#include <boost/gil.hpp>
#include <boost/gil/extension/io/jpeg.hpp>
#include <boost/gil/extension/numeric/sampler.hpp>
#include <boost/gil/extension/numeric/resample.hpp>
#include <boost/gil/extension/numeric/kernel.hpp>
#include <boost/gil/extension/numeric/convolve.hpp>

#include <cmath>

using namespace std;
using namespace boost::gil;

// value of strong edge pixel
static const int TOP_VALUE = 255;
// value of weak (maybe edge) pixel
static const int MIDDLE_VALUE = 150;
// value of definitely not-edge pixel
static const int BOTTOM_VALUE = 0;

// Example how to detect edges via Canny algorithm
// Read more about Canny edge detection algorithm on
// https://docs.opencv.org/3.1.0/da/d22/tutorial_py_canny.html

template <typename SrcView>
void gaussian_blur(const SrcView &src)
{
    //1-D Gaussian blur kernel with kernel size 5, Sigma 1.0
    float gaussian[] = {0.06136f, 0.24477f, 0.38774f, 0.24477f, 0.06136f};
    kernel_1d<float> kernel(gaussian, 5, 2);

    convolve_rows<gray32f_pixel_t>(src, kernel, src, convolve_option_output_ignore);
    convolve_cols<gray32f_pixel_t>(src, kernel, src, convolve_option_output_ignore);
}

// Function calculates image gradient (intensity and direction)
// Smoothened image is filtered with a Sobel kernels in both horizontal and vertical
// direction to get first derivative in horizontal direction (Gx) and vertical direction (Gy).
// Gradient manitude is found as G = sqrt(Gx^2 + Gy^2)
// Direction is found as angle = arctan(Gy / Gx)
// More information about sobel filtering on https://en.wikipedia.org/wiki/Sobel_operator
void sobel_filtering(const gray8c_view_t &src,
                     const gray8_view_t &magnitude, const gray32f_view_t &slope)
{
    // This we used 1-D convolution
    // Gx = second * (first * img)
    // Gy = first * (second * img)
    // where * is convolution
    float first_sobel[] = {1.f, 0.f, -1.f};
    float second_sobel[] = {1.f, 2.f, 1.f};

    kernel_1d<float> first_sobel_kernel(first_sobel, 3, 1);
    kernel_1d<float> second_sobel_kernel(second_sobel, 3, 1);

    //16 bits signed matrix is used in order to avoid char overflow
    gray16s_image_t vertical(src.dimensions());
    gray16s_image_t horizontal(src.dimensions());

    convolve_rows<gray32f_pixel_t>(src, first_sobel_kernel,
                                   view(vertical), convolve_option_output_zero);
    convolve_cols<gray32f_pixel_t>(const_view(vertical), second_sobel_kernel,
                                   view(vertical), convolve_option_output_zero);

    convolve_rows<gray32f_pixel_t>(src, second_sobel_kernel, view(horizontal),
                                   convolve_option_output_zero);
    convolve_cols<gray32f_pixel_t>(const_view(horizontal), first_sobel_kernel,
                                   view(horizontal), convolve_option_output_zero);

    // Magnitude and angle calculation
    auto ver_it = view(vertical).begin();
    auto hor_it = view(horizontal).begin();
    auto slope_it = slope.begin();

    for (auto mag_it = magnitude.begin(); mag_it != magnitude.end();
            ++mag_it, ++ver_it, ++hor_it, ++slope_it) {

        // std::min was used to be sure, that pixel max value less than 256
        *mag_it = std::min(UINT8_MAX,
                           (int)std::sqrt(
                               std::pow((int)(*ver_it), 2) + std::pow((int)(*hor_it), 2)));

        *slope_it = (float)std::atan2((int)(*hor_it), (int)(*ver_it));
    }
}

// Non-maximum suppression method is perfomed to thin out edges of the image
// Method goes throug all the points on the gradient intensity matrix and
// finds the pixels with maximum value in the edge direction
void non_maximal_suppression(const gray8c_view_t &magnitude, const gray32fc_view_t &angle,
                             const gray8_view_t &dst)
{
    // Dst matrix fills with 0
    fill_pixels(dst, int8_t(0));

    auto mag_loc = magnitude.xy_at(1, 1);
    int q, r;
    int index;
    int curVal;
    for (int y = 1; y < magnitude.height() - 1; ++y) {
        auto dst_it = dst.row_begin(y);
        auto angle_it = angle.row_begin(y);

        for (int x = 1; x < dst.width() - 1; ++x, ++angle_it, ++dst_it, ++mag_loc.x()) {
            // Index helps to find direction of the edge
            // Pixel has 8 neighbors and 4 possible directions
            // (-1,-1) (0,-1) (1,-1)
            // (-1, 0) (0, 0) (1, 0)
            // (-1, 1) (0, 1) (1, 1)
            // On each direction 2 possible neighbors
            // If both of them is smaller than current value (0, 0),
            // then current value is saved in output image
            index = (int)((*angle_it)[0] * 8 / M_PI);
            index = (index < 0) ? index + 8 : index;

            curVal = mag_loc(0, 0);

            switch (index) {
            // Horizontal direction
            case 0:
            case 7:
            case 8:
                q = mag_loc(1, 0);
                r = mag_loc(-1, 0);
                break;
            // 45 degree
            case 1:
            case 2:
                q = mag_loc(1, 1);
                r = mag_loc(-1, -1);
                break;
            // Vertical direction
            case 3:
            case 4:
                q = mag_loc(0, 1);
                r = mag_loc(0, -1);
                break;
            // 135 degree
            case 5:
            case 6:
                q = mag_loc(1, -1);
                r = mag_loc(-1, 1);
                break;
            }

            if ((curVal >= q) && (curVal >= r)) {
                *dst_it = curVal;
            }
        }
        mag_loc += point2<std::ptrdiff_t>(-dst.width() + 2, 1);
    }
}

void hysteresis_threshold(const gray8_view_t &dst, int minVal, int maxVal)
{
    // Histogram calculation of 3 groups: strong pixel, weak pixel, definitely not-edge pixel
    for_each_pixel(dst, [minVal, maxVal](gray8_pixel_t &pixel) {
        pixel = (pixel < minVal) ? BOTTOM_VALUE :
                ((pixel > maxVal) ? TOP_VALUE : MIDDLE_VALUE);
    });

    // Weak pixels check
    // If weak pixel has a strong neighbor, then it is strong one
    // otherwise it is definitely not-edge pixel
    auto dst_loc = dst.xy_at(1, 1);
    for (int i = 1; i < dst.height() - 1; ++i) {
        for (int j = 1; j < dst.width() - 1; ++j, ++dst_loc.x()) {
            if (dst_loc(0, 0) == MIDDLE_VALUE) {
                if (dst_loc(-1, -1) == TOP_VALUE || dst_loc(-1, 0) == TOP_VALUE ||
                        dst_loc(-1, 1) == TOP_VALUE || dst_loc(1, -1) == TOP_VALUE ||
                        dst_loc(1, 0) == TOP_VALUE || dst_loc(1, 1) == TOP_VALUE ||
                        dst_loc(0, -1) == TOP_VALUE || dst_loc(0, 1) == TOP_VALUE)
                    dst_loc(0, 0) = TOP_VALUE;
                else
                    dst_loc(0, 0) = BOTTOM_VALUE;
            }
        }
        dst_loc += point2<std::ptrdiff_t>(-dst.width() + 2, 1);
    }
}

void canny_edge_detection(const rgb8c_view_t &src,
                          const gray8_view_t &dst, int minVal, int maxVal)
{
    // Canny edge detection algorithm works on grayscale images only
    gray8_image_t gray_img(src.dimensions());
    copy_pixels(color_converted_view<gray8_pixel_t>(src), view(gray_img));

    gaussian_blur(view(gray_img));

    gray32f_image_t angle(src.dimensions());
    gray8_image_t magnitude(src.dimensions());
    sobel_filtering(view(gray_img), view(magnitude), view(angle));

    non_maximal_suppression(view(magnitude), view(angle), dst);

    hysteresis_threshold(dst, minVal, maxVal);
}

int main(int argc, char *argv[])
{
    char *input = "test.jpg";
    char *output = "canny.jpg";
    int min_threshold_value = 30;
    int max_threshold_value = 70;

    if (argc >= 2 && (argv[1][0] == '-') && (argv[1][1] == 'H' || argv[1][1] == 'h')) {
        printf("canny [path_to_input [path_to_output "
               "[min_threshold_value [max_threshold_value]]]]\n");
        return 0;
    }
    if (argc >= 2) input = argv[1];
    if (argc >= 3) output = argv[2];
    if (argc >= 4) min_threshold_value = atoi(argv[3]);
    if (argc >= 5) max_threshold_value = atoi(argv[4]);

    if (min_threshold_value > max_threshold_value)
        std::swap(min_threshold_value, max_threshold_value);

    rgb8_image_t img;
    read_image(input, img, jpeg_tag{});

    gray8_image_t res_img(img.dimensions());
    canny_edge_detection(const_view(img), view(res_img),
                         min_threshold_value, max_threshold_value);
    write_view(output, view(res_img), jpeg_tag{});
    return 0;
}


/**
 * @file
 * @brief Solution of source-free heat equation and computation of H1
 *  	  seminorms on different triangular meshes and refinement levels
 * @author Julien Gacon, Amélie Loher
 * @date   March 2019
 */

#include "unstablebvp.h"
// General includes
#include <array>
#include <fstream>
#include <memory>
#include <string>
// Eigen
#include <Eigen/Core>
#include <Eigen/SparseLU>
// Lehrfempp
#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/uscalfe/uscalfe.h>

namespace UnstableBVP {

std::shared_ptr<lf::refinement::MeshHierarchy> createMeshHierarchy(
    const int reflevels, const std::string &mesh_type) {
  // Helper object: mesh factory
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // Decide where the triangular domain should be located in x_2 direction
  // by adding an offset to the x_2 coordinate of the nodes
  double offset = 0;
  if (mesh_type == "top") {
    offset = 1.5;
  } else if (mesh_type == "bottom") {
    offset = -1.5;
  } else {
    // already at 0
  }

  // Define the nodes
  std::array<std::array<double, 2>, 3> node_coord{
      std::array<double, 2>({0.5, -0.5 + offset}),
      std::array<double, 2>({0, 0.5 + offset}),
      std::array<double, 2>({1, 0.5 + offset})};

  for (const auto &node : node_coord) {
    mesh_factory_ptr->AddPoint(Eigen::Vector2d({node[0], node[1]}));
  }

  // Initialize triangle
  mesh_factory_ptr->AddEntity(lf::base::RefEl::kTria(),
                              std::vector<lf::base::size_type>({0, 1, 2}),
                              std::unique_ptr<lf::geometry::Geometry>(nullptr));

  // Get a pointer to the mesh
  std::shared_ptr<lf::mesh::Mesh> mesh_p = mesh_factory_ptr->Build();

  // (optional) Print information about the mesh
  // std::cout << "   Mesh info\n" << *mesh_p;

  // Ask LehrFEM++ to create a hierarchy of nested meshes
  std::shared_ptr<lf::refinement::MeshHierarchy> multi_mesh_p =
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(mesh_p,
                                                              reflevels);

  return multi_mesh_p;
}

double solveTemperatureDistribution(
    std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  // **********************************************************************
  // Stage 0: provide all coefficient functions mainly through lambda
  //          functions and derived MeshFunctions
  // **********************************************************************

  // The boundary condition
  auto bc = [](Eigen::Vector2d x) -> double {
    return x[1] <= 0 ? 1 - x[1] : 0;
  };
  // Wrap into a MeshFunction
  lf::mesh::utils::MeshFunctionGlobal mf_bc{bc};

  // We use lowest-order (p.w. linear Lagrangian finite elements), for which
  // LehrFEM++ provides a built-in description according to the paradigm of
  // parametric finite elements.
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  // Reference to current mesh
  const lf::mesh::Mesh &mesh{*(fe_space->Mesh())};
  // Obtain local->global index mapping for current finite element space
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};

