//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2007.

//  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_STATS_FIND_SCALE_HPP
#define BOOST_STATS_FIND_SCALE_HPP

#include <boost/math/distributions/fwd.hpp> // for all distribution signatures.
#include <boost/math/distributions/complement.hpp>
#include <boost/math/policies/policy.hpp>
// using boost::math::policies::policy;
#include <boost/math/tools/traits.hpp>
#include <boost/static_assert.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/policies/error_handling.hpp>
// using boost::math::complement; // will be needed by users who want complement,
// but NOT placed here to avoid putting it in global scope.

namespace boost
{
  namespace math
  {
    // Function to find location of random variable z
    // to give probability p (given scale)
    // Applies to normal, lognormal, extreme value, Cauchy, (and symmetrical triangular),
    // distributions that have scale.
    // BOOST_STATIC_ASSERTs, see below, are used to enforce this.

    template <class Dist, class Policy>
    inline
      typename Dist::value_type find_scale( // For example, normal mean.
      typename Dist::value_type z, // location of random variable z to give probability, P(X > z) == p.
      // For example, a nominal minimum acceptable weight z, so that p * 100 % are > z
      typename Dist::value_type p, // probability value desired at x, say 0.95 for 95% > z.
      typename Dist::value_type location, // location parameter, for example, normal distribution mean.
      const Policy& pol
      )
    {
#if !defined(BOOST_NO_SFINAE) && !BOOST_WORKAROUND(__SUNPRO_CC, BOOST_TESTED_AT(0x590))
      BOOST_STATIC_ASSERT(::boost::math::tools::is_distribution<Dist>::value);
      BOOST_STATIC_ASSERT(::boost::math::tools::is_scaled_distribution<Dist>::value);
#endif
      static const char* function = "boost::math::find_scale<Dist, Policy>(%1%, %1%, %1%, Policy)";

      if(!(boost::math::isfinite)(p) || (p < 0) || (p > 1))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "Probability parameter was %1%, but must be >= 0 and <= 1!", p, pol);
      }
      if(!(boost::math::isfinite)(z))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "find_scale z parameter was %1%, but must be finite!", z, pol);
      }
      if(!(boost::math::isfinite)(location))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "find_scale location parameter was %1%, but must be finite!", location, pol);
      }

      //cout << "z " << z << ", p " << p << ",  quantile(Dist(), p) "
      //<< quantile(Dist(), p) << ", z - mean " << z - location
      //<<", sd " << (z - location)  / quantile(Dist(), p) << endl;

      //quantile(N01, 0.001) -3.09023
      //quantile(N01, 0.01) -2.32635
      //quantile(N01, 0.05) -1.64485
      //quantile(N01, 0.333333) -0.430728
      //quantile(N01, 0.5) 0
      //quantile(N01, 0.666667) 0.430728
      //quantile(N01, 0.9) 1.28155
      //quantile(N01, 0.95) 1.64485
      //quantile(N01, 0.99) 2.32635
      //quantile(N01, 0.999) 3.09023

      typename Dist::value_type result =
        (z - location)  // difference between desired x and current location.
        / quantile(Dist(), p); // standard distribution.

      if (result <= 0)
      { // If policy isn't to throw, return the scale <= 0.
        policies::raise_evaluation_error<typename Dist::value_type>(function,
          "Computed scale (%1%) is <= 0!" " Was the complement intended?",
          result, Policy());
      }
      return result;
    } // template <class Dist, class Policy> find_scale

    template <class Dist>
    inline // with default policy.
      typename Dist::value_type find_scale( // For example, normal mean.
      typename Dist::value_type z, // location of random variable z to give probability, P(X > z) == p.
      // For example, a nominal minimum acceptable z, so that p * 100 % are > z
      typename Dist::value_type p, // probability value desired at x, say 0.95 for 95% > z.
      typename Dist::value_type location) // location parameter, for example, mean.
    { // Forward to find_scale using the default policy.
      return (find_scale<Dist>(z, p, location, policies::policy<>()));
    } // find_scale

    template <class Dist, class Real1, class Real2, class Real3, class Policy>
    inline typename Dist::value_type find_scale(
      complemented4_type<Real1, Real2, Real3, Policy> const& c)
    {
      //cout << "cparam1 q " << c.param1 // q
      //  << ", c.dist z " << c.dist // z
      //  << ", c.param2 l " << c.param2 // l
      //  << ", quantile (Dist(), c.param1 = q) "
      //  << quantile(Dist(), c.param1) //q
      //  << endl;

#if !defined(BOOST_NO_SFINAE) && !BOOST_WORKAROUND(__SUNPRO_CC, BOOST_TESTED_AT(0x590))
      BOOST_STATIC_ASSERT(::boost::math::tools::is_distribution<Dist>::value);
      BOOST_STATIC_ASSERT(::boost::math::tools::is_scaled_distribution<Dist>::value);
#endif
      static const char* function = "boost::math::find_scale<Dist, Policy>(complement(%1%, %1%, %1%, Policy))";

      // Checks on arguments, as not complemented version,
      // Explicit policy.
      typename Dist::value_type q = c.param1;
      if(!(boost::math::isfinite)(q) || (q < 0) || (q > 1))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "Probability parameter was %1%, but must be >= 0 and <= 1!", q, c.param3);
      }
      typename Dist::value_type z = c.dist;
      if(!(boost::math::isfinite)(z))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "find_scale z parameter was %1%, but must be finite!", z, c.param3);
      }
      typename Dist::value_type location = c.param2;
      if(!(boost::math::isfinite)(location))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "find_scale location parameter was %1%, but must be finite!", location, c.param3);
      }

      typename Dist::value_type result =
        (c.dist - c.param2)  // difference between desired x and current location.
        / quantile(complement(Dist(), c.param1));
      //     (  z    - location) / (quantile(complement(Dist(),  q))
      if (result <= 0)
      { // If policy isn't to throw, return the scale <= 0.
        policies::raise_evaluation_error<typename Dist::value_type>(function,
          "Computed scale (%1%) is <= 0!" " Was the complement intended?",
          result, Policy());
      }
      return result;
    } // template <class Dist, class Policy, class Real1, class Real2, class Real3> typename Dist::value_type find_scale

    // So the user can start from the complement q = (1 - p) of the probability p,
    // for example, s = find_scale<normal>(complement(z, q, l));

    template <class Dist, class Real1, class Real2, class Real3>
    inline typename Dist::value_type find_scale(
      complemented3_type<Real1, Real2, Real3> const& c)
    {
      //cout << "cparam1 q " << c.param1 // q
      //  << ", c.dist z " << c.dist // z
      //  << ", c.param2 l " << c.param2 // l
      //  << ", quantile (Dist(), c.param1 = q) "
      //  << quantile(Dist(), c.param1) //q
      //  << endl;

#if !defined(BOOST_NO_SFINAE) && !BOOST_WORKAROUND(__SUNPRO_CC, BOOST_TESTED_AT(0x590))
      BOOST_STATIC_ASSERT(::boost::math::tools::is_distribution<Dist>::value);
      BOOST_STATIC_ASSERT(::boost::math::tools::is_scaled_distribution<Dist>::value);
#endif
      static const char* function = "boost::math::find_scale<Dist, Policy>(complement(%1%, %1%, %1%, Policy))";

      // Checks on arguments, as not complemented version,
      // default policy policies::policy<>().
      typename Dist::value_type q = c.param1;
      if(!(boost::math::isfinite)(q) || (q < 0) || (q > 1))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "Probability parameter was %1%, but must be >= 0 and <= 1!", q, policies::policy<>());
      }
      typename Dist::value_type z = c.dist;
      if(!(boost::math::isfinite)(z))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "find_scale z parameter was %1%, but must be finite!", z, policies::policy<>());
      }
      typename Dist::value_type location = c.param2;
      if(!(boost::math::isfinite)(location))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "find_scale location parameter was %1%, but must be finite!", location, policies::policy<>());
      }

      typename Dist::value_type result =
        (z - location)  // difference between desired x and current location.
        / quantile(complement(Dist(), q));
      //     (  z    - location) / (quantile(complement(Dist(),  q))
      if (result <= 0)
      { // If policy isn't to throw, return the scale <= 0.
        policies::raise_evaluation_error<typename Dist::value_type>(function,
          "Computed scale (%1%) is <= 0!" " Was the complement intended?",
          result, policies::policy<>()); // This is only the default policy - also Want a version with Policy here.
      }
      return result;
    } // template <class Dist, class Real1, class Real2, class Real3> typename Dist::value_type find_scale

  } // namespace boost
} // namespace math

#endif // BOOST_STATS_FIND_SCALE_HPP


// Copyright (c) 2018 Adam Butcher, Antony Polukhin
// Copyright (c) 2019-2022 Antony Polukhin
//
// 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)

#include <boost/pfr/core.hpp>
#include <boost/core/lightweight_test.hpp>

auto parseHex(char const* p, size_t limit = ~0u) {
    struct { size_t val; char const* rest; } res = { 0, p };
    while (limit) {
        int v = *res.rest;
        if (v >= '0' && v <= '9')
            v = v - '0';
        else if (v >= 'A' && v <= 'F')
            v = 10 + v - 'A';
        else if (v >= 'a' && v <= 'f')
            v = 10 + v - 'a';
        else
            break;
        res.val = (res.val << 4) + v;
        --limit;
        ++res.rest;
    }
    return res;
}

auto parseLinePrefix(char const* line) {
     struct {
          size_t byteCount, address, recordType; char const* rest;
     } res;
     using namespace boost::pfr;
     tie_from_structure (res.byteCount, line) = parseHex(line, 2);
     tie_from_structure (res.address, line) = parseHex(line, 4);
     tie_from_structure (res.recordType, line) = parseHex(line, 2);
     res.rest = line;
     return res;
}

int main() {
    auto line = "0860E000616263646566000063";
    auto meta = parseLinePrefix(line);
    BOOST_TEST_EQ(meta.byteCount, 8);
    BOOST_TEST_EQ(meta.address, 24800);
    BOOST_TEST_EQ(meta.recordType, 0);
    BOOST_TEST_EQ(meta.rest, line + 8);

    size_t val;
    using namespace boost::pfr;

    tie_from_structure (val, std::ignore) = parseHex("a73b");
    BOOST_TEST_EQ(val, 42811);

    tie_from_structure (std::ignore, line) = parseHex(line, 8);
    BOOST_TEST_EQ(line, meta.rest);

    return boost::report_errors();
}


/*
// Copyright 2018 University of Liege
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Authors:
// - Adrien Crovato
*/

//// Panel splitting
// Split a panel and compute AIC on each sub-panel
//
// Inputs:
// - idP: influencing panel index
// - idF: target field cell index
// - x0: x - coordinate of first corner point of panel in local axes
// - x1: x - coordinate of second corner point of panel in local axes
// - x2: x - coordinate of third corner point of panel in local axes
// - x3: x - coordinate of fourth corner point of panel in local axes
// - y0: y - coordinate of first corner point of panel in local axes
// - y1: y - coordinate of second corner point of panel in local axes
// - y2: y - coordinate of third corner point of panel in local axes
// - y3: y - coordinate of fourth corner point of panel in local axes
// - bPan: body panels (structure)
// - fPan: field panels (structure)
// - sp: sub-panels (structure)

#include <iostream>
#include <Eigen/Dense>
#include <array>
#include "split_panel.h"
#include "infcB.h"

#define NDIM 3
#define NSING 2

using namespace std;
using namespace Eigen;

