/* Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *  * Neither the name of NVIDIA CORPORATION nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <boost/multiprecision/cpp_int.hpp>

#include "util/numeric.hpp"

using namespace boost::multiprecision;

//------------------------------------
//              Factors
//------------------------------------

unsigned long Factors::ISqrt_(unsigned long x)
{
  register unsigned long op, res, one;

  op = x;
  res = 0;

  one = 1 << 30;
  while (one > op) one >>= 2;

  while (one != 0)
  {
    if (op >= res + one)
    {
      op -= res + one;
      res += one << 1;
    }
    res >>= 1;
    one >>= 2;
  }
  return res;
}

void Factors::CalculateAllFactors_()
{
  all_factors_.clear();
  for (unsigned long i = 1; i <= ISqrt_(n_); i++)
  {
    if (n_ % i == 0)
    {
      all_factors_.push_back(i);
      if (i * i != n_)
      {
        all_factors_.push_back(n_ / i);
      }
    }
  }
}

// Return a vector of all order-way cofactor sets of n.
std::vector<std::vector<unsigned long>>
Factors::MultiplicativeSplitRecursive_(unsigned long n, int order)
{
  if (order == 0)
  {
    return {{}};
  }
  else if (order == 1)
  {
    return {{n}};
  }
  else
  {
    std::vector<std::vector<unsigned long>> retval;
    for (auto factor = all_factors_.begin(); factor != all_factors_.end(); factor++)
    {
      // This factor is only acceptable if the residue is divisible by it.
      if (n % (*factor) == 0)
      {
        // Recursive call.
        std::vector<std::vector<unsigned long>> subproblem =
          MultiplicativeSplitRecursive_(n / (*factor), order - 1);

        // Append this factor to the end of each vector returned by the
        // recursive call.
        for (auto vector = subproblem.begin(); vector != subproblem.end(); vector++)
        {
          vector->push_back(*factor);
        }

        // Add all these appended vectors to my growing vector of vectors.
        retval.insert(retval.end(), subproblem.begin(), subproblem.end());
      }
      else
      {
        // Discard this factor.
      }
    }
    return retval;
  }
}

Factors::Factors() :
    n_(0)
{
}

Factors::Factors(const unsigned long n, const int order) :
    n_(n), cofactors_()
{
  CalculateAllFactors_();
  cofactors_ = MultiplicativeSplitRecursive_(n, order);
}

Factors::Factors(const unsigned long n, const int order, std::map<unsigned, unsigned long> given) :
    n_(n), cofactors_()
{
  assert(given.size() <= std::size_t(order));

  // If any of the given factors is not a factor of n, forcibly reset that to
  // be a free variable, otherwise accumulate them into a partial product.
  unsigned long partial_product = 1;
  for (auto f = given.begin(); f != given.end(); f++)
  {
    auto factor = f->second;
    if (n % (factor * partial_product) == 0)
    {
      partial_product *= factor;
    }
    else
    {
      std::cerr << "WARNING: cannot accept " << factor << " as a factor of " << n
                << " with current partial product " << partial_product;
#define SET_NONFACTOR_TO_FREE_VARIABLE
#ifdef SET_NONFACTOR_TO_FREE_VARIABLE
      // Ignore mapping constraint and set the factor to a free variable.
      std::cerr << ", ignoring mapping constraint and setting to a free variable."
                << std::endl;
      f = given.erase(f);
#else
      // Try to find the next *lower* integer that is a factor of n.
      // FIXME: there are multiple exceptions that can cause us to be here:
      // (a) factor doesn't divide into n.
      // (b) factor does divide into n but causes partial product to exceed n.
      // (c) factor does divide into n but causes partial product to not
      //     divide into n.
      // The following code only solves case (a).
      std::cerr << "FIXME: please fix this code." << std::endl;
      assert(false);

      for (; factor >= 1 && (n % factor != 0); factor--);
      std::cerr << ", setting this to " << factor << " instead." << std::endl;
      f->second = factor;
      partial_product *= factor;
#endif
    }
    assert(n % partial_product == 0);
  }

  CalculateAllFactors_();

  cofactors_ = MultiplicativeSplitRecursive_(n / partial_product, order - given.size());

  // Insert the given factors at the specified indices of each of the solutions.
  for (auto& cofactors : cofactors_)
  {
    for (auto& given_factor : given)
    {
      // Insert the given factor, pushing all existing factors back.
      auto index = given_factor.first;
      auto value = given_factor.second;
      assert(index <= cofactors.size());
      cofactors.insert(cofactors.begin() + index, value);
    }
  }
}

void Factors::PruneMax(std::map<unsigned, unsigned long>& max)
{
  // Prune the vector of cofactor sets by removing those sets that have factors
  // outside user-specified min/max range. We should really have done this during
  // MultiplicativeSplitRecursive. However, the "given" map complicates things
  // because given factors may be scattered, and we'll need a map table to
  // find the original rank from the "compressed" rank seen by
  // MultiplicativeSplitRecursive. Doing it now is slower but cleaner and less
  // bug-prone.

  auto cofactors_it = cofactors_.begin();
  while (cofactors_it != cofactors_.end())
  {
    bool illegal = false;
    for (auto& max_factor : max)
    {
      auto index = max_factor.first;
      auto max = max_factor.second;
      assert(index <= cofactors_it->size());
      auto value = cofactors_it->at(index);
      if (value > max)
      {
        illegal = true;
        break;
      }
    }

    if (illegal)
      cofactors_it = cofactors_.erase(cofactors_it);
    else
      cofactors_it++;
  }
}

std::vector<unsigned long>& Factors::operator[](int index)
{
  return cofactors_[index];
}

std::size_t Factors::size()
{
  return cofactors_.size();
}

void Factors::Print()
{
  PrintAllFactors();
  PrintCoFactors();
}

void Factors::PrintAllFactors()
{
  std::cout << "All factors of " << n_ << ": ";
  bool first = true;
  for (auto f = all_factors_.begin(); f != all_factors_.end(); f++)
  {
    if (first)
    {
      first = false;
    }
    else
    {
      std::cout << ", ";
    }
    std::cout << (*f);
  }
  std::cout << std::endl;
}

void Factors::PrintCoFactors()
{
  std::cout << *this;
}

std::ostream& operator<<(std::ostream& out, const Factors& f)
{
  out << "Co-factors of " << f.n_ << " are: " << std::endl;
  for (auto cset = f.cofactors_.begin(); cset != f.cofactors_.end(); cset++)
  {
    out << "    " << f.n_ << " = ";
    bool first = true;
    for (auto i = cset->begin(); i != cset->end(); i++)
    {
      if (first)
      {
        first = false;
      }
      else
      {
        out << " * ";
      }
      out << (*i);
    }
    out << std::endl;
  }
  return out;
}

//------------------------------------
//        PatternGenerator128
//------------------------------------

PatternGenerator128::PatternGenerator128(uint128_t bound) :
    bound_(bound)
{
}

SequenceGenerator128::SequenceGenerator128(uint128_t bound, bool autoloop) :
    PatternGenerator128(bound),
    autoloop_(autoloop),
    cur_(0)
{
}

uint128_t SequenceGenerator128::Next()
{
  auto retval = cur_;
  if (cur_ == bound_-1)
  {
    assert(autoloop_);
    cur_ = 0;
  }
  else
  {
    cur_++;
  }
  return retval;
}

RandomGenerator128::RandomGenerator128(uint128_t bound) :
    PatternGenerator128(bound),
    use_two_generators_(bound > uint128_t(uint64_max_)),
    low_gen_(0, use_two_generators_ ? uint64_max_ : (std::uint64_t)(bound - 1)),
    high_gen_(0, (std::uint64_t)(bound/uint64_max_ - 1))
{
}

uint128_t RandomGenerator128::Next()
{
  std::uint64_t low = low_gen_(engine_);
  std::uint64_t high = 0;

  if (use_two_generators_)
  {
    high = high_gen_(engine_);
  }

  uint128_t rand = low + ((uint128_t)high * uint64_max_);
  assert(rand < bound_);

  return rand;
}

//------------------------------------
//           Miscellaneous
//------------------------------------

// Returns the smallest factor of an integer and the quotient after
// division with the smallest factor.
void SmallestFactor(uint64_t n, uint64_t& factor, uint64_t& residue)
{
  for (uint64_t i = 2; i < n; i++)
  {
    if (n % i == 0)
    {
      factor = i;
      residue = n / i;
      return;
    }
  }
  factor = n;
  residue = 1;
}

// Helper function to get close-to-square layouts of arrays
// containing a given number of nodes.
void GetTiling(uint64_t num_elems, uint64_t& height, uint64_t& width)
{
  std::vector<uint64_t> factors;
  uint64_t residue = num_elems;
  uint64_t cur_factor;
  while (residue > 1)
  {
    SmallestFactor(residue, cur_factor, residue);
    factors.push_back(cur_factor);
  }

  height = 1;
  width = 1;
  for (uint64_t i = 0; i < factors.size(); i++)
  {
    if (i % 2 == 0)
      height *= factors[i];
    else
      width *= factors[i];
  }

  if (height > width)
  {
    uint64_t temp = height;
    height = width;
    width = temp;
  }
}

double LinearInterpolate(double x,
                         double x0, double x1,
                         double q0, double q1)
{
  double slope = (x0 == x1) ? 0 : (q1 - q0) / double(x1 - x0);
  return q0 + slope * (x - x0);
}

double BilinearInterpolate(double x, double y,
                           double x0, double x1,
                           double y0, double y1,
                           double q00, double q01, double q10, double q11)
{
  // Linear interpolate along x dimension.
  double qx0 = LinearInterpolate(x, x0, x1, q00, q10);
  double qx1 = LinearInterpolate(x, x0, x1, q01, q11);

  // Linear interpolate along y dimension.
  return LinearInterpolate(y, y0, y1, qx0, qx1);
}


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/dists.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/ten.hpp>
#include <boost/simd/constant/mten.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/valmax.hpp>

STF_CASE_TPL (" dists real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::dists;

  STF_EXPR_IS( dists(T(), T()), T );

#ifndef STF_NO_INVALIDS
  STF_IEEE_EQUAL(dists(bs::Inf<T>() , bs::Inf<T>()) , bs::Nan<T>());
  STF_IEEE_EQUAL(dists(bs::Minf<T>(), bs::Minf<T>()), bs::Nan<T>());
  STF_IEEE_EQUAL(dists(bs::Nan<T>() , bs::Nan<T>()) , bs::Nan<T>());
#endif

  STF_EQUAL(dists(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(dists(bs::Mone<T>(), bs::One<T>()), bs::Two<T>());
  STF_EQUAL(dists(bs::One<T>(), bs::Three<T>()), bs::Two<T>());
}

STF_CASE_TPL (" dists integer_ui",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::dists;

  STF_EXPR_IS( dists(T(), T()), T );

  STF_EQUAL(dists(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(dists(bs::One<T>(), bs::Three<T>()), bs::Two<T>());
  STF_EQUAL(dists(bs::Valmax<T>(), bs::Zero<T>()), bs::Valmax<T>());
  STF_EQUAL(dists(bs::Zero<T>(), bs::Valmax<T>()), bs::Valmax<T>());
}

STF_CASE_TPL (" dists integer_si",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::dists;
  STF_EXPR_IS( dists(T(), T()), T );

  STF_EQUAL(dists(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(dists(bs::Mone<T>(), bs::One<T>()), bs::Two<T>());
  STF_EQUAL(dists(bs::One<T>(), bs::Three<T>()), bs::Two<T>());
  STF_EQUAL(dists(bs::Valmax<T>(), bs::Zero<T>()), bs::Valmax<T>());
  STF_EQUAL(dists(bs::Zero<T>(), bs::Valmax<T>()), bs::Valmax<T>());
  STF_EQUAL(dists(bs::Zero<T>(), bs::Valmin<T>()), bs::Valmax<T>());
  STF_EQUAL(dists(bs::Valmin<T>(), bs::Zero<T>()), bs::Valmax<T>());

  STF_EQUAL(dists(bs::Ten<T>(), bs::Mten<T>()), (T(20)));
  STF_EQUAL(dists(bs::Mten<T>(), bs::Ten<T>()), (T(20)));
}


// system includes -----------------------------------------------
#include <Eigen/Geometry>
#include <boost/math/constants/constants.hpp>
#include <boost/program_options.hpp>
#include <iostream>
// own includes --------------------------------------------------
#include <base/eigen2hdf.hpp>
#include <base/init.hpp>
#include <base/timer.hpp>
#include <fft/fft2.hpp>
#include <fft/fft2_r2c.hpp>
#include <ridgelet/ridgelet_cell_array.hpp>
#include <ridgelet/ridgelet_frame.hpp>
#include <ridgelet/rt.hpp>

#include <boundary_conditions/inflow_bc.hpp>
#include <operators/operators.hpp>
#include <solver/cg.hpp>
#include <solver/ridgelet_solver.hpp>

using namespace std;

typedef RT<> RT_t;
typedef RT_t::array_t array_t;
typedef RT_t::complex_array_t complex_array_t;
typedef RT_t::rt_coeff_t rt_coeff_t;
typedef RidgeletCellArray<rt_coeff_t> rca_t;

namespace po = boost::program_options;

const double Lx = 1.2;
const double Ly = 1.2;

class RidgeletInflowTest
{
 public:
  RidgeletInflowTest(po::variables_map vm);

  void init_sigma(array_t& Sigma);
  /// initialize fft(f(t=0, x))
  void init_fh0(complex_array_t& Fh);
  /// prescribed f(x) in absorption layer
  void init_fbc(array_t& fbc, double vx, double vy, double ql, double qt);
  /// apply boundary conditions in absorption layer to fft(f(x))
  void apply_bc(complex_array_t& Fh, const array_t& fbc);
  /**
   *
   * @param vx
   * @param vy
   * @param ql    inflow left
   * @param qt    inflow top
   * @param rho   density
   */
  void run(double vx, double vy, double ql, double qt, double rho, int ntsteps);

  RidgeletSolver<rt_coeff_t>& solver() { return rt_solver; }

  void log_cg(bool flag) { log_hist = flag; }

 private:
  RidgeletFrame rf;
  RT_t rt;
  FFTr2c<PlannerR2C> fft;
  RidgeletSolver<rt_coeff_t> rt_solver;
  // parameters
  po::variables_map vm;
  bool log_hist = false;
};