  // **********************************************************************
  // Stage 1: Assemble finite element Galerkin matrix
  // **********************************************************************

  // Dimension of finite element space`
  const lf::base::size_type N_dofs(dofh.NumDofs());
  // Matrix in triplet format holding Galerkin matrix, zero initially.
  lf::assemble::COOMatrix<double> A(N_dofs, N_dofs);

  // Element matrix builder for the negative Laplacian
  lf::uscalfe::LinearFELaplaceElementMatrix elmat_builder{};

  // Invoke assembly on cells (co-dimension = 0 as first argument)
  // Information about the mesh and the local-to-global map is passed through
  // a Dofhandler object, argument 'dofh'. This function call adds triplets to
  // the internal COO-format representation of the sparse matrix A.
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elmat_builder, A);

  // **********************************************************************
  // Stage 2: Right-hand side vector
  // **********************************************************************

  // Define RHS vector
  // No source, hence it is simply zero
  Eigen::Matrix<double, Eigen::Dynamic, 1> phi(N_dofs);
  phi.setZero();

  // **********************************************************************
  // Stage 3: Fixing solution components according to essential (Dirichlet)
  //          boundary conditions
  // **********************************************************************

  // Obtain specification for shape functions on edges
  std::shared_ptr<const lf::uscalfe::ScalarReferenceFiniteElement<double>>
      rsf_edge_p = fe_space->ShapeFunctionLayout(lf::base::RefEl::kSegment());
  LF_ASSERT_MSG(rsf_edge_p != nullptr, "FE specification for edges missing");

  // Obtain an array of boolean flags for the edges (codim 1) of the mesh,
  // `true` indicates that the edge lies on the boundary
  auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(fe_space->Mesh(), 1)};

  // Fetch flags and values for degrees of freedom located on Dirichlet
  // edges.
  auto ess_bdc_flags_values{lf::uscalfe::InitEssentialConditionFromFunction(
      dofh, *rsf_edge_p,
      [&bd_flags](const lf::mesh::Entity &edge) -> bool {
        return (bd_flags(edge));
      },
      mf_bc)};

  // Eliminate Dirichlet dofs from linear system
  lf::assemble::FixFlaggedSolutionComponents<double>(
      [&ess_bdc_flags_values](lf::assemble::glb_idx_t gdof_idx) {
        return ess_bdc_flags_values[gdof_idx];
      },
      A, phi);

  // **********************************************************************
  // Stage 4: Solve LSE
  // **********************************************************************

  // Assembly completed: Convert COO matrix A into CRS format using Eigen's
  // internal conversion routines.
  Eigen::SparseMatrix<double> A_crs = A.makeSparse();

  // Solve linear system using Eigen's sparse direct elimination
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(A_crs);
  LF_VERIFY_MSG(solver.info() == Eigen::Success, "LU decomposition failed");
  Eigen::VectorXd sol_vec = solver.solve(phi);
  LF_VERIFY_MSG(solver.info() == Eigen::Success, "Solving LSE failed");

  // **********************************************************************
  // Stage 5: Compute H1 seminorm
  // **********************************************************************

  // Compute the difference to a function that's zero everywhere, hence
  // just the gradient of the solution (which is encapsulated in the fe_space).
  // We use this trick to avoid the manual computation and make use of the
  // LehrFEM facilities :)
  lf::uscalfe::MeshFunctionL2GradientDifference loc_comp(
      fe_space,
      lf::mesh::utils::MeshFunctionConstant(Eigen::Vector2d(0.0, 0.0)), 2);

  // Compute the norm of the ``difference'' (i.e. the norm of the gradient of
  // the solution)
  const double norm = lf::uscalfe::NormOfDifference(dofh, loc_comp, sol_vec);

  return norm;
}

}  // namespace UnstableBVP


#include "sv/util/eigen.h"

#include <Eigen/Cholesky>

#include "sv/util/logging.h"

namespace sv {

void StableRotateBlockTopLeft(MatrixXdRef H,
                              VectorXdRef b,
                              int block_ind,
                              int block_size) {
  CHECK_EQ(H.rows(), b.size());
  CHECK_EQ(H.cols(), b.size());
  CHECK_GE(block_ind, 0);
  CHECK_GT(block_size, 0);
  CHECK_LT(block_ind * block_size, H.rows());

  if (block_ind == 0) return;
  const auto n = block_size;

  // Permute block of rows up gradually, also need to permute b
  for (int i = block_ind; i > 0; --i) {
    const auto r = i * block_size;
    // swap current rows with above rows
    H.middleRows(r, n).swap(H.middleRows(r - n, n));
    b.segment(r, n).swap(b.segment(r - n, n));
  }

  // Permute block of cols
  for (int j = block_ind; j > 0; --j) {
    const auto c = j * block_size;
    // swap current cols with left cols
    H.middleCols(c, n).swap(H.middleCols(c - n, n));
  }
}

void FillLowerTriangular(MatrixXdRef M) {
  CHECK_EQ(M.rows(), M.cols());
  M.triangularView<Eigen::Lower>() =
      M.triangularView<Eigen::Upper>().transpose();
}

void FillUpperTriangular(MatrixXdRef M) {
  CHECK_EQ(M.rows(), M.cols());
  M.triangularView<Eigen::Upper>() =
      M.triangularView<Eigen::Lower>().transpose();
}

void MakeSymmetric(MatrixXdRef M) {
  CHECK_EQ(M.rows(), M.cols());
  M += M.transpose().eval();
  M.array() /= 2.0;
}

void MargTopLeftBlock(const MatrixXdCRef& Hf,
                      const VectorXdCRef& bf,
                      MatrixXdRef Hm,
                      VectorXdRef bm,
                      int dim) {
  // Pre-condition
  // 1. Hf is square and match bsc and symmetric
  // 2. Hm is quare and match bpr
  const auto nf = bf.size();
  const auto nm = bm.size();
  CHECK_GT(dim, 0);
  CHECK_EQ(nm + dim, nf);
  CHECK_EQ(Hf.rows(), nf);
  CHECK_EQ(Hf.cols(), nf);
  CHECK_EQ(Hm.rows(), nm);
  CHECK_EQ(Hm.cols(), nm);
  CHECK_EQ(Hf, Hf.transpose()) << "\n" << Hf;

  // Hf                     bf
  // [ H00 H01 ] [ x0 ] = [ b0 ]
  // [ H10 H11 ] [ x1 ] = [ b1 ]
  // Hm = H11 - H10 * H00^-1 * H01
  // bm =  b1 - H10 * H00^-1 * b0
  const auto H01 = Hf.topRightCorner(dim, nm);
  const auto H10 = Hf.bottomLeftCorner(nm, dim);

  // Benchmark shows that simply inverse has similar speed as llt
  // However to account for rank-deficiency we use ldlt to inverse
  const auto H00_inv = Hf.topLeftCorner(dim, dim)
                           .selfadjointView<Eigen::Lower>()
                           .ldlt()
                           .solve(Eigen::MatrixXd::Identity(dim, dim))
                           .eval();

  Hm = Hf.bottomRightCorner(nm, nm);
  Hm.noalias() -= H10 * H00_inv * H01;

  const auto b0 = bf.head(dim);
  bm = bf.tail(nm);  // b1
  bm.noalias() -= H10 * (H00_inv * b0);

  // Make sure Hpr is symmetric
  MakeSymmetric(Hm);

  // Post-condition
  // 1. Hpr shape doesn't change
  // 2. Hpr is symmetric
  CHECK_EQ(Hm.rows(), nm);
  CHECK_EQ(Hm.cols(), nm);
  CHECK_EQ(Hm, Hm.transpose()) << "\n" << Hm;
}

}  // namespace sv


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_TWOTOM10_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_TWOTOM10_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate 2 to the power -10 (\f$2^{-10}\f$)

    @par Header <boost/simd/constant/twotom10.hpp>

    @par Semantic:

    @code
    T r = Twotom10<T>();
    @endcode

    is similar to:

    @code
    T r = pow(2, -10);
    @endcode

    @return The Twotom10 constant for the proper type
  **/
  template<typename T> T Twotom10();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant twotom10.

      @return The Twotom10 constant for the proper type
    **/
    const boost::dispatch::functor<tag::twotom10_> twotom10 = {};
  }
} }
#endif

#include <boost/simd/constant/scalar/twotom10.hpp>
#include <boost/simd/constant/simd/twotom10.hpp>

#endif