array<RowVectorXd,NSING> split_panel(int idP, int idF,
                                  double x0, double x1, double x2, double x3, double y0, double y1, double y2, double y3,
                                  Network &bPan, Field &fPan, Subpanel &sp) {

    //// Initialization
    int idx; // counter
    double a0, a1 ,b0, b1; // interpolation coefficients
    VectorXd xV0, xV1, xV2, xV3, yV0, yV1, yV2, yV3; // sub-panel vertices
    xV0.resize(sp.NS);
    xV1.resize(sp.NS);
    xV2.resize(sp.NS);
    xV3.resize(sp.NS);
    yV0.resize(sp.NS);
    yV1.resize(sp.NS);
    yV2.resize(sp.NS);
    yV3.resize(sp.NS);
    Vector3d tgt; // target points
    tgt.resize(NDIM);

    array <RowVectorXd, NSING> coeff; // returned and temporary coefficients
    array <double, NSING> coeffT;
    for (int i = 0; i < NSING; i++)
        coeff[i].resize(sp.NS);

    //// Change coordinates
    // Cell center
    tgt(0) = bPan.l(idP,0)*fPan.CG(idF,0) + bPan.l(idP,1)*fPan.CG(idF,1) + bPan.l(idP,2)*fPan.CG(idF,2);
    tgt(1) = bPan.p(idP,0)*fPan.CG(idF,0) + bPan.p(idP,1)*fPan.CG(idF,1) + bPan.p(idP,2)*fPan.CG(idF,2);
    tgt(2) = bPan.n(idP,0)*(fPan.CG(idF,0) - bPan.CG(idP,0)) + bPan.n(idP,1)*(fPan.CG(idF,1) - bPan.CG(idP,1))
               + bPan.n(idP,2)*(fPan.CG(idF,2) - bPan.CG(idP,2));

    //// Split panel
    // Compute sub-panels vertices
    idx = 0;
    for (int j = 0; j < sp.NSs; j++) {
        for (int i = 0; i < sp.NSs; i++) {
            // Compute weight factors
            a0 = (double) i/sp.NSs;
            a1 = (double) (i+1)/sp.NSs;
            b0 = (double) j/sp.NSs;
            b1 = (double) (j+1)/sp.NSs;
            // Compute new vertices
            xV0(idx) = (1-b0)*((1-a0)*x0 + a0*x1) + b0*(a0*x2 +(1-a0)*x3);
            xV1(idx) = (1-b0)*((1-a1)*x0 + a1*x1) + b0*(a1*x2 +(1-a1)*x3);
            xV2(idx) = (1-b1)*((1-a1)*x0 + a1*x1) + b1*(a1*x2 +(1-a1)*x3);
            xV3(idx) = (1-b1)*((1-a0)*x0 + a0*x1) + b1*(a0*x2 +(1-a0)*x3);
            yV0(idx) = (1-b0)*((1-a0)*y0 + a0*y1) + b0*(a0*y2 +(1-a0)*y3);
            yV1(idx) = (1-b0)*((1-a1)*y0 + a1*y1) + b0*(a1*y2 +(1-a1)*y3);
            yV2(idx) = (1-b1)*((1-a1)*y0 + a1*y1) + b1*(a1*y2 +(1-a1)*y3);
            yV3(idx) = (1-b1)*((1-a0)*y0 + a0*y1) + b1*(a0*y2 +(1-a0)*y3);
            idx ++;
        }
    }

    //// Compute AIC
    for (int j = 0; j < sp.NS; j++) {
        // call infcBB
        coeffT = infcB(0, 0, 1, tgt(0), tgt(1), tgt(2), xV0(j), yV0(j), xV1(j), yV1(j), xV2(j), yV2(j), xV3(j), yV3(j));
        // Store to return
        coeff[0](j) = coeffT[0];
        coeff[1](j) = coeffT[1];
    }
    return coeff;
}


#ifndef AQ1_MAIN_HPP
#define AQ1_MAIN_HPP

#include <iostream>
#include <memory>
#include <optional>
#include <string>
#include <variant>
#include <vector>

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/rational.hpp>

namespace mp = boost::multiprecision;
using MPInt = mp::cpp_int;
using MPRational = boost::rational<MPInt>;
using MPFloat = mp::cpp_dec_float_100;

enum class TOK {
    NUMLIT,  // Numeric literal
    OWARI,   // End of file
    PLUS,
    MINUS,
    STAR,
    SLASH,
    PERCENT,
    NEWLINE,
    LPAREN,
    RPAREN,
    IDENT,
    COMMA,
    FMTLIT,
};

struct FormatLiteral {
    bool is_signed, is_zero_padded;
    int field_width, precision;
};

struct Token {
    TOK kind;
    std::variant<std::monostate, MPRational, std::string, FormatLiteral> data;

    static const Token &owari()
    {
        static const Token tok{TOK::OWARI, std::monostate{}};
        return tok;
    }
};

class Lex {
private:
    std::istream &is_;
    std::optional<Token> pending_;
    std::vector<char> history_;

    int getch();
    void putback(int ch);
    Token next_token();

public:
    Lex(std::istream &is) : is_(is), pending_(std::nullopt)
    {
    }

    // Get the next non-NEWLINE token.
    Token get();
    // Expect the next non-NEWLINE token.
    Token expect(TOK kind);
    // Return if the next non-NEWLINE token's kind is `kind`.
    bool match(TOK kind);

    // Peek the next token. This function will NOT skip NEWLINE.
    Token peek_next();

    std::string clear_history();
};

class ASTNode {
public:
    ASTNode() = default;

    // Thanks to:
    // https://github.com/isocpp/CppCoreGuidelines/blob/master/CppCoreGuidelines.md#Rc-copy-virtual
    // Thanks to:
    // https://github.com/isocpp/CppCoreGuidelines/blob/master/CppCoreGuidelines.md#Rc-five
    virtual ~ASTNode() = default;
    ASTNode(const ASTNode &) = delete;
    ASTNode &operator=(const ASTNode &) = delete;
    ASTNode(ASTNode &&) = delete;
    ASTNode &operator=(ASTNode &&) = delete;

    virtual MPRational eval() const = 0;
};

using ASTNodePtr = std::shared_ptr<ASTNode>;

enum class UNARYOP {
    PLUS,
    MINUS,
};

class UnaryOp : public ASTNode {
private:
    UNARYOP kind_;
    ASTNodePtr src_;

public:
    UnaryOp(UNARYOP kind, ASTNodePtr src) : kind_(kind), src_(std::move(src))
    {
    }

    MPRational eval() const override;
};

enum class BINOP {
    ADD,
    SUB,
    MUL,
    DIV,
};

class BinOp : public ASTNode {
private:
    BINOP kind_;
    ASTNodePtr lhs_, rhs_;

public:
    BinOp(BINOP kind, ASTNodePtr lhs, ASTNodePtr rhs)
        : kind_(kind), lhs_(std::move(lhs)), rhs_(std::move(rhs))
    {
    }

    MPRational eval() const override;
};

class NumImm : public ASTNode {
private:
    MPRational val_;

public:
    NumImm(MPRational val) : val_(std::move(val))
    {
    }

    MPRational eval() const override
    {
        return val_;
    }
};

class FuncCall : public ASTNode {
private:
    std::string name_;
    std::vector<ASTNodePtr> args_;

public:
    FuncCall(std::string name, std::vector<ASTNodePtr> args)
        : name_(std::move(name)), args_(std::move(args))
    {
    }

    MPRational eval() const override;
};

class FormatPrint : public ASTNode {
private:
    ASTNodePtr src_;
    int field_width_, precision_;

public:
    FormatPrint(ASTNodePtr src, int field_width, int precision)
        : src_(std::move(src)), field_width_(field_width), precision_(precision)
    {
    }

    MPRational eval() const override;
};

class Parser {
private:
    Lex &lex_;

    ASTNodePtr parse_primary();
    ASTNodePtr parse_unary();
    ASTNodePtr parse_multiplicative();
    ASTNodePtr parse_additive();
    ASTNodePtr parse_expr();

public:
    Parser(Lex &lex) : lex_(lex)
    {
    }

    ASTNodePtr parse();
};

#endif


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <string>
#include <list>
#include <vector>

using namespace std::string_literals;

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "vlasovpp/field.h"
#include "vlasovpp/complex_field.h"
#include "vlasovpp/weno.h"
#include "vlasovpp/fft.h"
#include "vlasovpp/array_view.h"
#include "vlasovpp/poisson.h"
#include "vlasovpp/rk.h"
#include "vlasovpp/config.h"
#include "vlasovpp/signal_handler.h"
#include "vlasovpp/iteration.h"
#include "vlasovpp/splitting.h"

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Zi(i) (i*f.step.dz+f.range.z_min)
#define Vkx(k) (k*f.step.dvx+f.range.vx_min)
#define Vky(k) (k*f.step.dvy+f.range.vy_min)
#define Vkz(k) (k*f.step.dvz+f.range.vz_min)

auto
maxwellian ( double rho , std::vector<double> u , double T ) {
  return [=](double z,double vx,double vy,double vz) {
    return rho/( std::pow(2.*math::pi<double>()*T,1.5) )*std::exp( -0.5*(SQ(vx-u[0])+SQ(vy-u[1])+SQ(vz-u[2]))/T );
  };
}

int
main ( int argc , char const * argv[] )
{
  std::string p("config.init");
  if ( argc > 1 )
    { p = argv[1]; }
  auto c = config(p);
  c.name = "vmls";

  c.create_output_directory();
  {
    std::ofstream ofconfig( c.output_dir / "config.init" );
    ofconfig << c << "\n";
    ofconfig.close();
  }

/* ------------------------------------------------------------------------- */
  field3d<double> f(boost::extents[c.Nvx][c.Nvy][c.Nvz][c.Nz]);
  complex_field<double,3> hf(boost::extents[c.Nvx][c.Nvy][c.Nvz][c.Nz]);

  const double Kx = 0.5;
  f.range.vx_min = -5.; f.range.vx_max = 5.;
  f.range.vy_min = -5.; f.range.vy_max = 5.;
  f.range.vz_min = -5.; f.range.vz_max = 5.;
  f.range.z_min =  0.;  f.range.z_max = 2./Kx*math::pi<double>();
  f.compute_steps();

  double dt = 0.1;

  ublas::vector<double> vx(c.Nv,0.),vy(c.Nv,0.),vz(c.Nv,0.);
  std::generate( vx.begin() , vx.end() , [&,k=0]() mutable {return (k++)*f.step.dvx+f.range.vx_min;} );
  std::generate( vy.begin() , vy.end() , [&,k=0]() mutable {return (k++)*f.step.dvy+f.range.vy_min;} );
  std::generate( vz.begin() , vz.end() , [&,k=0]() mutable {return (k++)*f.step.dvz+f.range.vz_min;} );

  ublas::vector<double> kx(c.Nx); // beware, Nx need to be odd
  {
    double l = f.range.len_z();
    for ( auto i=0u ; i<c.Nx/2 ; ++i ) { kx[i]      = 2.*math::pi<double>()*i/l; }
    for ( int i=-c.Nx/2 ; i<0 ; ++i ) { kx[c.Nx+i] = 2.*math::pi<double>()*i/l; }
  }

  auto M1 = maxwellian(1.,{0.,0.,0.},1.);
  for (std::size_t k_x=0u ; k_x<f.size(0) ; ++k_x ) {
    for (std::size_t k_y=0u ; k_y<f.size(1) ; ++k_y ) {
      for (std::size_t k_z=0u ; k_z<f.size(2) ; ++k_z ) {
        for (std::size_t i=0u ; i<f.size_x() ; ++i ) {
          f[k_x][k_y][k_z][i] = M1( Zi(i),Vkx(k_x),Vky(k_y),Vkz(k_z) ) * (1. + 0.5*std::cos(Kx*Zi(i)));
        }
        fft::fft(f[k_x][k_y][k_z].begin(),f[k_x][k_y][k_z].end(),hf[k_x][k_y][k_z].begin());
      }
    }
  }

  const double B0 = 1.;
  ublas::vector<double> Ex(c.Nz,0.),Ey(c.Nz,0.);
  ublas::vector<double> jcx(c.Nz,0.),jcy(c.Nz,0.);

  std::vector<double> electric_energy;  electric_energy.reserve(100);
  std::vector<double> electric_energy_x; electric_energy_x.reserve(100);
  std::vector<double> electric_energy_y; electric_energy_y.reserve(100);
  std::vector<double> times; times.reserve(100);

  hybird1dx3dv_b0<double> Lie( f , f.range.len_z() , B0 );
  double current_t = 0.;
  times.push_back(0.);

  auto compute_electric_energy = [&]( const ublas::vector<double> & E ) {
    double electric_energy = 0.;
    for ( const auto & ei : E ) { electric_energy += ei*ei*f.step.dz; }
      return electric_energy;
  };
  double eex=0.,eey=0.;
  eex = compute_electric_energy(Ex); eey = compute_electric_energy(Ey);
  electric_energy_x.push_back(std::sqrt(eex));
  electric_energy_y.push_back(std::sqrt(eey));
  electric_energy.push_back(std::sqrt(eex+eey));

  while ( current_t<c.Tf ) {
    std::cout << "\r" << current_t << " / " << c.Tf << std::flush;

/*
    Lie.H_E_tilde(dt,jcx,jcy,Ex,Ey,hf);
    //for ( int i = 0 ; i<c.Nz ; ++i ) { jcx[i] = 0.; jcy[i] = 0.; }
    Lie.H_jc(dt,jcx,jcy,Ex,Ey,hf);
    //for ( int i = 0 ; i<c.Nz ; ++i ) { jcx[i] = 0.; jcy[i] = 0.; }
    Lie.H_f_tilde(dt,jcx,jcy,Ex,Ey,hf);
    //for ( int i = 0 ; i<c.Nz ; ++i ) { jcx[i] = 0.; jcy[i] = 0.; }
*/

    Lie.H_E(dt,jcx,jcy,Ex,Ey,hf);
    //for ( int i = 0 ; i<c.Nz ; ++i ) { jcx[i] = 0.; jcy[i] = 0.; }

    Lie.H_jc(dt,jcx,jcy,Ex,Ey,hf);
    //for ( int i = 0 ; i<c.Nz ; ++i ) { jcx[i] = 0.; jcy[i] = 0.; }

    Lie.H_f(dt,jcx,jcy,Ex,Ey,hf);
    //for ( int i = 0 ; i<c.Nz ; ++i ) { jcx[i] = 0.; jcy[i] = 0.; }

    eex = compute_electric_energy(Ex); eey = compute_electric_energy(Ey);
    electric_energy.push_back( std::sqrt(eex+eey) );
    electric_energy_x.push_back(std::sqrt(eex));
    electric_energy_y.push_back(std::sqrt(eey));
    current_t += dt;
    times.push_back(current_t);
  }

  auto dt_y = [&,count=0] (auto const& y) mutable {
    std::stringstream ss; ss<<times[count++]<<" "<<y;
    return ss.str();
  };
  c << monitoring::data( "ee_tilde.dat"  , electric_energy   , dt_y );
  c << monitoring::data( "eex_tilde.dat" , electric_energy_x , dt_y );
  c << monitoring::data( "eey_tilde.dat" , electric_energy_y , dt_y );

  return 0;
}


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

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <geometry_test_common.hpp>

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

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/algorithms/make.hpp>

