/*
 * util.hh
 *
 *  Created on: May 3, 2015
 *      Author: sunayana_ghosh
 */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2015-2015 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

//In this file we implement some of the utility functions necessary for
//the classes WorkModel, ErrorEstimate and the main integrate function of the class EulerSDC.

#ifndef KASKADE_TIMESTEPPING_EULERSDC_UTIL_HH_
#define KASKADE_TIMESTEPPING_EULERSDC_UTIL_HH_

#include "norm.hh"

#include <cmath>          //std::abs
#include <functional>     //std::function
#include <vector>

#include <boost/timer/timer.hpp>

#include "dune/common/dynmatrix.hh"
#include "dune/common/dynvector.hh"

#include "fem/fixdune.hh"
#include "timestepping/sdc.hh"

namespace Kaskade {
  /**
   * \ingroup eulerSDC
   * \brief Some utility functions.
   */
//=================================================================================================
//  Utility function to find norm of a difference of vector of two Vectors
//=================================================================================================

/**
 * \ingroup eulerSDC
 * \brief Function to compute the norm of the difference of two vector of Vectors.
 *
 * @param y0  : A vector of Vectors, usually of type std::vector<Vector>
 * @param y1  : A vector of Vectors, usually of type std::vector<Vector>
 * @param norm  : A type of norm, in this case an object of type Kaskade::Norm
 * @return An object of type Vector::value_type is returned, it is usually a double.
 */
template<class Vector, class Norm>
typename Vector::value_type normVecDiff(std::vector<Vector> const& y0, std::vector<Vector> const& y1, Norm norm);

//=================================================================================================
//  Implementation for single sdc iteration step based on explicit Euler method.
//=================================================================================================
 /**
  * \ingroup eulerSDC
  * This function performs one spectral defect correction (SDC) iteration for a system of ODE's of the form:
  * \f$ y'(t) = f(y(t)) \f$, \f$ y(0) = y_0\f$ and \f$ t \in [0, T] \f$,
  * using the explicit Euler method on a given time grid. Here SDC is interpreted as fixed point iteration.
  * Given an approximate solution \f$ y^{[j]} \in \mathbb{P}_N \f$, the error function
  * \f[ \delta^{[j]} = y - y^{[j]} \f]
  * satisfies the defect equation
  * \f[ \delta^{[j]'}(t) = y'(t) - y^{[j]'}(t) = f(y(t)) - y^{[j]'}(t). \f]
  * The equivalent Picard equation is
  * \f[ \delta^{[j]}(t) = \int_{\tau=0}^t \left( f(y^{[j]}(\tau)+\delta^{[j]}(\tau)) - y^{[j]'}(\tau) \,d\tau\right)\f]
  * Evaluated at the grid nodes \f$ t_i\f$ we obtain
  * \f{eqnarray*}{
  * \delta^{[j]}_i & = & \delta^{[j]}_{i-1} + \int_{\tau=t_{i-1}}^{t_i} \left( f(y^{[j]}(\tau)+\delta^{[j]}(\tau)) - {y^{[j]}}'(\tau) \right) \,d\tau \\
  *                & = & \delta^{[j]}_{i-1} + \int_{\tau=t_{i-1}}^{t_i} \left( f(y^{[j]}(\tau)+\delta^{[j]}(\tau)) - f(y^{[j]}(\tau))\right) \,d\tau
  *               + \int_{\tau=t_{i-1}}^{t_i} \hspace{-0.5em}f(y^{[j]}(\tau)) \,d\tau - ( y^{[j]}_i-y^{[j]}_{i-1})
  * \f}
  * starting at \f$ \delta_0^{[j]} = 0. \f$ Using left looking rectangular rule for approximating the first integral, and the canonical quadrature by
  * polynomial interpolation on the nodes \f$ t_1,\ldots t_N \f$ for second integral, approximate values \f$ \hat{\delta}_i^{[j]} \f$ for
  * \f$ \delta_i^{[j]} \f$ can be evaluated. With the left looking rectangular rule, we obtain the explicit scheme
  * \f{eqnarray*}{
  *  \hat\delta^{[j]}_i & = &  \hat\delta^{[j]}_{i-1}
  * + (t_i-t_{i-1}) \left( f(y^{[j]}_{i-1}+\hat{\delta}^{[j]}_{i-1}) - f(y^{[j]}_{i-1})\right)
  * +  \sum_{k=1}^N S_{ik} f(y^{[j]}_k) - y^{[j]}_i+y^{[j]}_{i-1},
  * \f}
  * where the entries of the spectral quadrature matrix \f$ S \in \mathbb{R}^{N \times N} \f$ are defined in terms of the Lagrange polynomials
  * \f$ L_k \in \mathbb{P}_N \f$ satisfying \f$ L_k(t_i) = \delta_{ik} \f$ as
  * \f{eqnarray*}{
  *   S_{ik} & = & \int_{\tau=t_{i-1}}^{t_i} L_k(\tau) \,d\tau, \quad i,k =1,\dots, N.
  * \f}
  * An improved approximation \f$ y^{[j+1]} \f$ is then obtained by polynomial interpolation of \f$ \hat \delta^{[j]}_i \f$,
  * \f{eqnarray*}{
  * y^{[j+1]} = y^{[j]} + \hat\delta^{[j]}.
  * \f}
  *
  *\tparam Vector  a vector type, usually Dune::DenseVector.
  *
  * \param[in] grid the collocation time grid
  * \param[in] rhsFunc is the function which represents the right hand side \f$ f \f$ and is used in the computation of \f$ \delta y \f$.
  * \param[out] yi stores the current iterate of the solution \f$ y \f$.
  * \param[out] dyi is the approximate correction \f$ \delta y \f$
  * \param[in] verbose a boolean used to print out the iterates of \f$ y \f$.
  *
  */

  template<class Vector>
  void sdcExplicitEulerIterationStep(Kaskade::SDCTimeGrid const& grid,
                                     std::function<Vector(typename Vector::value_type, Vector const&)> rhsFunc,
                                     std::vector<Vector> & yi, std::vector<Vector> & dyi,
                                     bool verbose = false)
  {
    //extract time points from the grid
    auto const& pts = grid.points();
    //extracting values of the integration matrix corresponding to the time grid.
    auto const& integ = grid.integrationMatrix();
    //number of intervals
    int const n = pts.size() - 1;

    //including starting point for yi and dyi
    assert(yi.size() == n+1);
    assert(dyi.size() == n+1);

    //initialize all elements of dyi to zero.
    for (int i = 0; i <= n; ++i)
    {
      dyi[i] = Vector(0.0);
    }

    //declare Vector total to be used inside the for loop
    Vector total(0.0);

    //the SDC step for explicit Euler
    //perform n Euler steps
    boost::timer::cpu_timer timer;
    for (auto i = 1; i <= n; ++i)
    {
      //Initialize all the entries of total to zero
      total = Vector(0.0);
      //computation of the Lagrange interpolation
      for (int k = 0; k <= n; ++k)
      {
        total += integ[i-1][k] * rhsFunc(pts[k], yi[k]);
      }
      //approximate correction computation
      dyi[i] = dyi[i-1] + (pts[i] - pts[i-1]) *(rhsFunc(pts[i-1],yi[i-1]+dyi[i-1]) - rhsFunc(pts[i-1],yi[i-1])) + total - yi[i] + yi[i-1];
    }

    //Compute the next iteration of yi.
    for (auto i = 0; i <= n; ++i)
    {
      yi[i] += dyi[i];
      if (verbose)
        std::cout << "yi[" << i << "] = " << yi[i] << "\n";
    }

  }

  //=================================================================================================
   //  Implementation for single inexact sdc iteration step based on explicit Euler method.
   //=================================================================================================

   template<class Vector, class RealVector>
   void inexactSDCExplicitEulerIterationStep(Kaskade::SDCTimeGrid const& grid,
                                             std::function<Vector(typename Vector::value_type, Vector const&)> rhsFunc,
                                             RealVector const& toleranceVector, typename Vector::value_type rho,
                                             Kaskade::NormType norm_t,
                                             std::vector<Vector> & yi, std::vector<Vector> & dyi,
                                             bool verbose = false)
     {
       //extract time points from the grid
       auto const& tpts = grid.points();
       //extracting values of the integration matrix corresponding to the time grid.
       auto const& integ = grid.integrationMatrix();
       //number of intervals
       unsigned int const n = tpts.size() - 1;

       //including starting point for yi and dyi
       assert(yi.size() == n+1);
       assert(dyi.size() == n+1);

       //initialize all elements of dyi to zero.
       for (auto i = 0u; i <= n; ++i)
       {
         dyi[i] = Vector(0.0);
       }

       //declare Vector total to be used inside the for loop
       Vector total(0.0);

       //convert the toleranceVector which is a vector of doubles to a vector of Vectors.
       std::vector<Vector> errVector(n+1, Vector(0.0));
       //depending on the normType fill in err vector. This step is taken since the tolerance vector is a vector of scalars
       switch(norm_t)
       {
       case Kaskade::NormType::ONE_NORM:
       {
         auto sz = dyi[0].size();
         for (auto i = 1u; i <= n; ++i)
           errVector[i] = Vector(toleranceVector[i]/sz);
         break;
       }
       case Kaskade::NormType::MAX_NORM:
       {
         for (auto i = 1u; i <= n; ++i)
           errVector[i] = Vector(toleranceVector[i]);
         break;
        }
       }

       //the perturbed SDC step for explicit Euler
       //perform n Euler steps
       boost::timer::cpu_timer timer;
       for (auto i = 1u; i <= n; ++i)
       {
         //Initialize all the entries of total to zero
         total = Vector(0.0);

         //computation of the Lagrange interpolation
         for (auto k = 0u; k <= n; ++k)
         {
           total += integ[i-1][k] * (rhsFunc(tpts[k], yi[k]) + errVector[k]);
         }
         //approximate correction computation
         dyi[i] = dyi[i-1] + (tpts[i] - tpts[i-1]) *((rhsFunc(tpts[i-1],yi[i-1]+dyi[i-1]) - rhsFunc(tpts[i-1],yi[i-1]))- (1 - rho)*errVector[i-1]) + total - yi[i] + yi[i-1];
       }

       //Compute the next iteration of yi.
       for (auto i = 0u; i <= n; ++i)
       {
         yi[i] += dyi[i];
         if (verbose)
           std::cout << "yi[" << i << "] = " << yi[i] << "\n";
       }
     }

  //===================================================================================================
  //   Implementation of the abstract base class SDCUtil for all utility methods depending on the norms.
  //===================================================================================================

  template <class Vector, class Norm>
  class SDCUtil
  {
  public:
    using field_type = typename Vector::value_type;
    using RealVector = Dune::DynamicVector<double>;
    using RealMatrix = Dune::DynamicMatrix<double>;

    //virtual function
    /**
     * \ingroup eulerSDC
     * This function computes an estimate of the SDC contraction factor (\f$ \rho \f$) given three consecutive iterations of \f$ y \f$ and
     * depends on the given norm.
     * The estimate of \f$ \rho \f$ is given by:
     * \f{eqnarray*}{
     *  \rho & = & \frac{\|y^{[j+1]} - y^{[j]}\|}{\|y^{[j]} - y^{[j-1]}\| + \|y^{[j+1]} - y^{[j]}\|}
     * \f}
     *
     * \param[in] yPrev a Vector denoting previous iterate of \f$ y \f$.
     * \param[in] yCurrent a Vector denoting current iterate of \f$ y \f$.
     * \param[in] yNext a Vector denoting next iterate of \f$ y \f$   as obtained from SDC iteration step.
     * \param[in] norm an object of abstract base class type Norm, where we can provide the specific norm we consider for a problem.
     *
     * \return \f$ \rho \f$ an estimate of the SDC contraction factor. Return type is Vector::field_type
     */
    field_type sdcContractionFactor(std::vector<Vector> const& yPrev,
                                    std::vector<Vector> const& yCurrent,
                                    std::vector<Vector> const& yNext,
                                    Norm& norm);