/*
 * Copyright (c) 2013-2015 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef NEWTON_HPP
#define NEWTON_HPP

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <kv/autodif.hpp>
#include <limits>
#include <boost/random.hpp>
#include <ctime>
#include <cmath>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
bool
newton(F f, ub::vector<T>& x, T epsilon = std::numeric_limits<T>::epsilon(), int maxloop = 100)
{
	int s = x.size();
	int i, j, r;
	ub::vector<T> fx;
	ub::matrix<T> fdx;
	T norm1, norm2;

	for (i=0; i<maxloop; i++) {
		try {
			autodif<T>::split(f(autodif<T>::init(x)), fx, fdx);
			ub::permutation_matrix<> pm(s);
			r = ub::lu_factorize(fdx, pm);
			if (r != 0) return false;
			ub::lu_substitute(fdx, pm, fx);
		}
		catch (...) {
			return false;
		}

		norm1 = 1.;
		norm2 = 0.;
		for (j=0; j<s; j++) {
			using std::abs;
			norm1 = std::max(norm1, abs(x(j)));
			norm2 = std::max(norm2, abs(fx(j)));
		}

		x = x - fx;
		if (norm2 <= norm1 * epsilon) return true;
	}
	return false;
}

template <class T, class F>
bool
newton_random(F f, ub::vector<T>& x, T epsilon = std::numeric_limits<T>::epsilon(), int maxloop = 100)
{
	int s = x.size();
	int i;

	using namespace boost;
	// use "static" to be "randomized" only once
	static variate_generator< mt19937, normal_distribution<> > rand (mt19937(time(0)), normal_distribution<>(0., 10.));

	for (i=0; i<s; i++) x(i) = rand();
	return newton(f, x, epsilon, maxloop);
}

} // namespace kv

#endif // NEWTON_HPP


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

#include <nt2/core/functions/freqspace.hpp>
#include <nt2/include/functions/freqspace1.hpp>
#include <nt2/include/functions/colon.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/functions/scalar/floor.hpp>
#include <nt2/include/functions/scalar/rec.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/options.hpp>
#include <boost/mpl/bool.hpp>

namespace nt2 { namespace ext
{
  //============================================================================
  // This version of freqspace is called whenever a tie(...) = freqspace(...) is
  // captured before assign is resolved. As a tieable function, freqspace
  // retrieves rhs/lhs pair as inputs
  //============================================================================
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::freqspace_, tag::cpu_
                              , (A0)(N0)(A1)(N1)
                              , ((node_ < A0, nt2::tag::freqspace_
                                        , N0, nt2::container::domain
                                        >
                                ))
                                ((node_ < A1, nt2::tag::tie_
                                        , N1, nt2::container::domain
                                        >
                                ))
                            )
  {
    typedef void                                                    result_type;
    typedef typename boost::proto::result_of::child_c<A0&,0>::type       child0;
    typedef typename boost::proto::result_of::child_c<A1&,0>::type       child1;
    typedef typename boost::dispatch::meta::
            terminal_of< typename boost::dispatch::meta::
                         semantic_of<child0>::type
                       >::type                                            in0_t;
    typedef typename boost::dispatch::meta::
            terminal_of< typename boost::dispatch::meta::
                         semantic_of<child1>::type
                       >::type                                            out_t;

    typedef typename out_t::value_type                                  value_t;

    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      int n = 0, m = 0;
      bool whole =  false;
      bool meshgrid = false;
      getmn(a0, m, n, whole, meshgrid, N0(), N1() );
      compute(a1, m, n, whole, meshgrid, N1());
    }

    private:
    BOOST_FORCEINLINE void compute( A1 & a1, int m, int, bool whole
                                  , bool, boost::mpl::long_<1> const&
                                  ) const
    {
      if (whole)
        boost::proto
             ::child_c<0>(a1) = freqspace1(m,nt2::whole_,meta::as_<value_t>());
      else
        boost::proto::child_c<0>(a1) = freqspace1(m, meta::as_<value_t>());
    }

    void compute( A1 & a1, int m, int n, bool
                , bool /*meshgrid*/, boost::mpl::long_<2> const&
                ) const
    {
      value_t hvm = m*nt2::Half<value_t>();
      value_t hvn = n*nt2::Half<value_t>();
      value_t hm = nt2::rec(hvm);
      value_t hn = nt2::rec(hvn);
      value_t lm = -nt2::floor(hvm)*hm;
      value_t ln = -nt2::floor(hvn)*hn;

      // TODO: implement support for meshgrid option
      //if (meshgrid)
      // {
      boost::proto::child_c<0>(a1) = nt2::_(ln, hn, value_t(1)-value_t(2)/n);
      boost::proto::child_c<1>(a1) = nt2::_(lm, hm, value_t(1)-value_t(1)/m);
      // }
      // else
      // {
      //   boost::proto::child_c<0>(a1) = ??;
      //   boost::proto::child_c<1>(a1) = ??;
      // }
    }

    BOOST_FORCEINLINE  //[f]       = freqspace(n)
    void getmn(A0 const &a0, int &m,  int& n, bool&, bool&,
               boost::mpl::long_<3> const &,//number of inputs
               boost::mpl::long_<1> const &//number of outputs
               ) const
    {
      m = int(boost::proto::value(boost::proto::child_c<1>(a0)));
      n = 0;
    }

    BOOST_FORCEINLINE  //[f1, f2]       = freqspace(n)
    void getmn(A0 const &a0, int &m,  int& n, bool&, bool&
              , boost::mpl::long_<3> const &    //number of inputs
              , boost::mpl::long_<2> const &
              ) const//number of outputs
    {
      typedef typename boost::proto::result_of::child_c<A0&,1>::type child1;
      typedef typename boost::proto::result_of::value<child1>::type  type_t;
      typedef typename meta::is_scalar<type_t>::type               choice_t;
      m = getval(boost::proto::value(boost::proto::child_c<1>(a0)),0,choice_t());
      n = getval(boost::proto::value(boost::proto::child_c<1>(a0)),1,choice_t());
    }

    template < class T > static int getval(const T & a0, int,
                                           const boost::mpl::bool_<true> &)
      { return a0; }

    template < class T > static int getval(const T & a0, int i,
                                           const boost::mpl::bool_<false>  &)
      {return a0[i]; }

    BOOST_FORCEINLINE //[f]       = freqspace(n, whole_)
      void getmn( A0 const &a0, int &m, int& n, bool &whole, bool&
                , boost::mpl::long_<4> const &  //number of inputs
                , boost::mpl::long_<1> const &  //number of outputs
                ) const
    {
      m = int(boost::proto::value(boost::proto::child_c<1>(a0)));
      n = 0;
      whole =  true;
    }

    BOOST_FORCEINLINE //[f,g]       = freqspace(n, whole_)
    void getmn( A0 const &a0, int &m,  int& n, bool &whole, bool&
              , boost::mpl::long_<4> const &  //number of inputs
              , boost::mpl::long_<2> const &  //number of outputs
              ) const
    {
      m = int(boost::proto::value(boost::proto::child_c<1>(a0)));
      n = 0;
      whole =  true;
    }

    template < class Dummy >
    BOOST_FORCEINLINE // [f1, f2]  = freqspace([m, n])
    void getmn( A0 const &a0, int &m,  int& n, bool&, bool&
              , boost::mpl::long_<3> const &  //number of inputs
              , boost::mpl::long_<2> const &  //number of outputs
              , Dummy()
              ) const
    {
      typedef typename boost::proto::result_of::child_c<A0&,1>::type child1;
      typedef typename boost::proto::result_of::value<child1>::type  type_t;
      typedef typename meta::is_scalar<type_t>::type               choice_t;
      m = getval(boost::proto::value(boost::proto::child_c<1>(a0)),0,choice_t());
      n = getval(boost::proto::value(boost::proto::child_c<1>(a0)),1,choice_t());
    }

    template < class Dummy >
    BOOST_FORCEINLINE // [f1, f2]  = freqspace([m, n], meshgrid_)
    void getmn( A0 const &a0, int &m,  int& n, bool&, bool& meshgrid
              , boost::mpl::long_<4> const &  //number of inputs
              , boost::mpl::long_<2> const &  //number of outputs
              , Dummy()
              ) const
    {
      typedef typename boost::proto::result_of::child_c<A0&,1>::type child1;
      typedef typename boost::proto::result_of::value<child1>::type  type_t;
      typedef typename meta::is_scalar<type_t>::type               choice_t;
      m = getval(boost::proto::value(boost::proto::child_c<1>(a0)),0,choice_t());
      n = getval(boost::proto::value(boost::proto::child_c<1>(a0)),1,choice_t());
      meshgrid = true;
    }
  };
} }

#endif


#include <boost/test/unit_test.hpp>

#include <askr/utility/crc.h>
#include <string_view>

constexpr uint32_t crc32(std::string_view str)
{
    askr::crc<32> crc;
    crc.update(str);
    return crc.checksum();
}

BOOST_AUTO_TEST_SUITE(CRC)

BOOST_AUTO_TEST_CASE(CompileTimeTest)
{
    static_assert(crc32("123456789") == 0xCBF43926);
}

BOOST_AUTO_TEST_SUITE_END()


#include "include_and_types.cpp"
#include <set>
#include "my_transitive_closure.cpp"
#include <boost/graph/graphml.hpp>
#include <boost/property_map/dynamic_property_map.hpp>
#include <boost/graph/transitive_closure.hpp>

//Ad-hoc typedef interator_property_map types
typedef boost::iterator_property_map<__gnu_cxx::__normal_iterator<long unsigned int*, std::vector<long unsigned int> >,
        boost::vec_adj_list_vertex_id_map<boost::no_property, long unsigned int>,
                long unsigned int, long unsigned int&> typeVertex;

typedef boost::iterator_property_map<__gnu_cxx::__normal_iterator<std::set<long unsigned int>**,
        std::vector<std::set<long unsigned int>*> >,
        boost::vec_adj_list_vertex_id_map<boost::no_property, long unsigned int>, std::set<long unsigned int>*,
                std::set<long unsigned int>*&> typeSetVertex;

using namespace std;

int main(int, char*[]){

    //Reading graph from stdin
    Graph g;
    dynamic_properties dp;
    read_graphml(std::cin, g, dp);

    //Graph printing
    std::cout << "A directed graph:" << std::endl;
    print_graph(g, get(vertex_index, g));
    std::cout << std::endl;

    //Declaration of TransitiveClosure object
    TransitiveClosure <typeVertex, typeBool, typeInt, typeSetVertex> transitive_cl(&g);
    transitive_cl.transitive_closure_scc();

    //used only to compare results. This applies the built-in function of Boost for applying transitive closure
    std::cout << "BGL"<<std::endl;
    Graph gt;
    transitive_closure(g,gt);
    print_graph(gt, get(vertex_index,gt));

    return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IS_NOT_INFINITE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IS_NOT_INFINITE_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/true.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/is_not_equal.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF(is_not_infinite_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::integer_<A0>, X>
                          )
   {
     using result = bs::as_logical_t<A0>;
     BOOST_FORCEINLINE result operator()(const A0&) const BOOST_NOEXCEPT
     {
       return bs::True<result>();
     }
   };

   BOOST_DISPATCH_OVERLOAD_IF(is_not_infinite_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE bs::as_logical_t<A0> operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return is_not_equal(bs::abs(a0),bs::Inf<A0>());
      }
   };

} } }
#endif


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/ButterworthHPFilter.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../util/SlideUDFilter.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cmath>

namespace fluid {
namespace algorithm {

class EnvelopeGate
{

  using ArrayXd = Eigen::ArrayXd;

public:
  EnvelopeGate(index maxSize)
  {
    mInputStorage = ArrayXd(maxSize);
    mOutputStorage = ArrayXd(maxSize);
  }

  void init(double onThreshold, double offThreshold, double hiPassFreq,
            index minTimeAboveThreshold, index upwardLookupTime,
            index minTimeBelowThreshold, index downwardLookupTime)
  {
    using namespace std;

    mMinTimeAboveThreshold = minTimeAboveThreshold;
    mUpwardLookupTime = upwardLookupTime;
    mMinTimeBelowThreshold = minTimeBelowThreshold,
    mDownwardLookupTime = downwardLookupTime;
    mDownwardLatency = max<index>(minTimeBelowThreshold, mDownwardLookupTime);
    mLatency = max<index>(mMinTimeAboveThreshold + mUpwardLookupTime,
                          mDownwardLatency);
    if (mLatency < 0) mLatency = 1;
    mHiPassFreq = hiPassFreq;
    initFilters(mHiPassFreq);
    double initVal = min(onThreshold, offThreshold) - 1;
    initBuffers(initVal);
    mSlide.init(initVal);
    mInputState = false;
    mOutputState = false;
    mOnStateCount = 0;
    mOffStateCount = 0;
    mEventCount = 0;
    mSilenceCount = 0;
    mInitialized = true;
  }

  double processSample(const double in, double onThreshold, double offThreshold,
                       index rampUpTime, index rampDownTime, double hiPassFreq,
                       index minEventDuration, index minSilenceDuration)
  {
    using namespace std;
    assert(mInitialized);

    mSlide.updateCoeffs(rampUpTime, rampDownTime);

    double filtered = in;
    if (hiPassFreq != mHiPassFreq)
    {
      initFilters(hiPassFreq);
      mHiPassFreq = hiPassFreq;
    }
    if (mHiPassFreq > 0)
      filtered = mHiPass2.processSample(mHiPass1.processSample(in));

    double rectified = abs(filtered);
    double dB = 20 * log10(rectified);
    double floor = min(offThreshold, onThreshold) - 1;
    double clipped = max(dB, floor);
    double smoothed = mSlide.processSample(clipped);
    bool   forcedState = false;

    // case 1: we are waiting for event to finish
    if (mOutputState && mEventCount > 0)
    {
      if (mEventCount >= minEventDuration) { mEventCount = 0; }
      else
      {
        forcedState = true;
        mOutputBuffer(mLatency - 1) = 1;
        mEventCount++;
      }
      // case 2: we are waiting for silence to finish
    }
    else if (!mOutputState && mSilenceCount > 0)
    {
      if (mSilenceCount >= minSilenceDuration) { mSilenceCount = 0; }
      else
      {
        forcedState = true;
        mOutputBuffer(mLatency - 1) = 0;
        mSilenceCount++;
      }
    }
    // case 3: need to compute state
    if (!forcedState)
    {
      bool nextState = mInputState;
      if (!mInputState && smoothed >= onThreshold) { nextState = true; }
      if (mInputState && smoothed <= offThreshold) { nextState = false; }
      updateCounters(nextState);
      // establish and refine
      if (!mOutputState && mOnStateCount >= mMinTimeAboveThreshold &&
          mFillCount >= mLatency)
      {
        index onsetIndex =
            refineStart(mLatency - mMinTimeAboveThreshold - mUpwardLookupTime,
                        mUpwardLookupTime);
        mOutputBuffer.segment(onsetIndex, mLatency - onsetIndex) = 1;
        mEventCount = mOnStateCount;
        mOutputState = true; // we are officially on
      }
      else if (mOutputState && mOffStateCount >= mDownwardLatency &&
               mFillCount >= mLatency)
      {

        index offsetIndex =
            refineStart(mLatency - mDownwardLatency, mDownwardLookupTime);
        mOutputBuffer.segment(offsetIndex, mLatency - offsetIndex) = 0;
        mSilenceCount = mOffStateCount;
        mOutputState = false; // we are officially off
      }

      mOutputBuffer(mLatency - 1) = mOutputState ? 1 : 0;
      mInputState = nextState;
    }
    if (mLatency > 1)
    {
      mOutputBuffer.segment(0, mLatency - 1) =
          mOutputBuffer.segment(1, mLatency - 1);

      mInputBuffer.segment(0, mLatency - 1) =
          mInputBuffer.segment(1, mLatency - 1);
    }
    mInputBuffer(mLatency - 1) = smoothed;
    if (mFillCount < mLatency) mFillCount++;
    return mOutputBuffer(0);
  }