#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/geometries/box.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename G>
void test_get_set()
{
    typedef typename bg::coordinate_type<G>::type coordinate_type;

    G g;
    bg::set<0>(g, coordinate_type(1));
    bg::set<1>(g, coordinate_type(2));

    coordinate_type x = bg::get<0>(g);
    coordinate_type y = bg::get<1>(g);

    BOOST_CHECK_CLOSE(double(x), 1.0, 0.0001);
    BOOST_CHECK_CLOSE(double(y), 2.0, 0.0001);
}

template <typename G>
void test_indexed_get_set(G& g)
{
    bg::set<0, 0>(g, 1);
    bg::set<0, 1>(g, 2);
    bg::set<1, 0>(g, 3);
    bg::set<1, 1>(g, 4);

    typedef typename bg::coordinate_type<G>::type coordinate_type;
    coordinate_type x1 = bg::get<0, 0>(g);
    coordinate_type y1 = bg::get<0, 1>(g);
    coordinate_type x2 = bg::get<1, 0>(g);
    coordinate_type y2 = bg::get<1, 1>(g);

    BOOST_CHECK_CLOSE(double(x1), 1.0, 0.0001);
    BOOST_CHECK_CLOSE(double(y1), 2.0, 0.0001);
    BOOST_CHECK_CLOSE(double(x2), 3.0, 0.0001);
    BOOST_CHECK_CLOSE(double(y2), 4.0, 0.0001);
}

template <typename G, typename T>
void test_indexed_get(G const& g, T a, T b, T c, T d)
{
    T x1 = bg::get<0, 0>(g);
    T y1 = bg::get<0, 1>(g);
    T x2 = bg::get<1, 0>(g);
    T y2 = bg::get<1, 1>(g);

    BOOST_CHECK_CLOSE(double(x1), double(a), 0.0001);
    BOOST_CHECK_CLOSE(double(y1), double(b), 0.0001);
    BOOST_CHECK_CLOSE(double(x2), double(c), 0.0001);
    BOOST_CHECK_CLOSE(double(y2), double(d), 0.0001);
}

template <typename P>
void test_all()
{
    typedef typename bg::coordinate_type<P>::type coordinate_type;

    // POINT, setting coordinate
    test_get_set<P>();

    // BOX, setting left/right/top/bottom
    bg::model::box<P> b;
    test_indexed_get_set(b);

    // SEGMENT (in GGL not having default constructor; however that is not a requirement)
    P p1 = bg::make_zero<P>();
    P p2 = bg::make_zero<P>();
    bg::model::referring_segment<P> s(p1, p2);
    test_indexed_get_set(s);

    // CONST SEGMENT
    bg::set<0>(p1, 1); // we don't use assign because dim in {2,3}
    bg::set<1>(p1, 2);
    bg::set<0>(p2, 3);
    bg::set<1>(p2, 4);
    bg::model::referring_segment<P const> cs(p1, p2);
    test_indexed_get(cs,
        coordinate_type(1), coordinate_type(2),
        coordinate_type(3), coordinate_type(4));
}

int test_main(int, char* [])
{
    test_get_set<int[2]>();
    test_get_set<float[2]>();
    test_get_set<double[2]>();
    test_get_set<double[3]>();

    test_get_set<boost::tuple<double, double> >();

    test_all<bg::model::point<int, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<double, 2, bg::cs::cartesian> >();

    return 0;
}


#include <glm/models/binomial.hpp>

#include <armadillo>

using namespace arma;

binomial::binomial(const std::string &link_name)
    : glm_model( "binomial", link_name )
{
}

binomial::binomial(glm_link *link)
    : glm_model( "binomial", link )
{
}

binomial::~binomial()
{
}

bool
binomial::valid_mu(const arma::vec &mu) const
{
    for(int i = 0; i < mu.n_elem; i++)
    {
        if( mu[ i ] <= 0.0 || mu[ i ] >= 1.0 )
        {
            return false;
        }
    }

    return true;
}

vec
binomial::var(const arma::vec &mu) const
{
    return mu % ( 1.0 - mu );
}

double
binomial::dispersion(const arma::vec &mu, const arma::vec &y, const arma::uvec &missing, float k) const
{
    return 1.0;
}

double
binomial::likelihood(const arma::vec &mu, const arma::vec &y, const arma::uvec &missing, float dispersion) const
{
    double loglikelihood = 0.0;
    for(int i = 0; i < y.n_elem; i++)
    {
        if( missing[ i ] == 0 )
        {
            loglikelihood += y[ i ] * log( mu[ i ] ) + ( 1 - y[ i ] ) * log( 1 - mu[ i ] );
        }
    }

    return loglikelihood;
}

bool
binomial::is_binary() const
{
    return true;
}


#include <string>

#define BOOST_TEST_MODULE SparseCompareTests

#include "MIAConfig.h"

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif
#include "DenseMIA.h"
#include "SparseMIA.h"

template<typename MIAType, typename DenseMIAType>
void do_work(size_t dim1, size_t dim2, size_t dim3){

    MIAType test1(dim1,dim2,dim3), test2(dim1,dim2,dim3);
    test1.resize(2);
    test2.resize(2);
    std::fill(test1.data_begin(),test1.data_end(),1);
    std::fill(test2.data_begin(),test2.data_end(),1);
    *(test1.index_begin())=1;*(test2.index_begin())=1;
    *(test1.index_begin()+1)=2;*(test2.index_begin()+1)=2;
    BOOST_CHECK_MESSAGE(test1==test2,std::string("Comparison test 1 for ")+typeid(MIAType).name());
    *(test1.index_begin()+1)=2;*(test2.index_begin()+1)=3;
    BOOST_CHECK_MESSAGE(test1!=test2,std::string("Comparison test 2 for ")+typeid(MIAType).name());
    *(test1.index_begin()+1)=2;*(test2.index_begin()+1)=2;
    *(test1.data_begin())=2;
    BOOST_CHECK_MESSAGE(test1!=test2,std::string("Comparison test 3 for ")+typeid(MIAType).name());

    test1.resize(test1.dimensionality()/2);

    test1.randu(-50,50);
    test1.rand_indices();
    test1.collect_duplicates();
    test2.resize(test1.size());
    std::copy(test1.storage_begin(),test1.storage_end(),test2.storage_begin());

    BOOST_CHECK_MESSAGE(test1==test2,std::string("Comparison test 4 for ")+typeid(MIAType).name());

    //try comparing against a DenseMIA
    test1.resize(2);
    std::fill(test1.data_begin(),test1.data_end(),1);
    *(test1.index_begin())=1;*(test1.index_begin()+1)=2;

    DenseMIAType denseTest0;
    MIAType test0;
    BOOST_CHECK_MESSAGE(test0==denseTest0,std::string("Empty Dense Comparison for ")+typeid(MIAType).name());

    DenseMIAType denseTest1(dim1,dim2,dim3);
    denseTest1.atIdx(1)=1;
    denseTest1.atIdx(2)=1;
    BOOST_CHECK_MESSAGE(test1==denseTest1,std::string("Dense Comparison test 1 for ")+typeid(MIAType).name());
    BOOST_CHECK_MESSAGE(denseTest1==test1,std::string("Dense Comparison test 2 for ")+typeid(MIAType).name());
    denseTest1.atIdx(2)=2;
    BOOST_CHECK_MESSAGE(test1!=denseTest1,std::string("Dense Comparison test 3 for ")+typeid(MIAType).name());
    BOOST_CHECK_MESSAGE(denseTest1!=test1,std::string("Dense Comparison test 4 for ")+typeid(MIAType).name());
    denseTest1.atIdx(2)=1;
    denseTest1.atIdx(3)=2;
    BOOST_CHECK_MESSAGE(test1!=denseTest1,std::string("Dense Comparison test 5 for ")+typeid(MIAType).name());
    BOOST_CHECK_MESSAGE(denseTest1!=test1,std::string("Dense Comparison test 6 for ")+typeid(MIAType).name());

    BOOST_CHECK_MESSAGE(test1!=denseTest0,std::string("Empty/Non-empty Dense Comparison 1 for ")+typeid(MIAType).name());

    BOOST_CHECK_MESSAGE(test0!=denseTest1,std::string("Empty/Non-empty Dense Comparison 1 for ")+typeid(MIAType).name());

    denseTest1.randu(0,50);
    for(auto it=denseTest1.data_begin();it<denseTest1.data_end();++it)
        if(*it<40)
            *it=0;
    test1=denseTest1;
    BOOST_CHECK_MESSAGE(test1==denseTest1,std::string("Dense Comparison random test 1 for ")+typeid(MIAType).name());
}

BOOST_AUTO_TEST_CASE( SparseCompareTests )
{

    size_t dim1=3,dim2=4,dim3=5;
    //testing to make sure a copy constructor doesn't get called erroneously
    do_work<LibMIA::SparseMIA<float,3>, LibMIA::DenseMIA<float,3> >(dim1,dim2,dim3);
    do_work<LibMIA::SparseMIA<double,3>, LibMIA::DenseMIA<double,3> >(dim1,dim2,dim3);
    do_work<LibMIA::SparseMIA<int32_t,3>,LibMIA::DenseMIA<int32_t,3> >(dim1,dim2,dim3);
    do_work<LibMIA::SparseMIA<int64_t,3>,LibMIA::DenseMIA<int64_t,3> >(dim1,dim2,dim3);
}


//------------------------------------------------------------------------------
//                         SparseMatrixUtil1
//------------------------------------------------------------------------------
// GMAT: General Mission Analysis Tool.
//
// Copyright (c) 2002 - 2020 United States Government as represented by the
// Administrator of The National Aeronautics and Space Administration.
// All Other Rights Reserved.
//
// Author: Youngkwang Kim
// Created: 2015/07/08
//
// NOTE: This is a static utility class.  The constructors, destructor, and
// operator= are not implemented.  No instances of this class can be
// instantiated.
/**
 */