    //pure virtual functions
    /**
     * \ingroup eulerSDC
     * The function definition changes with the associated norm. For derivation details for the vectors \f$ \alpha \f$
     * and \f$ \Gamma \f$ see M. Weiser, S. Ghosh: Adaptive inexact SDC Methods.
     * The vector \f$ \alpha \f$ is given by \f$  \alpha = \{\alpha_1, \ldots , \alpha_N \}\f$, where
     * \f{eqnarray*}{
     * \alpha_i & = & \sum_{k=1}^N |S_{ki}| + (1 - \delta_{i,N})\,(t_{i+1} - t_i)\,(1 + \rho),
     * \f}
     * this is computed in case of \f$ 1\f$-norm and in the case of \f$ \max\f$-norm we have
     * \f$ \Gamma = \{\Gamma_1, \ldots, \Gamma_N \} \f$, where
     *  \f{eqnarray*}{
     *    \Gamma_i & = & \max_{1\leq n \leq N} \alpha_{n,i}, \textrm{ such that } \\
     *    \alpha_{n,i} & = & |S_{ni}| + \delta_{i,n-1}\,(t_i - t_{i-1})\,(1+\rho)
     *  \f}
     * @param[in] timePoints : Time points generated using the points() method of SDCTimeGrid.
     * @param[in] integrationMatrix : Integration matrix generated using the integrationMatrix() method of SDCTimeGrid.
     *                            Computed once for a given time grid.
     * @param[in] yPrev : Estimate of exact solution \f$ y \f$ using SDC iteration step
     * @param[in] yCurrent : The following estimate of \f$ y \f$ using SDC iteration step
     * @param[in] yNext : The next estimate of \f$ y \f$ after yCurrent using SDC iteration step
     *
     * \return Reference to a Vector, where the vector is \f$ \alpha \f$ or \f$ \Gamma \f$ depending on the norm.
     */

    virtual RealVector const& computeAlphaVec(RealVector const& timePoints,
                                              RealMatrix const& integrationMatrix,
                                              std::vector<Vector> const& yPrev,
                                              std::vector<Vector> const& yCurrent,
                                              std::vector<Vector> const& yNext) = 0;

    //virtual destructor
    virtual ~SDCUtil(){}
  };

  //===================================================================================================
  //   Implementation of the derived class SDCUtilOneNorm w.r.t. 1-norm.
  //===================================================================================================
  template<class Vector, class Norm>
  class SDCUtilOneNorm : public SDCUtil<Vector, Norm>
  {
  public:
    using field_type = typename Vector::value_type;
    using RealVector = Dune::DynamicVector<double>;
    using RealMatrix = Dune::DynamicMatrix<double>;

    /**
     *
     * @param timePts : Represents a vector of time points
     * @param integrationMatrix
     * @param yPrev
     * @param yCurrent
     * @param yNext
     * @return
     */

    virtual RealVector const& computeAlphaVec(RealVector const& timePts,
                                              RealMatrix const& integrationMatrix,
                                              std::vector<Vector> const& yPrev,
                                              std::vector<Vector> const& yCurrent,
                                              std::vector<Vector> const& yNext)
    {
      //number of collocation points
      auto nCollocationPts = timePts.size();
      alphaVec = RealVector(nCollocationPts-1, 0.0);

      //create one norm object
      Kaskade::OneNorm<Vector> on;
      //compute sdc contraction rate
      auto rho = Kaskade::SDCUtil<Vector, Norm>::sdcContractionFactor(yPrev, yCurrent, yNext, on);
      for (auto i = 0u; i < nCollocationPts-1; ++i)
      {
        field_type total = 0.0;
        for (auto k = 0u; k < nCollocationPts-1; ++k)
          total += std::abs(integrationMatrix[k][i]);
        if (i != nCollocationPts-2)
          alphaVec[i] = (total + (timePts[i+1] - timePts[i]) * (1 + rho));
        else
          alphaVec[i] = total;
      }
      return alphaVec;
    }

    virtual ~SDCUtilOneNorm(){}

  private:
    RealVector alphaVec;
    RealMatrix alphaMat;
  };

  //===================================================================================================
  //   Implementation of the derived class SDCUtilMaxNorm w.r.t. max-norm.
  //===================================================================================================
  template<class Vector, class Norm>
    class SDCUtilMaxNorm : public SDCUtil<Vector, Norm>
    {
    public:
      using field_type = typename Vector::value_type;
      using RealVector = Dune::DynamicVector<double>;
      using RealMatrix = Dune::DynamicMatrix<double>;

      //TODO: Doc me
      /**
       *
       * @param timePts
       * @param integrationMatrix
       * @param yPrev
       * @param yCurrent
       * @param yNext
       * @return
       */
      virtual RealVector const& computeAlphaVec(RealVector const& timePts,
                                                RealMatrix const& integrationMatrix,
                                                std::vector<Vector> const& yPrev,
                                                std::vector<Vector> const& yCurrent,
                                                std::vector<Vector> const& yNext)
      {
        //number of collocation points.
        auto nCollocationPts = timePts.size();
        gamma = RealVector(nCollocationPts, 0.0);

        //initialize the matrix alpha with size (nCollocationPts - 1) x nCollocationPts
        auto alphaMat = computeAlphaMat(timePts, integrationMatrix, yPrev, yCurrent, yNext, nCollocationPts);
        //Compute the vector Gamma.
        for (int i = 0; i < nCollocationPts -1; ++i)
        {
          for (int j = 0; j < nCollocationPts; ++j)
          {
            if (alphaMat[i][j] > gamma[j])
              gamma[j] = alphaMat[i][j];
          }
        }
        return gamma;
       }

      virtual ~SDCUtilMaxNorm() {};
    private:
      RealVector gamma;
      RealMatrix alphaMat;

      /**
       *
       * @param timePts
       * @param integrationMatrix
       * @param yPrev
       * @param yCurrent
       * @param yNext
       * @param nCollocationPts
       * @return
       */
      RealMatrix const& computeAlphaMat(RealVector const& timePts,
                                        RealMatrix const& integrationMatrix,
                                        std::vector<Vector> const& yPrev,
                                        std::vector<Vector> const& yCurrent,
                                        std::vector<Vector> const& yNext,
                                        int nCollocationPts);

    };

  //=================================================================================================
  //   FUNCTION DEFINITION BEGINS FROM HERE
  //=================================================================================================

  //=================================================================================================
  //  Utility function to find norm of a difference of vector of two Vectors
  //=================================================================================================

  template<class Vector, class Norm>
  typename Vector::value_type normVecDiff(std::vector<Vector> const& y0, std::vector<Vector> const& y1, Norm norm)
   {
     //Initialize the all the entries of dy to 0.0
     std::vector<Vector> dy(y0.size(), Vector(0.0));
     for (auto i = 0u; i < y0.size(); ++i)
     {
       dy[i] = y1[i] - y0[i];
     }
     return norm.value(dy);
   }

  //===================================================================================================
  //   Implementation of the abstract base class SDCUtil for all utility methods depending on the norms.
  //===================================================================================================

  template <class Vector, class Norm>
  typename Vector::value_type SDCUtil<Vector, Norm>::sdcContractionFactor(std::vector<Vector> const& yPrev,
                                                                          std::vector<Vector> const& yCurrent,
                                                                          std::vector<Vector> const& yNext,
                                                                          Norm& norm)
   {
    size_t n = yPrev.size();
    //Compute the differences: yPrev - yCurrent and yCurrent - yNext
    std::vector<Vector> y01(n, Vector(0.0));
    std::vector<Vector> y12(n, Vector(0.0));
    for (auto i = 0u; i < n; ++i)
    {
      y01[i] = yCurrent[i] - yPrev[i] ;
      y12[i] = yNext[i] - yCurrent[i];
    }
    auto val01 = norm.value(y01);
    auto val12 = norm.value(y12);
    auto beta = val12/(val01 + val12);
    auto val = val12/val01;
    if (val > 1 || val != val) //check if val > 1 or val is NaN.
    {
      if (beta != beta) //check if beta = NaN
        val = 0.70;
      else
        val = beta;
    }
    return val;
   }

  //===================================================================================================
  //   Implementation of the private method computeAlphaMat for the derived class SDCUtilMaxNorm
  //===================================================================================================
  using RealVector = Dune::DynamicVector<double>;
  using RealMatrix = Dune::DynamicMatrix<double>;

  template<class Vector, class Norm>
  RealMatrix const& SDCUtilMaxNorm<Vector, Norm>::computeAlphaMat(RealVector const& timePts,
                                                                  RealMatrix const& integrationMatrix,
                                                                  std::vector<Vector> const& yPrev,
                                                                  std::vector<Vector> const& yCurrent,
                                                                  std::vector<Vector> const& yNext,
                                                                  int nCollocationPts)
    {
    //create max norm object
    Kaskade::MaxNorm<Vector> mn;
    //compute sdc contraction rate
    auto rho = Kaskade::SDCUtil<Vector, Norm>::sdcContractionFactor(yPrev, yCurrent, yNext, mn);
    alphaMat = RealMatrix(nCollocationPts-1, nCollocationPts, 0.0);
    for (auto i = 0; i < nCollocationPts-1; ++i)
    {
      for (auto k = 0; k < nCollocationPts; ++k)
      {
        if (i == k)
          alphaMat[i][k] = std::abs(integrationMatrix[i][k]) + (timePts[i+1] - timePts[i]) * (1 + rho);
        else
          alphaMat[i][k] = std::abs(integrationMatrix[i][k]);
      }
    }
    return alphaMat;
    }

}//end namespace Kaskade

#endif /* KASKADE_TIMESTEPPING_EULERSDC_UTIL_HH_ */


#include <cstdlib>
#include <cmath>
#include <iostream>
#include <random>

#include "mkl.h"

#include <boost/timer.hpp>

#include "level0.hpp"
#include "level1.hpp"
#include "solver.hpp"
#include "make_csr.hpp"
#include "matrix.hpp"

int main(int argc, char **argv) {
  auto spm = math::make_csr();
  int m;
  std::cin >> m;
  math::Vector vec(m);
  for (int i = 0; i < m; ++i) {
    std::cin >> vec[i];
  }
  std::cerr << "start" << std::endl;
  std::cerr << spm.row_size << ',' << spm.col_size << ',' << spm.elems.size() << std::endl;
  boost::timer t;
  math::Vector res = math::CGLSMethod(spm, vec);
  math::Vector diff = spm * res - vec;
  std::cerr << "time: " << t.elapsed() << std::endl;
  for (math::Real val : res) {
    std::printf("%f\n", val);
  }
  double error = math::abs(diff);
  std::cerr << "error: " << error << std::endl;
  std::cerr << "standard deviation: " << (error/std::sqrt(spm.row_size)) << std::endl;
  std::cerr << "average absolute prediction error: " << math::average(diff) << std::endl;
  return 0;
}


#ifndef MMTBX_GEOMETRY_INDEXING_H
#define MMTBX_GEOMETRY_INDEXING_H

#include <mmtbx/geometry/flattening.hpp>
#include <scitbx/math/cartesian_product_fixed_size.hpp>

#include <boost/iterator/counting_iterator.hpp>
#include <boost/range/iterator_range.hpp>

#include <boost/numeric/conversion/converter.hpp>
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>

#include <boost/fusion/container/vector.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/container/vector/vector_fwd.hpp>
#include <boost/fusion/include/vector_fwd.hpp>
#include <boost/fusion/sequence/intrinsic/at_c.hpp>
#include <boost/fusion/include/at_c.hpp>
#include <boost/fusion/algorithm/iteration/fold.hpp>

#include <boost/mpl/vector.hpp>

#include <vector>

namespace mmtbx
{

namespace geometry
{

namespace indexing
{

template< typename Object, typename Vector >
class Linear
{
public:
  typedef Object object_type;
  typedef Vector vector_type;
  typedef std::vector< object_type > storage_type;
  typedef boost::iterator_range< typename storage_type::const_iterator > range_type;

private:
  storage_type objects_;

public:
  Linear();
  ~Linear();

  inline void add(const object_type& object, const vector_type& position);
  inline range_type close_to(const vector_type& centre) const;
  inline size_t size() const;
};

template< typename Continuous, typename Discrete >
class Discretizer
{
public:
  typedef Continuous continuous_type;
  typedef Discrete discrete_type;
  typedef boost::numeric::converter<
    Discrete,
    Continuous,
    boost::numeric::conversion_traits< Discrete, Continuous >,
    boost::numeric::def_overflow_handler,
    boost::numeric::Floor< Continuous >
    > converter_type;

private:
  continuous_type base_;
  continuous_type unit_;

public:
  Discretizer(const continuous_type& base, const continuous_type& unit);
  ~Discretizer();

  continuous_type const& base() const;
  discrete_type operator ()(const continuous_type& value) const;
};

template< typename Vector, typename Voxel, typename Discrete >
class Voxelizer
{
public:
  typedef Vector vector_type;
  typedef Voxel voxel_type;
  typedef typename vector_type::value_type continuous_type;
  typedef Discrete discrete_type;
  typedef Discretizer< continuous_type, discrete_type > discretizer_type;
  typedef boost::fusion::vector3<
    discretizer_type, discretizer_type, discretizer_type
    > discretizer_vector_type;

private:
  discretizer_vector_type discretizers_;

public:
  Voxelizer(const vector_type& base, const vector_type& step);
  ~Voxelizer();