  index getLatency() { return mLatency; }
  bool  initialized() { return mInitialized; }

private:
  void initBuffers(double initialValue)
  {
    using namespace std;
    mInputBuffer = mInputStorage.segment(0, max<index>(mLatency, 1))
                       .setConstant(initialValue);
    mOutputBuffer =
        mOutputStorage.segment(0, max<index>(mLatency, 1)).setZero();
    mInputState = false;
    mOutputState = false;
    mFillCount = max<index>(mLatency, 1);
  }

  void initFilters(double cutoff)
  {
    mHiPass1.init(cutoff);
    mHiPass2.init(cutoff);
  }

  index refineStart(index start, index nSamples)
  {
    if (nSamples < 2) return start + nSamples;
    ArrayXd        seg = mInputBuffer.segment(start, nSamples);
    ArrayXd::Index index;
    seg.minCoeff(&index);
    return start + index;
  }

  void updateCounters(bool nextState)
  {
    if (!mInputState && nextState)
    {
      mOffStateCount = 0;
      mOnStateCount = 1;
    }
    else if (mInputState && !nextState)
    {
      mOnStateCount = 0;
      mOffStateCount = 1;
    }
    else if (mInputState && nextState)
    {
      mOnStateCount++;
    }
    else if (!mInputState && !nextState)
    {
      mOffStateCount++;
    }
  }

  index  mLatency;
  index  mFillCount;
  double mHiPassFreq{0};

  index mMinTimeAboveThreshold{440};
  index mDownwardLookupTime{10};
  index mDownwardLatency;
  index mMinTimeBelowThreshold{10};
  index mUpwardLookupTime{24};

  ArrayXd mInputBuffer;
  ArrayXd mOutputBuffer;
  ArrayXd mInputStorage;
  ArrayXd mOutputStorage;

  bool mInputState{false};
  bool mOutputState{false};

  index mOnStateCount{0};
  index mOffStateCount{0};
  index mEventCount{0};
  index mSilenceCount{0};
  bool  mInitialized{false};

  ButterworthHPFilter mHiPass1;
  ButterworthHPFilter mHiPass2;
  SlideUDFilter       mSlide;
};
} // namespace algorithm
} // namespace fluid


// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  C++ sensor fusion/filtering code that uses flexkalman
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @ingroup aux_tracking
 */

#pragma once

#ifndef __cplusplus
#error "This header is C++-only."
#endif

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "flexkalman/AugmentedProcessModel.h"
#include "flexkalman/AugmentedState.h"
#include "flexkalman/BaseTypes.h"
#include "flexkalman/PoseState.h"

namespace xrt::auxiliary::tracking {

namespace types = flexkalman::types;
using flexkalman::types::Vector;

//! For things like accelerometers, which on some level measure the local vector
//! of a world direction.
template <typename State>
class WorldDirectionMeasurement : public flexkalman::MeasurementBase<WorldDirectionMeasurement<State>>
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	static constexpr size_t Dimension = 3;
	using MeasurementVector = types::Vector<Dimension>;
	using MeasurementSquareMatrix = types::SquareMatrix<Dimension>;
	WorldDirectionMeasurement(types::Vector<3> const &direction,
	                          types::Vector<3> const &reference,
	                          types::Vector<3> const &variance)
	    : direction_(direction.normalized()), reference_(reference.normalized()), covariance_(variance.asDiagonal())
	{}

	MeasurementSquareMatrix const &
	getCovariance(State const & /*s*/)
	{
		return covariance_;
	}

	types::Vector<3>
	predictMeasurement(State const &s) const
	{
		return s.getCombinedQuaternion() * reference_;
	}

	MeasurementVector
	getResidual(MeasurementVector const &predictedMeasurement, State const &s) const
	{
		return predictedMeasurement - reference_;
	}

	MeasurementVector
	getResidual(State const &s) const
	{
		return getResidual(predictMeasurement(s), s);
	}

private:
	types::Vector<3> direction_;
	types::Vector<3> reference_;
	MeasurementSquareMatrix covariance_;
};
#if 0
//! For things like accelerometers, which on some level measure the local vector
//! of a world direction.
class LinAccelWithGravityMeasurement
    : public flexkalman::MeasurementBase<LinAccelWithGravityMeasurement>
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	static constexpr size_t Dimension = 3;
	using MeasurementVector = types::Vector<Dimension>;
	using MeasurementSquareMatrix = types::SquareMatrix<Dimension>;
	LinAccelWithGravityMeasurement(types::Vector<3> const &direction,
	                          types::Vector<3> const &reference,
	                          types::Vector<3> const &variance)
	    : direction_(direction), reference_(reference),
	      covariance_(variance.asDiagonal())
	{}

	// template <typename State>
	MeasurementSquareMatrix const &
	getCovariance(State const & /*s*/)
	{
		return covariance_;
	}

	// template <typename State>
	types::Vector<3>
	predictMeasurement(State const &s) const
	{
		return reference_;
	}

	// template <typename State>
	MeasurementVector
	getResidual(MeasurementVector const &predictedMeasurement,
	            State const &s) const
	{
		s.getQuaternion().conjugate() *
		        predictedMeasurement return predictedMeasurement -
		    reference_.normalized();
	}

	template <typename State>
	MeasurementVector
	getResidual(State const &s) const
	{
		MeasurementVector residual =
		    direction_ - reference_ * s.getQuaternion();
		return getResidual(predictMeasurement(s), s);
	}

private:
	types::Vector<3> direction_;
	types::Vector<3> reference_;
	MeasurementSquareMatrix covariance_;
};
#endif

class BiasedGyroMeasurement : public flexkalman::MeasurementBase<BiasedGyroMeasurement>
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	static constexpr size_t Dimension = 3;
	using MeasurementVector = types::Vector<Dimension>;
	using MeasurementSquareMatrix = types::SquareMatrix<Dimension>;
	BiasedGyroMeasurement(types::Vector<3> const &angVel, types::Vector<3> const &variance)
	    : angVel_(angVel), covariance_(variance.asDiagonal())
	{}

	template <typename State>
	MeasurementSquareMatrix const &
	getCovariance(State const & /*s*/)
	{
		return covariance_;
	}

	template <typename State>
	types::Vector<3>
	predictMeasurement(State const &s) const
	{
		return s.b().stateVector() + angVel_;
	}

	template <typename State>
	MeasurementVector
	getResidual(MeasurementVector const &predictedMeasurement, State const &s) const
	{
		return predictedMeasurement - s.a().angularVelocity();
	}

	template <typename State>
	MeasurementVector
	getResidual(State const &s) const
	{
		return getResidual(predictMeasurement(s), s);
	}

private:
	types::Vector<3> angVel_;
	MeasurementSquareMatrix covariance_;
};
/*!
 * For PS Move-like things, where there's a directly-computed absolute position
 * that is not at the tracked body's origin.
 */
class AbsolutePositionLeverArmMeasurement : public flexkalman::MeasurementBase<AbsolutePositionLeverArmMeasurement>
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	using State = flexkalman::pose_externalized_rotation::State;
	static constexpr size_t Dimension = 3;
	using MeasurementVector = types::Vector<Dimension>;
	using MeasurementSquareMatrix = types::SquareMatrix<Dimension>;

	/*!
	 * @todo the point we get from the camera isn't the center of the ball,
	 * but the center of the visible surface of the ball - a closer
	 * approximation would be translation along the vector to the center of
	 * projection....
	 */
	AbsolutePositionLeverArmMeasurement(MeasurementVector const &measurement,
	                                    MeasurementVector const &knownLocationInBodySpace,
	                                    MeasurementVector const &variance)
	    : measurement_(measurement), knownLocationInBodySpace_(knownLocationInBodySpace),
	      covariance_(variance.asDiagonal())
	{}

	MeasurementSquareMatrix const &
	getCovariance(State const & /*s*/)
	{
		return covariance_;
	}

	types::Vector<3>
	predictMeasurement(State const &s) const
	{
		return s.getIsometry() * knownLocationInBodySpace_;
	}

	MeasurementVector
	getResidual(MeasurementVector const &predictedMeasurement, State const & /*s*/) const
	{
		return measurement_ - predictedMeasurement;
	}

	MeasurementVector
	getResidual(State const &s) const
	{
		return getResidual(predictMeasurement(s), s);
	}

private:
	MeasurementVector measurement_;
	MeasurementVector knownLocationInBodySpace_;
	MeasurementSquareMatrix covariance_;
};

} // namespace xrt::auxiliary::tracking


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


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2006, 2007, 2015 Ferdinando Ametrano
 Copyright (C) 2006 Cristina Duminuco
 Copyright (C) 2007 Giorgio Facchinetti
 Copyright (C) 2015 Paolo Mazzocchi

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef quantlib_abcdcalibration_hpp
#define quantlib_abcdcalibration_hpp

#include <ql/math/optimization/endcriteria.hpp>
#include <ql/math/optimization/projectedcostfunction.hpp>
#include <ql/math/array.hpp>

#include <boost/shared_ptr.hpp>

#include <vector>

namespace QuantLib {

    class Quote;
    class OptimizationMethod;
    class ParametersTransformation;

    class AbcdCalibration {
      private:
        class AbcdError : public CostFunction {
          public:
            AbcdError(AbcdCalibration* abcd) : abcd_(abcd) {}

            Real value(const Array& x) const {
                const Array y = abcd_->transformation_->direct(x);
                abcd_->a_ = y[0];
                abcd_->b_ = y[1];
                abcd_->c_ = y[2];
                abcd_->d_ = y[3];
                return abcd_->error();
            }
            Disposable<Array> values(const Array& x) const {
                const Array y = abcd_->transformation_->direct(x);
                abcd_->a_ = y[0];
                abcd_->b_ = y[1];
                abcd_->c_ = y[2];
                abcd_->d_ = y[3];
                return abcd_->errors();
            }
          private:
            AbcdCalibration* abcd_;
        };

        class AbcdParametersTransformation : public ParametersTransformation {
          public:
            AbcdParametersTransformation() : y_(Array(4)) {}
            // to constrained <- from unconstrained
            Array direct(const Array& x) const;
            // to unconstrained <- from constrained
            Array inverse(const Array& x) const;
          private:
            mutable Array y_;
        };