//------------------------------------------------------------------------------
#ifndef SparseMatrixUtil_hpp
#define SparseMatrixUtil_hpp

/// for std::vector's min_element, max_element
#include <algorithm>
#include "csaltdefs.hpp"
#include "Rvector.hpp"
#include "Rmatrix.hpp"

// ublas related preprocessor starts
/// we are not using any BOOST libraries
#define BOOST_ALL_NO_LIB
/// turning off unnecessary checks in debug mode.
#define BOOST_UBLAS_NDEBUG
/// include ublas related headers
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
// ublas related inclusion ends

using boost::numeric::ublas::compressed_matrix;

// define RSMatrix; they can be replaced with other
// data types.
typedef compressed_matrix<Real> RSMatrix;

// it is recommended not to use Iterators in the
// outside of sparse matrix utility!
// because using Iterators are BOOST dependent!

//typedef compressed_matrix<Real>::iterator1 Iterator1;
//typedef compressed_matrix<Real>::iterator2 Iterator2;
//typedef compressed_matrix<Real>::const_iterator1 ConstIterator1;
//typedef compressed_matrix<Real>::const_iterator2 ConstIterator2;

class CSALT_API SparseMatrixUtil
{
public:

   // set methods

   /// set the element of sparse matrix
   static void SetElement(RSMatrix  &spMat,
                          Integer rowIdx, Integer colIdx, Real value);

   /// set the size/resize of the sparse matrix
   static void SetSize(RSMatrix &spMat,
                       Integer rowSize, Integer colSize);

   /// set the sparsity pattern of the entire sparse matrix with zeros or ones
   /// NOTE that: changing sparsity pattern takes some time;
   ///      so it is desirable to set sparsity pattern with zeros, first;
   ///      then fill the values at the last.
   ///      this approach it useful for user jacobian and hessian matrices.
   static void SetSparsityPattern(RSMatrix &spMat,
                                  Integer  rowSize, Integer colSize,
                                  const IntegerArray *rowIdxVec,
                                  const IntegerArray *colIdxVec,
                                  bool  hasZeros = false);

   /// set the data of the entire sparse matrix
   static void SetSparseMatrix(RSMatrix &spMat,
                               Integer rowSize, Integer colSize,
                               const IntegerArray *rowIdxVec,
                               const IntegerArray *colIdxVec,
                               const std::vector<Real> *valueVec);

   /// set the data of the entire sparse matrix
   static void SetSparseMatrix(RSMatrix &spMat,
                               Integer rowSize, Integer colSize,
                               const IntegerArray *rowIdxVec,
                               const IntegerArray *colIdxVec,
                               const Rvector *valueVec);

   /// set the data of a part of the sparse matrix
   /// if the isNotAdding option is off, then
   /// the addition to the existing sparse matrix is done.
   static void SetSparseBLockMatrix(RSMatrix  &spMat,
                                    Integer   rowOffSet, Integer colOffSet,
                                    const IntegerArray *rowIdxVec,
                                    const IntegerArray *colIdxVec,
                                    const std::vector<Real> *valueVec,
                                    bool isNotAdding = true);

   /// set the data of a part of the sparse matrix
   /// if the isNotAdding option is off, then
   /// the addition to the existing sparse matrix is done.
   static void SetSparseBLockMatrix(RSMatrix  &spMat,
                                    Integer rowOffSet, Integer colOffSet,
                                    const IntegerArray *rowIdxVec,
                                    const IntegerArray *colIdxVec,
                                    const Rvector *valueVec,
                                    bool isNotAdding = true);

   /// set the data of a part of the sparse matrix
   /// if the isNotAdding option is off, then
   /// the addition to the existing sparse matrix is done.
   static void SetSparseBLockMatrix(RSMatrix  &spMat,
                                    Integer rowOffSet, Integer colOffSet,
                                    const RSMatrix  *spBlockMat,
                                    bool isNotAdding = true);

   /// set the data of a part of the sparse matrix
   /// if the isNotAdding option is off, then
   /// the addition to the existing sparse matrix is done.
   static void SetSparseBLockMatrix(RSMatrix  &spMat,
                                    Integer rowOffSet, Integer colOffSet,
                                    const Rmatrix  *blockMat,
                                    bool isNotAdding = true);

   /// get methods
   static Real GetElement(const RSMatrix  *spMat,
                          Integer rowIdx, Integer colIdx);

   static Integer GetNumRows(const RSMatrix  *spMat);
   static Integer GetNumColumns(const RSMatrix  *spMat);

   /// get the sparsity pattern of the sparse matrix
   static RSMatrix GetSparsityPattern(const RSMatrix *spMat,
                                      bool hasZeros = false);

   /// get the sparsity pattern of the sparse matrix
   static void GetSparsityPattern(const RSMatrix *spMat,
                                 IntegerArray &rowIdxVec,
                                 IntegerArray &colIdxVec);

   /// set the data of the entire sparse matrix
   static void GetThreeVectorForm(const RSMatrix  *spMat,
                                 IntegerArray &rowIdxVec,
                                 IntegerArray &colIdxVec,
                                 std::vector<Real> &valueVec);

   /// set the data of the entire sparse matrix
   static void GetThreeVectorForm(const RSMatrix *spMat,
                                 IntegerArray &rowIdxVec,
                                 IntegerArray &colIdxVec,
                                 Rvector &valueVec);

   /// set the data of a part of the sparse matrix
   static void GetThreeVectorForm(const RSMatrix *spMat,
                                 Integer rowIdxLB, Integer rowIdxUB,
                                 Integer colIdxLB, Integer colIdxUB,
                                 IntegerArray &rowIdxVec,
                                 IntegerArray &colIdxVec,
                                 std::vector<Real> &valueVec);

   /// set the data of a part of the sparse matrix
   static void GetThreeVectorForm(const RSMatrix *spMat,
                                 Integer rowIdxLB, Integer rowIdxUB,
                                 Integer colIdxLB, Integer colIdxUB,
                                 IntegerArray &rowIdxVec,
                                 IntegerArray &colIdxVec,
                                 Rvector &valueVec);

   /// get the number of non zero elements of the sparse matrix
   static Integer GetNumNonZeroElements(const RSMatrix *spMat);

   /// Get sum(abs(element)) throughout a sparse matrix.
   /// useful especially when calculating errors
   static Real GetAbsTotalSum(const RSMatrix *spMat);

   // various utilities

   /// perform product between sparse matrix and dense vector
   /// B = A*V (initialize = true); where A and B are sparse matrices, and
   /// V is a dense vector
   /// B += A*V (initialize = false); where A and B are sparse matrices, and
   /// V is a dense vector
   static void fast_prod(const RSMatrix *spMat,
                         const std::vector<Real> *vec,
                         std::vector<Real> &result, bool initialize = true);

   /// perform product between sparse matrix and dense vector
   /// B = A*V (initialize = true); where A and B are sparse matrices, and
   /// V is a dense vector
   /// B += A*V (initialize = false); where A and B are sparse matrices, and
   /// V is a dense vector
   static void fast_prod(const RSMatrix *spMat,
                         const Rvector  *vec,
                         Rvector &result, bool initialize = true);

   /// perform C = A*B (initialize = true);
   /// or C+= A*B (initialize = false) using axpy_prod of
   /// SparseMatrixUtil::ublas
   static void fast_prod(const RSMatrix *spMat1,
                         const RSMatrix *spMat2,
                         RSMatrix &resultMat,
                         bool initialize = true);

   /// print all the nonzero elements to the screen
   static void PrintNonZeroElements(const RSMatrix *spMat,
                                    bool rowOrder = true);

   /// replicate sparse block matrix pattern with row and column repetition and
   /// return spMat
   static void ReplicateSparseMatrix(const RSMatrix *spBlockMat,
                                    Integer numRowRepetition,
                                    Integer numColRepetition,
                                    RSMatrix &spMat);

   /// check whether the given matrix is zero matrix or not
   static bool IsZeroMatrix(const RSMatrix *spMat);

   // convert RSMatrix to Rmatrix
   static Rmatrix RSMatrixToRmatrix(const RSMatrix *spMat,
                                    Integer rowLB, Integer rowUB,
                                    Integer colLB, Integer colUB);

   // convert RSMatrix to Rmatrix
   static Rmatrix RSMatrixToRmatrix(const RSMatrix *spMat);

   /// copy the data of the input matrix into output matrix
   static RSMatrix CopySparseMatrix(const RSMatrix *spMat);

   /// copy the data of the input matrix into output matrix
   static void CopySparseMatrix(const RSMatrix &copyFrom, RSMatrix &copyTo);

private:
   /// private constructors, destructor, operator=   *** UNIMPLEMENTED ***
   SparseMatrixUtil();
   SparseMatrixUtil(const SparseMatrixUtil &copy);
   SparseMatrixUtil& operator=(const SparseMatrixUtil &copy);
   ~SparseMatrixUtil();
};

#endif // SparseMatrixUtil_hpp


#include <iostream>
#include <fstream>
#include <vector>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/impl/kdtree.hpp>
#include <pcl/filters/voxel_grid.h>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

ros::Publisher pub_cloud;

sensor_msgs::PointCloud2 local_map_pcl;
sensor_msgs::PointCloud2 local_depth_pcl;

ros::Subscriber odom_sub;
ros::Subscriber global_map_sub, local_map_sub;

ros::Timer local_sensing_timer;

bool has_global_map(false);
bool has_local_map(false);
bool has_odom(false);

nav_msgs::Odometry _odom;

double sensing_horizon, sensing_rate, estimation_rate;
double _x_size, _y_size, _z_size;
double _gl_xl, _gl_yl, _gl_zl;
double _resolution, _inv_resolution;
int _GLX_SIZE, _GLY_SIZE, _GLZ_SIZE;

ros::Time last_odom_stamp = ros::TIME_MAX;

inline Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i & index)
{
    Eigen::Vector3d pt;
    pt(0) = ((double)index(0) + 0.5) * _resolution + _gl_xl;
    pt(1) = ((double)index(1) + 0.5) * _resolution + _gl_yl;
    pt(2) = ((double)index(2) + 0.5) * _resolution + _gl_zl;

    return pt;
};

inline Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d & pt)
{
    Eigen::Vector3i idx;
    idx(0) = std::min( std::max( int( (pt(0) - _gl_xl) * _inv_resolution), 0), _GLX_SIZE - 1);
    idx(1) = std::min( std::max( int( (pt(1) - _gl_yl) * _inv_resolution), 0), _GLY_SIZE - 1);
    idx(2) = std::min( std::max( int( (pt(2) - _gl_zl) * _inv_resolution), 0), _GLZ_SIZE - 1);

    return idx;
};

void rcvOdometryCallbck(const nav_msgs::Odometry& odom)
{
  /*if(!has_global_map)
    return;*/
  has_odom = true;
  _odom = odom;
}

pcl::PointCloud<pcl::PointXYZ> _cloud_all_map, _local_map;
pcl::VoxelGrid<pcl::PointXYZ>  _voxel_sampler;
sensor_msgs::PointCloud2 _local_map_pcd;

pcl::search::KdTree<pcl::PointXYZ> _kdtreeLocalMap;
vector<int>     _pointIdxRadiusSearch;
vector<float>   _pointRadiusSquaredDistance;

void rcvGlobalPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map )
{
  if(has_global_map)
    return;

  ROS_WARN("[Depth Simulator] Global Pointcloud received..");

  pcl::PointCloud<pcl::PointXYZ> cloud_input;
  pcl::fromROSMsg(pointcloud_map, cloud_input);

  _voxel_sampler.setLeafSize(0.1f, 0.1f, 0.1f);
  _voxel_sampler.setInputCloud( cloud_input.makeShared() );
  _voxel_sampler.filter( _cloud_all_map );

  _kdtreeLocalMap.setInputCloud( _cloud_all_map.makeShared() );

  has_global_map = true;
  global_map_sub.shutdown();
}