  vector_type base() const;
  voxel_type operator ()(const vector_type& vector) const;
};

struct HashCombine
{
  typedef std::size_t result_type;

  template< typename T >
  result_type operator ()(const T& arg, result_type current) const;
};

template< typename FusionVector >
struct FusionVectorHasher
{
  typedef std::size_t result_type;
  result_type operator ()(const FusionVector& myvector) const;
};

template< typename Object, typename Vector, typename Discrete >
class Hash
{
public:
  typedef Object object_type;
  typedef Vector vector_type;
  typedef Discrete discrete_type;

private:
  typedef boost::counting_iterator< discrete_type > itercount;
  typedef boost::mpl::vector< itercount, itercount, itercount > itercount_list;
  typedef scitbx::math::cartesian_product::fixed_size_iterator< itercount_list >
    cartesian_type;

public:
  typedef typename vector_type::value_type distance_type;
  typedef typename cartesian_type::value_type voxel_type;
  typedef Voxelizer< vector_type, voxel_type, discrete_type > voxelizer_type;
  typedef std::vector< object_type > bucket_type;
  typedef FusionVectorHasher< voxel_type > hasher_type;
  typedef boost::unordered_map< voxel_type, bucket_type, hasher_type > storage_type;
  typedef boost::iterator_range< typename bucket_type::const_iterator >
    bucket_range_type;
  typedef utility::flattening_range< bucket_range_type > range_type;

private:
  voxelizer_type voxelizer_;
  storage_type objects_;
  discrete_type margin_;

public:
  Hash(const voxelizer_type& voxelizer, const discrete_type& margin);
  ~Hash();

  inline void add(const object_type& object, const vector_type& position);
  inline range_type close_to(const vector_type& centre) const;
  inline range_type approx_within_sphere(
    const vector_type& centre,
    const distance_type& radius
    ) const;
  inline size_t size() const;
  inline size_t cubes() const;

private:
  cartesian_type make_cartesian_iterator_around(
    const voxel_type& voxel,
    const voxel_type& margin
    ) const;
};

#include "indexing.hxx"

} // namespace indexing
} // namespace geometry
} // namespace mmtbx

#endif // MMTBX_GEOMETRY_INDEXING_H


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SCALAR_REM_PIO2_MEDIUM_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SCALAR_REM_PIO2_MEDIUM_HPP_INCLUDED
#include <nt2/toolbox/trigonometric/functions/rem_pio2_medium.hpp>
#include <nt2/toolbox/trigonometric/constants.hpp>
#include <nt2/include/functions/scalar/round.hpp>
#include <nt2/include/functions/scalar/fast_toint.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/fusion/tuple.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_medium_, tag::cpu_,
                             (A0),
                             (scalar_ < floating_<A0> > )
                             )
  {
    typedef boost::fusion::tuple<A0,A0,nt2::int32_t>           result_type;

    inline result_type operator()(A0 const& a0) const
    {
      result_type res;
      boost::fusion::at_c<2>(res) =
      nt2::rem_pio2_medium(a0,
                           boost::fusion::at_c<0>(res),
                           boost::fusion::at_c<1>(res)
                          );
      return res;
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  // reference based Implementation when real
  /////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_medium_, tag::cpu_,
                             (A0),
                             (scalar_ < floating_<A0> > )
                             (scalar_ < floating_<A0> > )
                             (scalar_ < floating_<A0> > )
                             )
  {
    typedef typename meta::as_integer<A0>::type result_type;
    inline result_type operator()(A0 const& t, A0 & xr, A0& xc) const
    {
      const A0 fn = nt2::round(t*Invpio_2<A0>());
      A0 r  = t-fn*Pio2_1<A0>();
      A0 w  = fn*Pio2_1t<A0>();
      A0 t2 = r;
      w  = fn*Pio2_2<A0>();
      r  = t2-w;
      w  = fn*Pio2_2t<A0>()-((t2-r)-w);
      t2 = r;
      w  = fn*Pio2_3<A0>();
      r  = t2-w;
      w  = fn*Pio2_3t<A0>()-((t2-r)-w);
      xr = r-w;
      xc = (r-xr)-w;
      return  fast_toint(fn)&3;
    }
  };
} }
#endif


//headers for stl
#include <sstream>
//headers for gazebo
#include <gazebo/gazebo.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/Link.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/math/gzmath.hh>
//headers for sdf
#include <sdf/Param.hh>
#include <sdf/sdf.hh>
//headers fot boost
#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
//headers for ROS
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Twist.h>

namespace gazebo
{
  class simple_driving_force_plugin : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr sdf)
    {
      // Store the pointer to the model
      this->model = _parent;
      this->LoadParams(sdf,"target_link",this->target_link);
      this->LoadParams(sdf,"target_joint",this->target_joint);
      std::string default_joint_states_topic = "/joint_states";
      this->LoadParams(sdf,"joint_state_topic",this->joint_state_topic,default_joint_states_topic);
      this->LoadParams(sdf,"driving_force_controller",this->driving_force_controller);
      nh.getParam(this->driving_force_controller+"/propeller/k0", k0);
      nh.getParam(this->driving_force_controller+"/propeller/k1", k1);
      nh.getParam(this->driving_force_controller+"/propeller/k2", k2);
      nh.getParam(this->driving_force_controller+"/propeller/fluid_density",fluid_density);
      nh.getParam(this->driving_force_controller+"/propeller/turning_radius", turning_radius);
      //nh.getParam(this->driving_force_controller+"/propeller/twist_topic",twist_topic);
      this->joint = this->model->GetJoint(this->target_joint);
      this->link = this->model->GetLink(target_link);
      driving_force_pub = nh.advertise<std_msgs::Float32>("/"+this->target_link+"/driving_force", 1);
      //twist_sub = nh.subscribe(twist_topic, 1, &simple_driving_force_plugin::twist_callback,this);
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&simple_driving_force_plugin::OnUpdate, this, _1));
    }

    double get_thrust(double rotational_speed,double inflow_rate)
    {
      if(rotational_speed == 0)
      {
        return 0;
      }
      if(rotational_speed > 0)
      {
        double Js = inflow_rate/rotational_speed*turning_radius;
        double Kt = k2*Js*Js + k1*Js + k0;
        double thrust = fluid_density*std::pow(rotational_speed,2)*std::pow(turning_radius,4)*Kt;
        return thrust;
      }
      if(rotational_speed < 0)
      {
        double Js = inflow_rate/rotational_speed*turning_radius;
        double Kt = k2*Js*Js + k1*Js + k0;
        double thrust = -fluid_density*std::pow(rotational_speed,2)*std::pow(turning_radius,4)*Kt;
        return thrust;
      }
    }

    // Called by the world update start event
    public: void OnUpdate(const common::UpdateInfo & /*_info*/)
    {
      inflow_rate = model->GetWorldLinearVel().x;
      std_msgs::Float32 driving_force_msg;
      double velocity = this->joint->GetVelocity(0);
      double driving_force = get_thrust(velocity,this->inflow_rate);
      this->link->AddForce(math::Vector3(driving_force, 0, 0));
      driving_force_msg.data = driving_force;
      driving_force_pub.publish(driving_force_msg);
    }

    template <typename T>
    bool LoadParams(sdf::ElementPtr sdf,std::string key,T& param)
    {
      std::string param_str;
      if(!sdf->HasElement(key))
      {
        ROS_WARN_STREAM("failed to get " << key);
        return false;
      }
      else
      {
        param_str = sdf->GetElement(key)->GetValue()->GetAsString();
      }
      try
      {
        param = boost::lexical_cast<T>(param_str);
      }
      catch(boost::bad_lexical_cast &)
      {
        ROS_WARN_STREAM("failed to casting " << key);
      }
      return true;
    }

    template <typename T>
    bool LoadParams(sdf::ElementPtr sdf,std::string key,T& param, T default_param)
    {
      std::string param_str;
      if(!sdf->HasElement(key))
      {
        param = default_param;
        ROS_INFO_STREAM("unable to get " << key << " ,set default_value = " << default_param);
        return false;
      }
      else
      {
        param_str = sdf->GetElement(key)->GetValue()->GetAsString();
      }
      try
      {
        param = boost::lexical_cast<T>(param_str);
      }
      catch(boost::bad_lexical_cast &)
      {
        ROS_WARN_STREAM("failed to casting " << key);
        param = default_param;
      }
      return true;
    }
    private: double rotational_speed_effort;
    private: std::string target_link,target_joint,joint_state_topic;
    private: physics::JointPtr joint;
    private: physics::ModelPtr model;
    private: physics::LinkPtr link;

    //ros publishers
    private: ros::NodeHandle nh;
    private: ros::Publisher driving_force_pub;
    //private: ros::Subscriber twist_sub;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    //propeller params
    std::string driving_force_controller;//,twist_topic;
    double k0,k1,k2;
    double fluid_density,turning_radius,inflow_rate;
  };
  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(simple_driving_force_plugin)
}


#include <vector>
#include <algorithm>

#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "MODI.hpp"
#include "Helper.hpp"

namespace ublas = boost::numeric::ublas;
using namespace std;

namespace t_simplex
{
	/**********************
	Vogel's initialization method
	**********************/
	void _initVogel(
		vector<double> &bi,
		vector<double> dj,
		TsBasic * basicsEnd,
		TsBasic ** facBasics,
		TsBasic ** cusBasics,
		bool ** isBasic,
		int m,
		int n,
		ublas::matrix<double> &cij,
		double &_tsMaxW)
	{
		int i, j;
		TsVogPen *srcPens = NULL;
		TsVogPen *snkPens = NULL;
		TsVogPen *pitra = NULL, *pitrb = NULL;  //iterators
		TsVogPen *maxPen = NULL;
		TsVogPen srcPenHead, snkPenHead;
		bool maxIsSrc = false;
		double lowVal = 0.0;

		try
		{
			srcPens = new TsVogPen[m];
			snkPens = new TsVogPen[n];
		}
		catch (std::bad_alloc)
		{
			delete[] srcPens;
			delete[] snkPens;
			throw;
		}

		srcPenHead.next = pitra = srcPens;
		for (i = 0; i < m; i++)
		{
			pitra->i = i;
			pitra->next = pitra + 1;
			pitra->prev = pitra - 1;
			pitra->one = pitra->two = 0;
			pitra->oneCost = pitra->twoCost = TSINFINITY;
			pitra++;
		}
		(--pitra)->next = NULL;
		srcPens[0].prev = &srcPenHead;

		snkPenHead.next = pitra = snkPens;
		for (i = 0; i < n; i++)
		{
			pitra->i = i;
			pitra->next = pitra + 1;
			pitra->prev = pitra - 1;
			pitra->one = pitra->two = 0;
			pitra->oneCost = pitra->twoCost = TSINFINITY;
			pitra++;
		}
		(--pitra)->next = NULL;
		snkPens[0].prev = &snkPenHead;

		for (pitra = srcPenHead.next, i = 0; pitra != NULL; pitra = pitra->next, i++)
		{
			for (pitrb = snkPenHead.next, j = 0; pitrb != NULL; pitrb = pitrb->next, j++)
			{
				//initialize Source Penalties;
				addPenalty(pitra, cij(i, j), j);
				addPenalty(pitrb, cij(i, j), i);
			}
		}

		while (srcPenHead.next != NULL && snkPenHead.next != NULL)
		{
			maxIsSrc = true;
			for (maxPen = pitra = srcPenHead.next; pitra != NULL; pitra = pitra->next)
				if ((pitra->twoCost - pitra->oneCost) > (maxPen->twoCost - maxPen->oneCost))
					maxPen = pitra;

			for (pitra = snkPenHead.next; pitra != NULL; pitra = pitra->next)
				if ((pitra->twoCost - pitra->oneCost) > (maxPen->twoCost - maxPen->oneCost))
				{
					maxPen = pitra;
					maxIsSrc = false;
				}

			if (maxIsSrc)
			{
				i = maxPen->i;
				j = maxPen->one;
			}
			else
			{
				j = maxPen->i;
				i = maxPen->one;
			}

			if (dj[j] - bi[i] > _tsMaxW * TSEPSILON
				|| (srcPenHead.next->next != NULL
				&& fabs(dj[i] - bi[j]) < _tsMaxW * TSEPSILON))
			{
				//delete source
				lowVal = bi[i];
				maxPen = srcPens + i;
				maxPen->prev->next = maxPen->next;
				if (maxPen->next != NULL)
					maxPen->next->prev = maxPen->prev;

				for (pitra = snkPenHead.next; pitra != NULL; pitra = pitra->next)
				{
					if (pitra->one == i || pitra->two == i)
					{
						pitra->oneCost = TSINFINITY;
						pitra->twoCost = TSINFINITY;
						for (pitrb = srcPenHead.next; pitrb != NULL; pitrb = pitrb->next)
							addPenalty(pitra, cij(pitrb->i, pitra->i), pitrb->i);
					}
				}
			}
			else
			{
				//delete sink
				lowVal = dj[j];
				maxPen = snkPens + j;
				maxPen->prev->next = maxPen->next;
				if (maxPen->next != NULL)
					maxPen->next->prev = maxPen->prev;

				for (pitra = srcPenHead.next; pitra != NULL; pitra = pitra->next)
				{
					if (pitra->one == j || pitra->two == j) {
						pitra->oneCost = TSINFINITY;
						pitra->twoCost = TSINFINITY;
						for (pitrb = snkPenHead.next; pitrb != NULL; pitrb = pitrb->next)
							addPenalty(pitra, cij(pitra->i, pitrb->i), pitrb->i);
					}
				}
			}

			bi[i] -= lowVal;
			dj[j] -= lowVal;

			isBasic[i][j] = 1;
			basicsEnd->val = lowVal;
			basicsEnd->i = i;
			basicsEnd->j = j;

			basicsEnd->nextCus = cusBasics[j];
			if (cusBasics[j] != NULL) cusBasics[j]->prevCus = basicsEnd;
			basicsEnd->nextFac = facBasics[i];
			if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

			facBasics[i] = basicsEnd;
			basicsEnd->prevCus = NULL;
			cusBasics[j] = basicsEnd;
			basicsEnd->prevFac = NULL;

			basicsEnd++;
		}

		delete[] srcPens;
		srcPens = NULL;
		delete[] snkPens;
		snkPens = NULL;
	}