      public:
        AbcdCalibration() {};
        AbcdCalibration(
             const std::vector<Real>& t,
             const std::vector<Real>& blackVols,
             Real aGuess = -0.06,
             Real bGuess =  0.17,
             Real cGuess =  0.54,
             Real dGuess =  0.17,
             bool aIsFixed = false,
             bool bIsFixed = false,
             bool cIsFixed = false,
             bool dIsFixed = false,
             bool vegaWeighted = false,
             const boost::shared_ptr<EndCriteria>& endCriteria
                      = boost::shared_ptr<EndCriteria>(),
             const boost::shared_ptr<OptimizationMethod>& method
                      = boost::shared_ptr<OptimizationMethod>());
        //! adjustment factors needed to match Black vols
        std::vector<Real> k(const std::vector<Real>& t,
                            const std::vector<Real>& blackVols) const;
        void compute();
        //calibration results
        Real value(Real x) const;
        Real error() const;
        Real maxError() const;
        Disposable<Array> errors() const;
        EndCriteria::Type endCriteria() const;
        Real a() const { return a_; }
        Real b() const { return b_; }
        Real c() const { return c_; }
        Real d() const { return d_; }
        bool aIsFixed_, bIsFixed_, cIsFixed_, dIsFixed_;
        Real a_, b_, c_, d_;
        boost::shared_ptr<ParametersTransformation> transformation_;
      private:
        // optimization method used for fitting
        mutable EndCriteria::Type abcdEndCriteria_;
        boost::shared_ptr<EndCriteria> endCriteria_;
        boost::shared_ptr<OptimizationMethod> optMethod_;
        mutable std::vector<Real> weights_;
        bool vegaWeighted_;
        //! Parameters
        std::vector<Real> times_, blackVols_;
    };

}

/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2006, 2015 Ferdinando Ametrano
 Copyright (C) 2006 Cristina Duminuco
 Copyright (C) 2005, 2006 Klaus Spanderen
 Copyright (C) 2007 Giorgio Facchinetti
 Copyright (C) 2015 Paolo Mazzocchi

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/math/optimization/method.hpp>
#include <ql/math/optimization/constraint.hpp>
#include <ql/math/optimization/levenbergmarquardt.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/termstructures/volatility/abcd.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/interpolations/abcdinterpolation.hpp>

namespace QuantLib {

    // to constrained <- from unconstrained
    inline Array AbcdCalibration::AbcdParametersTransformation::direct(const Array& x) const {
        y_[1] = x[1];
        y_[2] = std::exp(x[2]);
        y_[3] = std::exp(x[3]);
        y_[0] = std::exp(x[0]) - y_[3];
        return y_;
    }

    // to unconstrained <- from constrained
    inline Array AbcdCalibration::AbcdParametersTransformation::inverse(const Array& x) const {
        y_[1] = x[1];
        y_[2] = std::log(x[2]);
        y_[3] = std::log(x[3]);
        y_[0] = std::log(x[0] + x[3]);
        return y_;
    }

    // to constrained <- from unconstrained

    inline AbcdCalibration::AbcdCalibration(
               const std::vector<Real>& t,
               const std::vector<Real>& blackVols,
               Real a, Real b, Real c, Real d,
               bool aIsFixed, bool bIsFixed, bool cIsFixed, bool dIsFixed,
               bool vegaWeighted,
               const boost::shared_ptr<EndCriteria>& endCriteria,
               const boost::shared_ptr<OptimizationMethod>& optMethod)
    : aIsFixed_(aIsFixed), bIsFixed_(bIsFixed),
      cIsFixed_(cIsFixed), dIsFixed_(dIsFixed),
      a_(a), b_(b), c_(c), d_(d),
      abcdEndCriteria_(EndCriteria::None), endCriteria_(endCriteria),
      optMethod_(optMethod), weights_(blackVols.size(), 1.0/blackVols.size()),
      vegaWeighted_(vegaWeighted),
      times_(t), blackVols_(blackVols) {

        AbcdMathFunction::validate(a, b, c, d);

        QL_REQUIRE(blackVols.size()==t.size(),
                       "mismatch between number of times (" << t.size() <<
                       ") and blackVols (" << blackVols.size() << ")");

        // if no optimization method or endCriteria is provided, we provide one
        if (!optMethod_) {
            Real epsfcn = 1.0e-8;
            Real xtol = 1.0e-8;
            Real gtol = 1.0e-8;
            bool useCostFunctionsJacobian = false;
            optMethod_ = boost::shared_ptr<OptimizationMethod>(new
                LevenbergMarquardt(epsfcn, xtol, gtol, useCostFunctionsJacobian));
        }
        if (!endCriteria_) {
            Size maxIterations = 10000;
            Size maxStationaryStateIterations = 1000;
            Real rootEpsilon = 1.0e-8;
            Real functionEpsilon = 0.3e-4;     // Why 0.3e-4 ?
            Real gradientNormEpsilon = 0.3e-4; // Why 0.3e-4 ?
            endCriteria_ = boost::shared_ptr<EndCriteria>(new
                EndCriteria(maxIterations, maxStationaryStateIterations,
                            rootEpsilon, functionEpsilon, gradientNormEpsilon));
        }
    }

    inline void AbcdCalibration::compute() {
        if (vegaWeighted_) {
            Real weightsSum = 0.0;
            for (Size i=0; i<times_.size() ; i++) {
                Real stdDev = std::sqrt(blackVols_[i]* blackVols_[i]* times_[i]);
                // when strike==forward, the blackFormulaStdDevDerivative becomes
                weights_[i] = CumulativeNormalDistribution().derivative(.5*stdDev);
                weightsSum += weights_[i];
            }
            // weight normalization
            for (Size i=0; i<times_.size() ; i++) {
                weights_[i] /= weightsSum;
            }
        }

        // there is nothing to optimize
        if (aIsFixed_ && bIsFixed_ && cIsFixed_ && dIsFixed_) {
            abcdEndCriteria_ = EndCriteria::None;
            //error_ = interpolationError();
            //maxError_ = interpolationMaxError();
            return;
        } else {

            AbcdError costFunction(this);
            transformation_ = boost::shared_ptr<ParametersTransformation>(new
                AbcdParametersTransformation);

            Array guess(4);
            guess[0] = a_;
            guess[1] = b_;
            guess[2] = c_;
            guess[3] = d_;

            std::vector<bool> parameterAreFixed(4);
            parameterAreFixed[0] = aIsFixed_;
            parameterAreFixed[1] = bIsFixed_;
            parameterAreFixed[2] = cIsFixed_;
            parameterAreFixed[3] = dIsFixed_;

            Array inversedTransformatedGuess(transformation_->inverse(guess));

            ProjectedCostFunction projectedAbcdCostFunction(costFunction,
                            inversedTransformatedGuess, parameterAreFixed);

            Array projectedGuess
                (projectedAbcdCostFunction.project(inversedTransformatedGuess));

            NoConstraint constraint;
            Problem problem(projectedAbcdCostFunction, constraint, projectedGuess);
            abcdEndCriteria_ = optMethod_->minimize(problem, *endCriteria_);
            Array projectedResult(problem.currentValue());
            Array transfResult(projectedAbcdCostFunction.include(projectedResult));

            Array result = transformation_->direct(transfResult);
            AbcdMathFunction::validate(a_, b_, c_, d_);
            a_ = result[0];
            b_ = result[1];
            c_ = result[2];
            d_ = result[3];

        }
    }

    inline Real AbcdCalibration::value(Real x) const {
        return abcdBlackVolatility(x,a_,b_,c_,d_);
    }

    inline std::vector<Real> AbcdCalibration::k(const std::vector<Real>& t,
                                         const std::vector<Real>& blackVols) const {
        QL_REQUIRE(blackVols.size()==t.size(),
               "mismatch between number of times (" << t.size() <<
               ") and blackVols (" << blackVols.size() << ")");
        std::vector<Real> k(t.size());
        for (Size i=0; i<t.size() ; i++) {
            k[i]=blackVols[i]/value(t[i]);
        }
        return k;
    }

    inline Real AbcdCalibration::error() const {
        Size n = times_.size();
        Real error, squaredError = 0.0;
        for (Size i=0; i<times_.size() ; i++) {
            error = (value(times_[i]) - blackVols_[i]);
            squaredError += error * error * weights_[i];
        }
        return std::sqrt(n*squaredError/(n-1));
    }

    inline Real AbcdCalibration::maxError() const {
        Real error, maxError = QL_MIN_REAL;
        for (Size i=0; i<times_.size() ; i++) {
            error = std::fabs(value(times_[i]) - blackVols_[i]);
            maxError = std::max(maxError, error);
        }
        return maxError;
    }

    // calculate weighted differences
    inline Disposable<Array> AbcdCalibration::errors() const {
        Array results(times_.size());
        for (Size i=0; i<times_.size() ; i++) {
            results[i] = (value(times_[i]) - blackVols_[i])* std::sqrt(weights_[i]);
        }
        return results;
    }

    inline EndCriteria::Type AbcdCalibration::endCriteria() const{
        return abcdEndCriteria_;
    }

}

#endif


//  (C) Copyright John Maddock 2005.
//  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)

#ifndef BOOST_MATH_COMPLEX_ASIN_INCLUDED
#define BOOST_MATH_COMPLEX_ASIN_INCLUDED

#ifndef BOOST_MATH_COMPLEX_DETAILS_INCLUDED
#  include <boost/math/complex/details.hpp>
#endif
#ifndef BOOST_MATH_LOG1P_INCLUDED
#  include <boost/math/special_functions/log1p.hpp>
#endif
#include <boost/assert.hpp>

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::sqrt; using ::fabs; using ::acos; using ::asin; using ::atan; using ::atan2; }
#endif