void renderSensedPoints(const ros::TimerEvent & event)
{
    if(!has_global_map || !has_odom)
       return;

    Eigen::Quaterniond q;
    q.x() = _odom.pose.pose.orientation.x;
    q.y() = _odom.pose.pose.orientation.y;
    q.z() = _odom.pose.pose.orientation.z;
    q.w() = _odom.pose.pose.orientation.w;

    Eigen::Matrix3d rot;
    rot = q;
    Eigen::Vector3d yaw_vec = rot.col(0);

    _local_map.points.clear();
    pcl::PointXYZ searchPoint(_odom.pose.pose.position.x, _odom.pose.pose.position.y, _odom.pose.pose.position.z);
    _pointIdxRadiusSearch.clear();
    _pointRadiusSquaredDistance.clear();

    pcl::PointXYZ pt;
    if ( _kdtreeLocalMap.radiusSearch (searchPoint, sensing_horizon, _pointIdxRadiusSearch, _pointRadiusSquaredDistance) > 0 )
    {
       for (size_t i = 0; i < _pointIdxRadiusSearch.size (); ++i)
       {
          pt = _cloud_all_map.points[_pointIdxRadiusSearch[i]];

          if( (fabs(pt.z - _odom.pose.pose.position.z) / (sensing_horizon)) > tan(M_PI / 12.0) )
              continue;

          Vector3d pt_vec(pt.x - _odom.pose.pose.position.x, pt.y - _odom.pose.pose.position.y, pt.z - _odom.pose.pose.position.z);

          if(pt_vec.dot(yaw_vec) < 0 )
            continue;

          _local_map.points.push_back(pt);
       }
    }
    else
    {
       return;
    }

    _local_map.width = _local_map.points.size();
    _local_map.height = 1;
    _local_map.is_dense = true;

    pcl::toROSMsg(_local_map, _local_map_pcd);
    _local_map_pcd.header.frame_id = "map";

    pub_cloud.publish(_local_map_pcd);
}

void rcvLocalPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map )
{
  // do nothing, fix later
}

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

  nh.getParam("sensing_horizon", sensing_horizon);
  nh.getParam("sensing_rate",    sensing_rate);
  nh.getParam("estimation_rate", estimation_rate);

  nh.getParam("map/x_size",     _x_size);
  nh.getParam("map/y_size",     _y_size);
  nh.getParam("map/z_size",     _z_size);

  //subscribe point cloud
  global_map_sub = nh.subscribe( "global_map", 1,  rcvGlobalPointCloudCallBack);
  local_map_sub  = nh.subscribe( "local_map",  1,  rcvLocalPointCloudCallBack);
  odom_sub       = nh.subscribe( "odometry",   50, rcvOdometryCallbck   );

  //publisher depth image and color image
  pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("local_pointcloud", 10);

  double sensing_duration  = 1.0 / sensing_rate * 2.5;

  local_sensing_timer = nh.createTimer(ros::Duration(sensing_duration),  renderSensedPoints);

  _inv_resolution = 1.0 / _resolution;

  _gl_xl = -_x_size/2.0;
  _gl_yl = -_y_size/2.0;
  _gl_zl =   0.0;

  _GLX_SIZE = (int)(_x_size * _inv_resolution);
  _GLY_SIZE = (int)(_y_size * _inv_resolution);
  _GLZ_SIZE = (int)(_z_size * _inv_resolution);

  ros::Rate rate(100);
  bool status = ros::ok();
  while(status)
  {
    ros::spinOnce();
    status = ros::ok();
    rate.sleep();
  }
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

//
// Compare arithmetic results using fixed_int to GMP results.
//

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/multiprecision/cpp_int.hpp>
#include "test.hpp"

template <class Number, class BigNumber>
void test()
{
   using namespace boost::multiprecision;
   typedef Number test_type;

   test_type h = (std::numeric_limits<test_type>::max)();
   test_type l = (std::numeric_limits<test_type>::max)();
   BigNumber r;

   add(r, h, h);
   BOOST_CHECK_EQUAL(r, cpp_int(h) + cpp_int(h));

   multiply(r, h, h);
   BOOST_CHECK_EQUAL(r, cpp_int(h) * cpp_int(h));

   if(std::numeric_limits<test_type>::is_signed)
   {
      subtract(r, l, h);
      BOOST_CHECK_EQUAL(r, cpp_int(l) - cpp_int(h));
      subtract(r, h, l);
      BOOST_CHECK_EQUAL(r, cpp_int(h) - cpp_int(l));
      multiply(r, l, l);
      BOOST_CHECK_EQUAL(r, cpp_int(l) * cpp_int(l));
   }

   //
   // Try again with integer types as the source:
   //
   enum{ max_digits = std::numeric_limits<test_type>::is_signed ? std::numeric_limits<long long>::digits : std::numeric_limits<unsigned long long>::digits };
   enum{ require_digits = std::numeric_limits<test_type>::digits <= 2 * max_digits ? std::numeric_limits<test_type>::digits / 2 : max_digits };
   typedef typename boost::uint_t<require_digits>::least uint_least;
   typedef typename boost::int_t<require_digits>::least  int_least;
   typedef typename boost::mpl::if_c<std::numeric_limits<test_type>::is_signed, int_least, uint_least>::type i_type;

   i_type ih = (std::numeric_limits<i_type>::max)();
   i_type il = (std::numeric_limits<i_type>::max)();

   add(r, ih, ih);
   BOOST_CHECK_EQUAL(r, cpp_int(ih) + cpp_int(ih));

   multiply(r, ih, ih);
   BOOST_CHECK_EQUAL(r, cpp_int(ih) * cpp_int(ih));

   if(std::numeric_limits<test_type>::is_signed)
   {
      subtract(r, il, ih);
      BOOST_CHECK_EQUAL(r, cpp_int(il) - cpp_int(ih));
      subtract(r, ih, il);
      BOOST_CHECK_EQUAL(r, cpp_int(ih) - cpp_int(il));
      multiply(r, il, il);
      BOOST_CHECK_EQUAL(r, cpp_int(il) * cpp_int(il));
   }
}

void test_rational_mixed()
{
   using namespace boost::multiprecision;
   cpp_int a(2);
   cpp_rational r(10);

   BOOST_CHECK_EQUAL(a + -r, -8);
   BOOST_CHECK_EQUAL(-r + a, -8);
   BOOST_CHECK_EQUAL(-a + r, 8);
   BOOST_CHECK_EQUAL(r + -a, 8);

   BOOST_CHECK_EQUAL(a - -r, 12);
   BOOST_CHECK_EQUAL(-r - a, -12);
   BOOST_CHECK_EQUAL(-a - r, -12);
   BOOST_CHECK_EQUAL(r - -a, 12);

   BOOST_CHECK_EQUAL(a * -r, -20);
   BOOST_CHECK_EQUAL(-r * a, -20);
   BOOST_CHECK_EQUAL(-a * r, -20);
   BOOST_CHECK_EQUAL(r * -a, -20);

   BOOST_CHECK_EQUAL(a / -r, cpp_rational(-2, 10));
   BOOST_CHECK_EQUAL(-r / a, -5);
   BOOST_CHECK_EQUAL(cpp_rational(-a / r), cpp_rational(-2, 10));
   BOOST_CHECK_EQUAL(r / -a, -5);
}

int main()
{
   using namespace boost::multiprecision;

   test_rational_mixed();

   test<checked_int512_t, checked_int1024_t>();
   test<checked_int256_t, checked_int512_t>();
   test<number<cpp_int_backend<64, 64, signed_magnitude, checked, void>, et_off>, checked_int128_t>();
   test<boost::int64_t, checked_int128_t>();

   test<checked_uint512_t, checked_uint1024_t>();
   test<checked_uint256_t, checked_uint512_t>();
   test<number<cpp_int_backend<64, 64, unsigned_magnitude, checked, void>, et_off>, checked_uint128_t>();
   test<boost::uint64_t, checked_int128_t>();

   return boost::report_errors();
}


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

#include <nt2/table.hpp>
#include <nt2/include/functions/qr.hpp>
#include <nt2/include/functions/rand.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <magma.h>
#include "../../flops/qr.hpp"
#include <boost/fusion/include/at.hpp>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/absolute_time.hpp>
#include <nt2/sdk/bench/metric/gflops.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/constant.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;

template<typename T> struct qr_nt2
{
  template<typename Setup>
  qr_nt2(Setup const& s)
              :  w(boost::fusion::at_c<1>(s))
              ,  h(boost::fusion::at_c<0>(s))
  {
    result = nt2::zeros(h,w, nt2::meta::as_<T>());
    input  = nt2::rand(h,w, nt2::meta::as_<T>());
    }

  void operator()()
  {
  result = nt2::qr(input);
  }

  friend std::ostream& operator<<(std::ostream& os, qr_nt2<T> const& p)
  {
    return os << "(" << p.size() << ")";
  }

  std::size_t size() const { return h*w; }
  std::size_t flops() const {
  return (FLOPS_GEQRF(h,w)/size_t());
  }

private:
  std::size_t w,h,nr;
  nt2::table<T> input, result;
};

NT2_REGISTER_BENCHMARK_TPL( qr_nt2, (float)(double) )
{
  std::size_t size_min = args("size_min", 16);
  std::size_t size_max = args("size_max", 4096);
  std::size_t size_step = args("size_step", 10);

  run_during_with< qr_nt2<T> > ( 1.
                                , and_( geometric(size_min,size_max,size_step)
                                      , geometric(size_min,size_max,size_step)
                                      )
                                , gflops<stats::median_>()
                                );
}


/**
 * @file
 * Accumulates and process WebSocket and PCP Association timing stats.
 */

#pragma once

#include <pcp-test/connection_stats.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>

#include <boost/nowide/fstream.hpp>

#include <ostream>
#include <chrono>
#include <mutex>
#include <stdint.h>
#include <string>

namespace pcp_test {

using timing_accumulator_t = boost::accumulators::accumulator_set<
                                uint32_t,
                                boost::accumulators::stats<
                                    boost::accumulators::tag::max,
                                    boost::accumulators::tag::mean,
                                    boost::accumulators::tag::variance>>;
struct stats
{
    double   mean;
    double   stddev;
    uint32_t max;
    uint32_t count;

    stats();
    explicit stats(const timing_accumulator_t& a);
};

struct connection_stats
{
    stats tcp_us;
    stats ws_open_handshake_us;
    stats ws_close_handshake_us;
    stats association_ms;
    stats session_duration_ms;

    friend std::ostream& operator<< (std::ostream& out_stream,
                                     const connection_stats& c_s);

    friend std::ofstream & operator<< (boost::nowide::ofstream& out_stream,
                                       const connection_stats& c_s);
};

struct connection_timings_accumulator
{
  public:
    void accumulate_tcp_us(uint32_t interval);
    void accumulate_ws_open_handshake_us(uint32_t interval);
    void accumulate_ws_close_handshake_us(uint32_t interval);
    void accumulate_association_ms(uint32_t interval);
    void accumulate_session_duration_ms(uint32_t interval);

    connection_stats get_connection_stats() const;

  private:
    // Synchronizes accumulate() access to state
    mutable std::mutex the_mutex_;

    timing_accumulator_t tcp_us_acc_;
    timing_accumulator_t ws_open_handshake_us_acc_;
    timing_accumulator_t ws_close_handshake_us_acc_;
    timing_accumulator_t association_ms_acc_;
    timing_accumulator_t session_duration_ms_acc_;
};

}  // namespace pcp_test


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file iterative_rounding_example.cpp
 * @brief Iterative rounding example
 * This is an example implementation of an algorithm within the Iterative
 * Rounding framework.
 * The implemented algorithm is a vertex cover 2-approximation.
 * @author Piotr Godlewski
 * @version 1.0
 * @date 2014-03-24
 */

//! [Iterative Rounding Problem Example]
#include "paal/iterative_rounding/iterative_rounding.hpp"
#include "paal/utils/floating.hpp"
#include "paal/utils/functors.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/range/iterator_range.hpp>

#include <iostream>
#include <unordered_map>

namespace ir = paal::ir;

template <typename Graph, typename CostMap, typename OutputIter>
class vertex_cover {
public:
    vertex_cover(const Graph & g, CostMap cost_map, OutputIter cover) :
        m_g(g), m_cost_map(cost_map), m_cover(cover) {}

    using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
    using VertexMap = std::unordered_map<Vertex, paal::lp::col_id>;

    const Graph &get_graph() const { return m_g; }