RidgeletInflowTest::RidgeletInflowTest(po::variables_map vm_)
    : vm(vm_)
{
  unsigned int Jx = vm["Jx"].as<unsigned int>();
  unsigned int Jy = vm["Jy"].as<unsigned int>();
  unsigned int rho_x = vm["rx"].as<unsigned int>();
  unsigned int rho_y = vm["ry"].as<unsigned int>();

  rf = RidgeletFrame(Jx, Jy, rho_x, rho_y);
  rt = RT_t(rf);

  const unsigned int Nx = rf.Nx();  // #cols
  const unsigned int Ny = rf.Ny();  // #rows
  init_fftw(fft, FFTW_MEASURE, rf);
  fft.get_plan().create_and_get_plan(Ny / 2, Nx / 2, PlannerR2C::INV);
  fft.get_plan().create_and_get_plan(Ny / 2, Nx / 2, PlannerR2C::FWD);
}

void
RidgeletInflowTest::init_sigma(array_t& Sigma)
{
  double sigma = vm["sigma"].as<double>();
  unsigned int Nx = rf.Nx();
  unsigned int Ny = rf.Ny();

  Eigen::ArrayXd xi = Eigen::ArrayXd::LinSpaced(Nx + 1, 0, Lx).segment(0, Nx);
  Eigen::ArrayXd yi = Eigen::ArrayXd::LinSpaced(Ny + 1, 0, Ly).segment(0, Ny);

  Sigma.setZero();

  auto sigmaf_const = [sigma](double x, double y) {
    double d = std::max(x, y);
    if (d > 1) {
      return sigma;
    } else {
      return 0.0;
    }
  };
  auto sigmaf_lin = [sigma](double x, double y) {
    double d = std::max(x, y);
    if (d > 1 && d <= 1.1)
      return (d - 1) / 0.1 * sigma;
    else
      return sigma;
  };
  auto sigmaf_smooth = [sigma](double x, double y) {
    double d = std::max(x, y);
    double z = d - 1;
    if (d > 1 && d <= 1.2)
      return 400 * std::pow(z, 2) - 4000 * std::pow(z, 3) + 10000 * std::pow(z, 4);
    else
      return 0.;
  };
  std::function<double(double, double)> sigmaf;
  if (vm["sigmaf"].as<std::string>() == "const")
    sigmaf = sigmaf_const;
  else if (vm["sigmaf"].as<std::string>() == "lin")
    sigmaf = sigmaf_lin;
  else if (vm["sigmaf"].as<std::string>() == "smooth")
    sigmaf = sigmaf_smooth;
  else {
    std::cerr << "Option --sigmaf=" << vm["sigmaf"].as<std::string>() << " not found." << std::endl;
  }

  // initialize sigma
  for (unsigned int i = 0; i < Ny; ++i) {
    for (unsigned int j = 0; j < Nx; ++j) {
      if (yi[j] < 1 and yi[i] < 1) continue;
      Sigma(i, j) = sigmaf(xi[j], yi[i]);
    }
  }
}

void
RidgeletInflowTest::init_fh0(complex_array_t& Fh)
{
  unsigned int Nx = rf.Nx();
  unsigned int Ny = rf.Ny();

  Eigen::ArrayXd xi = Eigen::ArrayXd::LinSpaced(Nx / 2 + 1, 0, Lx).segment(0, Nx / 2);
  Eigen::ArrayXd yi = Eigen::ArrayXd::LinSpaced(Ny / 2 + 1, 0, Ly).segment(0, Ny / 2);

  array_t X = xi.transpose().replicate(Ny / 2, 1);
  array_t Y = yi.replicate(1, Nx / 2);
  array_t F = array_t::Ones(Ny / 2, Nx / 2);
  F = (X < 1 && Y < 1).select(F, array_t::Zero(Ny / 2, Nx / 2));
  complex_array_t fh(Ny / 2, Nx / 2);
  fft.ft(fh, F, false);
  Fh.setZero();
  ftcut(Fh, Ny / 2, Nx / 2) = 4 * fh;
}

void
RidgeletInflowTest::init_fbc(array_t& fbc, double vx, double vy, double ql, double qt)
{
  unsigned int Nx = rf.Nx();
  unsigned int Ny = rf.Ny();

  fbc.resize(Ny / 2, Nx / 2);
  make_inflow_source(fbc, vx, vy, Lx, Ly, ql, qt);

  Eigen::ArrayXd xi = Eigen::ArrayXd::LinSpaced(Nx / 2 + 1, 0, Lx).segment(0, Nx / 2);
  Eigen::ArrayXd yi = Eigen::ArrayXd::LinSpaced(Ny / 2 + 1, 0, Ly).segment(0, Ny / 2);

  array_t X = xi.transpose().replicate(Ny / 2, 1);
  array_t Y = yi.replicate(1, Nx / 2);
  fbc = (X < 1 && Y < 1).select(array_t::Zero(Ny / 2, Nx / 2), fbc);
}

void
RidgeletInflowTest::apply_bc(complex_array_t& Fh, const array_t& fbc)
{
  unsigned int Nx = rf.Nx();
  unsigned int Ny = rf.Ny();

  complex_array_t fh = ftcut(Fh, Ny / 2, Nx / 2);
  array_t F(Ny / 2, Nx / 2);
  fft.ift(F, fh);
  F /= 4;
  F = (fbc > 0).select(fbc, F);
  fft.ft(fh, F, false);
  Fh.setZero();
  ftcut(Fh, Ny / 2, Nx / 2) = 4 * fh;
}