	/**********************
	NW-Corner initialization method
	**********************/
	void _initNW(
		double *bi,
		double *dj,
		TsBasic * basicsEnd,
		TsBasic ** facBasics,
		TsBasic ** cusBasics,
		bool ** isBasic,
		int m,
		int n)
	{
		unsigned int i = 0, j = 0;
		double lowVal = 0.0;

		while (i < m && j < n)
		{
			// More capacity than Demand
			if (bi[i] >= dj[j] && dj[j] != 0.0)
			{
				lowVal = dj[j];
				bi[i] -= dj[j];
				dj[j] = 0.0;

				isBasic[i][j] = 1;
				basicsEnd->val = lowVal;
				basicsEnd->i = i;
				basicsEnd->j = j;

				basicsEnd->nextCus = cusBasics[j];
				if (cusBasics[j] != NULL) cusBasics[j]->prevCus = basicsEnd;
				basicsEnd->nextFac = facBasics[i];
				if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

				facBasics[i] = basicsEnd;
				basicsEnd->prevCus = NULL;
				cusBasics[j] = basicsEnd;
				basicsEnd->prevFac = NULL;

				basicsEnd++;

				// Test on Degeneration
				if (bi[i] == 0.0 && dj[j] == 0.0  && i < m - 1 && j < n - 1)
				{
					isBasic[i][j + 1] = 1;
					basicsEnd->val = lowVal;
					basicsEnd->i = i;
					basicsEnd->j = j + 1;

					basicsEnd->nextCus = cusBasics[j + 1];
					if (cusBasics[j + 1] != NULL) cusBasics[j + 1]->prevCus = basicsEnd;
					basicsEnd->nextFac = facBasics[i];
					if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

					facBasics[i] = basicsEnd;
					basicsEnd->prevCus = NULL;
					cusBasics[j + 1] = basicsEnd;
					basicsEnd->prevFac = NULL;

					basicsEnd++;

					i++;
					j++;
				}
				else
				{
					// Skip to next Customer
					j++;
				}
			}
			// Less Capacity than Demand
			else if (bi[i] < dj[j] && bi[i] != 0.0)
			{
				lowVal = bi[i];
				dj[j] -= bi[i];
				bi[i] = 0.0;

				isBasic[i][j] = 1;
				basicsEnd->val = lowVal;
				basicsEnd->i = i;
				basicsEnd->j = j;

				basicsEnd->nextCus = cusBasics[j];
				if (cusBasics[j] != NULL) cusBasics[j]->prevCus = basicsEnd;
				basicsEnd->nextFac = facBasics[i];
				if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

				facBasics[i] = basicsEnd;
				basicsEnd->prevCus = NULL;
				cusBasics[j] = basicsEnd;
				basicsEnd->prevFac = NULL;

				basicsEnd++;

				// Test on Degeneration
				if (bi[i] == 0.0 && dj[j] == 0.0 && i < m - 1 && j < n - 1)
				{
					isBasic[i][j+1] = 1;
					basicsEnd->val = lowVal;
					basicsEnd->i = i;
					basicsEnd->j = j+1;

					basicsEnd->nextCus = cusBasics[j+1];
					if (cusBasics[j+1] != NULL) cusBasics[j+1]->prevCus = basicsEnd;
					basicsEnd->nextFac = facBasics[i];
					if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

					facBasics[i] = basicsEnd;
					basicsEnd->prevCus = NULL;
					cusBasics[j+1] = basicsEnd;
					basicsEnd->prevFac = NULL;

					basicsEnd++;

					i++;
					j++;
				}
				else
				{
					// Skip to next Customer
					i++;
				}

			}
		}
	}

	/**********************
	LCM initialization method
	**********************/
	void _initLCM(
		double *bi,
		double *dj,
		TsBasic *basicsEnd,
		TsBasic **facBasics,
		TsBasic **cusBasics,
		bool ** isBasic,
		int m,
		int n,
		ublas::matrix<unsigned int> &cij_column_sorted,
		int *facilities)
	{
		unsigned int i = 0, j = 0;
		unsigned int indx_i = 0;
		double lowVal = 0.0;

		while (j < cij_column_sorted.size2())
		{
			indx_i = cij_column_sorted(i, j);

			// Can shift Capacity to fullfill Customer Demand
			if (bi[indx_i] != 0.0 && dj[j] != 0.0)
			{
				// More capacity than Demand
				if (bi[i] >= dj[j] && dj[j] != 0.0)
				{
					lowVal = dj[j];
					bi[i] -= dj[j];
					dj[j] = 0.0;

					isBasic[i][j] = 1;
					basicsEnd->val = lowVal;
					basicsEnd->i = i;
					basicsEnd->j = j;

					basicsEnd->nextCus = cusBasics[j];
					if (cusBasics[j] != NULL) cusBasics[j]->prevCus = basicsEnd;
					basicsEnd->nextFac = facBasics[i];
					if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

					facBasics[i] = basicsEnd;
					basicsEnd->prevCus = NULL;
					cusBasics[j] = basicsEnd;
					basicsEnd->prevFac = NULL;

					basicsEnd++;

					// Skip to next Customer
					j++;
					i = 0;
				}
				// Less Capacity than Demand
				else if (bi[i] < dj[j] && bi[i] != 0.0)
				{
					lowVal = bi[i];
					dj[j] -= bi[i];
					bi[i] = 0.0;

					isBasic[i][j] = 1;
					basicsEnd->val = lowVal;
					basicsEnd->i = i;
					basicsEnd->j = j;

					basicsEnd->nextCus = cusBasics[j];
					if (cusBasics[j] != NULL) cusBasics[j]->prevCus = basicsEnd;
					basicsEnd->nextFac = facBasics[i];
					if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

					facBasics[i] = basicsEnd;
					basicsEnd->prevCus = NULL;
					cusBasics[j] = basicsEnd;
					basicsEnd->prevFac = NULL;

					basicsEnd++;

					// Skip to next Customer
					i++;
				}
			}
		}

		// If its in the fictional Column
		if (n > cij_column_sorted.size2() && j == cij_column_sorted.size2())
		{
			for (i = 0; i != cij_column_sorted.size1(); ++i)
			{
				// If Capacity is not empty ship to customer
				if (bi[i] != 0.0)
				{
					lowVal = bi[i];
					bi[i] = 0.0;

					isBasic[i][j] = 1;
					basicsEnd->val = lowVal;
					basicsEnd->i = i;
					basicsEnd->j = j;

					basicsEnd->nextCus = cusBasics[j];
					if (cusBasics[j] != NULL) cusBasics[j]->prevCus = basicsEnd;
					basicsEnd->nextFac = facBasics[i];
					if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

					facBasics[i] = basicsEnd;
					basicsEnd->prevCus = NULL;
					cusBasics[j] = basicsEnd;
					basicsEnd->prevFac = NULL;

					basicsEnd++;
				}
			}
		}
	}
}


#include <iostream>
#include "raisim/World.hpp"
#include <Eigen/Core>

#include <iostream>
#include <fstream>
#include <time.h>

using std::cout; using std::ofstream;
using std::endl; using std::string;
using std::fstream;
#define OUT_FILE false
#define SWEEP false

using namespace std;

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

    // Hyper Parameter defination
     float inertia = 6*6*103E-6;     // joint inertia
//     float inertia = 0.008966;     // joint inertia
//    float inertia = 0.003708;
    float kp = 80.0f;//80.0f;           // joint stiffness
    float kd = 1.0f;//1.0f;            // joint damping
    float omega = 8.0f;//10.0f;        // pTarget swing frequency
    float dt = 2e-3;            // time step of calculation
//    float total_time = 15.0f * 10.0f;    // total simulation time
    float total_time = 20.0;    // total simulation time

//    raisim::World::setActivationKey(raisim::Path("./activation.raisim").getString());
    raisim::World::setActivationKey(raisim::Path("activation.raisim").getString());
    raisim::World world;

    Eigen::VectorXd jointDgain;
    Eigen::VectorXd jointPgain;
    Eigen::VectorXd Ptarget;
    Eigen::VectorXd Dtarget;
    Eigen::VectorXd gc_;
    Eigen::VectorXd gv_;
    raisim::VecDyn RotorInertia_;
    RotorInertia_.setZero(1);
    RotorInertia_[0] = inertia;

    time_t timep;

    auto nunchucks = world.addArticulatedSystem("test.urdf");
//    auto nunchucks = world.addArticulatedSystem("BlackPanther_2d.urdf");
    nunchucks->setControlMode(raisim::ControlMode::PD_PLUS_FEEDFORWARD_TORQUE);

    // nunchucks->setMass(2, inertia);
    nunchucks->setRotorInertia(RotorInertia_);
    nunchucks->updateMassInfo();
    cout << nunchucks->getMassMatrix() <<endl;
    world.setTimeStep(dt);

    int gcDim_ = nunchucks->getGeneralizedCoordinateDim();
    int gvDim_ = nunchucks->getDOF();

    jointDgain.setZero(gvDim_);
    jointPgain.setZero(gvDim_);
    gc_.setZero(gcDim_);
    gv_.setZero(gvDim_);
    Ptarget.setZero(gcDim_);
    Dtarget.setZero(gvDim_);

    jointDgain.tail(1) << kd;
    jointPgain.tail(1) << kp;

    nunchucks->setPdGains(jointPgain, jointDgain);
    nunchucks->setPdTarget(Ptarget, Dtarget);
    nunchucks->getState(gc_, gv_);
    gv_.tail(1) << 0;
    gc_.tail(1) << 0;
    nunchucks->setState(gc_, gv_);

    float current_time = 0.0;
    float pTarget_last = 0.0f;
    float pTarget_next = 0.0f;
    float ratio = 0.0;

#if SWEEP
#if OUT_FILE
    time(&timep);
    char filename[256] = {0};
    strftime( filename, sizeof(filename), "%Y.%m.%d %H-%M-%S.xls",localtime(&timep) );
    fstream file_out;
//    file_out.open("/home/wooden/Desktop/0322-0328_research_plan/data/"+std::string(filename), std::ios_base::out);
    file_out.open("/home/wooden/Desktop/0329-0411/data/"+std::string(filename), std::ios_base::out);
    file_out << "des_q0"<< "\t" << "raw_q0" << "\t" << "raw_dq0" << endl;
    for(int i=0; i<int(total_time/dt); i++){
        // omega = int(current_time/15.0f)+1.0f;  // change omega
        omega = 8;
        Ptarget.tail(1) << sin(2*3.1415926*omega*floor(current_time/0.001)*0.001);
        nunchucks->setPdTarget(Ptarget, Dtarget);
        nunchucks->setPdGains(jointPgain, jointDgain);
        world.integrate();
        file_out << Ptarget.tail(1) << "\t" << nunchucks->getGeneralizedCoordinate() << "\t" << nunchucks->getGeneralizedVelocity() << endl;
        current_time += dt;
    }
    file_out.close();
#else

#endif