namespace boost{ namespace math{

template<class T>
inline std::complex<T> asin(const std::complex<T>& z)
{
   //
   // This implementation is a transcription of the pseudo-code in:
   //
   // "Implementing the complex Arcsine and Arccosine Functions using Exception Handling."
   // T E Hull, Thomas F Fairgrieve and Ping Tak Peter Tang.
   // ACM Transactions on Mathematical Software, Vol 23, No 3, Sept 1997.
   //

   //
   // These static constants should really be in a maths constants library:
   //
   static const T one = static_cast<T>(1);
   //static const T two = static_cast<T>(2);
   static const T half = static_cast<T>(0.5L);
   static const T a_crossover = static_cast<T>(1.5L);
   static const T b_crossover = static_cast<T>(0.6417L);
   static const T s_pi = boost::math::constants::pi<T>();
   static const T half_pi = s_pi / 2;
   static const T log_two = boost::math::constants::ln_two<T>();
   static const T quarter_pi = s_pi / 4;
#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127)
#endif
   //
   // Get real and imaginary parts, discard the signs as we can
   // figure out the sign of the result later:
   //
   T x = std::fabs(z.real());
   T y = std::fabs(z.imag());
   T real, imag;  // our results

   //
   // Begin by handling the special cases for infinities and nan's
   // specified in C99, most of this is handled by the regular logic
   // below, but handling it as a special case prevents overflow/underflow
   // arithmetic which may trip up some machines:
   //
   if((boost::math::isnan)(x))
   {
      if((boost::math::isnan)(y))
         return std::complex<T>(x, x);
      if((boost::math::isinf)(y))
      {
         real = x;
         imag = std::numeric_limits<T>::infinity();
      }
      else
         return std::complex<T>(x, x);
   }
   else if((boost::math::isnan)(y))
   {
      if(x == 0)
      {
         real = 0;
         imag = y;
      }
      else if((boost::math::isinf)(x))
      {
         real = y;
         imag = std::numeric_limits<T>::infinity();
      }
      else
         return std::complex<T>(y, y);
   }
   else if((boost::math::isinf)(x))
   {
      if((boost::math::isinf)(y))
      {
         real = quarter_pi;
         imag = std::numeric_limits<T>::infinity();
      }
      else
      {
         real = half_pi;
         imag = std::numeric_limits<T>::infinity();
      }
   }
   else if((boost::math::isinf)(y))
   {
      real = 0;
      imag = std::numeric_limits<T>::infinity();
   }
   else
   {
      //
      // special case for real numbers:
      //
      if((y == 0) && (x <= one))
         return std::complex<T>(std::asin(z.real()), z.imag());
      //
      // Figure out if our input is within the "safe area" identified by Hull et al.
      // This would be more efficient with portable floating point exception handling;
      // fortunately the quantities M and u identified by Hull et al (figure 3),
      // match with the max and min methods of numeric_limits<T>.
      //
      T safe_max = detail::safe_max(static_cast<T>(8));
      T safe_min = detail::safe_min(static_cast<T>(4));

      T xp1 = one + x;
      T xm1 = x - one;

      if((x < safe_max) && (x > safe_min) && (y < safe_max) && (y > safe_min))
      {
         T yy = y * y;
         T r = std::sqrt(xp1*xp1 + yy);
         T s = std::sqrt(xm1*xm1 + yy);
         T a = half * (r + s);
         T b = x / a;

         if(b <= b_crossover)
         {
            real = std::asin(b);
         }
         else
         {
            T apx = a + x;
            if(x <= one)
            {
               real = std::atan(x/std::sqrt(half * apx * (yy /(r + xp1) + (s-xm1))));
            }
            else
            {
               real = std::atan(x/(y * std::sqrt(half * (apx/(r + xp1) + apx/(s+xm1)))));
            }
         }

         if(a <= a_crossover)
         {
            T am1;
            if(x < one)
            {
               am1 = half * (yy/(r + xp1) + yy/(s - xm1));
            }
            else
            {
               am1 = half * (yy/(r + xp1) + (s + xm1));
            }
            imag = boost::math::log1p(am1 + std::sqrt(am1 * (a + one)));
         }
         else
         {
            imag = std::log(a + std::sqrt(a*a - one));
         }
      }
      else
      {
         //
         // This is the Hull et al exception handling code from Fig 3 of their paper:
         //
         if(y <= (std::numeric_limits<T>::epsilon() * std::fabs(xm1)))
         {
            if(x < one)
            {
               real = std::asin(x);
               imag = y / std::sqrt(-xp1*xm1);
            }
            else
            {
               real = half_pi;
               if(((std::numeric_limits<T>::max)() / xp1) > xm1)
               {
                  // xp1 * xm1 won't overflow:
                  imag = boost::math::log1p(xm1 + std::sqrt(xp1*xm1));
               }
               else
               {
                  imag = log_two + std::log(x);
               }
            }
         }
         else if(y <= safe_min)
         {
            // There is an assumption in Hull et al's analysis that
            // if we get here then x == 1.  This is true for all "good"
            // machines where :
            //
            // E^2 > 8*sqrt(u); with:
            //
            // E =  std::numeric_limits<T>::epsilon()
            // u = (std::numeric_limits<T>::min)()
            //
            // Hull et al provide alternative code for "bad" machines
            // but we have no way to test that here, so for now just assert
            // on the assumption:
            //
            BOOST_ASSERT(x == 1);
            real = half_pi - std::sqrt(y);
            imag = std::sqrt(y);
         }
         else if(std::numeric_limits<T>::epsilon() * y - one >= x)
         {
            real = x/y; // This can underflow!
            imag = log_two + std::log(y);
         }
         else if(x > one)
         {
            real = std::atan(x/y);
            T xoy = x/y;
            imag = log_two + std::log(y) + half * boost::math::log1p(xoy*xoy);
         }
         else
         {
            T a = std::sqrt(one + y*y);
            real = x/a; // This can underflow!
            imag = half * boost::math::log1p(static_cast<T>(2)*y*(y+a));
         }
      }
   }

   //
   // Finish off by working out the sign of the result:
   //
   if((boost::math::signbit)(z.real()))
      real = (boost::math::changesign)(real);
   if((boost::math::signbit)(z.imag()))
      imag = (boost::math::changesign)(imag);

   return std::complex<T>(real, imag);
#ifdef BOOST_MSVC
#pragma warning(pop)
#endif
}

} } // namespaces

#endif // BOOST_MATH_COMPLEX_ASIN_INCLUDED


#include <opencv2/opencv.hpp>
#include <sophus/se3.h>
#include <Eigen/Core>
#include <vector>
#include <string>
#include <boost/format.hpp>
#include <pangolin/pangolin.h>

using namespace std;
using namespace Eigen;
using namespace cv;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
// 内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// 基线
double baseline = 0.573;
// paths
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png");    // other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// TODO implement this function
/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationMultiLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3 &T21
);

// TODO implement this function
/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3 &T21
);

// bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
            (1 - xx) * (1 - yy) * data[0] +
            xx * (1 - yy) * data[1] +
            (1 - xx) * yy * data[img.step] +
            xx * yy * data[img.step + 1]
    );
}

inline bool OutOfImg(float u, float v, int col, int row){
	if(u >= 0 && u < col && v >= 0 && v < row)
		return false;
	else{
		return true;
	}
}

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

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 1000;
    int boarder = 40;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3 T_cur_ref;

    for (int i = 1; i < 6; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
       // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);    // first you need to test single layer
        DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
    }
}

void DirectPoseEstimationSingleLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3 &T21
) {

    // parameters
    int half_patch_size = 4;
    int iterations = 100;

    double cost = 0, lastCost = 0;
    int nGood = 0;  // good projections
    VecVector2d goodProjection;

    for (int iter = 0; iter < iterations; iter++) {
        nGood = 0;
        goodProjection.clear();

        // Define Hessian and bias
        Matrix6d H = Matrix6d::Zero();  // 6x6 Hessian
        Vector6d b = Vector6d::Zero();  // 6x1 bias

        for (size_t i = 0; i < px_ref.size(); i++) {

            // compute the projection in the second image
            // TODO START YOUR CODE HERE
            float u = px_ref[i](0), v = px_ref[i](1);
			double zCam1 = depth_ref[i];
			double xCam1 = double(zCam1 / fx * (u - cx));
			double yCam1 = double(zCam1 / fy * (v - cy));
			Vector3d pCam1(xCam1, yCam1, zCam1);
			Vector3d pCam2 = T21 * pCam1;
			double xCam2 = pCam2[0], yCam2 = pCam2[1], zCam2 = pCam2[2];
			float u2 = float(fx * xCam2 / zCam2 + cx);
			float v2 = float(fy * yCam2 / zCam2 + cy);

			if(OutOfImg(u-half_patch_size, v-half_patch_size, img1.cols, img1.rows)
			  || OutOfImg(u+half_patch_size-1, v+half_patch_size-1, img1.cols, img1.rows)
			  || OutOfImg(u2-half_patch_size, v2-half_patch_size, img2.cols, img2.rows)
			  || OutOfImg(u2+half_patch_size-1, v2+half_patch_size-1, img2.cols, img2.rows)
			  ){
			//	cout << "[DM] pixel out of img " << endl;
			//	cout << u << "  " << v << " " << img1.cols << " " << img1.rows << endl;
			//	cout << u2 << "  " << v2 << " " << img2.cols << " " << img2.rows << endl;
				continue;
			}

//			cout << "uv in cam1:" << u << " " << v << endl;
//			cout << "uv in cam2:" << u2 << " " << v2 << endl;
//			cout << "p in cam2: " << pCam2 << endl;
//			cout << "p in cam1: " << pCam1 << endl;
            nGood++;
            goodProjection.push_back(Eigen::Vector2d(u2, v2));

			double xCam2_2 = xCam2 * xCam2, yCam2_2 = yCam2 * yCam2, zCam2_2 = zCam2 * zCam2;
			Matrix26d J_pixel_xi;   // pixel to \xi in Lie algebra
			J_pixel_xi(0,0) = fx / zCam2;
			J_pixel_xi(0,1) = 0;
		    J_pixel_xi(0,2) = - fx * xCam2 / zCam2_2;
		    J_pixel_xi(0,3) = - fx * xCam2 * yCam2 / zCam2_2;
		    J_pixel_xi(0,4) = fx + fx * xCam2_2 / zCam2_2;
		    J_pixel_xi(0,5) = - fx * yCam2 / zCam2;
		    J_pixel_xi(1,0) = 0;
		    J_pixel_xi(1,1) = fy / zCam2;
		    J_pixel_xi(1,2) = -fy * yCam2 / zCam2_2;
		    J_pixel_xi(1,3) = -fy - fy * yCam2_2 / zCam2_2;
		    J_pixel_xi(1,4) = fy * xCam2 * yCam2 / zCam2_2;
		    J_pixel_xi(1,5) = fy * xCam2 / zCam2;

			// and compute error and jacobian
            for (int x = -half_patch_size; x < half_patch_size; x++)
                for (int y = -half_patch_size; y < half_patch_size; y++) {

                    double error = 0;
                    Eigen::Vector2d J_img_pixel;    // image gradients

                    float u1_patch = u + x, v1_patch = v + y;
					float u2_patch = u2 + x, v2_patch = v2 + y;
					error = GetPixelValue(img1, u1_patch, v1_patch) - GetPixelValue(img2, u2_patch, v2_patch);
		    		J_img_pixel[0] = (GetPixelValue(img2, u2_patch + 1, v2_patch) - GetPixelValue(img2, u2_patch - 1, v2_patch)) / 2;
		  			J_img_pixel[1] = (GetPixelValue(img2, u2_patch, v2_patch + 1) - GetPixelValue(img2, u2_patch, v2_patch - 1)) / 2 ;
					// total jacobian
                    Vector6d J = -J_pixel_xi.transpose() * J_img_pixel;

                    H += J * J.transpose();
                    b += -error * J;
                    cost += error * error;
                }
            // END YOUR CODE HERE
        }

        // solve update and put it into estimation
        // TODO START YOUR CODE HERE
        Vector6d update;
		update = H.ldlt().solve(b);
        T21 = Sophus::SE3::exp(update) * T21;
        // END YOUR CODE HERE

        cost /= nGood;

        if (std::isnan(update[0])) {
            // sometimes occurred when we have a black or white patch and H is irreversible
            cout << "update is nan" << endl;
            break;
        }
        if (iter > 0 && cost > lastCost) {
//            cout << "cost increased: " << cost << ", " << lastCost << endl;
            break;
        }
        lastCost = cost;
//        cout << "cost = " << cost << ", good = " << nGood << endl;
    }
//    cout << "good projection: " << nGood << endl;
//    cout << "T21 = \n" << T21.matrix() << endl;

    // in order to help you debug, we plot the projected pixels here
//    cv::Mat img1_show, img2_show;
//    cv::cvtColor(img1, img1_show, CV_GRAY2BGR);
//    cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
//    for (auto &px: px_ref) {
//        cv::rectangle(img1_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
//                      cv::Scalar(0, 250, 0));
//    }
//    for (auto &px: goodProjection) {
//        cv::rectangle(img2_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
//                      cv::Scalar(0, 250, 0));
//    }
//    cv::imshow("reference", img1_show);
//    cv::imshow("current", img2_show);
//    cv::waitKey();
}

void DirectPoseEstimationMultiLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3 &T21
) {

    // parameters
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    // create pyramids
    vector<cv::Mat> pyr1, pyr2; // image pyramids
    // TODO START YOUR CODE HERE
	for(int i = 0; i < pyramids; i++){
		Mat img1_resize, img2_resize;
		cv::resize(img1, img1_resize, Size(img1.cols * scales[i], img1.rows * scales[i]));
		cv::resize(img2, img2_resize, Size(img2.cols * scales[i], img2.rows * scales[i]));
		pyr1.push_back(img1_resize);
		pyr2.push_back(img2_resize);
	}
    // END YOUR CODE HERE

    double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // backup the old values
    for (int level = pyramids - 1; level >= 0; level--) {
        VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
        for (auto &px: px_ref) {
            px_ref_pyr.push_back(scales[level] * px);
        }

        // TODO START YOUR CODE HERE
        // scale fx, fy, cx, cy in different pyramid levels
		fx = fxG * scales[level];
		fy = fyG * scales[level];
		cx = cxG * scales[level];
		cy = cyG * scales[level];

        // END YOUR CODE HERE
        DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);//T21 have add before's cal result.
    }
	cout << "Multi T21 = \n" << T21.matrix() << endl;
}


//
//  AMGCLSolver.hpp
//  IPC
//
//  Created by Minchen Li on 11/06/19.
//

#ifndef AMGCLSolver_hpp
#define AMGCLSolver_hpp

#include "LinSysSolver.hpp"

#include <amgcl/make_solver.hpp>
#include <amgcl/solver/cg.hpp>
#include <amgcl/solver/lgmres.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/coarsening/smoothed_aggregation.hpp>
#include <amgcl/relaxation/spai0.hpp>
#include <amgcl/relaxation/gauss_seidel.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/block_matrix.hpp>
#include <amgcl/value_type/static_matrix.hpp>

#include <Eigen/Eigen>

#include <vector>
#include <set>

#define USE_BW_BACKEND // use blockwise backend, must undef USE_BW_AGGREGATION
// #define USE_BW_AGGREGATION // use scalar backend but blockwise aggregation, must undef USE_BW_BACKEND
// if neither of above is defined, will use scalar backend and aggregation

// #define USE_AMG_SOLVER // use a single V-cycle to approximately solve the linear system
// if not defined then will use V-cycle preconditioned CG to solve the linear system

namespace IPC {

template <typename vectorTypeI, typename vectorTypeS>
class AMGCLSolver : public LinSysSolver<vectorTypeI, vectorTypeS> {
    typedef LinSysSolver<vectorTypeI, vectorTypeS> Base;

#ifdef USE_BW_BACKEND
    typedef amgcl::static_matrix<double, DIM, DIM> value_type;
    typedef amgcl::static_matrix<double, DIM, 1> rhs_type;
    typedef amgcl::backend::builtin<value_type> BBackend;
    using Solver = amgcl::make_solver<
        // Use AMG as preconditioner:
        amgcl::amg<
            BBackend,
            amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::gauss_seidel>,
        // And BiCGStab as iterative solver:
        amgcl::solver::lgmres<BBackend>>;
#else
    typedef amgcl::backend::builtin<double> Backend;
    // Use AMG as preconditioner:
    typedef amgcl::make_solver<
        // Use AMG as preconditioner:
        amgcl::amg<
            Backend,
            amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::gauss_seidel>,
        // And CG as iterative solver:
        amgcl::solver::lgmres<Backend>>
        Solver;
#endif

protected:
    Solver* solver;
    std::vector<int> _ia, _ja;
    std::vector<double> _a;

public:
    AMGCLSolver(void);
    ~AMGCLSolver(void);

    void set_pattern(const std::vector<std::set<int>>& vNeighbor,
        const std::set<int>& fixedVert);
    void load(const char* filePath, Eigen::VectorXd& rhs);

    void load_AMGCL(const char* filePath, Eigen::VectorXd& rhs);
    void write_AMGCL(const char* filePath, const Eigen::VectorXd& rhs) const;

    void copyOffDiag_IJ(void);
    void copyOffDiag_a(void);

    void analyze_pattern(void);

    bool factorize(void);

    void solve(Eigen::VectorXd& rhs,
        Eigen::VectorXd& result);
};

} // namespace IPC

#endif /* AMGCLSolver_hpp */


#include <Eigen/Eigen>
#include <boost/geometry.hpp>
#include <limits>
#include <lmlib/intersection.h>

namespace lm
{
box2f get_box(Eigen::Vector2f extents) { return box2f{-extents, extents}; }

float distance(Eigen::Vector2f point, Eigen::Vector2f box_extents)
{
    box2f box = get_box(box_extents);

    if (boost::geometry::within(point, box))
    {
        Eigen::Vector2f a{-box_extents[0], -box_extents[1]},
          b{-box_extents[0], box_extents[1]}, c{box_extents[0], box_extents[1]},
          d{box_extents[0], -box_extents[1]};

        std::initializer_list<segment2f> segments{
          {a, b}, {b, c}, {c, d}, {d, a}};

        float distance{std::numeric_limits<float>::max()};

        for (auto segment : segments)
        {
            distance = std::min(
              (float)boost::geometry::distance(segment, point), distance);
        }
        return -distance;
    }

    return boost::geometry::distance(point, box);
}

std::pair<segment2f, float>
  closest_edge(Eigen::Vector2f point, Eigen::Vector2f box_extents)
{
    Eigen::Vector2f a{-box_extents[0], -box_extents[1]},
      b{-box_extents[0], box_extents[1]}, c{box_extents[0], box_extents[1]},
      d{box_extents[0], -box_extents[1]};

    std::initializer_list<segment2f> segments{{a, b}, {b, c}, {c, d}, {d, a}};

    float min_distance{std::numeric_limits<float>::max()};
    segment2f closest;

    for (auto segment : segments)
    {
        double segment_distance = boost::geometry::distance(segment, point);
        if (segment_distance < min_distance)
        {
            min_distance = segment_distance;
            closest = segment;
        }
    }

    return {closest, min_distance};
}
} // namespace lm


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::gamma::is_log_concave               //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_GAMMA_IS_LOG_CONCAVE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_GAMMA_IS_LOG_CONCAVE_HPP_ER_2009
#include <cmath>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/policies/policy.hpp>

namespace boost{
namespace math{

template <class T, class P>
inline bool is_log_concave(
    const boost::math::gamma_distribution<T, P>& dist
){
    return ( dist.shape() > static_cast<T>(1) );
}

}// math
}// boost

#endif


/*-------------Lanczos.cpp----------------------------------------------------//
*
* Purpose: To diagonalize a random matrix using the Lanczos algorithm
*
*   Notes: Compile with (for Arch systems):
*              g++ -I /usr/include/eigen3/ Lanczos.cpp
*          0's along the prime diagonal. I don't know what this means.
*
*-----------------------------------------------------------------------------*/

#include <iostream>
#include <Eigen/Core>
#include <Eigen/QR>
#include <random>
#include <vector>
#include <math.h>

using namespace Eigen;

// Function for the lanczos algorithm, returns Tri-diagonal matrix
MatrixXd lanczos(MatrixXd &d_matrix, int row_num);

// Function for QR decomposition
MatrixXd qrdecomp(MatrixXd &Tridiag);

// Function to perform the Power Method
void p_method(MatrixXd &Tridiag, MatrixXd &Q);

// Function to return sign of value (signum function)
int sign(double value);

// Function to check eigenvectors and values
void eigentest(MatrixXd &d_matrix, MatrixXd &Q);

/*----------------------------------------------------------------------------//
* MAIN
*-----------------------------------------------------------------------------*/

int main(){

    int size = 200;
    MatrixXd d_matrix(size,size);

    // set up random device
    static std::random_device rd;
    int seed = rd();
    static std::mt19937 gen(seed);
    std::uniform_real_distribution<double> dist(0,1);

    for (size_t i = 0; i < d_matrix.rows(); ++i){
        for (size_t j = 0; j <= i; ++j){
            d_matrix(i,j) = dist(gen);
            d_matrix(j,i) = d_matrix(i,j);
        }
    }

    MatrixXd Tridiag = lanczos(d_matrix, 20);

    //std::cout << '\n' << "Tridiagonal matrix is: \n" << Tridiag << '\n';

    /*
    // Testing for eigenvalue determination
    MatrixXd Tridiag(3,3);
    Tridiag << 1, 2, 0,
               2, 1, 0,
               0, 0, -3;

    MatrixXd Tridiag(5,5);
    Tridiag << 5, 0, 0, 0, 0,
               0, 4, 0, 0, 0,
               0, 0, 3, 0, 0,
               0, 0, 0, 2, 0,
               0, 0, 0, 0, 7;
    Tridiag = lanczos(Tridiag, 5);
    */

    //std::cout << "Tridiag is: " << '\n' << Tridiag << '\n';

    MatrixXd Q = qrdecomp(Tridiag);

    MatrixXd Qtemp = Q;

    //std::cout << "Q is: " << '\n';
    //std::cout << Q << '\n';

    std::cout << "Finding eigenvalues: " << '\n';
    p_method(Tridiag, Q);

    //std::cout << "Q is: " << '\n' << Q << '\n';

    Qtemp = Qtemp - Q;
    //std::cout << "After the Power Method: " << Qtemp.squaredNorm() << '\n';
    eigentest(Tridiag, Q);

}

/*----------------------------------------------------------------------------//
* SUBROUTINE
*-----------------------------------------------------------------------------*/

// Function for the lanczos algorithm, returns Tri-diagonal matrix
MatrixXd lanczos(MatrixXd &d_matrix, int row_num){

    // Creating random device
    static std::random_device rd;
    int seed = rd();
    static std::mt19937 gen(seed);
    std::uniform_real_distribution<double> dist(0,1);

    // Defining values
    double threshold = 0.01;
    int j = 0;
    int size = d_matrix.rows();

    // Setting beta arbitrarily large for now
    double beta = 10;

    // generating the first rayleigh vector
    // alpha is actually just a double... sorry about that.
    MatrixXd rayleigh(d_matrix.rows(),1), q(d_matrix.rows(),1),
             alpha(1, 1);
    MatrixXd identity = MatrixXd::Identity(d_matrix.rows(), d_matrix.cols());

    // krylov is the krylovian subspace... Note, there might be a dynamic way to
    // do this. Something like:
    //std::vector <MatrixXd> krylov;
    MatrixXd krylov(d_matrix.rows(), row_num);

    for (size_t i = 0; i < size; ++i){
        rayleigh(i) = dist(gen);
    }

    //std::cout << rayleigh << '\n';

    //while (beta > threshold){
    for (size_t i = 0; i < row_num; ++i){
        beta = rayleigh.norm();
        //std::cout << "beta is: \n" << beta << '\n';

        q = rayleigh / beta;
        //std::cout << "q is: \n" << q << '\n';

        alpha = q.transpose() * d_matrix * q;
        //std::cout << "alpha is \n" << alpha << '\n';

        if (j == 0){
            rayleigh = (d_matrix - alpha(0,0) * identity) * q;
        }
        else{
            rayleigh = (d_matrix - alpha(0,0) * identity) * q
                       - beta * krylov.col(j - 1);

        }
        //std::cout << "rayleigh is: \n" << rayleigh <<'\n';
        //std::cout << "i is: " << i << '\n';

        //krylov.push_back(q);
        krylov.col(j) = q;
        j = j+1;
        // std::cout << j << '\n';
    }

    /*
    MatrixXd krylov_id = krylov.transpose() * krylov;
    std::cout << "The identity matrix from the krylov subspace is: \n"
              << krylov_id << '\n';
    */

    MatrixXd T(row_num,row_num);
    T = krylov.transpose() * d_matrix * krylov;

    return T;
}