void
RidgeletInflowTest::run(double vx, double vy, double ql, double qt, double rho, int ntsteps)
{
  rca_t x(rf);
  double dt = vm["dt"].as<double>();
  int cg_maxit = vm["cg_maxit"].as<int>();
  double cg_tol = vm["cg_tol"].as<double>();

  unsigned int Nx = rf.Nx();
  unsigned int Ny = rf.Ny();

  array_t Sigma(Ny, Nx);
  this->init_sigma(Sigma);

  array_t fbc(Ny / 2, Nx / 2);
  this->init_fbc(fbc, vx, vy, ql, qt);

  hid_t file, group_id;
  if (vm.count("save"))
    file = H5Fcreate("advection_inflow.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);

  array_t q(Ny, Nx);
  double r = sqrt(vx * vx + vy * vy);
  q.setZero();
  make_inflow_source(q, vx, vy, Lx, Ly, ql, qt);
  q *= (Sigma * r);

  if (vm.count("save")) {
    eigen2hdf::save(file, "q", q);
    eigen2hdf::save(file, "Sigma", Sigma);
    eigen2hdf::save(file, "fbc", fbc);
  }
  complex_array_t qh(Ny, Nx);
  fft.ft(qh, q, false);

  // make F decay smoothly in the absorption layer
  // F *= (array_t::Ones(Ny, Nx) -Sigma/sigma);
  complex_array_t Fh(Ny, Nx);
  this->init_fh0(Fh);

  auto& x_coeffs = x.coeffs();
  rt.rt(x_coeffs, Fh);

  // initialize operators
  // AhA = T'T
  AhAOpsigma AhA(Sigma, vx, vy, Lx, Ly, Nx, Ny, dt);
  // preconditioned operator
  RDTSCTimer timer;
  complex_array_t Bh(Ny, Nx);
  complex_array_t Rhsh(Ny, Nx);  // right hand side
  rca_t b(rf);
  rt_solver = RidgeletSolver<rt_coeff_t>(rf, vx, vy);
  PTransportOpBC<RT_t> A(rt, AhA, vx, vy);
  TransportOperatorBC T(Sigma, vx, vy, Lx, Ly, Nx, Ny, dt);  // required for rhs

  if (vm.count("save")) {
    complex_array_t solh(Ny / 2, Nx / 2);
    solh = ftcut(Fh, Ny / 2, Nx / 2);
    array_t sol(Ny / 2, Nx / 2);
    // scale fourier coefficients by grid_out factor
    // ftcut(solh, Ny, Nx) = Fh;
    fft.ift(sol, solh);
    sol /= 4;
    group_id = H5Gcreate(
        file, boost::lexical_cast<string>(0).c_str(), H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
    eigen2hdf::save(group_id, "sol", sol);
    H5Gclose(group_id);
  }
  // do not print log to stdout
  Logger::GetInstance().detach_stdout();
  for (int i = 1; i <= ntsteps; ++i) {
    this->apply_bc(Fh, fbc);
    // T'*Fh
    Rhsh = Fh + dt * qh;  // rhs in fourier space
    T.apply(Bh, Rhsh, true /* hermitian transpose */);
    rt.rt(b.coeffs(), Bh);

    timer.start();
    Logger::GetInstance().push_prefix(boost::lexical_cast<std::string>(i) + " CGSOLVER");
    rt_solver.solve(x, A, b, cg_tol, cg_maxit, log_hist);
    Logger::GetInstance().pop_prefix();
    auto nc_solve = timer.stop();

    cout << "RidgeletSolver took: " << nc_solve / 1e9 << " Gcycles\n";
    // write arrays to hdf5
    // complex_array_t tmp(Ny, Nx);
    rt.irt(Fh, x.coeffs());

    // eigen output
    if (vm.count("save")) {
      complex_array_t solh(Ny / 2, Nx / 2);
      solh = ftcut(Fh, Ny / 2, Nx / 2);
      array_t sol(Ny / 2, Nx / 2);
      // scale fourier coefficients by grid_out factor
      // ftcut(solh, Ny, Nx) = Fh;
      fft.ift(sol, solh);
      sol /= 4;
      group_id = H5Gcreate(
          file, boost::lexical_cast<string>(i).c_str(), H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
      eigen2hdf::save(group_id, "sol", sol);
      H5Gclose(group_id);
    }
  }
  if (vm.count("save")) H5Fclose(file);
}


/*
This code is based on the original code by Quentin "Leph" Rouxel and Team Rhoban.
The original files can be found at:
https://github.com/Rhoban/model/
*/
#ifndef QUINTICWALK_HPP
#define QUINTICWALK_HPP

#include <algorithm>
#include <Eigen/Dense>
#include "Footstep.hpp"
#include "bitbots_splines/TrajectoryUtils.h"
#include <tf/transform_datatypes.h>
#include <Eigen/Dense>
#include <tf_conversions/tf_eigen.h>
#include <eigen_conversions/eigen_msg.h>
#include "bitbots_splines/SplineContainer.hpp"
#include "bitbots_splines/AxisAngle.h"
#include "bitbots_splines/Angle.h"
#include "bitbots_splines/Euler.h"
#include "bitbots_quintic_walk/common.h"
#include <math.h>

namespace bitbots_quintic_walk {

    struct WalkingParameter {
        //Full walk cycle frequency
        //(in Hz, > 0)
        double freq;
        //Length of double support phase in half cycle
        //(ratio, [0:1])
        double doubleSupportRatio;
        //Lateral distance between the feet center
        //(in m, >= 0)
        double footDistance;
        //Maximum flying foot height
        //(in m, >= 0)
        double footRise;
        // Pause of Z movement on highest point
        //(single support cycle ratio, [0,1])
        double footZPause;
        //Let the foot's downward trajectory end above the ground
        //this is helpful if the support leg bends
        //(in m, >= 0)
        double footPutDownZOffset;
        //Phase time for moving the foot from Z offset to ground,
        // also used for X and Y since they should not move after contact to the ground
        //(phase between apex and single support end [0:1])
        double footPutDownPhase;
        //Phase of flying foot apex
        //(single support cycle phase, [0:1])
        double footApexPhase;
        //Foot X/Y overshoot in ratio of step length
        //(ratio, >= 0)
        double footOvershootRatio;
        //Foot X/Y overshoot phase
        //(single support cycle phase, [footApexPhase:1]
        double footOvershootPhase;
        //Height of the trunk from ground
        //(in m, > 0)
        double trunkHeight;
        //Trunk pitch orientation
        //(in rad)
        double trunkPitch;
        //Phase offset of trunk oscillation
        //(half cycle phase, [0:1])
        double trunkPhase;
        //Trunk forward offset
        //(in m)
        double trunkXOffset;
        //Trunk lateral offset
        //(in m)
        double trunkYOffset;
        //Trunk lateral oscillation amplitude ratio
        //(ratio, >= 0)
        double trunkSwing;
        //Trunk swing pause length in phase at apex
        //(half cycle ratio, [0:1])
        double trunkPause;
        //Trunk forward offset proportional to forward step
        //(in 1)
        double trunkXOffsetPCoefForward;
        //Trunk forward offset proportional to rotation step
        //(in m/rad)
        double trunkXOffsetPCoefTurn;
        //Trunk pitch orientation proportional to forward step
        //(in rad/m)
        double trunkPitchPCoefForward;
        //Trunk pitch orientation proportional to rotation step
        //(in 1)
        double trunkPitchPCoefTurn;
        double trunkYOnlyInDoubleSupport;
        double kickLength;
        double kickPhase;
        double footPutDownRollOffset;
        double kickVel;
        double pauseDuration;
        double firstStepSwingFactor;
    };

/**
 * QuinticWalk
 *
 * Holonomic and open loop walk
 * generator based on footstep control
 * and quintic splines in cartesian space.
 * Expressed all target state in cartesian
 * space with respect to current cupport foot
 */
    class QuinticWalk {
    public:

        /**
         * Initialization
         */
        QuinticWalk();

        /**
         * Return current walk phase
         * between 0 and 1
         */
        double getPhase() const;

        /**
         * Return current time between
         * 0 and half period for
         * trajectories evaluation
         */
        double getTrajsTime() const;

        /**
         * Get the footstep object.
         */
        Footstep getFootstep();

        /**
         * Return if true if left is current support foot
         */
        bool isLeftSupport();

        /**
         * Return true if both feet are currently on the ground
         */
        bool isDoubleSupport();

        /**
         * Assign given parameters vector
         */
        void setParameters(const WalkingParameter &params);

        /**
         * Update the internal walk state
         * (pĥase, trajectories) from given
         * elapsed time since last update() call
         */
        bool updateState(double dt, const Eigen::Vector3d &orders, bool walkableState);

        /**
         * Compute current cartesian
         * target from trajectories and assign
         * it to given model through inverse
         * kinematics.
         * Return false is the target is
         * unreachable.
         */
        void computeCartesianPosition(Eigen::Vector3d &trunkPos, Eigen::Vector3d &trunkAxis,
                                      Eigen::Vector3d &footPos, Eigen::Vector3d &footAxis, bool &isLeftsupportFoot);

        void
        computeCartesianPositionAtTime(Eigen::Vector3d &trunkPos, Eigen::Vector3d &trunkAxis, Eigen::Vector3d &footPos,
                                       Eigen::Vector3d &footAxis, bool &isLeftsupportFoot, double time);

        void requestKick(bool left);

        void requestPause();

        /**
         * Ends the current step earlier. Useful if foot hits ground to early.
         */
        void endStep();

        /**
         * Completely reset the engine, e.g. when robot fell down
         */
        void reset();

        std::string getState();

    private:

        std::string _engineState;

        /**
         * Current footstep support
         * and flying last and next pose
         */
        Footstep _footstep;

        /**
         * Movement phase between 0 and 1
         */
        double _phase;
        double _lastPhase;

        double _timePaused;

        /**
         * Currently used parameters
         */
        WalkingParameter _params;

        bool _leftKickRequested;
        bool _rightKickRequested;
        bool _pauseRequested;

        /**
         * Trunk pose and orientation
         * position, velocity and acceleration
         * at half cycle start
         */
        Eigen::Vector3d _trunkPosAtLast;
        Eigen::Vector3d _trunkVelAtLast;
        Eigen::Vector3d _trunkAccAtLast;
        Eigen::Vector3d _trunkAxisPosAtLast;
        Eigen::Vector3d _trunkAxisVelAtLast;
        Eigen::Vector3d _trunkAxisAccAtLast;

        /**
         * Generated half walk
         * cycle trajectory
         */
        bitbots_splines::Trajectories _trajs;

        void updatePhase(double dt);

        void buildNormalTrajectories(const Eigen::Vector3d &orders);

        void buildKickTrajectories(const Eigen::Vector3d &orders);

        void buildStartTrajectories(const Eigen::Vector3d &orders);

        void buildStopStepTrajectories(const Eigen::Vector3d &orders);

        void buildStopMovementTrajectories(const Eigen::Vector3d &orders);

        void buildTrajectories(const Eigen::Vector3d &orders, bool startMovement, bool startStep, bool kickStep);

        void buildWalkDisableTrajectories(const Eigen::Vector3d &orders, bool footInIdlePosition);

        void saveCurrentTrunkState();

        void useCurrentTrunkState();

        void point(std::string spline, double t, double pos, double vel = 0, double acc = 0);

        /**
         * Reset the trunk position and
         * orientation state vectors at last
         * half cycle as stopped pose
         */
        void resetTrunkLastState();
    };

}
#endif


// Copyright (c) John Morrison, all rights reserved.  See the
// accompanying LICENSE file for more information.

#include <math.h>
#include <Accuracy.h>
#include <CartesianCoordinates.h>
#include <CoordinateConversionException.h>
#include <CoordinateConversionService.h>
#include <GeodeticCoordinates.h>
#include <GeodeticParameters.h>
#include <HeightType.h>
#include <LocalCartesianParameters.h>
#include <geocon/geodetic2ecef.h>
#include <Eigen/Eigen>
#include <miniglog/logging.h>
#include "geocon_init.h"

namespace geocon {

using namespace MSP::CCS;
using namespace Eigen;

static GeodeticParameters kGeodeticParameters(
    CoordinateType::geodetic,
    HeightType::ellipsoidHeight);

static GeodeticParameters kECEFParameters(
    CoordinateType::geocentric,
    HeightType::ellipsoidHeight);

geodetic2ecef* geodetic2ecef::Create() {
  if (!init()) {
    LOG(ERROR) << "Failed to initialize GeoCon";
    return nullptr;
  }

  try {
    return new geodetic2ecef(
        new CoordinateConversionService("WGE", &kGeodeticParameters,
                                        "WGE", &kECEFParameters));
  } catch (CoordinateConversionException& e) {
    LOG(ERROR) << "Failed to create ccs_: " << e.getMessage();
  }
  return nullptr;
}

geodetic2ecef::geodetic2ecef(CoordinateConversionService* ccs) : ccs_(ccs) {}

bool geodetic2ecef::to_ecef(
    double latitude, double longitude, double altitude, double std,
    CartesianCoordinates* cart, Accuracy* acc) const {
  CHECK_NOTNULL(cart);
  CHECK_NOTNULL(acc);
  GeodeticCoordinates geo_coords(CoordinateType::geodetic,
                                 longitude, latitude, altitude);
  Accuracy geo_acc;

  // 90% confidence interval is 1.64 STD
  geo_acc.setSphericalError90(std * 1.64);

  try {
    ccs_->convertSourceToTarget(&geo_coords, &geo_acc, *cart, *acc);
  } catch (CoordinateConversionException& e) {
    LOG(ERROR) << "Failed to convert coordinates: " << e.getMessage();
    return false;
  }

  return true;
}

bool geodetic2ecef::to_geodetic(
    double x, double y, double z, double std,
    GeodeticCoordinates* geo, Accuracy* acc) const {
  CHECK(false);
  return false;
}
}  // namespace geocon


/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#include <Eigen/Dense>
#include "octopus_search.hpp"
#include "mader_types.hpp"
#include "utils.hpp"
#include "ros/ros.h"
#include "visualization_msgs/MarkerArray.h"
#include "visualization_msgs/Marker.h"
#include "decomp_ros_msgs/PolyhedronArray.h"
#include <decomp_ros_utils/data_ros_utils.h>  //For DecompROS::polyhedron_array_to_ros

Eigen::Vector3d getPosDynObstacle(double t)
{
  double t_scaled = 4.3 * t;

  Eigen::Vector3d pos;
  pos << sin(t_scaled) + 2 * sin(2 * t_scaled),  //////////////////////
      cos(t_scaled) - 2 * cos(2 * t_scaled),     //////////////////////
      -sin(3 * t_scaled);                        //////////////////////

  pos.x() = pos.x() / 2.5;
  pos.y() = pos.y() / 2.5;
  pos.z() = pos.z() / 1.5;

  pos.z() = pos.z() + 1.0;

  return pos;
}

ConvexHullsOfCurve createStaticObstacle(double x, double y, double z, int num_pol, double bbox_x, double bbox_y,
                                        double bbox_z)
{
  ConvexHullsOfCurve hulls_curve;
  std::vector<Point_3> points;

  points.push_back(Point_3(x - bbox_x / 2.0, y - bbox_y / 2.0, z - bbox_z / 2.0));
  points.push_back(Point_3(x - bbox_x / 2.0, y - bbox_y / 2.0, z + bbox_z / 2.0));
  points.push_back(Point_3(x - bbox_x / 2.0, y + bbox_y / 2.0, z + bbox_z / 2.0));
  points.push_back(Point_3(x - bbox_x / 2.0, y + bbox_y / 2.0, z - bbox_z / 2.0));

  points.push_back(Point_3(x + bbox_x / 2.0, y + bbox_y / 2.0, z + bbox_z / 2.0));
  points.push_back(Point_3(x + bbox_x / 2.0, y + bbox_y / 2.0, z - bbox_z / 2.0));
  points.push_back(Point_3(x + bbox_x / 2.0, y - bbox_y / 2.0, z + bbox_z / 2.0));
  points.push_back(Point_3(x + bbox_x / 2.0, y - bbox_y / 2.0, z - bbox_z / 2.0));

  CGAL_Polyhedron_3 hull_interval = cu::convexHullOfPoints(points);

  for (int i = 0; i < num_pol; i++)
  {
    hulls_curve.push_back(hull_interval);  // static obstacle
  }

  return hulls_curve;
}

visualization_msgs::Marker getMarker(Eigen::Vector3d& center, double bbox_x, double bbox_y, double bbox_z, double t_min,
                                     double t_final, double t, int id)
{
  visualization_msgs::Marker m;
  m.type = visualization_msgs::Marker::CUBE;
  m.header.frame_id = "world";
  m.header.stamp = ros::Time::now();
  m.ns = "marker_dyn_obs";
  m.action = visualization_msgs::Marker::ADD;
  m.id = id;
  m.color = mu::getColorJet(t, t_min, t_final);
  m.scale.x = bbox_x;
  m.scale.y = bbox_y;  // rviz complains if not
  m.scale.z = bbox_z;  // rviz complains if not
  m.pose.position.x = center.x();
  m.pose.position.y = center.y();
  m.pose.position.z = center.z();
  m.pose.orientation.w = 1.0;
  return m;

  // visualization_msgs::MarkerArray marker_array;

  // if (data.size() == 0)
  // {
  //   return marker_array;
  // }

  // int j = 9000;
  // for (int i = 0; i < data.size(); i = i + 1)
  // {
  //   visualization_msgs::Marker m;
  //   m.type = visualization_msgs::Marker::CUBE;
  //   m.header.frame_id = "world";
  //   m.header.stamp = ros::Time::now();
  //   m.ns = ns;
  //   m.action = visualization_msgs::Marker::ADD;
  //   m.id = j;
  //   m.color = mu::getColorJet(t, t_min, t_final);
  //   m.scale.x = bbox_x;
  //   m.scale.y = bbox_y;  // rviz complains if not
  //   m.scale.z = bbox_z;  // rviz complains if not
  //   m.pose.position.x = data[i].x();
  //   m.pose.position.y = data[i].y();
  //   m.pose.position.z = data[i].z();
  //   m.pose.orientation.w = 1.0;

  //   marker_array.markers.push_back(m);
  //   j = j + 1;
  // }
  // return marker_array;
}

ConvexHullsOfCurve createDynamicObstacle(std::vector<visualization_msgs::MarkerArray>& ma_vector, double x, double y,
                                         double z, int num_pol, double bbox_x, double bbox_y, double bbox_z,
                                         double t_min, double t_max)
{
  double time_per_interval = (t_max - t_min) / num_pol;
  double time_discretization = 0.01;

  ConvexHullsOfCurve hulls_curve;

  ma_vector.clear();
  // ma.markers.clear();

  int j = 0;
  for (int interval_index = 0; interval_index < num_pol; interval_index++)
  {
    std::vector<Point_3> points_interval;

    visualization_msgs::MarkerArray ma;
    std::cout << "--------------Interval " << interval_index << std::endl;
    for (double t = interval_index * time_per_interval; t < (interval_index + 1) * time_per_interval;
         t = t + time_discretization)
    {
      Eigen::Vector3d current_pos = getPosDynObstacle(t);
      x = current_pos.x();
      y = current_pos.y();
      z = current_pos.z();

      double bbox_x_infl = bbox_x + 0.03;
      double bbox_y_infl = bbox_y + 0.03;
      double bbox_z_infl = bbox_z + 0.03;

      points_interval.push_back(Point_3(x - bbox_x_infl / 2.0, y - bbox_y_infl / 2.0, z - bbox_z_infl / 2.0));
      points_interval.push_back(Point_3(x - bbox_x_infl / 2.0, y - bbox_y_infl / 2.0, z + bbox_z_infl / 2.0));
      points_interval.push_back(Point_3(x - bbox_x_infl / 2.0, y + bbox_y_infl / 2.0, z + bbox_z_infl / 2.0));
      points_interval.push_back(Point_3(x - bbox_x_infl / 2.0, y + bbox_y_infl / 2.0, z - bbox_z_infl / 2.0));

      points_interval.push_back(Point_3(x + bbox_x_infl / 2.0, y + bbox_y_infl / 2.0, z + bbox_z_infl / 2.0));
      points_interval.push_back(Point_3(x + bbox_x_infl / 2.0, y + bbox_y_infl / 2.0, z - bbox_z_infl / 2.0));
      points_interval.push_back(Point_3(x + bbox_x_infl / 2.0, y - bbox_y_infl / 2.0, z + bbox_z_infl / 2.0));
      points_interval.push_back(Point_3(x + bbox_x_infl / 2.0, y - bbox_y_infl / 2.0, z - bbox_z_infl / 2.0));

      ma.markers.push_back(getMarker(current_pos, bbox_x, bbox_y, bbox_z, t_min, t_max, t, j));
      j = j + 1;
    }

    ma_vector.push_back(ma);

    CGAL_Polyhedron_3 hull_interval = cu::convexHullOfPoints(points_interval);
    hulls_curve.push_back(hull_interval);
  }
  return hulls_curve;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "testOctopusSearch");
  ros::NodeHandle nh("~");
  ros::Publisher trajectories_found_pub =
      nh.advertise<visualization_msgs::MarkerArray>("/trajectories_found", 1000, true);

  ros::Publisher best_trajectory_found_pub =
      nh.advertise<visualization_msgs::MarkerArray>("/best_trajectory_found", 1000, true);
  ros::Publisher convex_hulls_pub = nh.advertise<visualization_msgs::Marker>("/convex_hulls", 1, true);

  std::vector<ros::Publisher> jps_poly_pubs;  // = nh.advertise<decomp_ros_msgs::PolyhedronArray>("poly_jps", 1, true);
  std::vector<ros::Publisher> traj_obstacle_colored_pubs;
  std::vector<ros::Publisher> best_trajectory_found_intervals_pubs;

  int num_pol = 7;
  int deg_pol = 3;

  for (int i = 0; i < num_pol; i++)
  {
    ros::Publisher tmp =
        nh.advertise<visualization_msgs::MarkerArray>("/traj_obstacle_colored_int_" + std::to_string(i), 1, true);
    traj_obstacle_colored_pubs.push_back(tmp);

    ros::Publisher tmp2 = nh.advertise<decomp_ros_msgs::PolyhedronArray>("/poly_jps_int_" + std::to_string(i), 1, true);
    jps_poly_pubs.push_back(tmp2);

    ros::Publisher tmp3 =
        nh.advertise<visualization_msgs::MarkerArray>("/best_trajectory_found_int_" + std::to_string(i), 1, true);
    best_trajectory_found_intervals_pubs.push_back(tmp3);
  }

  // ros::Publisher traj_obstacle_colored_int0_pub =
  //     nh.advertise<visualization_msgs::MarkerArray>("traj_obstacle_colored_int0", 1, true);

  std::string basis;

  nh.getParam("basis", basis);

  std::cout << "Basis= " << basis << std::endl;

  int samples_x = 5;  // odd number
  int samples_y = 5;  // odd number
  int samples_z = 5;  // odd number

  double alpha_shrink = 0.9;

  double fraction_voxel_size = 0.0;  // grid used to prune nodes that are on the same cell

  double runtime = 0.1;    //[seconds]
  double goal_size = 0.1;  //[meters]

  Eigen::Vector3d v_max(7.0, 7.0, 7.0);
  Eigen::Vector3d a_max(400000.0, 4000000.0, 4000000.0);

  Eigen::Vector3d q0(-3, 0.5, 1);
  Eigen::Vector3d q1 = q0;
  Eigen::Vector3d q2 = q1;
  Eigen::Vector3d goal(5.0, 0, 1);

  double t_min = 0.0;
  double t_max = t_min + (goal - q0).norm() / (0.8 * v_max(0));

  std::cout << "t_min= " << t_min << std::endl;
  std::cout << "t_max= " << t_max << std::endl;
  std::cout << "=========================" << std::endl;

  ConvexHullsOfCurves hulls_curves;

  double bbox_x = 0.4;
  double bbox_y = 0.4;
  double bbox_z = 0.4;

  double dc = 0.002;  // Simply used for visualization

  int num_of_obs = 1;  // odd number
  double separation = 0.4;

  int num_of_obs_up = (num_of_obs - 1) / 2.0;

  std::vector<visualization_msgs::MarkerArray> ma_vector;

  // ConvexHullsOfCurve hulls_curve = createStaticObstacle(0.0, 0.0, bbox_z / 2.0, num_pol, bbox_x, bbox_y, bbox_z);
  ConvexHullsOfCurve hulls_curve =
      createDynamicObstacle(ma_vector, 0.0, 0.0, bbox_z / 2.0, num_pol, bbox_x, bbox_y, bbox_z, t_min, t_max);
  hulls_curves.push_back(hulls_curve);  // only one obstacle

  for (int i = 1; i <= num_of_obs_up; i++)
  {
    ConvexHullsOfCurve hulls_curve =
        createStaticObstacle(0.0, i * (bbox_y + separation), 0.0, num_pol, bbox_x, bbox_y, bbox_z);
    hulls_curves.push_back(hulls_curve);  // only one obstacle

    hulls_curve = createStaticObstacle(0.0, -i * (bbox_y + separation), 0.0, num_pol, bbox_x, bbox_y, bbox_z);
    hulls_curves.push_back(hulls_curve);  // only one obstacle
  }

  for (int i = 0; i < ma_vector.size(); i++)
  {
    traj_obstacle_colored_pubs[i].publish(ma_vector[i]);
  }

  std::cout << "hulls_curves.size()= " << hulls_curves.size() << std::endl;

  mt::ConvexHullsOfCurves_Std hulls_std = cu::vectorGCALPol2vectorStdEigen(hulls_curves);
  // vec_E<Polyhedron<3>> jps_poly = cu::vectorGCALPol2vectorJPSPol(hulls_curves);

  for (int i = 0; i < num_pol; i++)
  {
    ConvexHullsOfCurve tmp2;
    ConvexHullsOfCurves tmp;

    tmp2.push_back(hulls_curve[i]);
    tmp.push_back(tmp2);

    // convert the obstacles polyhedron arrays
    decomp_ros_msgs::PolyhedronArray poly_msg = DecompROS::polyhedron_array_to_ros(cu::vectorGCALPol2vectorJPSPol(tmp));
    poly_msg.header.frame_id = "world";
    jps_poly_pubs[i].publish(poly_msg);
  }

  // hull.push_back(Eigen::Vector3d(-1.0, -1.0, -700.0));
  // hull.push_back(Eigen::Vector3d(-1.0, -1.0, 700.0));
  // hull.push_back(Eigen::Vector3d(-1.0, 1.0, 700.0));
  // hull.push_back(Eigen::Vector3d(-1.0, 1.0, -700.0));

  // hull.push_back(Eigen::Vector3d(1.0, 1.0, 700.0));
  // hull.push_back(Eigen::Vector3d(1.0, 1.0, -700.0));
  // hull.push_back(Eigen::Vector3d(1.0, -1.0, 700.0));
  // hull.push_back(Eigen::Vector3d(1.0, -1.0, -700.0));

  // Assummes static obstacle
  /*  for (int i = 0; i < num_pol; i++)
    {
      hulls_curve.push_back(hull);
    }

    hulls_curves.push_back(hulls_curve);*/

  OctopusSearch myAStarSolver(basis, num_pol, deg_pol, alpha_shrink);
  myAStarSolver.setUp(t_min, t_max, hulls_std);

  myAStarSolver.setq0q1q2(q0, q1, q2);
  myAStarSolver.setGoal(goal);

  myAStarSolver.setXYZMinMaxAndRa(-1e6, 1e6, -1e6, 1e6, -1.0, 10.0, 1e6);  // limits for the search, in world frame
  myAStarSolver.setBBoxSearch(30.0, 30.0, 30.0);                           // limits for the search, centered on q2
  myAStarSolver.setMaxValuesAndSamples(v_max, a_max, samples_x, samples_y, samples_z, fraction_voxel_size);

  myAStarSolver.setRunTime(runtime);
  myAStarSolver.setGoalSize(goal_size);

  myAStarSolver.setBias(1.0);

  myAStarSolver.setVisual(false);

  std::vector<Eigen::Vector3d> q;
  std::vector<Eigen::Vector3d> n;
  std::vector<double> d;
  bool solved = myAStarSolver.run(q, n, d);

  // Recover all the trajectories found and the best trajectory
  std::vector<mt::trajectory> all_trajs_found;
  myAStarSolver.getAllTrajsFound(all_trajs_found);

  mt::trajectory best_traj_found;
  mt::PieceWisePol pwp_best_traj_found;
  myAStarSolver.getBestTrajFound(best_traj_found, pwp_best_traj_found, dc);

  // for (double t = t_min; t <= t_max; t = t + dc)
  // {
  //   Eigen::Vector3d pos = pwp_best_traj_found.eval(t);
  //   std::cout << "At t= " << t << " pos=" << pos.transpose() << " pos_obstacle=" << getPosDynObstacle(t).transpose()
  //             << std::endl;
  // }

  // Convert to marker arrays
  //---> all the trajectories found
  int increm = 2;
  int increm_best = 1;
  double scale = 0.02;
  int j = 0;

  visualization_msgs::MarkerArray marker_array_all_trajs;
  for (auto traj : all_trajs_found)
  {
    visualization_msgs::MarkerArray marker_array_traj = mu::trajectory2ColoredMarkerArray(
        traj, v_max.maxCoeff(), increm, "traj" + std::to_string(j), scale, "time", 0, 1);
    // std::cout << "size of marker_array_traj= " << marker_array_traj.markers.size() << std::endl;
    for (auto marker : marker_array_traj.markers)
    {
      marker_array_all_trajs.markers.push_back(marker);
    }
    j++;
  }

  //---> the best trajectory found
  scale = 0.1;
  visualization_msgs::MarkerArray marker_array_best_traj;
  marker_array_best_traj = mu::trajectory2ColoredMarkerArray(best_traj_found, v_max.maxCoeff(), increm_best,
                                                             "traj" + std::to_string(j), scale, "time", 0, 1);

  double entries_per_interval = marker_array_best_traj.markers.size() / num_pol;
  for (int i = 0; i < num_pol; i++)
  {
    std::vector<visualization_msgs::Marker> tmp(                                 /////////
        marker_array_best_traj.markers.begin() + i * entries_per_interval,       /////////
        marker_array_best_traj.markers.begin() + (i + 1) * entries_per_interval  /////////
    );

    visualization_msgs::MarkerArray ma;
    ma.markers = tmp;
    best_trajectory_found_intervals_pubs[i].publish(ma);
  }

  best_trajectory_found_pub.publish(marker_array_best_traj);

  // Get the edges of the convex hulls and publish them
  mt::Edges edges_convex_hulls;
  myAStarSolver.getEdgesConvexHulls(edges_convex_hulls);
  convex_hulls_pub.publish(mu::edges2Marker(edges_convex_hulls, mu::color(mu::red_normal)));

  // publish the trajectories
  trajectories_found_pub.publish(marker_array_all_trajs);

  ros::spinOnce();

  /*
    vectorOfNodes2vectorOfStates()

        traj_committed_colored_ = stateVector2ColoredMarkerArray(data, type, par_.v_max, increm, name_drone_);
    pub_traj_committed_colored_.publish(traj_committed_colored_);*/

  // if (solved == true)
  // {
  //   std::cout << "This is the result" << std::endl;
  //   for (auto qi : q)
  //   {
  //     std::cout << qi.transpose() << std::endl;
  //   }
  // }
  // else
  // {
  //   std::cout << "A* didn't find a solution" << std::endl;
  // }

  // std::cout << "Normal Vectors: " << std::endl;
  // for (auto ni : n)
  // {
  //   std::cout << ni.transpose() << std::endl;
  // }

  // std::cout << "D coefficients: " << std::endl;
  // for (auto di : d)
  // {
  //   std::cout << di << std::endl;
  // }

  ros::spin();

  return 0;
}


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

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

const int debug_level = 0;

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

struct DebtRelation
{
  int i, j, d;
};

void testcase()
{
  int n, m;
  std::cin >> n >> m;
  assert(n >= 1 && n <= 1e3 && m >= 0 && m <= 1e5);

  std::vector<int> balances_by_province(n);
  for (int &v : balances_by_province)
  {
    std::cin >> v;
    assert(abs(v) <= (1 << 20));
  }

  std::vector<DebtRelation> debt_relations(m);
  for (DebtRelation &r : debt_relations)
  {
    std::cin >> r.i >> r.j >> r.d;
    assert(r.i != r.j && r.i >= 0 && r.i < n && r.j >= 0 && r.j < n && r.d > 0 && r.d <= (1 << 20));
  }

  Graph G(n + 2);
  const int node_source = n;
  const int node_sink = n + 1;
  auto add_edge = [&G](int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const Graph::edge_descriptor e = boost::add_edge(from, to, G).first;
    const Graph::edge_descriptor rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  };

  long base_flow = 0;
  for (int i = 0; i < n; i++)
  {
    const int balance = balances_by_province.at(i);
    if (balance >= 0)
    {
      DEBUG(3, "non-negative balance " << balance << " i " << i);
      add_edge(node_source, i, balance);
      base_flow += balance;
    }
    else
    {
      add_edge(i, node_sink, -balance);
    }
  }

  for (const DebtRelation &r : debt_relations)
  {
    add_edge(r.i, r.j, r.d);
  }

  long flow = boost::push_relabel_max_flow(G, node_source, node_sink);
  DEBUG(1, "flow " << flow << " base_flow " << base_flow);
  std::cout << (flow < base_flow ? "yes" : "no") << "\n";
}

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

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

  return 0;
}


#pragma once

#include <boost/utility.hpp>
#include <iterator>
#include <MicropolygonMesh.hpp>
#include <vector>
#include <VertexType.hpp>

template <class CurveType>
struct CurveSubdivider {
	typedef typename VertexType<CurveType>::type VertexType;
	typedef typename ScalarType<VertexType>::type ScalarType;

	private:
		struct NodeType {
			VertexType vertex_;

			Vec4 rhw_;
			Vec2 projected_position_;

			ScalarType u_;

			NodeType(CurveType const & curve, ScalarType const & u)
				: vertex_(curve(u)), rhw_(rhw_position(vertex_)),
					projected_position_(rhw_.x(), rhw_.y()), u_(u)
			{
				projected_position_ /= (rhw_.z() * rhw_.w());
			}
		};

		typedef std::vector<NodeType> NodeBuffer;

		struct TransposeIterator_ {
			typedef typename MicropolygonMesh<VertexType>::iterator base_iterator;

			TransposeIterator_(base_iterator begin, base_iterator end, int stride)
				: begin_(begin), it_(begin), end_(end), stride_(stride) { }

			void accumulate(VertexType const & vertex) {
				*it_ = vertex;
				it_ += stride_;
				if (it_ >= end_) { it_ = ++begin_; }
			}

			private:
				base_iterator begin_, it_, end_;
				int stride_;
		};

		struct TransposeIterator {
			TransposeIterator (TransposeIterator_ & base)
				: base_(base) { }

			TransposeIterator & operator * () { return *this; }

			TransposeIterator operator ++ (int) { return *this; }

			TransposeIterator & operator ++ () { return *this; }

			TransposeIterator & operator = (VertexType const & vertex) {
				base_.accumulate(vertex);
			}

			private:
				TransposeIterator_ & base_;
		};

	public:
		CurveSubdivider(CurveType const & curve, float width = 1.0f)
			: curve_(curve), half_width_(width / 2.0f)
		{
			ping_ = {
				NodeType(curve_, static_cast<ScalarType>(0.0)),
				NodeType(curve_, static_cast<ScalarType>(1.0))
			};

			calc_max_tol();
			subdivide();
		}

		MicropolygonMesh<VertexType> * get_mesh() const {
			MicropolygonMesh<VertexType> * mesh = new MicropolygonMesh<VertexType>(ping_.size(), 3);

			TransposeIterator_ out_(mesh->begin(), mesh->end(), ping_.size());
			TransposeIterator out(out_);

			auto right = ping_.begin(), left = right++;
			auto rhw_left = rhw_position(left->vertex_);
			auto proj_left = Vec2(rhw_left.x(), rhw_left.y()) / (rhw_left.z() * rhw_left.w());

			Vec2 ortho(0, 0);

			for (; right != ping_.end(); ++left, ++right) {
				auto rhw_right = rhw_position(right->vertex_);
				auto proj_right = Vec2(rhw_right.x(), rhw_right.y()) / (rhw_right.z() * rhw_right.w());

				ortho = rot_90(proj_right - proj_left);
				scale_length_to(ortho, half_width_);

				splay(out, left->vertex_, Vec4(ortho.x(), ortho.y(), 0.0f, 0.0f));

				rhw_left = rhw_right;
				proj_left = proj_right;
			}

			splay(out, left->vertex_, Vec4(ortho.x(), ortho.y(), 0.0f, 0.0f));

			return mesh;
		}

	private:
		NodeType split(NodeType const & left, NodeType const & right) const {
			ScalarType u = .5 * left.u_ + .5 * right.u_;
			return NodeType(curve_, u);
		}

		float tolerance_metric(NodeType const & left, NodeType const & right) const {
			return dist_sq(left.projected_position_, right.projected_position_);
		}

		void calc_max_tol() {
			max_tol_ = 0.0f;

			auto next = ping_.begin(), prev = next++;
			for (; next != ping_.end(); ++prev, ++next) {
				max_tol_ = (std::max)(max_tol_, tolerance_metric(*prev, *next));
			}
		}

		void subdivide() {
			float const tolerance = 1.0f;

			while (max_tol_ >= tolerance) {
				auto left = ping_.begin();

				pong_.push_back(*left);

				for (auto right = left + 1; right != ping_.end(); ++right) {
					pong_.push_back(split(*left, *right));
					pong_.push_back(*right);
					left = right;
				}

				ping_.swap(pong_);
				pong_.clear();
				calc_max_tol();
			}
		}

		CurveType const & curve_;
		NodeBuffer ping_, pong_;

		float max_tol_, half_width_;
};


#include <mex.h>
#include <iostream>
#include <Eigen/Dense>
#include "drakeUtil.h"
#include "RigidBodyManipulator.h"

using namespace Eigen;
using namespace std;

/*
 * A C version of the getCMM function
 */

void mexFunction( int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[] ) {

  if (nrhs < 2) {
    mexErrMsgIdAndTxt("Drake:getCMMmex:NotEnoughInputs","Usage getCMMmex(model_ptr,q_cache) or getCMMmex(model_ptr,q_cache,qd)");
  }

  // first get the model_ptr back from matlab
  RigidBodyManipulator *model= (RigidBodyManipulator*) getDrakeMexPointer(prhs[0]);

  double* q = mxGetPr(prhs[1]);
  for (int i = 0; i < model->num_dof; i++) {
    if (q[i] - model->cached_q[i] > 1e-8 || q[i] - model->cached_q[i] < -1e-8) {
      mexErrMsgIdAndTxt("Drake:getCMMmex:InvalidKinematics","This kinsol is no longer valid.  Somebody has called doKinematics with a different q since the solution was computed.");
    }
  }

  plhs[0] = mxCreateDoubleMatrix(6,model->num_dof,mxREAL);
  Map<MatrixXd> A(mxGetPr(plhs[0]),6,model->num_dof);
  plhs[1] = mxCreateDoubleMatrix(6,model->num_dof,mxREAL);
  Map<MatrixXd> Adot(mxGetPr(plhs[1]),6,model->num_dof);

  double* qd;
  if (nrhs > 2) {
    qd = mxGetPr(prhs[2]);
  }
  else {
    qd = new double[model->num_dof];
    for (int i = 0; i < model->num_dof; i++) {
      qd[i] = 0;
    }
  }

  model->getCMM(q,qd,A,Adot);
  return;
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "graphs-precomp.h"  // Precompiled headers

#include <mrpt/graphs/ScalarFactorGraph.h>
#include <mrpt/system/CTicTac.h>
#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::graphs;
using namespace std;

#if EIGEN_VERSION_AT_LEAST(3, 1, 0)  // Requires Eigen>=3.1
#include <Eigen/SparseCore>
#include <Eigen/SparseQR>
#endif

ScalarFactorGraph::FactorBase::~FactorBase() = default;
ScalarFactorGraph::ScalarFactorGraph() : COutputLogger("GMRF") {}
void ScalarFactorGraph::clear()
{
	MRPT_LOG_DEBUG("clear() called");

	m_numNodes = 0;
	m_factors_unary.clear();
	m_factors_binary.clear();
}

void ScalarFactorGraph::initialize(const size_t nodeCount)
{
	MRPT_LOG_DEBUG_STREAM("initialize() called, nodeCount=" << nodeCount);

	m_numNodes = nodeCount;
}

void ScalarFactorGraph::addConstraint(const UnaryFactorVirtualBase& c)
{
	m_factors_unary.push_back(&c);
}
void ScalarFactorGraph::addConstraint(const BinaryFactorVirtualBase& c)
{
	m_factors_binary.push_back(&c);
}

bool ScalarFactorGraph::eraseConstraint(const FactorBase& c)
{
	{
		auto it = std::find(m_factors_unary.begin(), m_factors_unary.end(), &c);
		if (it != m_factors_unary.end())
		{
			m_factors_unary.erase(it);
			return true;
		}
	}
	{
		auto it =
			std::find(m_factors_binary.begin(), m_factors_binary.end(), &c);
		if (it != m_factors_binary.end())
		{
			m_factors_binary.erase(it);
			return true;
		}
	}
	return false;
}

/* Method:
  (\Sigma)^{-1/2) *  d( h(x) )/d( x )  * x_incr = - (\Sigma)^{-1/2) * r(x)
  ===================================            ========================
			  =A                                           =b

   A * x_incr = b         --> SparseQR.
*/
void ScalarFactorGraph::updateEstimation(
	/** Output increment of the current estimate. Caller must add this
	   vector to current state vector to obtain the optimal estimation. */
	mrpt::math::CVectorDouble& solved_x_inc,
	/** If !=nullptr, the covariance of the estimate will be stored here. */
	mrpt::math::CVectorDouble* solved_variances)
{
	ASSERTMSG_(m_numNodes > 0, "numNodes=0. Have you called initialize()?");

	m_timelogger.enable(m_enable_profiler);

#if EIGEN_VERSION_AT_LEAST(3, 1, 0)

	// Number of vertices:
	const size_t n = m_numNodes;
	solved_x_inc.setZero(n, 1);

	// Number of edges:
	const size_t m1 = m_factors_unary.size(), m2 = m_factors_binary.size();
	const size_t m = m1 + m2;

	// Build Ax=b
	// -----------------------
	m_timelogger.enter("GMRF.build_A_tri");

	std::vector<Eigen::Triplet<double>> A_tri;
	A_tri.reserve(m1 + 2 * m2);

	Eigen::VectorXd g;  // Error vector

	g.setZero(m);
	int edge_counter = 0;
	for (const auto& e : m_factors_unary)
	{
		ASSERT_(e != nullptr);
		// Jacob:
		const double w = std::sqrt(e->getInformation());
		double dr_dx;
		e->evalJacobian(dr_dx);
		const int node_id = e->node_id;
		A_tri.emplace_back(edge_counter, node_id, w * dr_dx);
		// gradient:
		g[edge_counter] -= w * e->evaluateResidual();

		++edge_counter;
	}
	for (const auto& e : m_factors_binary)
	{
		ASSERT_(e != nullptr);
		// Jacob:
		const double w = std::sqrt(e->getInformation());
		double dr_dxi, dr_dxj;
		e->evalJacobian(dr_dxi, dr_dxj);
		const int node_id_i = e->node_id_i, node_id_j = e->node_id_j;
		A_tri.emplace_back(edge_counter, node_id_i, w * dr_dxi);
		A_tri.emplace_back(edge_counter, node_id_j, w * dr_dxj);
		// gradient:
		g[edge_counter] -= w * e->evaluateResidual();

		++edge_counter;
	}
	m_timelogger.leave("GMRF.build_A_tri");

	// Compress sparse
	// -----------------------
	Eigen::SparseMatrix<double> A(m, n);
	{
		mrpt::system::CTimeLoggerEntry tle(
			m_timelogger, "GMRF.build_A_compress");

		A.setFromTriplets(A_tri.begin(), A_tri.end());
		A.makeCompressed();
	}

	// Solve increment
	// -----------------------
	Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>
		solver;
	{
		mrpt::system::CTimeLoggerEntry tle(m_timelogger, "GMRF.solve");

		solver.compute(A);
		solved_x_inc = solver.solve(g);
	}

	// Recover covariance
	// -----------------------
	if (solved_variances)
	{
		mrpt::system::CTimeLoggerEntry tle(m_timelogger, "GMRF.variance");

		solved_variances->resize(n);

		// VARIANCE SIGMA = inv(P) * inv( P*H*inv(P) ) * P
		// Get triangular supperior P*H*inv(P) = UT' * UT = P * R'*R * inv(P)
		// (QR factor: Use UT=R)

		MRPT_TODO("Use compressed access instead of coeff() below");

		Eigen::SparseMatrix<double> UT = solver.matrixR();
		Eigen::SparseMatrix<double> solved_covariance(n, n);
		solved_covariance.reserve(UT.nonZeros());

		// Apply custom equations to obtain the inverse -> inv( P*H*inv(P) )
		const int show_progress_steps = std::max(int(20), int(n / 20));
		for (int l = n - 1; l >= 0; l--)
		{
			if (!(l % show_progress_steps))
				MRPT_LOG_DEBUG_FMT(
					"Computing variance %6.02f%%... \r",
					(100.0 * (n - l - 1)) / n);

			// Computes variances in the inferior submatrix of "l"
			double subSigmas = 0.0;
			for (size_t j = l + 1; j < n; j++)
			{
				if (UT.coeff(l, j) != 0)
				{
					// Compute off-diagonal variances Sigma(j,l) = Sigma(l,j);

					// SUM 1
					double sum = 0.0;
					for (size_t i = l + 1; i <= j; i++)
					{
						if (UT.coeff(l, i) != 0)
						{
							sum +=
								UT.coeff(l, i) * solved_covariance.coeff(i, j);
						}
					}
					// SUM 2
					for (size_t i = j + 1; i < n; ++i)
					{
						if (UT.coeff(l, i) != 0)
						{
							sum +=
								UT.coeff(l, i) * solved_covariance.coeff(j, i);
						}
					}
					// Save off-diagonal variance (only Upper triangular)
					solved_covariance.insert(l, j) = (-sum / UT.coeff(l, l));
					subSigmas += UT.coeff(l, j) * solved_covariance.coeff(l, j);
				}
			}

			solved_covariance.insert(l, l) =
				(1 / UT.coeff(l, l)) * (1 / UT.coeff(l, l) - subSigmas);
		}

		MRPT_LOG_DEBUG_FMT("Computing variance %6.02f%%... \r", 100.0);

		for (unsigned int i = 0; i < n; i++)
		{
			const int idx = (int)solver.colsPermutation().indices().coeff(i);
			const double variance = solved_covariance.coeff(i, i);
			(*solved_variances)[idx] = variance;
		}

	}  // end calc variances

#else
	THROW_EXCEPTION("This method requires Eigen 3.1.0 or above");
#endif
}


#include "fbstab/components/full_variable.h"

#include <Eigen/Dense>
#include <cmath>
#include <memory>

#include "tools/utilities.h"

namespace fbstab {

using VectorXd = Eigen::VectorXd;
using MatrixXd = Eigen::MatrixXd;

FullVariable::FullVariable(int nz, int nl, int nv) {
  if (nz <= 0 || nl < 0 || nv <= 0) {
    throw std::runtime_error(
        "All size inputs to FullVariable::FullVariable must be >= 1.");
  }

  nz_ = nz;
  nl_ = nl;
  nv_ = nv;

  z_storage_ = tools::make_unique<VectorXd>(nz_);
  l_storage_ = tools::make_unique<VectorXd>(nl_);
  v_storage_ = tools::make_unique<VectorXd>(nv_);
  y_storage_ = tools::make_unique<VectorXd>(nv_);

  z_ = z_storage_.get();
  l_ = l_storage_.get();
  v_ = v_storage_.get();
  y_ = y_storage_.get();

  z_->setConstant(0.0);
  l_->setConstant(0.0);
  v_->setConstant(0.0);
  y_->setConstant(0.0);
}

void FullVariable::Fill(double a) {
  z_->setConstant(a);
  l_->setConstant(a);
  v_->setConstant(a);
  InitializeConstraintMargin();
}

void FullVariable::InitializeConstraintMargin() {
  NullDataCheck();
  // y = b - A*z
  y_->setConstant(0.0);
  data_->axpyb(1.0, y_);
  data_->gemvA(*z_, -1.0, 1.0, y_);
}

void FullVariable::axpy(double a, const FullVariable& x) {
  NullDataCheck();

  z_->noalias() += a * (*x.z_);
  l_->noalias() += a * (*x.l_);
  v_->noalias() += a * (*x.v_);

  // y <- y + a*(x.y - b)
  y_->noalias() += a * (*x.y_);
  data_->axpyb(-a, y_);
}

void FullVariable::Copy(const FullVariable& x) {
  *z_ = *x.z_;
  *l_ = *x.l_;
  *v_ = *x.v_;
  *y_ = *x.y_;
  data_ = x.data_;
}

void FullVariable::ProjectDuals() { *v_ = v_->cwiseMax(0); }

double FullVariable::Norm() const {
  const double t1 = z_->norm();
  const double t2 = l_->norm();
  const double t3 = v_->norm();

  return sqrt(t1 * t1 + t2 * t2 + t3 * t3);
}

bool FullVariable::SameSize(const FullVariable& x) const {
  return (x.nz_ == nz_ && x.nl_ == nl_ && x.nv_ == nv_);
}

void FullVariable::NullDataCheck() const {
  if (data_ == nullptr) {
    throw std::runtime_error(
        "FullVariable tried to access problem data before it's "
        "linked.");
  }
}

}  // namespace fbstab


#include <gtest/gtest.h>

#include <iostream>

#include <Eigen/Core>

#include "rtff/abstract_filter.h"
#include "rtff/filter.h"
#include "wave/file.h"

const std::string gResourcePath(TEST_RESOURCES_PATH);

class MyFilter : public rtff::AbstractFilter {
private:
  void ProcessTransformedBlock(std::vector<std::complex<float>*> data,
                               uint32_t size) override {
    for (uint8_t channel_idx = 0; channel_idx < data.size(); channel_idx++) {
      auto buffer = Eigen::Map<Eigen::VectorXcf>(data[channel_idx], size);
      ASSERT_EQ(size, fft_size() / 2 + 1);
      buffer.block(20, 0, 50, 1) *= 0;
    }
  }
};

TEST(RTFF, Basis) {
  // Read input file content
  wave::File file;
  file.Open(gResourcePath + "/Untitled3.wav", wave::OpenMode::kIn);
  std::vector<float> content(file.frame_number() * file.channel_number());
  ASSERT_EQ(file.Read(&content), wave::Error::kNoError);

  // Initialize filter
  auto block_size = 2048;
  auto channel_number = file.channel_number();

  MyFilter filter;
  std::error_code err;
  filter.Init(channel_number, err);
  ASSERT_FALSE(err);
  filter.set_block_size(block_size);

  rtff::AudioBuffer buffer(block_size, channel_number);

  // For debug. From this point, the application shouldn't allocate any memory.
  Eigen::internal::set_is_malloc_allowed(false);

  // Extract each frames (add latency)
  auto multichannel_buffer_size = block_size * channel_number;

  for (uint32_t sample_idx = 0;
       sample_idx < content.size() - multichannel_buffer_size;
       sample_idx += multichannel_buffer_size) {
    // process the input buffer
    float* sample_ptr = content.data() + sample_idx;

    buffer.fromInterleaved(sample_ptr);
    filter.ProcessBlock(&buffer);
    buffer.toInterleaved(sample_ptr);

    // to write, we compensate the latency
    int output_sample_idx =
    sample_idx - (filter.FrameLatency() * channel_number);
    if (output_sample_idx < 0) {
      // begining of the file. As we create latency, the first few samples will
      // be zeros. To compensate, we just remove them
      float* output_sample_ptr = content.data();
      float* processed_sample_ptr = sample_ptr + abs(output_sample_idx);
      auto size_to_copy = block_size - filter.FrameLatency();
      memcpy(output_sample_ptr, processed_sample_ptr,
             size_to_copy * channel_number * sizeof(float));
    } else {
      // after the first few buffers, we are on general case. We just have the
      // write taking the latency into consideration
      float* output_sample_ptr = content.data() + output_sample_idx;
      memcpy(output_sample_ptr, sample_ptr,
             block_size * channel_number * sizeof(float));
    }

    // display the current status
    std::cout << round(double(sample_idx * 100) /
                       (file.frame_number() * file.channel_number()))
    << "%" << std::endl;
  }

  // For debug. From this point, the application can allocate memory
  Eigen::internal::set_is_malloc_allowed(true);

  // Write the output file content
  wave::File output;
  output.Open("/tmp/rtff_res.wav", wave::OpenMode::kOut);
  output.set_sample_rate(file.sample_rate());
  output.set_channel_number(file.channel_number());
  output.set_bits_per_sample(file.bits_per_sample());
  output.Write(content);
}

// This test makes sure that changing the block size doesn't crash the process
TEST(RTFF, ChangeBlockSize) {
  MyFilter filter;
  std::error_code err;
  auto channel_number = 1;
  filter.Init(channel_number, err);
  ASSERT_FALSE(err);

  auto block_size = 512;
  filter.set_block_size(block_size);

  auto buffer = std::make_shared<rtff::AudioBuffer>(block_size, channel_number);
  // queue 50 buffer
  for (auto index = 0; index < 50; index++) {
    memset(buffer->data(0), 0, block_size);
    filter.ProcessBlock(buffer.get());
  }
  block_size = 1024;
  filter.set_block_size(block_size);
  buffer = std::make_shared<rtff::AudioBuffer>(block_size, channel_number);
  // queue 50 other buffer
  for (auto index = 0; index < 50; index++) {
    memset(buffer->data(0), 0, block_size);
    filter.ProcessBlock(buffer.get());
  }
}

// this test makes sure that filter class syntax is functionnal
TEST(RTFF, Filter) {
  rtff::Filter filter;
  std::error_code err;
  auto channel_number = 1;
  filter.Init(channel_number, err);
  filter.execute = [](std::vector<std::complex<float>*> data, uint32_t size) {
    for (uint8_t channel_idx = 0; channel_idx < data.size(); channel_idx++) {
      auto buffer = Eigen::Map<Eigen::VectorXcf>(data[channel_idx], size);
      buffer = Eigen::VectorXcf::Random(size);
    }
  };

  ASSERT_FALSE(err);
  auto block_size = 512;
  filter.set_block_size(block_size);

  rtff::AudioBuffer buffer(block_size, channel_number);
  // queue 50 buffer
  for (auto index = 0; index < 50; index++) {
    memset(buffer.data(0), 0, block_size);
    filter.ProcessBlock(&buffer);
  }
}

uint32_t GetLatency(rtff::Filter& filter);

TEST(RTFF, Latency) {
  rtff::Filter filter;
  std::error_code err;
  filter.Init(1, err);
  ASSERT_FALSE(err);

  // case blocksize % fftsize == 0
  filter.set_block_size(512);
  ASSERT_EQ(filter.FrameLatency(), GetLatency(filter));

  // case block size > fft size
  filter.set_block_size(filter.fft_size() + 100);
  ASSERT_EQ(filter.FrameLatency(), GetLatency(filter));

  // case block_size < fft_size and blocksize % fftsize != 0
  filter.set_block_size(filter.fft_size() - 100);
  ASSERT_EQ(filter.FrameLatency(), GetLatency(filter));
}

TEST(RTFF, Latency4096) {
  rtff::Filter filter;
  std::error_code err;
  filter.Init(1, 4096, 4096 * 0.75, err);
  ASSERT_FALSE(err);

  // case blocksize % fftsize == 0
  filter.set_block_size(512);
  ASSERT_EQ(filter.FrameLatency(), GetLatency(filter));

  // case block size > fft size
  filter.set_block_size(filter.fft_size() + 100);
  ASSERT_EQ(filter.FrameLatency(), GetLatency(filter));

  // case block_size < fft_size and blocksize % fftsize != 0
  filter.set_block_size(filter.fft_size() - 100);
  ASSERT_EQ(filter.FrameLatency(), GetLatency(filter));
}

TEST(RTFF, Latency0Overlap) {
  rtff::Filter filter;
  std::error_code err;
  filter.Init(1, 4096, 0, err);
  ASSERT_FALSE(err);

  // case blocksize % fftsize == 0
  filter.set_block_size(512);
  ASSERT_EQ(filter.FrameLatency(), GetLatency(filter));

  // case blocksize % fftsize == 0
  filter.set_block_size(filter.fft_size());
  ASSERT_EQ(filter.FrameLatency(), GetLatency(filter));

  // case block size > fft size
  filter.set_block_size(filter.fft_size() + 100);
  ASSERT_EQ(filter.FrameLatency(), GetLatency(filter));

  // case block_size < fft_size and blocksize % fftsize != 0
  filter.set_block_size(filter.fft_size() - 100);
  ASSERT_EQ(filter.FrameLatency(), GetLatency(filter));
}

// This class make sure processing little block size does not crash
TEST(RTFF, LittleBlockSize) {
  rtff::Filter filter;
  std::error_code err;
  auto channel_number = 1;
  filter.Init(channel_number, 2048, 2048*0.75, err);
  ASSERT_FALSE(err);

  filter.execute = [](std::vector<std::complex<float>*> data, uint32_t size) {
    for (uint8_t channel_idx = 0; channel_idx < data.size(); channel_idx++) {
      auto buffer = Eigen::Map<Eigen::VectorXcf>(data[channel_idx], size);
      buffer = Eigen::VectorXcf::Random(size);
    }
  };

  // Testing 64 bytes block
  auto block_size = 64;
  rtff::AudioBuffer buffer(block_size, channel_number);
  filter.set_block_size(block_size);
  // queue 50 buffer
  for (auto index = 0; index < 50; index++) {
    memset(buffer.data(0), 0, block_size);
    filter.ProcessBlock(&buffer);
  }

  // Testing 43 bytes block
  block_size = 43;
  buffer = rtff::AudioBuffer(block_size, channel_number);
  filter.set_block_size(block_size);
  // queue 50 buffer
  for (auto index = 0; index < 50; index++) {
    memset(buffer.data(0), 0, block_size);
    filter.ProcessBlock(&buffer);
  }

}

// Compute the filter latency by sending a Dirac and checking the filter output
uint32_t GetLatency(rtff::Filter& filter) {
  rtff::AudioBuffer buffer(filter.block_size(), filter.channel_count());
  auto block_size = filter.block_size();

  // generate a dirac
  auto sample_rate = 44100;
  auto pre_dirac_samples = sample_rate * 1;
  std::vector<float> content(pre_dirac_samples * 2 + 1, 0);
  content[pre_dirac_samples] = 1;

  // run data into the filter
  for (uint32_t sample_idx = 0; sample_idx < content.size() - block_size;
       sample_idx += block_size) {
    float* sample_ptr = content.data() + sample_idx;
    memcpy(buffer.data(0), sample_ptr, block_size * sizeof(float));

    filter.ProcessBlock(&buffer);

    memcpy(sample_ptr, buffer.data(0), block_size * sizeof(float));
  }
  uint32_t max_index = 0;
  Eigen::Map<const Eigen::Matrix<float, Eigen::Dynamic, 1>>(content.data(),
                                                            content.size())
  .maxCoeff(&max_index);
  auto latency = max_index - pre_dirac_samples;
  return latency;
}

// Test the Hann window
TEST(RTFF, HannWindow) {
  rtff::Filter filter;
  std::error_code err;
  auto channel_number = 1;
  filter.Init(channel_number, 2048, 1024, rtff::fft_window::Type::Hann, err);
  ASSERT_FALSE(err);

  filter.execute = [](std::vector<std::complex<float>*> data, uint32_t size) {
    for (uint8_t channel_idx = 0; channel_idx < data.size(); channel_idx++) {
      auto buffer = Eigen::Map<Eigen::VectorXcf>(data[channel_idx], size);
      buffer = Eigen::VectorXcf::Random(size);
    }
  };
  rtff::AudioBuffer buffer(filter.block_size(), filter.channel_count());
  for (auto index = 0; index < 50; index++) {
    filter.ProcessBlock(&buffer);
  }
}


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


#ifndef __MANIPULATION_EKF__
#define __MANIPULATION_EKF__
#include <ros/ros.h>
#include <Eigen/Dense>
#include <math.h>
#include <pr2_algorithms/algorithm_base.hpp>
#include <limits>
#include <stdexcept>

namespace manipulation_algorithms{
/**
  Class that implements an extended kalman filter observer to estimate
  the state of the dexterous manipulation controller.
*/
class ManipulationEKF : public AlgorithmBase
{
public:
  ManipulationEKF();
  ~ManipulationEKF();

  /**
    Initialize the EKF observer.

    The observer will keep estimates for the contact point between grasped object and surface,
    the angle between the object and the surface place, and the contact force.

    @param init_x The initial estimate for the system state: x = [x_c, theta_c, f_c]
  */
  void initialize(const Eigen::VectorXd &init_x);

  /**
    Update the EKF estimate.
    Updates the EKF estimate based on the current actuation commands and measured force, torque and end-effector orientation.

    @param u The commanded system imput.
    @param y The measurements vector.
    @param x_e The current end-effector state: [x_e, y_e, theta_e].
    @param dt The time-step since the last estimate update
    @return The new estimate values
  */
  Eigen::VectorXd estimate(const Eigen::Vector3d &u, const Eigen::VectorXd &y, const Eigen::Vector3d &x_e, const double dt);

  /**
    Obtain the parameters relevant to the estimator from the parameter server.
    The method will fetch the Q and R covariance matrices from the server.

    @param n The nodehandle that will be used to query the parameter server
    @return False in case of error
  */
  bool getParams(const ros::NodeHandle &n);

private:
  Eigen::VectorXd x_hat_;
  Eigen::MatrixXd P_, Q_, R_;
  double k_s_, theta_o_, x_o_;

  void initializeMatrices(int dim, Eigen::MatrixXd &A, Eigen::MatrixXd &C, Eigen::MatrixXd &G, Eigen::MatrixXd &I, Eigen::MatrixXd &K, Eigen::MatrixXd &P);
  void computeA(Eigen::MatrixXd &A, const double y_e_dot, const double theta_e_dot, const double x_e, const double x_c, const double theta_c, const double f_c_y, const double f_c_x, const double k_s);
  void computeC(Eigen::MatrixXd &C, const double x_e, const double x_c, const double theta_c, const double f_c_y, const double f_c_x, const double k_s);
  void computeG(Eigen::MatrixXd &G, const double x_e, const double x_c, const double theta_c, const double theta_e, const double f_c_x, const double f_c_y, const double k_s);
};
}
#endif


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS

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

#include <boost/simd/arch/common/detail/generic/erf_kernel.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/six.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/expx2.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/negif.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/is_nan.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/signnz.hpp>
#endif
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( erfcx_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 x) const BOOST_NOEXCEPT
    {
      #ifndef BOOST_SIMD_NO_INVALIDS
      if(is_nan(x)) return x;
      #endif
      A0 y =  bs::abs(x);
      if (y <= Ratio<A0, 15, 32>()) // 0.46875
      {
        A0 ysq;
        A0 res = detail::erf_kernel1<A0>::erf1(x, y, ysq);
        return oneminus(res)*exp(ysq);
      }
      else if (y <= 4)
      {
        A0 res = detail::erf_kernel1<A0>::erf2(x, y);
        if (is_ltz(x))
        {
          detail::erf_kernel1<A0>::finalize3(res, x);
        }
         return res;
      }
      else
      {
        A0 res = detail::erf_kernel1<A0>::erf3(x, y);
        if (is_ltz(x))
        {
          detail::erf_kernel1<A0>::finalize3(res, x);
        }
        return res;
      }
    }
  };

  BOOST_DISPATCH_OVERLOAD ( erfcx_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      #ifndef BOOST_SIMD_NO_INFINITIES
      if(a0 == Inf<A0>()) return Zero<A0>();
      if(is_nan(a0)) return a0;
      #endif
      if (a0 < Ratio<A0, 2, 3>())
      {
        return expx2(a0)*erfc(a0);
      }
      else
      {
        A0 z =  a0/inc(a0) - Ratio<A0, 2, 5>();
        return detail::erf_kernel<A0>::erfc2(z);
      }
    }
  };
} } }