    auto get_cost(Vertex v)->decltype(std::declval<CostMap>()(v)) {
        return m_cost_map(v);
    }

    void bind_col_to_vertex(paal::lp::col_id col, Vertex v) {
        m_vertex_map.insert(typename VertexMap::value_type(v, col));
    }

    paal::lp::col_id vertex_to_column(Vertex v) { return m_vertex_map[v]; }

    void add_to_cover(Vertex v) {
        *m_cover = v;
        ++m_cover;
    }

  private:
    const Graph &m_g;
    CostMap m_cost_map;
    OutputIter m_cover;
    VertexMap m_vertex_map;
};
//! [Iterative Rounding Problem Example]

//! [Iterative Rounding Components Example]
struct vertex_cover_init {
    template <typename Problem, typename LP>
    void operator()(Problem &problem, LP &lp) {
        lp.set_optimization_type(paal::lp::MINIMIZE);

        // variables for vertices
        for (auto v :
             boost::make_iterator_range(vertices(problem.get_graph()))) {
            problem.bind_col_to_vertex(lp.add_column(problem.get_cost(v)), v);
        }

        // x_u + x_v >= 1 for each edge e=(u,v)
        for (auto e : boost::make_iterator_range(edges(problem.get_graph()))) {
            auto x_u = problem.vertex_to_column(source(e, problem.get_graph()));
            auto x_v = problem.vertex_to_column(target(e, problem.get_graph()));
            lp.add_row(x_u + x_v >= 1);
        }
    }
};

struct vertex_cover_set_solution {
    template <typename Problem, typename GetSolution>
    void operator()(Problem &problem, const GetSolution &solution) {
        // add vertices with column value equal 1 to cover
        for (auto v :
             boost::make_iterator_range(vertices(problem.get_graph()))) {
            if (m_compare.e(solution(problem.vertex_to_column(v)), 1)) {
                problem.add_to_cover(v);
            }
        }
    }

private:
    const paal::utils::compare<double> m_compare;
};

using vertex_cover_ir_components =
    ir::IRcomponents<vertex_cover_init, ir::round_condition_greater_than_half,
                     paal::utils::always_false, vertex_cover_set_solution>;
//! [Iterative Rounding Components Example]

//! [Iterative Rounding Example]
int main() {
    using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
        boost::no_property, boost::no_property>;
    using Vertex = boost::graph_traits<Graph>::vertex_descriptor;

    // sample problem
    std::vector<std::pair<int, int>> edges{ { 0, 1 }, { 0, 2 }, { 1, 2 },
                                            { 1, 3 }, { 1, 4 }, { 1, 5 },
                                            { 5, 0 }, { 3, 4 } };
    std::vector<int> costs{ 1, 2, 1, 2, 1, 5 };

    Graph g(edges.begin(), edges.end(), 6);
    auto vertex_costs = paal::utils::make_array_to_functor(costs);
    std::vector<Vertex> result_cover;
    auto insert_iter = std::back_inserter(result_cover);

    vertex_cover<Graph, decltype(vertex_costs), decltype(insert_iter)> problem(
        g, vertex_costs, insert_iter);

    // solve it
    auto result =
        ir::solve_iterative_rounding(problem, vertex_cover_ir_components());

    // print result
    if (result.first == paal::lp::OPTIMAL) {
        std::cout << "Vertices in the cover:" << std::endl;
        for (auto v : result_cover) {
            std::cout << "Vertex " << v << std::endl;
        }
        std::cout << "Cost of the solution: " << *(result.second) << std::endl;
    } else {
        std::cout << "The instance is infeasible" << std::endl;
    }
    paal::lp::glp::free_env();
    return 0;
}
//! [Iterative Rounding Example]


#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#include "sunaga.hpp"
#include <boost/test/unit_test.hpp>

using namespace interval_algebra::sunaga;

BOOST_AUTO_TEST_CASE(single_float) {
  Interval<float> A(6.1);
  BOOST_CHECK_EQUAL(A.l0(), float(6.1));
  BOOST_CHECK_EQUAL(A.l1(), float(6.1));
  Interval<float> B(1.2, 4);
  BOOST_CHECK_EQUAL(B.l0(), float(1.2));
  BOOST_CHECK_EQUAL(B.l1(), float(4));
  Interval<float> C(4, 1);
  Interval<float> D(-std::numeric_limits<float>::infinity(),
                    +std::numeric_limits<float>::infinity());
  Interval<float> E(+std::numeric_limits<float>::infinity(),
                    -std::numeric_limits<float>::infinity());
  BOOST_CHECK_EQUAL(A.empty(), false);
  BOOST_CHECK_EQUAL(B.empty(), false);
  BOOST_CHECK_EQUAL(C.empty(), true);
  BOOST_CHECK_EQUAL(D.empty(), false);
  BOOST_CHECK_EQUAL(E.empty(), true);
  BOOST_CHECK_EQUAL(A.full(), false);
  BOOST_CHECK_EQUAL(B.full(), false);
  BOOST_CHECK_EQUAL(C.full(), false);
  BOOST_CHECK_EQUAL(D.full(), true);
  BOOST_CHECK_EQUAL(E.full(), false);
  BOOST_CHECK_EQUAL(Interval<float>::add(A, B).l0(), A.l0() + B.l0());
  BOOST_CHECK_EQUAL(Interval<float>::add(A, B).l1(), A.l1() + B.l1());
  BOOST_CHECK_EQUAL(Interval<float>::sub(A, B).l0(), A.l0() - B.l1());
  BOOST_CHECK_EQUAL(Interval<float>::sub(A, B).l1(), A.l1() - B.l0());
  BOOST_CHECK_EQUAL(Interval<float>::incl(B, A), false);
  BOOST_CHECK_EQUAL(Interval<float>::incl(A, B), false);
  BOOST_CHECK_EQUAL(Interval<float>::incl(D, A), true);
  BOOST_CHECK_EQUAL(Interval<float>::coinside(B, A), false);
  BOOST_CHECK_EQUAL(Interval<float>::coinside(B, B), true);
}

BOOST_AUTO_TEST_CASE(double_float) {
  Interval<double> A(6.1);
  BOOST_CHECK_EQUAL(A.l0(), double(6.1));
  BOOST_CHECK_EQUAL(A.l1(), double(6.1));
  Interval<double> B(1.2, 4);
  BOOST_CHECK_EQUAL(B.l0(), double(1.2));
  BOOST_CHECK_EQUAL(B.l1(), double(4));
  Interval<double> C(4, 1);
  Interval<double> D(-std::numeric_limits<double>::infinity(),
                     +std::numeric_limits<double>::infinity());
  Interval<double> E(+std::numeric_limits<double>::infinity(),
                     -std::numeric_limits<double>::infinity());
  BOOST_CHECK_EQUAL(A.empty(), false);
  BOOST_CHECK_EQUAL(B.empty(), false);
  BOOST_CHECK_EQUAL(C.empty(), true);
  BOOST_CHECK_EQUAL(D.empty(), false);
  BOOST_CHECK_EQUAL(E.empty(), true);
  BOOST_CHECK_EQUAL(A.full(), false);
  BOOST_CHECK_EQUAL(B.full(), false);
  BOOST_CHECK_EQUAL(C.full(), false);
  BOOST_CHECK_EQUAL(D.full(), true);
  BOOST_CHECK_EQUAL(E.full(), false);
  BOOST_CHECK_EQUAL(Interval<double>::add(A, B).l0(), A.l0() + B.l0());
  BOOST_CHECK_EQUAL(Interval<double>::add(A, B).l1(), A.l1() + B.l1());
  BOOST_CHECK_EQUAL(Interval<double>::sub(A, B).l0(), A.l0() - B.l1());
  BOOST_CHECK_EQUAL(Interval<double>::sub(A, B).l1(), A.l1() - B.l0());
  BOOST_CHECK_EQUAL(Interval<double>::incl(B, A), false);
  BOOST_CHECK_EQUAL(Interval<double>::incl(A, B), false);
  BOOST_CHECK_EQUAL(Interval<double>::incl(D, A), true);
  BOOST_CHECK_EQUAL(Interval<double>::coinside(B, A), false);
  BOOST_CHECK_EQUAL(Interval<double>::coinside(B, B), true);
}


// -------------------------------------------------------------------------------------------------
//                              Copyright 2016 - NumScale SAS
//
//                   Distributed under the Boost Software License, Version 1.0.
//                        See accompanying file LICENSE.txt or copy at
//                            http://www.boost.org/LICENSE_1_0.txt
// -------------------------------------------------------------------------------------------------

/// bench for functor reversebits in scalar mode for int8_t type with no decorator (regular call).
#include <simd_bench.hpp>
#include <boost/simd/function/reversebits.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/function/reversebits.hpp>

namespace nsb = ns::bench;
namespace bs =  boost::simd;
DEFINE_BENCH_MAIN()
{
  using T = int8_t;
  using U = T;
  run<T>(bs::reversebits, nsbg::rand<T>(bs::Valmin<U>(), bs::Valmax<U>()));
}


// Copyright (c) by respective owners including Yahoo!, Microsoft, and
// individual contributors. All rights reserved. Released under a BSD (revised)
// license as described in the file LICENSE.

#include <boost/test/unit_test.hpp>
#include <boost/test/test_tools.hpp>

#include "fast_pow10.h"
#include "test_common.h"

bool are_same(float a, float b) { return std::abs(a - b) < std::numeric_limits<float>::epsilon(); }

BOOST_AUTO_TEST_CASE(pow10_tests)
{
  // In reality most of these tests would all evaluate to the same. The reason is that the epsilon is usually around 1.2e-7
  const float base = 10;
  BOOST_CHECK(are_same(VW::fast_pow10(-127), static_cast<float>(std::pow(base, -127))));
  BOOST_CHECK(are_same(VW::fast_pow10(-46), static_cast<float>(std::pow(base, -46))));
  BOOST_CHECK(are_same(VW::fast_pow10(-45), static_cast<float>(std::pow(base, -45))));
  BOOST_CHECK(are_same(VW::fast_pow10(-44), static_cast<float>(std::pow(base, -44))));
  BOOST_CHECK(are_same(VW::fast_pow10(-40), static_cast<float>(std::pow(base, -40))));
  BOOST_CHECK(are_same(VW::fast_pow10(-38), static_cast<float>(std::pow(base, -38))));
  BOOST_CHECK(are_same(VW::fast_pow10(-37), static_cast<float>(std::pow(base, -37))));
  BOOST_CHECK(are_same(VW::fast_pow10(-10), static_cast<float>(std::pow(base, -10))));
  BOOST_CHECK(are_same(VW::fast_pow10(-5), static_cast<float>(std::pow(base, -5))));
  BOOST_CHECK_CLOSE(VW::fast_pow10(0), 1.f, FLOAT_TOL);
  BOOST_CHECK_CLOSE(VW::fast_pow10(5), 1e5f, FLOAT_TOL);
  BOOST_CHECK_CLOSE(VW::fast_pow10(10), 1e10f, FLOAT_TOL);
  BOOST_CHECK_CLOSE(VW::fast_pow10(37), 1e37f, FLOAT_TOL);
  BOOST_CHECK_CLOSE(VW::fast_pow10(38), 1e38f, FLOAT_TOL);
  BOOST_CHECK(std::isinf(VW::fast_pow10(39)));
  BOOST_CHECK(std::isinf(VW::fast_pow10(127)));
}