#else
#if OUT_FILE
    time(&timep);
    char filename[256] = {0};
    strftime( filename, sizeof(filename), "%Y.%m.%d %H-%M-%S.txt",localtime(&timep) );
    fstream file_out;
    file_out.open("/home/wooden/Desktop/0322-0328_research_plan/data/"+std::string(filename), std::ios_base::out);
    file_out << "inertia " << inertia << " kp " << kp << " kd " << kd << " omega " << omega << " dt " << dt << endl;
    for (int i = 0; i < int(total_time / dt); i++) {
        file_out << "t: " << current_time << " q " << nunchucks->getGeneralizedCoordinate() << " dq " << nunchucks->getGeneralizedVelocity() << endl;
        // Ptarget.tail(1) << sin(2*3.14*omega*current_time);
        Ptarget.tail(1) << sin(2*3.1415926*omega*floor(current_time/0.002)*0.002);
//        if(fmod(current_time,0.002)<1e-6){
//            pTarget_last = pTarget_next;
//            pTarget_next = sin(2*3.1415926*omega*floor(current_time/0.002)*0.002);
//        }
//        ratio = fmod(current_time,0.002)/0.002;
//        Ptarget.tail(1) <<  ratio*pTarget_next+(1.0-ratio)*pTarget_last;
        nunchucks->setPdTarget(Ptarget, Dtarget);
        nunchucks->setPdGains(jointPgain, jointDgain);
        world.integrate();
        current_time += dt;
//        cout << nunchucks->getGeneralizedCoordinate() << " " << (nunchucks->getGeneralizedForce()) << endl;
//        cout << nunchucks->getMassMatrix() <<endl;
    }
    file_out.close();

    // M(u+-u-)=dt(-kd((u++u-)/2-u_ref))
    //
#else
    for (int i = 0; i < int(total_time / dt); i++) {
//        file_out << "t: " << current_time << " q " << nunchucks->getGeneralizedCoordinate() << " dq " << nunchucks->getGeneralizedVelocity() << endl;
        // Ptarget.tail(1) << sin(2 * 3.14 * omega * current_time);
        Ptarget.tail(1) << sin(2*3.1415926*omega*floor(current_time/0.002)*0.002);
        nunchucks->setPdTarget(Ptarget, Dtarget);
        nunchucks->setPdGains(jointPgain, jointDgain);
        world.integrate();
        current_time += dt;
//        cout << "target" << Ptarget.tail(1) <<
//        " " << nunchucks->getGeneralizedCoordinate() <<
//        " " << (nunchucks->getGeneralizedVelocity()) <<
//        " "<< nunchucks->getGeneralizedForce()<<endl;
//        cout << nunchucks->getMassMatrix() <<endl;
    }
#endif
#endif

    std::cout << "Finished!" << std::endl;
    return 0;
}


#ifndef STAN_OPTIMIZATION_BFGS_LINESEARCH_HPP
#define STAN_OPTIMIZATION_BFGS_LINESEARCH_HPP

#include <boost/math/special_functions/fpclassify.hpp>
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <string>
#include <limits>

namespace stan {
  namespace optimization {
    /**
     * Find the minima in an interval [loX, hiX] of a cubic function which
     * interpolates the points, function values and gradients provided.
     *
     * Implicitly, this function constructs an interpolating polynomial
     *     g(x) = a_3 x^3 + a_2 x^2 + a_1 x + a_0
     * such that g(0) = 0, g(x1) = f1, g'(0) = df0, g'(x1) = df1 where
     *     g'(x) = 3 a_3 x^2 + 2 a_2 x + a_1
     * is the derivative of g(x).  It then computes the roots of g'(x) and
     * finds the minimal value of g(x) on the interval [loX,hiX] including
     * the end points.
     *
     * This function implements the full parameter version of CubicInterp().
     *
     * @param df0 First derivative value, f'(x0)
     * @param x1 Second point
     * @param f1 Second function value, f(x1)
     * @param df1 Second derivative value, f'(x1)
     * @param loX Lower bound on the interval of solutions
     * @param hiX Upper bound on the interval of solutions
     **/
    template<typename Scalar>
    Scalar CubicInterp(const Scalar &df0,
                       const Scalar &x1, const Scalar &f1, const Scalar &df1,
                       const Scalar &loX, const Scalar &hiX) {
      const Scalar c3((-12*f1 + 6*x1*(df0 + df1))/(x1*x1*x1));
      const Scalar c2(-(4*df0 + 2*df1)/x1 + 6*f1/(x1*x1));
      const Scalar &c1(df0);

      const Scalar t_s = std::sqrt(c2*c2 - 2.0*c1*c3);
      const Scalar s1 = - (c2 + t_s)/c3;
      const Scalar s2 = - (c2 - t_s)/c3;

      Scalar tmpF;
      Scalar minF, minX;

      // Check value at lower bound
      minF = loX*(loX*(loX*c3/3.0 + c2)/2.0 + c1);
      minX = loX;

      // Check value at upper bound
      tmpF = hiX*(hiX*(hiX*c3/3.0 + c2)/2.0 + c1);
      if (tmpF < minF) {
        minF = tmpF;
        minX = hiX;
      }

      // Check value of first root
      if (loX < s1 && s1 < hiX) {
        tmpF = s1*(s1*(s1*c3/3.0 + c2)/2.0 + c1);
        if (tmpF < minF) {
          minF = tmpF;
          minX = s1;
        }
      }

      // Check value of second root
      if (loX < s2 && s2 < hiX) {
        tmpF = s2*(s2*(s2*c3/3.0 + c2)/2.0 + c1);
        if (tmpF < minF) {
          minF = tmpF;
          minX = s2;
        }
      }

      return minX;
    }

    /**
     * Find the minima in an interval [loX, hiX] of a cubic function which
     * interpolates the points, function values and gradients provided.
     *
     * Implicitly, this function constructs an interpolating polynomial
     *     g(x) = a_3 x^3 + a_2 x^2 + a_1 x + a_0
     * such that g(x0) = f0, g(x1) = f1, g'(x0) = df0, g'(x1) = df1 where
     *     g'(x) = 3 a_3 x^2 + 2 a_2 x + a_1
     * is the derivative of g(x).  It then computes the roots of g'(x) and
     * finds the minimal value of g(x) on the interval [loX,hiX] including
     * the end points.
     *
     * @param x0 First point
     * @param f0 First function value, f(x0)
     * @param df0 First derivative value, f'(x0)
     * @param x1 Second point
     * @param f1 Second function value, f(x1)
     * @param df1 Second derivative value, f'(x1)
     * @param loX Lower bound on the interval of solutions
     * @param hiX Upper bound on the interval of solutions
     **/
    template<typename Scalar>
    Scalar CubicInterp(const Scalar &x0, const Scalar &f0, const Scalar &df0,
                       const Scalar &x1, const Scalar &f1, const Scalar &df1,
                       const Scalar &loX, const Scalar &hiX) {
      return x0 + CubicInterp(df0, x1-x0, f1-f0, df1, loX-x0, hiX-x0);
    }

    /**
     * An internal utility function for implementing WolfeLineSearch()
     **/
    template<typename FunctorType, typename Scalar, typename XType>
    int WolfLSZoom(Scalar &alpha, XType &newX, Scalar &newF, XType &newDF,
                   FunctorType &func,
                   const XType &x, const Scalar &f, const Scalar &dfp,
                   const Scalar &c1dfp, const Scalar &c2dfp, const XType &p,
                   Scalar alo, Scalar aloF, Scalar aloDFp,
                   Scalar ahi, Scalar ahiF, Scalar ahiDFp,
                   const Scalar &min_range) {
      Scalar d1, d2, newDFp;
      int itNum(0);

      while (1) {
        itNum++;

        if (std::fabs(alo-ahi) < min_range)
          return 1;

        if (itNum%5 == 0) {
          alpha = 0.5*(alo+ahi);
        } else {
          // Perform cubic interpolation to determine next point to try
          d1 = aloDFp + ahiDFp - 3*(aloF-ahiF)/(alo-ahi);
          d2 = std::sqrt(d1*d1 - aloDFp*ahiDFp);
          if (ahi < alo)
            d2 = -d2;
          alpha = ahi
            - (ahi - alo) * (ahiDFp + d2 - d1) / (ahiDFp - aloDFp + 2*d2);
          if (!boost::math::isfinite(alpha) ||
              alpha < std::min(alo, ahi) + 0.01 * std::fabs(alo - ahi) ||
              alpha > std::max(alo, ahi) - 0.01 * std::fabs(alo - ahi))
            alpha = 0.5 * (alo + ahi);
        }

        newX = x + alpha * p;
        while (func(newX, newF, newDF)) {
          alpha = 0.5 * (alpha + std::min(alo, ahi));
          if (std::fabs(std::min(alo, ahi) - alpha) < min_range)
            return 1;
          newX = x + alpha * p;
        }
        newDFp = newDF.dot(p);
        if (newF > (f + alpha * c1dfp) || newF >= aloF) {
          ahi = alpha;
          ahiF = newF;
          ahiDFp = newDFp;
        } else {
          if (std::fabs(newDFp) <= -c2dfp)
            break;
          if (newDFp*(ahi-alo) >= 0) {
            ahi = alo;
            ahiF = aloF;
            ahiDFp = aloDFp;
          }
          alo = alpha;
          aloF = newF;
          aloDFp = newDFp;
        }
      }
      return 0;
    }

    /**
     * Perform a line search which finds an approximate solution to:
     * \f[
     *       \min_\alpha f(x_0 + \alpha p)
     * \f]
     * satisfying the strong Wolfe conditions:
     *  1) \f$ f(x_0 + \alpha p) \leq f(x_0) + c_1 \alpha p^T g(x_0) \f$
     *  2) \f$ \vert p^T g(x_0 + \alpha p) \vert \leq c_2 \vert p^T g(x_0) \vert \f$
     * where \f$g(x) = \frac{\partial f}{\partial x}\f$ is the gradient of f(x).
     *
     * @tparam FunctorType A type which supports being called as
     *        ret = func(x,f,g)
     * where x is the input point, f and g are the function value and
     * gradient at x and ret is non-zero if function evaluation fails.
     *
     * @param func Function which is being minimized.
     *
     * @param alpha First value of \f$ \alpha \f$ to try.  Upon return this
     * contains the final value of the \f$ \alpha \f$.
     *
     * @param x1 Final point, equal to \f$ x_0 + \alpha p \f$.
     *
     * @param f1 Final point function value, equal to \f$ f(x_0 + \alpha p) \f$.
     *
     * @param gradx1 Final point gradient, equal to \f$ g(x_0 + \alpha p) \f$.
     *
     * @param p Search direction.  It is assumed to be a descent direction such
     * that \f$ p^T g(x_0) < 0 \f$.
     *
     * @param x0 Value of starting point, \f$ x_0 \f$.
     *
     * @param f0 Value of function at starting point, \f$ f(x_0) \f$.
     *
     * @param gradx0 Value of function gradient at starting point,
     *    \f$ g(x_0) \f$.
     *
     * @param c1 Parameter of the Wolfe conditions. \f$ 0 < c_1 < c_2 < 1 \f$
     * Typically c1 = 1e-4.
     *
     * @param c2 Parameter of the Wolfe conditions. \f$ 0 < c_1 < c_2 < 1 \f$
     * Typically c2 = 0.9.
     *
     * @param minAlpha Smallest allowable step-size.
     *
     * @param maxLSIts Maximum number line search iterations.
     *
     * @param maxLSRestarts Maximum number of times line search will
     * restart with \f$ f() \f$ failing.
     *
     * @return Returns zero on success, non-zero otherwise.
     **/
    template<typename FunctorType, typename Scalar, typename XType>
    int WolfeLineSearch(FunctorType &func,
                        Scalar &alpha,
                        XType &x1, Scalar &f1, XType &gradx1,
                        const XType &p,
                        const XType &x0, const Scalar &f0, const XType &gradx0,
                        const Scalar &c1, const Scalar &c2,
                        const Scalar &minAlpha, const Scalar &maxLSIts,
                        const Scalar &maxLSRestarts) {
      const Scalar dfp(gradx0.dot(p));
      const Scalar c1dfp(c1*dfp);
      const Scalar c2dfp(c2*dfp);

      Scalar alpha0(minAlpha);
      Scalar alpha1(alpha);

      Scalar prevF(f0);
      XType prevDF(gradx0);
      Scalar prevDFp(dfp);
      Scalar newDFp;

      int retCode = 0, nits = 0, lsRestarts = 0, ret;

      while (1) {
        if (nits >= maxLSIts) {
          retCode = 1;
          break;
        }

        x1.noalias() = x0 + alpha1 * p;
        ret = func(x1, f1, gradx1);
        if (ret != 0) {
          if (lsRestarts >= maxLSRestarts) {
            retCode = 1;
            break;
          }

          alpha1 = 0.5 * (alpha0 + alpha1);
          lsRestarts++;
          continue;
        }
        lsRestarts = 0;

        newDFp = gradx1.dot(p);
        if ((f1 > f0 + alpha * c1dfp) || (f1 >= prevF && nits > 0)) {
          retCode = WolfLSZoom(alpha, x1, f1, gradx1,
                               func,
                               x0, f0, dfp,
                               c1dfp, c2dfp, p,
                               alpha0, prevF, prevDFp,
                               alpha1, f1, newDFp,
                               1e-16);
          break;
        }
        if (std::fabs(newDFp) <= -c2dfp) {
          alpha = alpha1;
          break;
        }
        if (newDFp >= 0) {
          retCode = WolfLSZoom(alpha, x1, f1, gradx1,
                               func,
                               x0, f0, dfp,
                               c1dfp, c2dfp, p,
                               alpha1, f1, newDFp,
                               alpha0, prevF, prevDFp,
                               1e-16);
          break;
        }

        alpha0 = alpha1;
        prevF = f1;
        std::swap(prevDF, gradx1);
        prevDFp = newDFp;

        alpha1 *= 10.0;

        nits++;
      }
      return retCode;
    }
  }
}