#endif


#include "ndt_feature_finder/utils.hpp"

#include <iostream>
#define BOOST_TEST_DYN_LINK

#define BOOST_TEST_MODULE MyTest
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(trying)
{

	Eigen::Vector3d ray_direction;
	Eigen::Vector3d ray_point;
	Eigen::Vector3d ray_direction_second;
	Eigen::Vector3d ray_point_second;

	ray_direction << 1, 0, 0;
	ray_direction_second << 0, 1, 0;

	std::cout << "Test 1 " << std::endl;
	ray_point << 0, 0, 0;
	ray_point_second << 0, 0, 0;
	Eigen::Vector3d out = perception_oru::ndt_feature_finder::collisionRay(ray_direction, ray_point, ray_direction_second, ray_point_second);
	Eigen::Vector3d test;
	test << 0,0,0;
	BOOST_CHECK_EQUAL(out, test);
// 	ray_point << 0, 0, 0;
	ray_point_second << -1, 0, 0;
	out = perception_oru::ndt_feature_finder::collisionRay(ray_direction, ray_point, ray_direction_second, ray_point_second);
	test << -1,0,0;
	std::cout << "Test 2 " << std::endl;
	BOOST_CHECK_EQUAL(out, test);

	std::cout << "Test 3 " << std::endl;
	ray_point_second << -1, 1, 0;
	out = perception_oru::ndt_feature_finder::collisionRay(ray_direction, ray_point, ray_direction_second, ray_point_second);
	BOOST_CHECK_EQUAL(out, test);

	std::cout << "Test 4 " << std::endl;
	ray_direction_second << 1, 1, 0;
	ray_point_second << -1, -1, 0;
	out = perception_oru::ndt_feature_finder::collisionRay(ray_direction, ray_point, ray_direction_second, ray_point_second);
	test << 0,0,0;
	BOOST_CHECK_EQUAL(out, test);

	ray_direction << 1, 1, 0;
	ray_point << -1, 1, 0;
	ray_direction_second << -1, 1, 0;
	ray_point_second << -2, -1, 0;
	out = perception_oru::ndt_feature_finder::collisionRay(ray_direction, ray_point, ray_direction_second, ray_point_second);
	test << -2, 1, 0;
	std::cout << "Test 5 " << std::endl;
	BOOST_CHECK_EQUAL(out, test);

	ray_direction << 1, 1, 0;
	ray_point << -1, 1, 0;
	ray_direction_second << 1, -1, 0;
	ray_point_second << -2, -1, 0;
	out = perception_oru::ndt_feature_finder::collisionRay(ray_direction, ray_point, ray_direction_second, ray_point_second);
	test << -2, 1, 0;

	std::cout << std::endl << ray_point - out << std::endl;
	std::cout << std::endl << ray_point_second - out << std::endl;
	std::cout << "Test 6 " << std::endl;
	BOOST_CHECK_EQUAL(out, test);

	auto mean_cell_tmp = ray_point;
	auto vector2 = ray_point - out;
	double angle_to = atan2(vector2(1), vector2(0)) - atan2(0, 1);
	std::cout << "angle to " << angle_to << std::endl;
	if (angle_to < 0) angle_to += 2 * M_PI;
// 	if(angle_to > M_PI) angle_to = (2 * M_PI) - angle_to;

	Eigen::Vector3d horizon;
	horizon << 1, 0, 0 ;
	double rotation_degree = perception_oru::ndt_feature_finder::getAngle(horizon,vector2);
	if(rotation_degree > M_PI){
		rotation_degree = (M_PI * 2) - rotation_degree;
	}
	double rotation_degree2 = perception_oru::ndt_feature_finder::getAngle(horizon,ray_direction);
	if(rotation_degree2 > M_PI){
		rotation_degree2 = (M_PI * 2) - rotation_degree2;
	}

// 	BOOST_CHECK_EQUAL(angle_to, rotation_degree);
// 	BOOST_CHECK_EQUAL(rotation_degree2, rotation_degree);

	double rotation_degree4 = perception_oru::ndt_feature_finder::getAngle(ray_direction, ray_direction_second);
	if(rotation_degree4 > M_PI){
		rotation_degree4 = (M_PI * 2) - rotation_degree4;
	}

	auto vector3 = ray_point_second - out;
	std::cout << "VEC2 !" << vector2<<std::endl;
	std::cout << "VEC3 !" << vector3<<std::endl;
	double angle_to2 = atan2(vector3(1), vector3(0)) - atan2(0, 1);
	if (angle_to2 < 0) angle_to2 += 2 * M_PI;
// 	if(angle_to2 > M_PI) angle_to2 = (2 * M_PI) - angle_to2;
	std::cout << "angle to " << angle_to2 << std::endl;

	double a_b = perception_oru::ndt_feature_finder::getAngle(vector2, vector3);
	if(a_b > M_PI){
		a_b = (M_PI * 2) - a_b;
	}
	double width = std::abs(angle_to - angle_to2);
	if(width > M_PI){
		width = (M_PI * 2) - width;
	}
	BOOST_CHECK_EQUAL(width, a_b);
	BOOST_CHECK_EQUAL(width, rotation_degree4);

	std::cout << ray_point << " - " << out << " = "  << vector2 << " \n\n"<< ray_direction << std::endl;
	int a;
	std::cin >> a;

}