//==================================================================================================
/*!
  @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_FUNCTION_CUMPROD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CUMPROD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-reduction
    Function object implementing cumprod capabilities

    Computes the cumulated product of the vector elements

    Take care that overflow is very easy to get here especially for integral types.
    cumprod of char and unsigned char almost always overflow in SIMD !

    @par semantic:
    For any given vector @c x of type @c T:

    @code
    T r = cumprod(x);
    @endcode

    is similar to:

    @code
    auto r = x;
    for(int i=1;i < T::static_size; ++i)
      r[i] *= r[i-1];
    @endcode

  **/
  Value cumprod(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/cumprod.hpp>
#include <boost/simd/function/simd/cumprod.hpp>

#endif


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================
//
//  Sample output
//
// DFS categorized directed graph
// tree: 0 --> 2
// tree: 2 --> 1
// back: 1 --> 1
// tree: 1 --> 3
// back: 3 --> 1
// tree: 3 --> 4
// back: 4 --> 0
// back: 4 --> 1
// forward or cross: 2 --> 3

// BFS categorized directed graph
// tree: 0 --> 2
// tree: 2 --> 1
// tree: 2 --> 3
// cycle: 1 --> 1
// cycle: 1 --> 3
// cycle: 3 --> 1
// tree: 3 --> 4
// cycle: 4 --> 0
// cycle: 4 --> 1

#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <vector>
#include <map>
#include <algorithm>
#include <utility>
#include <string>
#include <range/v3/algorithm/transform.hpp>

#include <boost/graph/visitors.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/depth_first_search.hpp>

template<class Tag>
struct edge_printer : public boost::base_visitor<edge_printer<Tag>>
{
  std::map<int, std::string> idbag_;
  int *n = 0;
  typedef Tag event_filter;
  edge_printer(std::string edge_t) : m_edge_type(edge_t) {}
  template<class Edge, class Graph>
  void operator()(Edge e, Graph &G)
  {
    std::cout << m_edge_type << ": " << source(e, G) << " --> " << target(e, G) << std::endl;
    //std::cout << m_edge_type << ": " << idbag_[source(e, G)] << " --> " << idbag_[target(e, G)] << std::endl;
    if (n) {

      *n += 1;
    }
  }
  int Count() const
  {
    return *n;
  }

  std::string m_edge_type;
};
template<class Tag>
edge_printer<Tag> print_edge(std::string type, Tag)
{
  return edge_printer<Tag>(type);
}

typedef boost::adjacency_list<> Graph;
typedef std::pair<int, int> E;
struct Bags
{
  int test(const Graph &G, int n) const
  {

    using std::endl;
    using std::cout;
    using namespace boost;

    E edges[] = { E(0, 2), E(1, 1), E(1, 3), E(2, 1), E(2, 3), E(3, 1), E(3, 4), E(4, 0), E(4, 1), E(5, 6), E(6, 5) };
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
    Graph Gb(5);
    for (std::size_t j = 0; j < sizeof(edges) / sizeof(E); ++j)
      add_edge(edges[j].first, edges[j].second, G);
#else
    Graph Gb(edges, edges + sizeof(edges) / sizeof(E), 5);
#endif

    typedef boost::graph_traits<Graph>::vertices_size_type size_type;

    //std::vector<size_type> d(num_vertices(G));
    //std::vector<size_type> f(num_vertices(G));

    cout << "DFS categorized directed graph" << endl;
    depth_first_search(Gb,
      visitor(make_dfs_visitor(make_list(print_edge("tree", on_tree_edge()),
        print_edge("back", on_back_edge()),
        print_edge("forward or cross", on_forward_or_cross_edge())))));

    //cout << endl
    //     << "BFS categorized directed graph" << endl;
    //boost::breadth_first_search(G, vertex(0, G), visitor(make_bfs_visitor(std::make_pair(print_edge("tree", on_tree_edge()), print_edge("cycle", on_non_tree_edge())))));
    cout << endl
         << "BFS jiayuehua categorized directed graph" << endl;
    auto ep = print_edge("tree", on_tree_edge());
    auto epb = print_edge("tree", on_non_tree_edge());
    ep.idbag_ = idbag_;
    epb.idbag_ = idbag_;
    int num = 0;
    ep.n = #
    boost::breadth_first_search(G, vertex(n, G), visitor(make_bfs_visitor(std::make_pair(ep, print_edge("cycle", on_non_tree_edge())))));
    cout << endl;

    return num;
  }
  std::vector<E> edges_;
  int shinygoldid_ = 0;
  Graph G_;
  std::map<int, std::string> idbag_;

public:
  int shinygoldidNum() const
  {
    return test(G_, shinygoldid_);
  }
  friend std::istream &operator>>(std::istream &is, Bags &bags)
  {
    std::map<std::string, int> bagid;
    std::multimap<std::string, std::string> stredges_;
    std::string color, attr;
    std::string line;

    for (int i = 0, n = 0; std::getline(is, line); ++n) {
      std::istringstream iss(line);
      iss >> color >> attr;
      //std::pair<std::string, std::string> edge;
      auto edgedst = color + attr;
      if (auto [k, v] = bagid.insert(std::pair{ edgedst, i }); v) {
        bags.idbag_[i] = edgedst;
        ++i;
      }

      for (; iss >> color >> attr;) {
        if (color + attr != "noother") {

          if (auto [k, v] = bagid.insert(std::pair{ color + attr, i }); v) {
            bags.idbag_[i] = color + attr;
            ++i;
          }

          stredges_.insert(std::pair{ color + attr, edgedst });
        }
      }
    }
    bags.edges_.resize(stredges_.size());
    ranges::transform(
      stredges_, bags.edges_.begin(), [bagid](auto pair) mutable { return std::pair{ bagid[pair.first], bagid[pair.second] }; });

    Graph G(bags.edges_.begin(), bags.edges_.end(), bagid.size());
    bags.shinygoldid_ = bagid["shinygold"];
    bags.G_ = G;

    return is;
  }
};
int main(int argc, char **argv)
{
  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    std::string color, attr;
    Bags bags;
    ifs >> bags;
    std::cout << bags.shinygoldidNum() << std::endl;
  }
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <opencv2/xfeatures2d/nonfree.hpp>
#include <opencv2/opencv.hpp>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <Eigen/Core>

#include <chrono>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
using namespace std;
using namespace cv;

void writeResults( const string& filename, const vector<string>& timestamps, const vector<Mat>& Rt )
{
    CV_Assert( timestamps.size() == Rt.size() );

    ofstream file( filename.c_str() );
    if( !file.is_open() )
        return;

    cout.precision(4);
    for( size_t i = 0; i < Rt.size(); i++ )
    {
        const Mat& Rt_curr = Rt[i];
        if( Rt_curr.empty() )
            continue;

        CV_Assert( Rt_curr.type() == CV_64FC1 );

        Mat R = Rt_curr(Rect(0,0,3,3)), rvec;
        Rodrigues(R, rvec);
        double alpha = norm( rvec );
        if(alpha > DBL_MIN)
            rvec = rvec / alpha;

        double cos_alpha2 = std::cos(0.5 * alpha);
        double sin_alpha2 = std::sin(0.5 * alpha);

        rvec *= sin_alpha2;

        CV_Assert( rvec.type() == CV_64FC1 );
        // timestamp tx ty tz qx qy qz qw
        file << timestamps[i] << " " << fixed
             << Rt_curr.at<double>(0,3) << " " << Rt_curr.at<double>(1,3) << " " << Rt_curr.at<double>(2,3) << " "
             << rvec.at<double>(0) << " " << rvec.at<double>(1) << " " << rvec.at<double>(2) << " " << cos_alpha2 << endl;

    }
    file.close();
}

void find_feature_matches(
 const Mat &img_1, const Mat &img_2, std::vector<KeyPoint> &keypoints_1,vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches, const Mat &img_3);
 void find_feature_matches_another(
 const Mat &img_1, const Mat &img_2, std::vector<KeyPoint> &keypoints_1,vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches, const Mat &img_3);

// // 像素坐标转相机归一化坐标
 Point2d pixel2cam(const Point2d &p, const Mat &K);

// BA by g2o
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

Mat bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose,
  const string& filename,
  const vector<string>& timestamps
);