#endif


//
// Created by yanhang on 4/21/16.
//

#include "warping.h"
#include "gridenergy.h"
#include "utility.h"
#include <Eigen/Sparse>
#include <Eigen/SPQRSupport>
#include <fstream>

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

namespace substab{

	GridWarpping::GridWarpping(const int w, const int h, const int gw, const int gh) : width(w), height(h), gridW(gw), gridH(gh) {
		blockW = (double) width / gridW;
		blockH = (double) height / gridH;
		gridLoc.resize((size_t) (gridW + 1) * (gridH + 1));
		for (auto x = 0; x <= gridW; ++x) {
			for (auto y = 0; y <= gridH; ++y) {
				gridLoc[y * (gridW + 1) + x] = Eigen::Vector2d(blockW * x, blockH * y);
				if (x == gridW)
					gridLoc[y * (gridW + 1) + x][0] -= 1.1;
				if (y == gridH)
					gridLoc[y * (gridW + 1) + x][1] -= 1.1;
			}
		}
	}

	void GridWarpping::getGridIndAndWeight(const Eigen::Vector2d &pt, Eigen::Vector4i &ind,
										   Eigen::Vector4d &w) const {
		CHECK_LE(pt[0], width - 1);
		CHECK_LE(pt[1], height - 1);
		int x = (int) floor(pt[0] / blockW);
		int y = (int) floor(pt[1] / blockH);

		//////////////
		// 1--2
		// |  |
		// 4--3
		/////////////
		ind = Vector4i(y * (gridW + 1) + x, y * (gridW + 1) + x + 1, (y + 1) * (gridW + 1) + x + 1,
					   (y + 1) * (gridW + 1) + x);

		const double &xd = pt[0];
		const double &yd = pt[1];
		const double xl = gridLoc[ind[0]][0];
		const double xh = gridLoc[ind[2]][0];
		const double yl = gridLoc[ind[0]][1];
		const double yh = gridLoc[ind[2]][1];

		w[0] = (xh - xd) * (yh - yd);
		w[1] = (xd - xl) * (yh - yd);
		w[2] = (xd - xl) * (yd - yl);
		w[3] = (xh - xd) * (yd - yl);

		double s = w[0] + w[1] + w[2] + w[3];
		CHECK_GT(s, 0) << pt[0] << ' '<< pt[1];
		w = w / s;

		Vector2d pt2 =
				gridLoc[ind[0]] * w[0] + gridLoc[ind[1]] * w[1] + gridLoc[ind[2]] * w[2] + gridLoc[ind[3]] * w[3];
		double error = (pt2 - pt).norm();
		CHECK_LT(error, 0.0001) << pt[0] << ' ' << pt[1] << ' ' << pt2[0] << ' ' << pt2[1];
	}

	void GridWarpping::visualizeGrid(const std::vector<Eigen::Vector2d>& grid, cv::Mat &img) const {
		CHECK_EQ(grid.size(), gridLoc.size());
		CHECK_EQ(img.cols, width);
		CHECK_EQ(img.rows, height);
		//img = Mat(height, width, CV_8UC3, Scalar(0,0,0));
		for(auto gy=0; gy<gridH; ++gy) {
			for (auto gx = 0; gx < gridW; ++gx){
				const int gid1 = gy * (gridW+1) + gx;
				const int gid2 = (gy+1) * (gridW+1) + gx;
				const int gid3 = (gy+1)*(gridW+1)+gx+1;
				const int gid4= gy * (gridW+1) + gx+1;
				if(grid[gid1][0] > 0 && grid[gid2][0] > 0)
					cv::line(img, cv::Point(grid[gid1][0], grid[gid1][1]), cv::Point(grid[gid2][0], grid[gid2][1]), Scalar(255,255,255));
				if(grid[gid2][0] > 0 && grid[gid3][0] > 0)
					cv::line(img, cv::Point(grid[gid2][0], grid[gid2][1]), cv::Point(grid[gid3][0], grid[gid3][1]), Scalar(255,255,255));
				if(grid[gid3][0] > 0 && grid[gid4][0] > 0)
					cv::line(img, cv::Point(grid[gid3][0], grid[gid3][1]), cv::Point(grid[gid4][0], grid[gid4][1]), Scalar(255,255,255));
				if(grid[gid4][0] > 0 && grid[gid1][0] > 0)
					cv::line(img, cv::Point(grid[gid4][0], grid[gid4][1]), cv::Point(grid[gid1][0], grid[gid1][1]), Scalar(255,255,255));
			}
		}
	}

	void GridWarpping::computeSimilarityWeight(const cv::Mat &input, std::vector<double>& saliency) const {
		saliency.resize((size_t)(gridW*gridH));
		for(auto y=0; y<gridH; ++y){
			for(auto x=0; x<gridW; ++x){
				const int gid = gridInd(x,y);
				vector<vector<double> > pixs(3);
				for(auto x1=(int)gridLoc[gridInd(x,y)][0]; x1<gridLoc[gridInd(x+1,y+1)][0]; ++x1){
					for(auto y1=(int)gridLoc[gridInd(x,y)][1]; y1<gridLoc[gridInd(x+1,y+1)][1]; ++y1){
						Vec3b pix = input.at<Vec3b>(y1,x1);
						pixs[0].push_back((double)pix[0] / 255.0);
						pixs[1].push_back((double)pix[1] / 255.0);
						pixs[2].push_back((double)pix[2] / 255.0);
					}
				}
				Vector3d vars(math_util::variance(pixs[0]),math_util::variance(pixs[1]),math_util::variance(pixs[2]));
				saliency[gid] = vars.norm();
			}
		}
	}

	void GridWarpping::warpImageCloseForm(const cv::Mat &input, cv::Mat &output, const vector<Vector2d>& pts1, const vector<Vector2d>& pts2, const int id) const {
		CHECK_EQ(pts1.size(), pts2.size());

		char buffer[1024] = {};

		vector<Vector2d> resGrid(gridLoc.size());
		const int kDataTerm = (int)pts1.size() * 2;
		const int kSimTerm = (gridW-1)*(gridH-1)*8;
		const int kVar = (int)gridLoc.size() * 2;

		vector<Eigen::Triplet<double> > triplets;
		VectorXd B(kDataTerm+kSimTerm);
		//add data constraint
		const double wdata = 1.0;
		const double wsimilarity = 20;
		int cInd = 0;
		for(auto i=0; i<pts2.size(); ++i) {
			if (pts2[i][0] < 0 || pts2[i][1] < 0 || pts2[i][0] >= width - 1 || pts2[i][1] >= height - 1)
				continue;
			Vector4i indRef;
			Vector4d bwRef;
			CHECK_LT(cInd + 1, B.rows());

			getGridIndAndWeight(pts2[i], indRef, bwRef);
			for (auto j = 0; j < 4; ++j) {
				CHECK_LT(indRef[j]*2+1, kVar);
				triplets.push_back(Triplet<double>(cInd, indRef[j] * 2, wdata * bwRef[j]));
				triplets.push_back(Triplet<double>(cInd + 1, indRef[j] * 2 + 1, wdata * bwRef[j]));
			}
			B[cInd] = wdata * pts1[i][0];
			B[cInd + 1] = wdata * pts1[i][1];
			cInd += 2;
		}

		// Mat inputOutput = input.clone();
		// visualizeGrid(gridLoc, inputOutput);
		// for(const auto& pt: pts1)
		// 	cv::circle(inputOutput, cv::Point2d(pt[0], pt[1]), 1, Scalar(0,0,255), 2);
		// sprintf(buffer, "vis_input%05d.jpg", id);
		// imwrite(buffer, inputOutput);

//		vector<double> saliency;
//		computeSimilarityWeight(input, saliency);

		auto getLocalCoord = [](const Vector2d& p1, const Vector2d& p2, const Vector2d& p3){
			Vector2d axis1 = p3 - p2;
			Vector2d axis2(-1*axis1[1], axis1[0]);
			Vector2d v = p1 - p2;
			return Vector2d(v.dot(axis1)/axis1.squaredNorm(), v.dot(axis2)/axis2.squaredNorm());
		};
		{
			//test for local coord
//			Vector2d p1(0,0), p2(0,-1), p3(1,0);
//			Vector2d uv = getLocalCoord(p1,p2,p3);
//			printf("(%.2f,%.2f)\n", uv[0], uv[1]);
		}
		for(auto y=1; y< gridH; ++y) {
			for (auto x = 1; x < gridW; ++x) {
				vector<Vector2i> gids{
						Vector2i(gridInd(x - 1, y), gridInd(x, y - 1)),
						Vector2i(gridInd(x, y - 1), gridInd(x + 1, y)),
						Vector2i(gridInd(x + 1, y), gridInd(x, y + 1)),
						Vector2i(gridInd(x, y + 1), gridInd(x - 1, y))
				};
				const int cgid = gridInd(x, y);
//				printf("-----------------------\n");
				for (const auto &gid: gids) {
					Vector2d refUV = getLocalCoord(gridLoc[cgid], gridLoc[gid[0]], gridLoc[gid[1]]);
//					printf("(%.2f,%.2f),(%.2f,%.2f),(%.2f,%.2f), u:%.2f,v:%.2f\n", gridLoc[cgid][0], gridLoc[cgid][1],
//						   gridLoc[gid[0]][0], gridLoc[gid[0]][1], gridLoc[gid[1]][0], gridLoc[gid[1]][1], refUV[0],
//						   refUV[1]);
					//x coordinate
					triplets.push_back(Triplet<double>(cInd, cgid * 2, wsimilarity));
					triplets.push_back(Triplet<double>(cInd, gid[0]*2, -1 * wsimilarity));
					triplets.push_back(Triplet<double>(cInd, gid[0] * 2, refUV[0] * wsimilarity));
					triplets.push_back(Triplet<double>(cInd, gid[1] * 2, -1 * refUV[0] * wsimilarity));

					triplets.push_back(Triplet<double>(cInd, gid[0] * 2 + 1, -1 * refUV[1] * wsimilarity));
					triplets.push_back(Triplet<double>(cInd, gid[1] * 2 + 1, refUV[1] * wsimilarity));
					B[cInd] = 0;

					//y coordinate
					triplets.push_back(Triplet<double>(cInd + 1, cgid * 2 + 1, wsimilarity));
					triplets.push_back(Triplet<double>(cInd + 1, gid[0] * 2 + 1, -1 * wsimilarity));
					triplets.push_back(Triplet<double>(cInd + 1, gid[0] * 2 + 1, refUV[0] * wsimilarity));
					triplets.push_back(Triplet<double>(cInd + 1, gid[1] * 2 + 1, -1 * refUV[0] * wsimilarity));
					triplets.push_back(Triplet<double>(cInd + 1, gid[0] * 2, refUV[1] * wsimilarity));
					triplets.push_back(Triplet<double>(cInd + 1, gid[1] * 2, -1 * refUV[1] * wsimilarity));
					B[cInd + 1] = 0;
					cInd += 2;
				}
			}
		}

//		const double wregular = 0.1;
//		for(auto x=0; x<=gridW; ++x){
//			for(auto y=0; y<=gridH; ++y){
//				int gid = gridInd(x,y);
//				triplets.push_back(Triplet<double>(cInd, gid*2, wregular));
//				triplets.push_back(Triplet<double>(cInd+1, gid*2+1, wregular));
//				B[cInd] = wregular * gridLoc[gid][0];
//				B[cInd+1] = wregular * gridLoc[gid][1];
//				cInd +=2;
//			}
//		}
		CHECK_LE(cInd, kDataTerm+kSimTerm);
		SparseMatrix<double> A(cInd, kVar);
		A.setFromTriplets(triplets.begin(), triplets.end());

		Eigen::SPQR<SparseMatrix<double> > solver(A);
		VectorXd res = solver.solve(B.block(0,0,cInd,1));
		CHECK_EQ(res.rows(), kVar);

		vector<Vector2d> vars(gridLoc.size());
		for(auto i=0; i<vars.size(); ++i){
			vars[i][0] = res[2*i];
			vars[i][1] = res[2*i+1];
		}

		output = Mat(height, width, CV_8UC3, Scalar::all(0));
		for(auto y=0; y<height; ++y){
			for(auto x=0; x<width; ++x){
				Vector4i ind;
				Vector4d w;
				getGridIndAndWeight(Vector2d(x,y), ind, w);
				Vector2d pt(0,0);
				for(auto i=0; i<4; ++i){
					pt[0] += vars[ind[i]][0] * w[i];
					pt[1] += vars[ind[i]][1] * w[i];
				}
				if(pt[0] < 0 || pt[1] < 0 || pt[0] > width - 1 || pt[1] > height - 1)
					continue;
				Vector3d pixO = interpolation_util::bilinear<uchar,3>(input.data, input.cols, input.rows, pt);
				output.at<Vec3b>(y,x) = Vec3b((uchar) pixO[0], (uchar)pixO[1], (uchar)pixO[2]);
			}
		}

		// Mat outputOutput = input.clone();
		// visualizeGrid(vars, outputOutput);
		// for(const auto& pt: pts2)
		// 	cv::circle(outputOutput, cv::Point2d(pt[0], pt[1]), 1, Scalar(0,0,255), 2);
		// sprintf(buffer, "vis_output%05d.jpg", id);
		// imwrite(buffer, outputOutput);
	}

}//namespace substablas