// Function for QR decomposition
// Because we only need Q for the power method, I will retun only Q
MatrixXd qrdecomp(MatrixXd &Tridiag){
    // Q is and orthonormal vector => Q'Q = 1
    MatrixXd Id = MatrixXd::Identity(Tridiag.rows(), Tridiag.cols());
    MatrixXd Q = Id;
    MatrixXd P(Tridiag.rows(), Tridiag.cols());

    // R is the upper triangular matrix
    MatrixXd R = Tridiag;

    int row_num = Tridiag.rows();
    int countx = 0, county = 0;

    // Scale R
    double sum = 0.0, sigma, tau, fak, max_val = 0;

    bool sing;

    // Defining vectors for algorithm
    MatrixXd diag(row_num,1);

    //std::cout << R << '\n';

    for (int i = 0; i < row_num-1; ++i){
        diag = MatrixXd::Zero(row_num, 1);

        sum = 0;
        for (size_t j = i; j < row_num; ++j){
            sum += R(j,i) * R(j,i);
            //std::cout << R(j,i) << '\n';
        }
        sum = sqrt(sum);

        if (R(i,i) > 0){
            sigma = -sum;
        }
        else{
            sigma = sum;
        }

        //std::cout << "sigma is: " << sigma << '\n';

        sum = 0;
        //diag = R.block(i,i, row_num - i, 1);
        //std::cout << "diag is: " << '\n';
        for (int j = i; j < row_num; ++j){
            //std::cout << i << '\t' << j << '\n';
            if (j == i){
                diag(j) = R(j,i) + sigma;
            }
            else{
                diag(j) = R(j, i);
            }
            //std::cout << diag(j) << '\n';
            sum = sum + diag(j) * diag(j);
        }
        sum = sqrt(sum);

        //std::cout << "sum is: " << sum << '\n';

        if (sum > 0.000000000000001){

            for (int j = i; j < row_num; ++j){
                diag(j) = diag(j) / sum;
            }

            //std::cout << "normalized diag is: " << '\n' << diag << '\n';

            P = Id - (diag * diag.transpose()) * 2.0;

            R = P * R;
            Q = Q * P;
        }

        //std::cout << "R is: " << R << '\n';

    }
    //std::cout << "R is: " << R << '\n';
    //std::cout << "Q is: " << Q << '\n';

    //std::cout << "QR is: " << '\n' << Q*R << '\n';

    //std::cout << "Q^T * Q is: " << '\n' << Q.transpose() * Q << '\n' << '\n';
    //std::cout << "QR - A is: " << '\n' << Q*R - Tridiag << '\n';
    //std::cout << "Q^T * A - R: " << '\n'
    //          << Q.transpose() * Tridiag - R << '\n' << '\n';

    return Q;
}

// Function to perform the Power Method
void p_method(MatrixXd &Tridiag, MatrixXd &Q){

    //std::cout << "Q is: " << '\n' << Q << '\n';
    //std::cout << "Tridiag is : " << '\n' << Tridiag << '\n';

    // Find all eigenvectors
    MatrixXd eigenvectors(Tridiag.rows(), Tridiag.cols());
    MatrixXd Z(Tridiag.rows(), Tridiag.cols());
    MatrixXd Qtemp = Q;

    // Iteratively defines eigenvectors
    for (int i = 0; i < Tridiag.rows(); ++i){
        Z = Tridiag * Q;
        Q = qrdecomp(Z);

    }

    Qtemp = Qtemp - Q;
    //std::cout << "This should not be 0: " << Qtemp.squaredNorm() << '\n';

}

// Function to return sign of value (signum function)
int sign(double value){
    if (value < 0.0){
        return -1;
    }
    else if (value > 0){
        return 1;
    }
    else {
        return 0;
    }
}

// Function to check eigenvectors and values
void eigentest(MatrixXd &Tridiag, MatrixXd &Q){

    // Calculating the Rayleigh quotient (v^t * A * v) / (v^t * v)
    // Note, that this should be a representation of eigenvalues

    std::vector<double> eigenvalues(Tridiag.rows());
    MatrixXd checkvector(Tridiag.rows(),1);
    double QQ, QAQ;

    for (size_t i = 0; i < Tridiag.rows(); ++i){
        QQ = Q.col(i).transpose() * Q.col(i);
        QAQ = Q.col(i).transpose() * Tridiag * Q.col(i);
        eigenvalues[i] =  QAQ / QQ;
        std::cout << "eigenvalue is: " << eigenvalues[i] << '\n';

        checkvector = ((Tridiag * Q.col(i)) / eigenvalues[i]) - Q.col(i);
        //std::cout << checkvector << '\n' << '\n';
        std::cout << "This should be 0: " << '\t'
                  << checkvector.squaredNorm() << '\n';

    }

}


#ifndef SOLVERSPOINTS_HPP
#define SOLVERSPOINTS_HPP

#include <Eigen/Core>
#include <vector>

// ##########################################################
// Solver for 6L. The solver is from Stewenius paper,
// and was implemented by Kneip.
// ##########################################################
template <typename floatPrec>
std::vector<Eigen::Matrix<floatPrec,4,4>> solver3Q(
    std::vector<std::pair<Eigen::Matrix<floatPrec,4,1>, Eigen::Matrix<floatPrec,4,1>>> ptPair,
	std::vector<std::pair<Eigen::Matrix<floatPrec,4,1>, Eigen::Matrix<floatPrec,4,1>>> plPair,
	std::vector<std::pair<std::pair<Eigen::Matrix<floatPrec,4,1>, Eigen::Matrix<floatPrec,4,1>>,
        std::pair<Eigen::Matrix<floatPrec,4,1>, Eigen::Matrix<floatPrec,4,1>>>> lPair
);

#endif


#include "nmea/vtg.h"
#include <ros/ros.h>
#include <boost/algorithm/string.hpp>
#include "geometry_msgs/Vector3Stamped.h"
#include "angles/angles.h"

using std::string;
using std::vector;
using std::tuple;
using std::get;

nmea::vtg
v3s_to_vtg_ros_msg(geometry_msgs::Vector3Stamped const &message);

nmea::vtg
v3s_to_vtg_ros_msg(geometry_msgs::Vector3Stamped const &message)
{
  double const speedMagMps = sqrt(message.vector.x * message.vector.x +
                                  message.vector.y * message.vector.y +
                                  message.vector.z * message.vector.z);

  double const trackFromEastRad = atan2(-message.vector.y, message.vector.x);
  double const trackFromNorthDeg = angles::to_degrees(trackFromEastRad);

  nmea::vtg ros_msg;
  ros_msg.true_track_made_good = trackFromNorthDeg;
  ros_msg.magnetic_track_made_good_valid = false;
  ros_msg.ground_speed_knots = speedMagMps * 1.94384;
  ros_msg.ground_speed_kph = speedMagMps * 3.6;
  return ros_msg;
}

class NmeaSubVelToNmeaPubVtg
{
public:
  inline NmeaSubVelToNmeaPubVtg(ros::NodeHandle *const nh)
      : vtgPub(nh->advertise<nmea::vtg>("vtg", 10))
  {
  }

  inline void Callback(geometry_msgs::Vector3Stamped const &message)
  {
    this->vtgPub.publish(v3s_to_vtg_ros_msg(message));
  }

private:
  ros::Publisher vtgPub;
};

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "vel_to_vtg");

  ros::NodeHandle n;
  NmeaSubVelToNmeaPubVtg nmea_sub_vel_to_nmea_pub_vtg(&n);
  ros::Subscriber sub =
      n.subscribe("gps/fix_velocity", 10, &NmeaSubVelToNmeaPubVtg::Callback,
                  &nmea_sub_vel_to_nmea_pub_vtg);

  ros::spin();

  return EXIT_SUCCESS;
}


/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020-2021 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

#include <Eigen/QR>
#include <wpi/MathExtras.h>

#include "frc/geometry/Pose2d.h"
#include "frc/system/LinearSystem.h"
#include "frc/system/plant/LinearSystemId.h"
#include "frc/trajectory/TestTrajectory.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "frc/trajectory/constraint/DifferentialDriveVelocitySystemConstraint.h"
#include "gtest/gtest.h"

// TODO: The constraint used in this test violates the max voltage, but not
// doing so would mean the only way to reach the max steady-state velocity for
// that voltage is open-loop exponential convergence. The constraint's
// optimization intent should be clarified so we can write a better test.
TEST(DifferentialDriveVelocitySystemTest, DISABLED_Constraint) {
  constexpr auto kMaxVoltage = 10_V;

  // Pick an unreasonably large kA to ensure the constraint has to do some work
  const frc::LinearSystem<2, 2, 2> system =
      frc::LinearSystemId::IdentifyDrivetrainSystem(
          1_V / 1_mps, 3_V / 1_mps_sq, 1_V / 1_rad_per_s, 3_V / 1_rad_per_s_sq);
  const frc::DifferentialDriveKinematics kinematics{0.5_m};
  auto config = frc::TrajectoryConfig(12_mps, 12_mps_sq);
  config.AddConstraint(frc::DifferentialDriveVelocitySystemConstraint(
      system, kinematics, kMaxVoltage));

  auto trajectory = frc::TestTrajectory::GetTrajectory(config);

  auto time = 0_s;
  auto dt = 20_ms;
  auto duration = trajectory.TotalTime();

  while (time < duration) {
    auto point = trajectory.Sample(time);
    time += dt;

    const frc::ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
                                           point.velocity * point.curvature};

    auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);

    auto x = frc::MakeMatrix<2, 1>(left.to<double>(), right.to<double>());

    // Not really a strictly-correct test as we're using the chassis accel
    // instead of the wheel accel, but much easier than doing it "properly" and
    // a reasonable check anyway
    auto xDot = frc::MakeMatrix<2, 1>(point.acceleration.to<double>(),
                                      point.acceleration.to<double>());

    Eigen::Matrix<double, 2, 1> u =
        system.B().householderQr().solve(xDot - system.A() * x);

    EXPECT_GE(u(0), -kMaxVoltage.to<double>() - 0.5);
    EXPECT_LE(u(0), kMaxVoltage.to<double>() + 0.5);
    EXPECT_GE(u(1), -kMaxVoltage.to<double>() - 0.5);
    EXPECT_LE(u(1), kMaxVoltage.to<double>() + 0.5);
  }
}

TEST(DifferentialDriveVelocitySystemTest, HighCurvature) {
  constexpr auto kMaxVoltage = 10_V;

  const frc::LinearSystem<2, 2, 2> system =
      frc::LinearSystemId::IdentifyDrivetrainSystem(
          1_V / 1_mps, 3_V / 1_mps_sq, 1_V / 1_rad_per_s, 3_V / 1_rad_per_s_sq);
  // Large trackwidth - need to test with radius of curvature less than half of
  // trackwidth
  const frc::DifferentialDriveKinematics kinematics{3_m};

  auto config = frc::TrajectoryConfig(12_fps, 12_fps_sq);
  config.AddConstraint(frc::DifferentialDriveVelocitySystemConstraint(
      system, kinematics, kMaxVoltage));

  EXPECT_NO_FATAL_FAILURE(frc::TrajectoryGenerator::GenerateTrajectory(
      frc::Pose2d{1_m, 0_m, frc::Rotation2d{90_deg}}, {},
      frc::Pose2d{0_m, 1_m, frc::Rotation2d{180_deg}}, config));

  config.SetReversed(true);

  EXPECT_NO_FATAL_FAILURE(frc::TrajectoryGenerator::GenerateTrajectory(
      frc::Pose2d{0_m, 1_m, frc::Rotation2d{180_deg}}, {},
      frc::Pose2d{1_m, 0_m, frc::Rotation2d{90_deg}}, config));
}