int main(int argc, char **argv) {
	if(argc != 4){
        cout << "Format: file_with_rgb_depth_pairs trajectory_file odometry_name [Rgbd or ICP or RgbdICP or FastICP]" << endl;
        return -1;
   }

   vector<string> timestamps;
   vector<Mat> Rts, Rts_ba;

   const string filename = argv[1];
   ifstream file( filename.c_str() );
   if( !file.is_open() )
      return -1;
   char dlmrt = '/';
   size_t pos = filename.rfind(dlmrt);
   string dirname = pos == string::npos ? "" : filename.substr(0, pos) + dlmrt;

   const int timestampLength = 17;
   const int rgbPathLehgth = 17+8;
   const int depthPathLehgth = 17+10;

   float fx = 517.3f, // default
         fy = 516.5f,
         cx = 318.6f,
         cy = 255.3f;
   string datas[793];
   string str1;
   std::getline(file, str1);
    datas[0] = str1;

    for(int i = 1; !file.eof(); i++){
        string str;
        std::getline(file, str);
        datas[i] = str;
        if(str.empty()) break;
        if(str.at(0) == '#') continue; /* comment */
        cout << " previous image: " << datas[i-1] << "\n" << " current image "<< str << endl;
        Mat image, depth, image1, depth1, image2, depth2;
        // if(i > 2) {
        //     string rgbFilename2 = datas[i-2].substr(timestampLength + 1, rgbPathLehgth );
        //     string timestap2 = datas[i-2].substr(0, timestampLength);
        //     string depthFilename2 = datas[i-2].substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );

        //     image2 = imread(dirname + rgbFilename2);
        //     depth2 = imread(dirname + depthFilename2, -1);
        // }

        string rgbFilename1 = datas[i-1].substr(timestampLength + 1, rgbPathLehgth );
        string timestap1 = datas[i-1].substr(0, timestampLength);
        string depthFilename1 = datas[i-1].substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
        image1 = imread(dirname + rgbFilename1);
        depth1 = imread(dirname + depthFilename1, -1);

        string rgbFilename = str.substr(timestampLength + 1, rgbPathLehgth );
        string timestap = str.substr(0, timestampLength);
        string depthFilename = str.substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
        image = imread(dirname + rgbFilename);
        depth = imread(dirname + depthFilename, -1);

        // cout << "prev prev " << datas[i-2] << " previous image: " << datas[i-1] << " current image "<< str << endl;
        // CV_Assert(!image.empty());
        // CV_Assert(!depth.empty());
        // CV_Assert(!image1.empty());
        // CV_Assert(!depth1.empty());
        // CV_Assert(depth.type() == CV_16UC1);
        // CV_Assert(depth1.type() == CV_16UC1);

        // if(i > 2){
        //     CV_Assert(!image2.empty());
        //     CV_Assert(!depth2.empty());
        //     CV_Assert(depth2.type() == CV_16UC1);
        // }

        std::vector<KeyPoint> keypoints_1, keypoints_2, key1, key2;
        vector<DMatch> matches;
        Ptr<FeatureDetector> detector = ORB::create();
        detector->detect(image1, key1);
        detector->detect(image, key2);
        if(key1.size() == 0 || key2.size() == 0){
            find_feature_matches_another(image1, image, keypoints_1, keypoints_2, matches, image2);
            cout << "第二個: " <<  "一共找到了" << matches.size() << "组匹配点" << endl;
        }
        else{
            find_feature_matches(image1, image, keypoints_1, keypoints_2, matches, image2);
            cout << "第一個: " <<"一共找到了" << matches.size() << "组匹配点" << endl;
        }

        // 建立3D点
        //Mat d1 = imread(depth1, IMREAD_UNCHANGED);       // 深度图为16位无符号数，单通道图像
        Mat K = (Mat_<double>(3, 3) << 517.3f, 0, 318.6f, 0, 516.5f, 255.3f, 0, 0, 1);
        vector<Point2f> pts_2d_old;
        vector<Point2f> pts_2d;
        int index = 1;
        for (DMatch m:matches) {
                ushort d = depth1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
                // cout << "depth " << d << endl;
                if (d == 0){   // bad depth
                    continue;
                // d = 1;
                }
                // cout << "this is matches: " <<  index << endl;
                float dd = d / 5000.0;
                // cout << "keypoints_1[m.queryIdx].pt " << keypoints_1[m.queryIdx].pt << endl;
                Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
                // cout << "p1.x " << p1.x << " p1.y " << p1.y << endl;
                // cout << "keypoints_2[m.trainIdx].pt " << keypoints_2[m.trainIdx].pt << endl;
                pts_2d_old.push_back(Point2f(p1.x * dd, p1.y * dd));
                pts_2d.push_back(keypoints_2[m.trainIdx].pt);
                index += 1;
            // }
        }
        bool b = false;
        // if(i ==373) {b = true;}
        cout << "2d-2d pairs: " << pts_2d_old.size() << " " << pts_2d.size() << " "  << i <<  endl;
        vector<Scalar> colors;
        cv::RNG rng;
        for(int i = 0; i < 100; i++){
            int r = rng.uniform(0, 256);
            int g = rng.uniform(0, 256);
            int b = rng.uniform(0, 256);
            colors.push_back(Scalar(r,g,b));
        }
        // vector<Point2f> p_old, p_new;
        // cv::goodFeaturesToTrack(image1, p_old, 100, 0.3, 7, Mat(), 7, false, 0.04);
        // // Calculate optical flow
        vector<uchar> status;
        vector<float> err;
        Mat mask = Mat::zeros(image1.size(), image1.type());
        cv::TermCriteria criteria = TermCriteria((TermCriteria::COUNT) + (TermCriteria::EPS), 10, 0.03);
        cv::calcOpticalFlowPyrLK(image, image1, pts_2d_old, pts_2d, status, err, Size(15,15), 2, criteria);
        vector<Point2f> good_new;
        // Visualization part
        for(uint i = 0; i < pts_2d_old.size(); i++){
            // Select good points
            if(status[i] == 1) {
                good_new.push_back(pts_2d[i]);
                // Draw the tracks
                line(mask,pts_2d[i], pts_2d_old[i], colors[i], 2);
                circle(image, pts_2d[i], 5, colors[i], -1);
            }
        }
        // Display the demo
        Mat img;
        cv::add(image, mask, img);
        // if (save) {
        //     string save_path = "./optical_flow_frames/frame_" + to_string(counter) + ".jpg";
        //     imwrite(save_path, img);
        // }
        cv::imshow("flow", img);
        // int keyboard = cv::waitKey(25);
        // if (keyboard == 'q' || keyboard == 27)
        //     break;
        // Update the previous frame and previous points
        image1 = image.clone();
        pts_2d_old = good_new;

//       chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
//       Mat r, t, inliers;
//       solvePnPRansac(pts_3d, pts_2d, K, Mat(), r, t, b, 300, 0.05, 0.95, inliers);
//       // cout << inliers << inliers.size()  << inliers.at<int>(1,0)  << inliers.at<int>(3,0)<< endl;
//       for (int i=0; i<inliers.rows; i++){
//          cout << "inliers -> keypoints_1: " << keypoints_1[inliers.at<int>(i,0)].pt << "inliers -> keypoints_2: " << keypoints_2[inliers.at<int>(i,0)].pt << endl;
//       }
//       // cout << "inliers "<< inliers << endl;
//       cout<<"pnp OK = "<<b<<", inliers point num = "<<inliers.rows<<endl;
//       Mat R;
//       cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
//       chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
//       chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
//       cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;

//       cout << "R=" << endl << R << endl;
//       cout << "t=" << endl << t << endl;
//       timestamps.push_back( timestap );
//       Mat output;
//       hconcat(R, t, output);

//       Mat Rt = Mat::eye(4,4,CV_64FC1);
//       Rt.at<double>(0,0) = R.at<double>(0,0);
//       Rt.at<double>(0,1) = R.at<double>(0,1);
//       Rt.at<double>(0,2) = R.at<double>(0,2);
//       Rt.at<double>(1,0) = R.at<double>(1,0);
//       Rt.at<double>(1,1) = R.at<double>(1,1);
//       Rt.at<double>(1,2) = R.at<double>(1,2);
//       Rt.at<double>(2,0) = R.at<double>(2,0);
//       Rt.at<double>(2,1) = R.at<double>(2,1);
//       Rt.at<double>(2,2) = R.at<double>(2,2);
//       Rt.at<double>(0,3) = t.at<double>(0,0);
//       Rt.at<double>(1,3) = t.at<double>(0,1);
//       Rt.at<double>(2,3) = t.at<double>(0,2);
//       cout << "Rt " << Rt << endl;
//       Rts.push_back(Rt);

//       Mat Rt_ba;

//       VecVector3d pts_3d_eigen;
//       VecVector2d pts_2d_eigen;
//       for (size_t i = 0; i < pts_3d.size(); ++i) {
//          // cout << "vector3d " << Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z) << "vector2d " << Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y) << endl;
//          pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
//          pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
//       }
//       // for (size_t i = 0; i < pts_3d.size(); ++i) {
//       //    for (size_t j = 0; j < 3; ++j) {
//       //       cout << " eigen3d " << pts_3d_eigen[i][j] << cout << " eigen2d "  <<*pts_2d_eigen[i][j];
//       //    }
//       // }
//       cout << "calling bundle adjustment by g2o" << endl;
//       Sophus::SE3d pose_g2o;
//       t1 = chrono::steady_clock::now();
//       Rt_ba = bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, K, pose_g2o, argv[2], timestamps);
//       Rts_ba.push_back(Rt_ba);
//       t2 = chrono::steady_clock::now();
//       time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
//       cout << "solve pnp by g2o cost time: " << time_used.count() << " seconds." << endl;
   }
//    writeResults(argv[2], timestamps, Rts);
//    //writeResults(argv[2], timestamps, Rts_ba);

  return 0;
}
void find_feature_matches_another(const Mat &img_1, const Mat &img_2,
                           std::vector<KeyPoint> &keypoints_1,
			                  std::vector<KeyPoint> &keypoints_2,
                           std::vector<DMatch> &matches,
                           const Mat &img_3) {
   Mat descriptors_1, descriptors_2, descriptors_3, descriptors_4;
   Ptr<FeatureDetector> detector = AgastFeatureDetector::create();
   Ptr<DescriptorExtractor> descriptor = AgastFeatureDetector::create();
   Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
   detector->detect(img_1, keypoints_1);
   detector->detect(img_2, keypoints_2);
   cout << "keypoints_1.size() "<< keypoints_1.size() << " keypoints_2.size() " << keypoints_2.size() << endl;
   // if(keypoints_1.size() != 0 && keypoints_2.size() != 0){
      for (int i=0; i< keypoints_1.size(); i++){
         // cout << "keypoint1 " << keypoints_1[i].pt << "keypoint2 " << keypoints_2[i].pt << endl;
      }
      descriptor->compute(img_1, keypoints_1, descriptors_1);
      descriptor->compute(img_2, keypoints_2, descriptors_2);
   // }
   int eee = descriptors_1.empty();
   int ddd = descriptors_2.empty();

   vector<DMatch> match;
   matcher->match(descriptors_1, descriptors_2, match);
   double min_dist = 10000, max_dist = 0;
     for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
     if (dist < min_dist) min_dist = dist;
     if (dist > max_dist) max_dist = dist;
   }
   printf("-- Max dist : %f \n", max_dist);
   printf("-- Min dist : %f \n", min_dist);
   for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 10.0)) {
       matches.push_back(match[i]);
     }
   }
 }
 void find_feature_matches(const Mat &img_1, const Mat &img_2,
                           std::vector<KeyPoint> &keypoints_1,
			                  std::vector<KeyPoint> &keypoints_2,
                           std::vector<DMatch> &matches,
                           const Mat &img_3) {

   Mat descriptors_1, descriptors_2, descriptors_3, descriptors_4;
   Ptr<FeatureDetector> detector = ORB::create();
   Ptr<DescriptorExtractor> descriptor = ORB::create();
   Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
   detector->detect(img_1, keypoints_1);
   detector->detect(img_2, keypoints_2);
   cout << "keypoints_1.size() "<< keypoints_1.size() << " keypoints_2.size() " << keypoints_2.size() << endl;
      for (int i=0; i< keypoints_1.size(); i++){
         // cout << "keypoint1 " << keypoints_1[i].pt << "keypoint2 " << keypoints_2[i].pt << endl;
      }
      descriptor->compute(img_1, keypoints_1, descriptors_1);
      descriptor->compute(img_2, keypoints_2, descriptors_2);
   int eee = descriptors_1.empty();
   int ddd = descriptors_2.empty();
   vector<DMatch> match;
   matcher->match(descriptors_1, descriptors_2, match);
   double min_dist = 10000, max_dist = 0;
     for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
     if (dist < min_dist) min_dist = dist;
     if (dist > max_dist) max_dist = dist;
   }

   printf("-- Max dist : %f \n", max_dist);
   printf("-- Min dist : %f \n", min_dist);
   for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 10.0)) {
       matches.push_back(match[i]);
     }
   }
 }

 Point2d pixel2cam(const Point2d &p, const Mat &K) {
   return Point2d
     (
       (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
       (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
     );
 }

/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
   public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

   virtual void setToOriginImpl() override {
      _estimate = Sophus::SE3d();
   }

   /// left multiplication on SE3
   virtual void oplusImpl(const double *update) override {
      Eigen::Matrix<double, 6, 1> update_eigen;
      update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
      _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
   }

   virtual bool read(istream &in) override {}

   virtual bool write(ostream &out) const override {}
};

class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
   public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

      EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}

   virtual void computeError() override {
      const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
      Sophus::SE3d T = v->estimate();
      Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
      pos_pixel /= pos_pixel[2];
      _error = _measurement - pos_pixel.head<2>();
   }

   virtual void linearizeOplus() override {
      const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
      Sophus::SE3d T = v->estimate();
      Eigen::Vector3d pos_cam = T * _pos3d;
      double fx = _K(0, 0);
      double fy = _K(1, 1);
      double cx = _K(0, 2);
      double cy = _K(1, 2);
      double X = pos_cam[0];
      double Y = pos_cam[1];
      double Z = pos_cam[2];
      double Z2 = Z * Z;
      _jacobianOplusXi
         << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
         0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
   }

   virtual bool read(istream &in) override {}

   virtual bool write(ostream &out) const override {}

   private:
      Eigen::Vector3d _pos3d;
      Eigen::Matrix3d _K;
};

Mat bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose,
  const string& filename,
  const vector<string>& timestamps) {

  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // pose is 6, landmark is 3
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
  g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

  // vertex
  VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose
  vertex_pose->setId(0);
  vertex_pose->setEstimate(Sophus::SE3d());
  optimizer.addVertex(vertex_pose);

  // K
  Eigen::Matrix3d K_eigen;
  K_eigen <<
          K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
    K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
    K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

  // edges
  int index = 1;
  for (size_t i = 0; i < points_2d.size(); ++i) {
    auto p2d = points_2d[i];
    auto p3d = points_3d[i];
    EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);
    edge->setId(index);
    edge->setVertex(0, vertex_pose);
    edge->setMeasurement(p2d);
    edge->setInformation(Eigen::Matrix2d::Identity());
    optimizer.addEdge(edge);
    index++;
  }

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.setVerbose(true);
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
  cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;
//   cout << "pose matrix "  << vertex_pose->estimate().matrix()(0) << vertex_pose->estimate().matrix()(1) << vertex_pose->estimate().matrix()(4) << endl;

  Mat Rt = Mat::eye(4,4,CV_64FC1);
  Rt.at<double>(0,0) = vertex_pose->estimate().matrix()(0);
  Rt.at<double>(1,0) = vertex_pose->estimate().matrix()(1);
  Rt.at<double>(2,0) = vertex_pose->estimate().matrix()(2);
  Rt.at<double>(3,0) = vertex_pose->estimate().matrix()(3);
  Rt.at<double>(0,1) = vertex_pose->estimate().matrix()(4);
  Rt.at<double>(1,1) = vertex_pose->estimate().matrix()(5);
  Rt.at<double>(2,1) = vertex_pose->estimate().matrix()(6);
  Rt.at<double>(3,1) = vertex_pose->estimate().matrix()(7);
  Rt.at<double>(0,2) = vertex_pose->estimate().matrix()(8);
  Rt.at<double>(1,2) = vertex_pose->estimate().matrix()(9);
  Rt.at<double>(2,2) = vertex_pose->estimate().matrix()(10);
  Rt.at<double>(3,2) = vertex_pose->estimate().matrix()(11);
  Rt.at<double>(0,3) = vertex_pose->estimate().matrix()(12);
  Rt.at<double>(1,3) = vertex_pose->estimate().matrix()(13);
  Rt.at<double>(2,3) = vertex_pose->estimate().matrix()(14);
  Rt.at<double>(3,3) = vertex_pose->estimate().matrix()(15);

//   writeResults(filename, timestamps, vertex_pose->estimate().matrix());
  pose = vertex_pose->estimate();
  return Rt;
}