// Parse memory specifications like "100 MB" orr "100MiB"
// Authors: Adrien Barral, Max Schwarz

#include "bytes_parser.h"

#include <boost/spirit/include/classic.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/qi.hpp>

#include <ros/console.h>

namespace rosmon
{
namespace launch
{

std::tuple<uint64_t, bool> parseMemory(const std::string& memory)
{
	using boost::phoenix::ref;
	using boost::spirit::qi::_1;
	using boost::spirit::qi::double_;
	using boost::spirit::qi::no_case;
	using boost::spirit::qi::phrase_parse;

	struct bytes_decades_ : boost::spirit::qi::symbols<char, uint64_t>
	{
		bytes_decades_(){
			add("kB", 1e3)("mB", 1e6)("gB", 1e9)("tB", 1e12)("KB", 1e3)("MB", 1e6)("GB", 1e9)(
				"TB", 1e12)("B", 1)("KiB", static_cast<uint64_t>(1ull<<10))("MiB",
				static_cast<uint64_t>(1ull<<20))("GiB", static_cast<uint64_t>(1ull<<30))("TiB",
				static_cast<uint64_t>(1ull<<40));
		}
	} byte_decades;

	auto it = memory.begin();
	double res = 0.0;
	bool ok = phrase_parse(it, memory.end(),
		(double_[ref(res) = _1] >> -(byte_decades[ref(res) *= _1])),
		boost::spirit::ascii::space
	);

	return std::make_tuple(static_cast<uint64_t>(res), ok);
}

}
}


//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2019
//
//============================================================================

#ifndef __Thea_MatVec_hpp__
#define __Thea_MatVec_hpp__

#include "Common.hpp"
#include "MatrixFormat.hpp"

// Resolve a conflict between X.h (which #defines Success) and Eigen (which has an enum called Success). Fortunately both
// headers currently assign the same value to Success (0) so the #undef should be all that's needed.
// https://stackoverflow.com/questions/22400905/eigen-and-cimg-compatibility-issues
#ifdef Success
#  undef Success
#endif
#include <Eigen/Core>
#include <Eigen/Geometry>  // for Quaternion, and because for some weird reason Eigen defines cross() here

#include <complex>

namespace Thea {

#ifdef THEA_ROW_MAJOR
  int const DEFAULT_MATRIX_LAYOUT = MatrixLayout::ROW_MAJOR;
#else  // nothing provided or THEA_COLUMN_MAJOR defined
  int const DEFAULT_MATRIX_LAYOUT = MatrixLayout::COLUMN_MAJOR;
#endif

// Typedef common instantiations. These are fully instantiated and have no further template parameters.
// Alignment is DISABLED for fixed-size vectorizable types (Vector4f, Matrix4f, Vector2d etc). These require special handling
// for every class which directly or indirectly has such a member, and could lead to unexpected bugs and considerably less ease
// of use.
//
// See:
// - http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
// - https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html
// - http://eigen.tuxfamily.org/dox/group__TopicFixedSizeVectorizable.html
// - https://stackoverflow.com/q/41087043
//
#define THEA_DECL_MATRIX_TYPEDEFS(scalar, suffix)                                                                             \
    typedef Eigen::Matrix< scalar, 2, 2,              DEFAULT_MATRIX_LAYOUT      | Eigen::DontAlign >  Matrix2    ## suffix;  \
    typedef Eigen::Matrix< scalar, 3, 3,              DEFAULT_MATRIX_LAYOUT      | Eigen::DontAlign >  Matrix3    ## suffix;  \
    typedef Eigen::Matrix< scalar, 4, 4,              DEFAULT_MATRIX_LAYOUT      | Eigen::DontAlign >  Matrix4    ## suffix;  \
    typedef Eigen::Matrix< scalar, 2, 1,              MatrixLayout::COLUMN_MAJOR | Eigen::DontAlign >  Vector2    ## suffix;  \
    typedef Eigen::Matrix< scalar, 3, 1,              MatrixLayout::COLUMN_MAJOR | Eigen::DontAlign >  Vector3    ## suffix;  \
    typedef Eigen::Matrix< scalar, 4, 1,              MatrixLayout::COLUMN_MAJOR | Eigen::DontAlign >  Vector4    ## suffix;  \
    typedef Eigen::Matrix< scalar, 1, 2,              MatrixLayout::ROW_MAJOR    | Eigen::DontAlign >  RowVector2 ## suffix;  \
    typedef Eigen::Matrix< scalar, 1, 3,              MatrixLayout::ROW_MAJOR    | Eigen::DontAlign >  RowVector3 ## suffix;  \
    typedef Eigen::Matrix< scalar, 1, 4,              MatrixLayout::ROW_MAJOR    | Eigen::DontAlign >  RowVector4 ## suffix;  \
    typedef Eigen::Matrix< scalar, 2, Eigen::Dynamic, DEFAULT_MATRIX_LAYOUT                         >  Matrix2X   ## suffix;  \
    typedef Eigen::Matrix< scalar, 3, Eigen::Dynamic, DEFAULT_MATRIX_LAYOUT                         >  Matrix3X   ## suffix;  \
    typedef Eigen::Matrix< scalar, 4, Eigen::Dynamic, DEFAULT_MATRIX_LAYOUT                         >  Matrix4X   ## suffix;  \
    typedef Eigen::Matrix< scalar, Eigen::Dynamic, 2, DEFAULT_MATRIX_LAYOUT                         >  MatrixX2   ## suffix;  \
    typedef Eigen::Matrix< scalar, Eigen::Dynamic, 3, DEFAULT_MATRIX_LAYOUT                         >  MatrixX3   ## suffix;  \
    typedef Eigen::Matrix< scalar, Eigen::Dynamic, 4, DEFAULT_MATRIX_LAYOUT                         >  MatrixX4   ## suffix;

THEA_DECL_MATRIX_TYPEDEFS(Real, )
THEA_DECL_MATRIX_TYPEDEFS(float, f)
THEA_DECL_MATRIX_TYPEDEFS(double, d)
THEA_DECL_MATRIX_TYPEDEFS(std::complex<float>, cf)
THEA_DECL_MATRIX_TYPEDEFS(std::complex<double>, cd)
THEA_DECL_MATRIX_TYPEDEFS(int, i)

#undef THEA_DECL_MATRIX_TYPEDEFS

// Typedef additional instantiations of fully resizable matrices, NOT including the "plain X" versions which will be declared
// below with (fully optional) template arguments to avoid having to repeatedly type
// Matrix<Eigen::Dynamic, Eigen::Dynamic, T> in templated code.
#define THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS(scalar, suffix)                                                                   \
    typedef Eigen::Matrix< scalar, Eigen::Dynamic, Eigen::Dynamic, DEFAULT_MATRIX_LAYOUT      >  MatrixX    ## suffix;        \
    typedef Eigen::Matrix< scalar, Eigen::Dynamic, 1             , MatrixLayout::COLUMN_MAJOR >  VectorX    ## suffix;        \
    typedef Eigen::Matrix< scalar, 1,              Eigen::Dynamic, MatrixLayout::ROW_MAJOR    >  RowVectorX ## suffix;

THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS(float, f)
THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS(double, d)
THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS(std::complex<float>, cf)
THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS(std::complex<double>, cd)
THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS(int, i)

#undef THEA_DECL_RESIZABLE_MATRIX_TYPEDEFS

/**
 * General 2D dense matrix template, alias for Eigen::Matrix with a custom default layout (row or column major) and
 * alignment preference.
 *
 * This alias currently disables alignment for all fixed-size types: fixed-size vectorizable types (Vector4f, Matrix4f,
 * Vector2d etc) require special handling for every class which directly or indirectly has such a member, and could lead to
 * unexpected bugs and considerably less ease of use.
 *
 * @see
 *   - http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
 *   - https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html
 *   - http://eigen.tuxfamily.org/dox/group__TopicFixedSizeVectorizable.html
 *   - https://stackoverflow.com/q/41087043
 */
template <int Rows, int Cols, typename T = Real,
          int Options = DEFAULT_MATRIX_LAYOUT,
          int MaxRowsAtCompileTime = Rows,
          int MaxColsAtCompileTime = Cols>
using Matrix = Eigen::Matrix<T, Rows, Cols,
                             Options | ((Options & Eigen::DontAlign) == 0 && (Rows == Eigen::Dynamic || Cols == Eigen::Dynamic)
                                      ? Eigen::AutoAlign : Eigen::DontAlign),
                             MaxRowsAtCompileTime, MaxColsAtCompileTime>;

/**
 * General 2D dense matrix template with dynamic resizing, alias for Eigen::Matrix with Eigen::Dynamic and a custom
 * default layout (row or column major).
 */
template <typename T = Real,
          int Options = DEFAULT_MATRIX_LAYOUT,
          int MaxRowsAtCompileTime = Eigen::Dynamic,
          int MaxColsAtCompileTime = Eigen::Dynamic>
using MatrixX = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime>;

/**
 * General 1D dense column vector template, alias for Eigen::Matrix<T, Size, 1,...>, with a custom alignment
 * preference.
 *
 * This alias currently disables alignment for all fixed-size types: fixed-size vectorizable types (Vector4f, Vector2d
 * etc) require special handling for every class which directly or indirectly has such a member, and could lead to unexpected
 * bugs and considerably less ease of use.
 *
 * @see
 *   - http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
 *   - https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html
 *   - http://eigen.tuxfamily.org/dox/group__TopicFixedSizeVectorizable.html
 *   - https://stackoverflow.com/q/41087043
 */
template <int Size, typename T = Real,
          int Options = MatrixLayout::COLUMN_MAJOR,
          int MaxRowsAtCompileTime = Size>
using Vector = Eigen::Matrix<T, Size, 1,
                             Options | ((Options & Eigen::DontAlign) == 0 && Size == Eigen::Dynamic
                                      ? Eigen::AutoAlign : Eigen::DontAlign),
                             MaxRowsAtCompileTime, 1>;

/**
 * General 1D dense column vector template with dynamic resizing, alias for Eigen::Matrix<T, Eigen::Dynamic, 1,...>
 * with a custom default layout (row or column major).
 */
template <typename T = Real,
          int Options = MatrixLayout::COLUMN_MAJOR,
          int MaxRowsAtCompileTime = Eigen::Dynamic>
using VectorX = Eigen::Matrix<T, Eigen::Dynamic, 1, Options, MaxRowsAtCompileTime, 1>;

/**
 * General 1D dense row vector template, alias for Eigen::Matrix<T, 1, Size,...>, with a custom alignment
 * preference.
 *
 * This alias currently disables alignment for all fixed-size types: fixed-size vectorizable types (RowVector4f,
 * RowVector2d etc) require special handling for every class which directly or indirectly has such a member, and could lead to
 * unexpected bugs and considerably less ease of use.
 *
 * @see
 *   - http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
 *   - https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html
 *   - http://eigen.tuxfamily.org/dox/group__TopicFixedSizeVectorizable.html
 *   - https://stackoverflow.com/q/41087043
 */
template <int Size, typename T = Real,
          int Options = MatrixLayout::ROW_MAJOR,
          int MaxColsAtCompileTime = Size>
using RowVector = Eigen::Matrix<T, 1, Size,
                                Options | ((Options & Eigen::DontAlign) == 0 && Size == Eigen::Dynamic
                                         ? Eigen::AutoAlign : Eigen::DontAlign),
                                1, MaxColsAtCompileTime>;

/**
 * General 1D dense row vector template with dynamic resizing, alias for Eigen::Matrix<T, 1, Eigen::Dynamic,...>
 * with a custom default layout (row or column major).
 */
template <typename T = Real,
          int Options = MatrixLayout::ROW_MAJOR,
          int MaxColsAtCompileTime = Eigen::Dynamic>
using RowVectorX = Eigen::Matrix<T, 1, Eigen::Dynamic, Options, 1, MaxColsAtCompileTime>;

//=============================================================================================================================
// Typedef Eigen::Map wrappers for interpreting raw data as common Eigen types.
//=============================================================================================================================

#define THEA_DECL_MATRIX_MAP_TYPEDEFS(suffix)                                 \
    typedef Eigen::Map< Matrix2    ## suffix >  Matrix2    ## suffix ## Map;  \
    typedef Eigen::Map< Matrix3    ## suffix >  Matrix3    ## suffix ## Map;  \
    typedef Eigen::Map< Matrix4    ## suffix >  Matrix4    ## suffix ## Map;  \
    typedef Eigen::Map< Vector2    ## suffix >  Vector2    ## suffix ## Map;  \
    typedef Eigen::Map< Vector3    ## suffix >  Vector3    ## suffix ## Map;  \
    typedef Eigen::Map< Vector4    ## suffix >  Vector4    ## suffix ## Map;  \
    typedef Eigen::Map< RowVector2 ## suffix >  RowVector2 ## suffix ## Map;  \
    typedef Eigen::Map< RowVector3 ## suffix >  RowVector3 ## suffix ## Map;  \
    typedef Eigen::Map< RowVector4 ## suffix >  RowVector4 ## suffix ## Map;  \
    typedef Eigen::Map< Matrix2X   ## suffix >  Matrix2X   ## suffix ## Map;  \
    typedef Eigen::Map< Matrix3X   ## suffix >  Matrix3X   ## suffix ## Map;  \
    typedef Eigen::Map< Matrix4X   ## suffix >  Matrix4X   ## suffix ## Map;  \
    typedef Eigen::Map< MatrixX2   ## suffix >  MatrixX2   ## suffix ## Map;  \
    typedef Eigen::Map< MatrixX3   ## suffix >  MatrixX3   ## suffix ## Map;  \
    typedef Eigen::Map< MatrixX4   ## suffix >  MatrixX4   ## suffix ## Map;  \
    \
    typedef Eigen::Map< Matrix2    ## suffix const >  Matrix2    ## suffix ## ConstMap;  \
    typedef Eigen::Map< Matrix3    ## suffix const >  Matrix3    ## suffix ## ConstMap;  \
    typedef Eigen::Map< Matrix4    ## suffix const >  Matrix4    ## suffix ## ConstMap;  \
    typedef Eigen::Map< Vector2    ## suffix const >  Vector2    ## suffix ## ConstMap;  \
    typedef Eigen::Map< Vector3    ## suffix const >  Vector3    ## suffix ## ConstMap;  \
    typedef Eigen::Map< Vector4    ## suffix const >  Vector4    ## suffix ## ConstMap;  \
    typedef Eigen::Map< RowVector2 ## suffix const >  RowVector2 ## suffix ## ConstMap;  \
    typedef Eigen::Map< RowVector3 ## suffix const >  RowVector3 ## suffix ## ConstMap;  \
    typedef Eigen::Map< RowVector4 ## suffix const >  RowVector4 ## suffix ## ConstMap;  \
    typedef Eigen::Map< Matrix2X   ## suffix const >  Matrix2X   ## suffix ## ConstMap;  \
    typedef Eigen::Map< Matrix3X   ## suffix const >  Matrix3X   ## suffix ## ConstMap;  \
    typedef Eigen::Map< Matrix4X   ## suffix const >  Matrix4X   ## suffix ## ConstMap;  \
    typedef Eigen::Map< MatrixX2   ## suffix const >  MatrixX2   ## suffix ## ConstMap;  \
    typedef Eigen::Map< MatrixX3   ## suffix const >  MatrixX3   ## suffix ## ConstMap;  \
    typedef Eigen::Map< MatrixX4   ## suffix const >  MatrixX4   ## suffix ## ConstMap;

THEA_DECL_MATRIX_MAP_TYPEDEFS()
THEA_DECL_MATRIX_MAP_TYPEDEFS(f)
THEA_DECL_MATRIX_MAP_TYPEDEFS(d)
THEA_DECL_MATRIX_MAP_TYPEDEFS(cf)
THEA_DECL_MATRIX_MAP_TYPEDEFS(cd)
THEA_DECL_MATRIX_MAP_TYPEDEFS(i)

#undef THEA_DECL_MATRIX_MAP_TYPEDEFS

#define THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS(suffix)                       \
    typedef Eigen::Map< MatrixX    ## suffix >  MatrixX    ## suffix ## Map;  \
    typedef Eigen::Map< VectorX    ## suffix >  VectorX    ## suffix ## Map;  \
    typedef Eigen::Map< RowVectorX ## suffix >  RowVectorX ## suffix ## Map;  \
    \
    typedef Eigen::Map< MatrixX    ## suffix const >  MatrixX    ## suffix ## ConstMap;  \
    typedef Eigen::Map< VectorX    ## suffix const >  VectorX    ## suffix ## ConstMap;  \
    typedef Eigen::Map< RowVectorX ## suffix const >  RowVectorX ## suffix ## ConstMap;

THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS(f)
THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS(d)
THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS(cf)
THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS(cd)
THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS(i)

#undef THEA_DECL_RESIZABLE_MATRIX_MAP_TYPEDEFS

/** Alias for Eigen::Map< Matrix<...> >. */
template < int Rows, int Cols, typename T = Real,
           int Options = DEFAULT_MATRIX_LAYOUT,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int MaxRowsAtCompileTime = Rows,
           int MaxColsAtCompileTime = Cols >
using MatrixMap = Eigen::Map< Matrix<Rows, Cols, T, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime>,
                              MapOptions, StrideType >;

/** Alias for Eigen::Map< Matrix<...> const >. */
template < int Rows, int Cols, typename T = Real,
           int Options = DEFAULT_MATRIX_LAYOUT,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int MaxRowsAtCompileTime = Rows,
           int MaxColsAtCompileTime = Cols >
using MatrixConstMap = Eigen::Map< Matrix<Rows, Cols, T, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime> const,
                                   MapOptions, StrideType >;

/** Alias for Eigen::Map< Vector<...> >. */
template < int Size, typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::COLUMN_MAJOR,
           int MaxRowsAtCompileTime = Size >
using VectorMap = Eigen::Map< Vector<Size, T, Options, MaxRowsAtCompileTime>, MapOptions, StrideType >;

/** Alias for Eigen::Map< Vector<...> const >. */
template < int Size, typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::COLUMN_MAJOR,
           int MaxRowsAtCompileTime = Size >
using VectorConstMap = Eigen::Map< Vector<Size, T, Options, MaxRowsAtCompileTime> const, MapOptions, StrideType >;

/** Alias for Eigen::Map< RowVector<...> >. */
template < int Size, typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::ROW_MAJOR,
           int MaxColsAtCompileTime = Size >
using RowVectorMap = Eigen::Map< RowVector<Size, T, Options, MaxColsAtCompileTime>, MapOptions, StrideType >;

/** Alias for Eigen::Map< RowVector<...> const >. */
template < int Size, typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::ROW_MAJOR,
           int MaxColsAtCompileTime = Size >
using RowVectorConstMap = Eigen::Map< RowVector<Size, T, Options, MaxColsAtCompileTime> const, MapOptions, StrideType >;

/** Alias for Eigen::Map< MatrixX<...> >. */
template < typename T = Real,
           int Options = DEFAULT_MATRIX_LAYOUT,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int MaxRowsAtCompileTime = Eigen::Dynamic,
           int MaxColsAtCompileTime = Eigen::Dynamic >
using MatrixXMap = Eigen::Map< MatrixX<T, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime>, MapOptions, StrideType >;

/** Alias for Eigen::Map< MatrixX<...> const >. */
template < typename T = Real,
           int Options = DEFAULT_MATRIX_LAYOUT,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int MaxRowsAtCompileTime = Eigen::Dynamic,
           int MaxColsAtCompileTime = Eigen::Dynamic >
using MatrixXConstMap = Eigen::Map< MatrixX<T, Options, MaxRowsAtCompileTime, MaxColsAtCompileTime> const,
                                    MapOptions, StrideType >;

/** Alias for Eigen::Map< VectorX<...> >. */
template < typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::COLUMN_MAJOR,
           int MaxRowsAtCompileTime = Eigen::Dynamic >
using VectorXMap = Eigen::Map< VectorX<T, Options, MaxRowsAtCompileTime>, MapOptions, StrideType >;

/** Alias for Eigen::Map< VectorX<...> const >. */
template < typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::COLUMN_MAJOR,
           int MaxRowsAtCompileTime = Eigen::Dynamic >
using VectorXConstMap = Eigen::Map< VectorX<T, Options, MaxRowsAtCompileTime> const, MapOptions, StrideType >;

/** Alias for Eigen::Map< RowVectorX<...> >. */
template < typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::ROW_MAJOR,
           int MaxColsAtCompileTime = Eigen::Dynamic >
using RowVectorXMap = Eigen::Map< RowVectorX<T, Options, MaxColsAtCompileTime>, MapOptions, StrideType >;

/** Alias for Eigen::Map< RowVectorX<...> const >. */
template < typename T = Real,
           typename StrideType = Eigen::Stride<0, 0>,
           int MapOptions = Eigen::Unaligned,
           int Options = MatrixLayout::ROW_MAJOR,
           int MaxColsAtCompileTime = Eigen::Dynamic >
using RowVectorXConstMap = Eigen::Map< RowVectorX<T, Options, MaxColsAtCompileTime> const, MapOptions, StrideType >;

//=============================================================================================================================
// Typedef other useful Eigen classes.
//=============================================================================================================================

/** Alias for Eigen::Quaternion<...>. */
template <typename T = Real> using Quaternion = Eigen::Quaternion<T>;

} // namespace Thea

#include "MatrixUtil.hpp"

#endif


#include <assert.h>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <iostream>
#include <vector>

struct Point
{
    constexpr Point() = default;
    constexpr Point(long _id, std::vector<long>&& l)
        : id(_id)
    {
        assert(l.size() == 9);
        auto it = l.begin();
        x = *it++;
        y = *it++;
        z = *it++;
        v_x = *it++;
        v_y = *it++;
        v_z = *it++;
        a_x = *it++;
        a_y = *it++;
        a_z = *it++;
    }

    long id = 0;
    long x = 0, y = 0, z = 0;
    long v_x = 0, v_y = 0, v_z = 0;
    long a_x = 0, a_y = 0, a_z = 0;
    long len = 0;
    long acc = 0;

    void step() noexcept
    {
        v_x += a_x;
        v_y += a_y;
        v_z += a_z;
        x += v_x;
        y += v_y;
        z += v_z;
        len = std::abs(x) + std::abs(y) + std::abs(z);
        acc = std::abs(a_x) + std::abs(a_y) + std::abs(a_z);
    }

    bool operator<(const Point& b) const noexcept {
        return std::tie(len, acc) < std::tie(b.len, b.acc);
    }
};

int main()
{
    // p=<-4897,3080,2133>, v=<-58,-15,-78>, a=<17,-7,0>
    // -4897,3080,2133,-58,-15,-78,17,-7,0
    std::vector<Point> points;
    std::string line;
    long id = 0;
    while (std::getline(std::cin, line)) {
        std::vector<std::string> words;
        boost::algorithm::split(
            words, line, boost::is_any_of(", "), boost::algorithm::token_compress_on);
        std::vector<long> nums;
        for (const auto& word : words) {
            nums.push_back(atol(&word[0]));
        }
        points.emplace_back(Point(id++, std::move(nums)));
    }

    for (int i = 0; i < 500; ++i) {
        std::sort(points.begin(), points.end());
        for (int j =0; j < 10; ++j) {
            std::cout << points[j].id << ", ";
        }
        std::cout << "\n";
        for (auto& p : points) {
            p.step();
        }
    }

    return 0;
}