#include <iostream>
#include <vector>

#include <algorithm>
#include <unordered_map>
#include <set>
#include <utility>
#include <stdexcept>
#include <cmath>
#include <chrono>
#include <exception>
#include <boost/filesystem.hpp>

#include "dynet/lstm.h"
#include "dynet/training.h"
#include "dynet/dynet.h"
#include "dynet/dict.h"

#include <boost/functional/hash.hpp>
#include <boost/program_options.hpp>
#include <boost/serialization/unordered_set.hpp>

#include "lemon_inc.h"
#include "serialization.h"
#include "nn/tagger.h"
#include "nn/biaffine_parser.h"
#include "utils.h"

#include "dependency.h"
#include "conll.h"
#include "activation_function.h"
#include "probs.h"

int main(int argc, char **argv)
{
    std::string test_path;
    std::string model_path;
    std::string output_path;

    bool attachment_score;
    bool full;

    namespace po = boost::program_options;
    po::options_description desc("Options");
    desc.add_options()
        ("test", po::value<std::string>(&test_path)->required(), "")
        ("model", po::value<std::string>(&model_path)->required(), "")
        ("output", po::value<std::string>(&output_path)->required(), "")
        ("attachment-score", po::value<bool>(&attachment_score)->default_value(true))
        ("full", po::value<bool>(&full)->default_value(false))
    ;

    po::positional_options_description pod;
    pod.add("test", 1);
    pod.add("model", 1);
    pod.add("output", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(pod).run(), vm);
    po::notify(vm);

    dynet::initialize(argc, argv);

    ConllSettings conll_settings;
    read_object(model_path + ".conll_settings.param", conll_settings);

    std::cerr << "Reading test data..." << std::endl << std::flush;
    Conll conll_test(conll_settings);
    std::vector<IntSentence> test_data;
    conll_test.read(test_path);
    conll_test.as_int_sentence([&](const IntSentence& s) { test_data.push_back(s); });

    // Compute and update POS
    {
        // Neural Network
        dynet::Model model;

        RNNSettings node_rnn_settings;
        read_object(model_path + ".tagger.rnn_settings", node_rnn_settings);

        NeuralTaggerSettings nn_settings;
        read_object(model_path + ".tagger.nn_settings", nn_settings);

        NeuralTagger<dynet::LSTMBuilder> rnn(model, nn_settings, node_rnn_settings);

        read_object(model_path + ".tagger.param", model);

        for (IntSentence& sentence : test_data)
        {
            dynet::ComputationGraph cg;
            rnn.compute(
                cg,
                sentence,
                [&] (unsigned index, dynet::expr::Expression& expr) -> void
                {
                    auto vec = dynet::as_vector(expr.value());
                    int predicted = std::distance(std::begin(vec), std::max_element(std::begin(vec), std::end(vec)));
                    sentence[index+1].pos = predicted;
                }
            );
        }
    }

    // Compute and update dependencies
    {
        dynet::Model model;

        RNNSettings node_rnn_settings;
        read_object(model_path + ".parser.rnn_settings", node_rnn_settings);

        Probs attachment_probs;
        read_object(model_path + ".attachment-probs", attachment_probs);

        NeuralBiaffineParserSettings nn_settings;
        read_object(model_path + ".parser.nn_settings", nn_settings);

        NeuralBiaffineParser<dynet::LSTMBuilder> rnn(model, nn_settings, node_rnn_settings);
        read_object(model_path + ".parser.param", model);

        for (IntSentence& sentence : test_data)
        {
            dynet::ComputationGraph cg;

            LDigraph lemon_graph;
            LArcMap lemon_weights(lemon_graph);

            for (unsigned i = 0 ; i <= sentence.size() ; ++i)
            {
                LNode node = lemon_graph.addNode();
                assert(lemon_graph.id(node) == (int) i);
            }

            rnn.compute(
                cg,
                sentence,
                [&] (const unsigned head, const unsigned modifier, double score, dynet::expr::Expression& expr) -> void
                {
                    unused_parameter(expr);

                    int mod_pos = sentence[modifier].pos;

                    if (head == 0u)
                    {
                        auto f = attachment_probs.head.find(mod_pos);

                        if (!full)
                        {
                            // skip if not candidate for dependency
                            if (f == std::end(attachment_probs.head))
                                return;

                            if (attachment_score)
                                score += log(f->second);
                        }
                    }
                    else
                    {
                        int head_pos = sentence[head].pos;
                        auto f = attachment_probs.pos.find(std::make_pair(head_pos, mod_pos));

                        if (!full)
                        {
                            // skip if not candidate for dependency
                            if (f == std::end(attachment_probs.pos))
                                return;

                            if (attachment_score)
                                score += log(f->second);
                        }
                    }
                    LArc lemon_arc = lemon_graph.addArc(
                        lemon_graph.nodeFromId(head),
                        lemon_graph.nodeFromId(modifier)
                    );
                    lemon_weights[lemon_arc] = -score;
                }
            );

            MSA msa(lemon_graph, lemon_weights);
            msa.run(lemon_graph.nodeFromId(0));

            for (unsigned modifier = 1 ; modifier <= sentence.size() ; ++modifier)
            {
                auto msa_pred = msa.pred(lemon_graph.nodeFromId(modifier));
                assert(msa_pred != lemon::INVALID);

                int predicted = lemon_graph.id(lemon_graph.source(msa_pred));
                sentence[modifier].head = predicted;
            }
        }
    }

    // Output
    for (unsigned i = 0u ; i < test_data.size() ; ++i)
    {
        const IntSentence& int_sentence = test_data.at(i);
        ConllSentence& conll_sentence = conll_test.at(i);

        for (unsigned j = 0u ; j < int_sentence.size() ; ++ j)
        {
            const auto& int_token = int_sentence[j+1];
            auto& conll_token = conll_sentence.at(j);

            conll_token.head = std::to_string(int_token.head);

            if (conll_settings.use_cpos)
            {
                conll_token.cpostag = conll_settings.pos_dict.convert(int_token.pos);
                conll_token.postag = "_";
            }
            else
            {
                conll_token.cpostag = "_";
                conll_token.postag = conll_settings.pos_dict.convert(int_token.pos);
            }
        }
    }

    std::ofstream f(output_path);
    f << conll_test;
    f.close();
}


/*
 * img_align.cpp
 *
 *  Created on: Aug 22, 2012
 *      Author: cforster
 */

#include <vector>
#include <string>
#include <stdint.h>
#include <stdio.h>
#include <iostream>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <opencv2/opencv.hpp>
#include <vikit/math_utils.h>
#include <vikit/vision.h>
#include <vikit/pinhole_camera.h>
#include <vikit/nlls_solver.h>
#include <vikit/performance_monitor.h>
#include <vikit/img_align.h>
#include <sophus/se3.h>

namespace vk {

/*******************************************************************************
 * Forward Compositional
 */
ForwardCompositionalSE3::
ForwardCompositionalSE3( vector<PinholeCamera>& cam_pyr,
                         vector<cv::Mat>& depth_pyr,
                         vector<cv::Mat>& img_pyr,
                         vector<cv::Mat>& tpl_pyr,
                         vector<cv::Mat>& img_pyr_dx,
                         vector<cv::Mat>& img_pyr_dy,
                         SE3& init_model,
                         int n_levels,
                         int n_iter,
                         float res_thresh,
                         bool display,
                         Method method,
                         int test_id) :
      cam_pyr_(cam_pyr),
      depth_pyr_(depth_pyr),
      img_pyr_(img_pyr),
      tpl_pyr_(tpl_pyr),
      img_pyr_dx_(img_pyr_dx),
      img_pyr_dy_(img_pyr_dy),
      display_(display),
      log_(test_id < 0),
      res_thresh_(res_thresh)
{
  n_iter_ = n_iter;
  method_ = method;

  // Init Performance Monitor
#if 0
  if(log_)
  {
    permon_.init("forward", ros::package::getPath("rpl_examples") + "/trace/img_align/data",
                 test_id, true);
    permon_.addLog("iter");
    permon_.addLog("level");
    permon_.addLog("mu");
    permon_.addLog("chi2");
    permon_.addLog("trials");
  }
#endif

  runOptimization(init_model);

}

ForwardCompositionalSE3::
ForwardCompositionalSE3( vector<PinholeCamera>& cam_pyr,
                         vector<cv::Mat>& depth_pyr,
                         vector<cv::Mat>& img_pyr,
                         vector<cv::Mat>& tpl_pyr,
                         vector<cv::Mat>& img_pyr_dx,
                         vector<cv::Mat>& img_pyr_dy,
                         int n_levels,
                         int n_iter,
                         float res_thresh,
                         bool display,
                         Method method,
                         int test_id) :
      cam_pyr_(cam_pyr),
      depth_pyr_(depth_pyr),
      img_pyr_(img_pyr),
      tpl_pyr_(tpl_pyr),
      img_pyr_dx_(img_pyr_dx),
      img_pyr_dy_(img_pyr_dy),
      display_(display),
      log_(test_id < 0),
      res_thresh_(res_thresh)
{
  n_iter_ = n_iter;
  method_ = method;

  // Init Performance Monitor
#if 0
  if(log_)
  {
    permon_.init("forward", ros::package::getPath("rpl_examples") + "/trace/img_align/data",
                 test_id, true);
    permon_.addLog("iter");
    permon_.addLog("level");
    permon_.addLog("mu");
    permon_.addLog("chi2");
    permon_.addLog("trials");
  }
#endif

}

void ForwardCompositionalSE3::
runOptimization(SE3& model, int levelBegin, int levelEnd)
{
  if(levelBegin < 0 || levelBegin > n_levels_-1)
    levelBegin = n_levels_-1;
  if(levelEnd < 0)
    levelEnd = 1;

  // Perform Pyramidal optimization
  for(level_=levelBegin; level_>=levelEnd; --level_)
  {
    mu_ = 0.1;
    cout << endl << "PYRAMID LEVEL " << level_
         << endl << "---------------" << endl;
    optimize(model);
  }
}

double ForwardCompositionalSE3::
computeResiduals (const SE3& model, bool linearize_system, bool compute_weight_scale)
{
  // Warp the image such that it aligns with the template image
  double chi2 = 0;
  size_t n_pixels = 0;

  if(linearize_system)
    resimg_ = cv::Mat(tpl_pyr_[level_].size(), CV_32F, cv::Scalar(1));

  for( int v=0; v<depth_pyr_[level_].rows; ++v )
  {
    for( int u=0; u<depth_pyr_[level_].cols; ++u )
    {
      // compute pixel location in new img
      cv::Vec3f cv_float3 = depth_pyr_[level_].at<cv::Vec3f>(v,u);
      Vector3d xyz_tpl(cv_float3[0], cv_float3[1], cv_float3[2]);
      Vector3d xyz_img(model*xyz_tpl);
      Vector2f uv_img_pyr = cam_pyr_[level_].world2cam(xyz_img).cast<float>(); // apply cam model
      if( cam_pyr_[level_].isInFrame(uv_img_pyr.cast<int>(), 2) )
      {
        // compare image values
        float intensity_tpl =
            tpl_pyr_[level_].at<float>(v,u);
        float intensity_img =
            interpolateMat_32f(img_pyr_[level_], uv_img_pyr[0], uv_img_pyr[1]);

        // compute residual (opposite to 2d case because of other jacobian)
        float res = intensity_tpl-intensity_img;

        // robustification
        if(res > res_thresh_)  res = res_thresh_;
        if(res < -res_thresh_) res = -res_thresh_;
        chi2 += res*res;
        n_pixels++;

        if(linearize_system)
        {
          // get gradient of warped image (~gradient at warped position)
          float dx = 0.5*interpolateMat_32f(img_pyr_dx_[level_], uv_img_pyr[0], uv_img_pyr[1]);
          float dy = 0.5*interpolateMat_32f(img_pyr_dy_[level_], uv_img_pyr[0], uv_img_pyr[1]);

          // evaluate jacobian
          Matrix<double,2,6> frame_jac;
          frameJac_xyz2uv(xyz_img, cam_pyr_[level_].fx(), frame_jac);

          // compute steppest descent images
          Vector6d J = dx*frame_jac.row(0) + dy*frame_jac.row(1);

          // compute Hessian and
          H_ += J*J.transpose();
          Jres_ += J*res;

          resimg_.at<float>(v,u) = -res;
        }
      }
    }
  }
  chi2 /= n_pixels;
  return chi2;
}

int ForwardCompositionalSE3::
solve()
{
  x_ = H_.ldlt().solve(-Jres_);
  if((bool) std::isnan((double) x_[0]))
    return 0;
  return 1;
}

void ForwardCompositionalSE3::
update(const ModelType& old_model,  ModelType& new_model)
{
  new_model = SE3::exp(x_)*(old_model);
}

void ForwardCompositionalSE3::
startIteration()
{
#if 0
  if(log_)
    permon_.newMeasurement();
#endif
}

void ForwardCompositionalSE3::
finishIteration()
{
#if 0
  if(log_)
  {
    permon_.log("iter", iter_);
    permon_.log("level", level_);
    permon_.log("mu", mu_);
    permon_.log("chi2", chi2_);
    permon_.log("trials", n_trials_);
  }
#endif

  if(display_)
  {
    cv::namedWindow("residuals", CV_WINDOW_AUTOSIZE);
    cv::imshow("residuals", resimg_*3);
    cv::waitKey(0);
  }
}

/*******************************************************************************
 * Efficient Second Order Minimization (ESM)
 */
SecondOrderMinimisationSE3::
SecondOrderMinimisationSE3( vector<PinholeCamera>& cam_pyr,
                            vector<cv::Mat>& depth_pyr,
                            vector<cv::Mat>& img_pyr,
                            vector<cv::Mat>& tpl_pyr,
                            vector<cv::Mat>& img_pyr_dx,
                            vector<cv::Mat>& img_pyr_dy,
                            vector<cv::Mat>& tpl_pyr_dx,
                            vector<cv::Mat>& tpl_pyr_dy,
                            SE3& init_model,
                            int n_levels,
                            int n_iter,
                            float res_thresh,
                            bool display,
                            Method method,
                            int test_id) :
      cam_pyr_(cam_pyr),
      depth_pyr_(depth_pyr),
      img_pyr_(img_pyr),
      tpl_pyr_(tpl_pyr),
      img_pyr_dx_(img_pyr_dx),
      img_pyr_dy_(img_pyr_dy),
      tpl_pyr_dx_(tpl_pyr_dx),
      tpl_pyr_dy_(tpl_pyr_dy),
      display_(display),
      log_(test_id < 0),
      res_thresh_(res_thresh)
{
  n_iter_ = n_iter;
  method_ = method;
  verbose_ = false;

#if 0
  if(log_)
  {
    // Init Performance Monitor
    permon_.init("esm", ros::package::getPath("rpl_examples") + "/trace/img_align/data",
                 test_id, true);
    permon_.addLog("iter");
    permon_.addLog("level");
    permon_.addLog("mu");
    permon_.addLog("chi2");
    permon_.addLog("trials");
  }
#endif

  // perform pyramidal optimization
  for(level_=n_levels-1; level_>2; --level_)
  //level_ = n_levels-1;
  {
    // Optimize
    mu_ = 0.01f;
    if(display_)
    {
      cout << endl << "PYRAMID LEVEL " << level_
           << endl << "patch-width = " << img_pyr_[level_].cols
           << endl << "---------------" << endl;
    }
    optimize(init_model);
  }
}

double SecondOrderMinimisationSE3::
computeResiduals (const SE3& model, bool linearize_system, bool compute_weight_scale)
{
  // Warp the image such that it aligns with the template image
  double chi2 = 0;
  size_t n_pixels = 0;

  // TODO: to improve access speed, use a pointer and increment every iteration

  // Compute Warp
  cv::Mat mask = cv::Mat_<bool>(tpl_pyr_[level_].rows, tpl_pyr_[level_].cols, false);
  cv::Mat img_warped = cv::Mat_<float>(tpl_pyr_[level_].rows, tpl_pyr_[level_].cols, 1.0);

  for( int v=0; v<depth_pyr_[level_].rows; ++v )
  {
    for( int u=0; u<depth_pyr_[level_].cols; ++u )
    {
      // compute pixel location in new img
      cv::Vec3f cv_float3 = depth_pyr_[level_].at<cv::Vec3f>(v,u);
      Vector3d xyz_tpl(cv_float3[0], cv_float3[1], cv_float3[2]);
      Vector3d xyz_img(model*xyz_tpl);
      Vector2f uv_img_pyr = cam_pyr_[level_].world2cam(xyz_img).cast<float>(); // apply cam model
      if( cam_pyr_[level_].isInFrame(uv_img_pyr.cast<int>(), 1) )
      {
        img_warped.at<float>(v,u) = interpolateMat_32f(img_pyr_[level_], uv_img_pyr[0], uv_img_pyr[1]);

        if( cam_pyr_[level_].isInFrame(uv_img_pyr.cast<int>(), 2) )
          mask.at<bool>(v,u) = true;
      }
    }
  }

  // Compute Warp derivative
  cv::Mat img_warped_dx, img_warped_dy;
  cv::Sobel(img_warped, img_warped_dx, CV_32F, 1, 0, 1);
  cv::Sobel(img_warped, img_warped_dy, CV_32F, 0, 1, 1);

  // Compute Jacobian
  if(linearize_system)
    resimg_ = cv::Mat_<float>(tpl_pyr_[level_].size(), 1.0);

  for( int v=0; v<depth_pyr_[level_].rows; ++v )
  {
    for( int u=0; u<depth_pyr_[level_].cols; ++u )
    {
      if (mask.at<bool>(v,u))
      {
        // compare image values
        float intensity_tpl = tpl_pyr_[level_].at<float>(v,u);
        float intensity_img = img_warped.at<float>(v,u);

        // compute residual  (opposite to 2d case because of other jacobian)
        float res = intensity_tpl-intensity_img;

        // robustification
        if(res > res_thresh_)  res = res_thresh_;
        if(res < -res_thresh_) res = -res_thresh_;
        chi2 += res*res;
        n_pixels++;

        if(linearize_system)
        {
          // 0.25 because we have two 0.5 factors. First from adding the two gradients
          // and the second when we compute the sobel mask
          float dx = 0.25*(tpl_pyr_dx_[level_].at<float>(v,u) + img_warped_dx.at<float>(v,u));
          float dy = 0.25*(tpl_pyr_dy_[level_].at<float>(v,u) + img_warped_dy.at<float>(v,u));

          // evaluate jacobian
          cv::Vec3f cv_float3 = depth_pyr_[level_].at<cv::Vec3f>(v,u);
          Vector3d xyz_tpl(cv_float3[0], cv_float3[1], cv_float3[2]);
          Vector3d xyz_img(model*xyz_tpl);
          Matrix<double,2,6> frame_jac;
          frameJac_xyz2uv(xyz_tpl, cam_pyr_[level_].fx(), frame_jac);

          // compute steppest descent images
          Vector6d J = dx*frame_jac.row(0) + dy*frame_jac.row(1);

          // compute Hessian
          H_ += J*J.transpose();
          Jres_ += J*res;
          resimg_.at<float>(v,u) = res;
        }
      }
    }
  }
  chi2 /= n_pixels;
  return chi2;
}

int SecondOrderMinimisationSE3::
solve()
{
  x_ = H_.ldlt().solve(-Jres_);
  if((bool) std::isnan((double) x_[0]))
    return 0;
  return 1;
}

void SecondOrderMinimisationSE3::
update(const ModelType& old_model,  ModelType& new_model)
{
  new_model = SE3::exp(x_)*old_model;
}

void SecondOrderMinimisationSE3::
startIteration()
{
#if 0
  if(log_)
    permon_.newMeasurement();
#endif
}

void SecondOrderMinimisationSE3::
finishIteration()
{
#if 0
  if(log_)
  {
    permon_.log("iter", iter_);
    permon_.log("level", level_);
    permon_.log("mu", mu_);
    permon_.log("chi2", chi2_);
    permon_.log("trials", n_trials_);
  }
#endif

  if(display_)
  {
    cv::namedWindow("residuals", CV_WINDOW_AUTOSIZE);
    cv::imshow("residuals", resimg_*3);
    cv::waitKey(0);
  }
}

} // end namespace vk


#include <stan/math/torsten/test/unit/test_fixture_onecpt_effcpt.hpp>
#include <stan/math/torsten/test/unit/test_functors.hpp>
#include <stan/math/torsten/pmx_onecpt_model.hpp>
#include <stan/math/torsten/pmx_ode_model.hpp>
#include <boost/mp11.hpp>

TYPED_TEST_SUITE_P(test_onecpt_effcpt);
TYPED_TEST_P(test_onecpt_effcpt, multiple_bolus) {
  this -> compare_solvers_val();
  this -> compare_solvers_adj(this -> amt, 1.e-8, "AMT");
  this -> compare_solvers_adj(this -> rate, 1.e-8, "RATE");
  this -> compare_solvers_adj(this -> ii, 1.e-8, "II");
  this -> compare_solvers_adj(this -> theta[0], 5.e-6, "theta");
  this -> compare_solvers_adj(this -> biovar[0], 1.e-6, "bioavailability");
  this -> compare_solvers_adj(this -> tlag[0], 1.e-8, "lag time");
}

TYPED_TEST_P(test_onecpt_effcpt, multiple_bolus_addl) {
  this -> ii[0] = 1.55;
  this -> compare_solvers_val();
  this -> compare_solvers_adj(this -> amt, 1.e-8, "AMT");
  this -> compare_solvers_adj(this -> rate, 1.e-8, "RATE");
  this -> compare_solvers_adj(this -> ii, 5.e-6, "II");
  this -> compare_solvers_adj(this -> theta[0], 5.e-6, "theta");
  this -> compare_solvers_adj(this -> biovar[0], 1.e-6, "bioavailability");
  this -> compare_solvers_adj(this -> tlag[0], 1.e-8, "lag time");
}

REGISTER_TYPED_TEST_SUITE_P(test_onecpt_effcpt,
                            multiple_bolus,
                            multiple_bolus_addl);

using test_types = boost::mp11::mp_product<
  std::tuple,
  ::testing::Types<pmx_solve_onecpt_effcpt_functor>, // solver 1
  ::testing::Types<pmx_solve_linode_functor,
                   pmx_solve_rk45_functor>, // solver 2
  ::testing::Types<double>,  // TIME
  ::testing::Types<double, stan::math::var_value<double>>,  // AMT
  ::testing::Types<double, stan::math::var_value<double>> , // RATE
  ::testing::Types<double> , // II
  ::testing::Types<double, stan::math::var_value<double>> , // PARAM
  ::testing::Types<double> , // BIOVAR
  ::testing::Types<double> , // TLAG
  ::testing::Types<torsten::PMXOneCptEffCptODE>             // ODE
    >;

INSTANTIATE_TYPED_TEST_SUITE_P(PMX, test_onecpt_effcpt, test_types);


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

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of polynom components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 06/03/2011
///
#include <nt2/include/functions/polysub.hpp>
#include <nt2/include/functions/conv.hpp>
#include <nt2/include/functions/isequal.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/table.hpp>

#include <boost/array.hpp>

NT2_TEST_CASE_TPL ( polysub_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::polysub;
  using nt2::tag::polysub_;
  nt2::table<T, nt2::_2D> a =  nt2::_(T(1), T(4));
  nt2::table<T, nt2::_2D> b =  nt2::_(T(1), T(2));
  nt2::table<T, nt2::_2D> c;
  T aab[] = { 1, 2, 2, 2};
  nt2::table<T> d(nt2::of_size(1, 4), &aab[0], &aab[4]);
  c = polysub(a, b);

  NT2_DISPLAY(a);
  NT2_DISPLAY(b);
  NT2_DISPLAY(c);
  NT2_DISPLAY(polysub(a, b));
  NT2_DISPLAY(polysub(b, a));
  NT2_TEST(nt2::isequal(d,polysub(a, b)));
  NT2_TEST(nt2::isequal(-d,polysub(b, a)));
} // end of test for floating_


#include <iostream>
#include <string>
#include <array>
#include <vector>
#include <deque>
#include <list>
#include <set>
#include <map>
#include <unordered_set>
#include <unordered_map>
#include <stack>
#include <queue>
#include <algorithm>
#include <numeric>
#include <cmath>
#define BOOST_TEST_MODULE Wildcard Matching
#include <boost/test/included/unit_test.hpp>

using namespace std;

class Solution {
public:
    bool isMatch(string s, string p) {
        auto lenS = s.size();
        auto lenP = p.size();
        vector<vector<bool>> dp {lenS+1, vector<bool>(lenP+1, false)};

        dp[0][0] = true;
        for (auto j = 1; j <= lenP; ++j) {
            if (p[j-1] == '*') {
                dp[0][j] = dp[0][j-1];
            }
        }

        for (auto i = 1; i <= lenS; ++i) {
            for (auto j = 1; j <= lenP; ++j) {
                if ( p[j-1] == '?' || p[j-1] == s[i-1]) { // match single
                    dp[i][j] = dp[i-1][j-1];
                } else if (p[j-1] == '*') {
                    dp[i][j] = dp[i][j-1] || dp[i-1][j-1] || dp[i-1][j];
                }
            }
        }

        // cout << s << '|' << p << endl;
        // for (auto row : dp) {
        //     for (auto val : row)
        //         cout << val << ' ';
        //     cout << endl;
        // }
        return dp[lenS][lenP];
    }
};

BOOST_AUTO_TEST_CASE( my_test )
{
    Solution s;

    BOOST_TEST( ! s.isMatch("aa",   "a")     );
    BOOST_TEST(   s.isMatch("aa",   "aa")    );
    BOOST_TEST( ! s.isMatch("aaa",  "aa")    );
    BOOST_TEST(   s.isMatch("aa",   "*")     );
    BOOST_TEST(   s.isMatch("aa",   "a*")    );
    BOOST_TEST(   s.isMatch("ab",   "?*")    );
    BOOST_TEST( ! s.isMatch("aab",  "c*a*b") );
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/combinatorial/include/functions/primes.hpp>
#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/saturate.hpp>

#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>

#include <nt2/constant/constant.hpp>
#include <nt2/table.hpp>
#include <nt2/include/functions/tic.hpp>
#include <nt2/include/functions/toc.hpp>

NT2_TEST_CASE_TPL ( primes_integer__1_0,  NT2_TYPES)
{

  using nt2::primes;
  using nt2::tag::primes_;

  T p[6] = {2, 3, 5, 7, 11, 13};
  nt2::table<T> myp = primes(T(15));
  // specific values tests
  NT2_DISPLAY(myp);
  for(size_t i=1; i <= 6; ++i)
  {
    NT2_TEST_EQUAL(p[i-1], myp(i));
  }
}


#ifndef MPLLIBS_METAMONAD_TEST_COMMON_HPP
#define MPLLIBS_METAMONAD_TEST_COMMON_HPP

// Copyright Abel Sinkovics (abel@sinkovics.hu) 2010.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/metafunction.hpp>
#include <mpllibs/metamonad/lazy_metafunction.hpp>
#include <mpllibs/metamonad/returns.hpp>
#include <mpllibs/metamonad/name.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/divides.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/equal_to.hpp>

namespace
{
  using namespace mpllibs::metamonad::name;

  typedef boost::mpl::int_<0> int0;
  typedef boost::mpl::int_<1> int1;
  typedef boost::mpl::int_<2> int2;
  typedef boost::mpl::int_<3> int3;
  typedef boost::mpl::int_<4> int4;
  typedef boost::mpl::int_<6> int6;
  typedef boost::mpl::int_<9> int9;
  typedef boost::mpl::int_<11> int11;
  typedef boost::mpl::int_<13> int13;
  typedef boost::mpl::int_<24> int24;
  typedef boost::mpl::int_<26> int26;
  typedef boost::mpl::int_<28> int28;
  typedef boost::mpl::int_<37> int37;

  MPLLIBS_METAFUNCTION(eval, (T)) ((T));

  MPLLIBS_METAFUNCTION(double_value, (T)) ((boost::mpl::times<T, int2>));

  MPLLIBS_LAZY_METAFUNCTION(lazy_double_value, (T))
  ((boost::mpl::times<T, int2>));

  MPLLIBS_LAZY_METAFUNCTION(lazy_plus, (A)(B)) ((boost::mpl::plus<A, B>));

  MPLLIBS_LAZY_METAFUNCTION(double_lazy_plus, (A)(B))
  ((boost::mpl::plus<typename A::type, typename B::type>));

  MPLLIBS_LAZY_METAFUNCTION(lazy_times, (A)(B)) ((boost::mpl::times<A, B>));

  MPLLIBS_LAZY_METAFUNCTION(lazy_equal_to, (A)(B))
  ((boost::mpl::equal_to<A, B>));

  // Needed by laziness tests

  typedef mpllibs::metamonad::returns<int13> returns13;

  MPLLIBS_METAFUNCTION(non_lazy_plus, (A)(B))
  ((boost::mpl::int_<A::value + B::value>));

  MPLLIBS_METAFUNCTION(custom_eval_if, (C)(T)(F))
  ((boost::mpl::if_<C, T, F>));

  typedef boost::mpl::divides<int1, int0> breaking_expr;
}

#endif


#include "metaheuristic/methods/giant_tour_creation.h"
#include "metaheuristic/methods/create_zone_matrix.h"
#include "metaheuristic/methods/neighborhood.h"
#include "metaheuristic/methods/vnd.h"
#include "metaheuristic/model/instance.h"
#include "metaheuristic/model/solution.h"
#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <ctime>
#include <fstream>
#include <iostream>
#include <iterator>
#include <limits>
#include <omp.h>
#include <sstream>
#include <cmath>

#define length(x) (sizeof(x) / sizeof(x[0]))

using namespace std;

template < typename Type > std::string to_str (const Type & t)
{
    std::ostringstream os;
    os << t;
    return os.str ();
}

/*
 * RUN SEARCH IS THE MAIN ALGORITHM FUNCTION
 * INPUTS:
 * randomization --> int Indicate randomization value for construction of initial solutions with random_nearest_neighbor
 * seed --> int Random number generator state
 * instance_name --> string  "route_"+route_number+".txt";
 * in_test_phase --> 0 if currently Train phase, 1 if currently apply/test phase
 * neighborhood_combinations_sliced --> vector of vectors containing the Neighborhoods for VND
 * first_improvement --> Boolean indicating if first_improvement strategy or best_improvement strategy is used in the VND (Default=false, use best_improvement)
 * normalize --> Indicate if normalize costs of the objective function. Default=True
 */

void run_search(int randomization, int seed,  const string& instance_name, int in_test_phase,  vector<vector<TSPNeighborhood*>>neighborhood_combinations_sliced, bool first_improvement, bool normalize) {
    string direc_input = "";
    string direc_output = "";
    string timing_direc = "";
    string local_optima_direc = "";

    //WRITE OUTPUT CSV'S TO DIFFERENT FOLDERS, DEPENDING ON TRAINING PHASE OR APPLY PHASE
    if (in_test_phase == 0) {
        direc_input = "../data/model_build_outputs/parsed_files/";
        timing_direc = "../data/model_build_outputs/timing/";
        direc_output = "../data/model_build_outputs/grasp_routes_prob_random_"+ to_str(randomization)+"/";
        local_optima_direc = "../data/model_build_outputs/local_optima_analisis/";
    }
    else if (in_test_phase == 1) {
        direc_input = "../data/model_apply_outputs/parsed_files_val/";
        timing_direc = "../data/model_apply_outputs/timing/";
        direc_output = "../data/model_apply_outputs/grasp_routes_prob_random_"+ to_str(randomization)+"/";
        local_optima_direc = "../data/model_apply_outputs/local_optima_analisis/";

    }

    vector<string> splitting_result;
    boost::split(splitting_result, instance_name, boost::is_any_of("."));

    string global_results = direc_output +"solution_ " + splitting_result[0];

    vector<string> route_and_number;
    boost::split(route_and_number, splitting_result[0], boost::is_any_of("_"));
    TSPInstance instance = TSPInstance(direc_input + instance_name, route_and_number[1], in_test_phase);
    cout << "Parsing instance done" << endl;

    //ASSIGN RANDOM_SEED_STATE TO THE METHOD FOR CREATING RANDOM FURTHEST NEIGHBOR SOLUTION
    vector<TSPGiantTour *> giant_tour_heuristics;
    vector<TSPGiantTour *> furthest_heuristics = {new TSP_GiantTour_FN(seed)};  //Furthest neighbor for max initial_solution time

    // CREATE STRUCTURES FOR STORING INITIAL SOLUTIONS - CURRENT BEST SOLUTION - FINAL BEST SOLUTION
    //TSPSolution *giant_tour_solution;
    TSPSolution *initial_furthest_solution;
    TSPSolution *best_solution = new TSPSolution();
    TSPSolution *best_initial_solution = new TSPSolution();
    TSPSolution *train_best_initial_solution = new TSPSolution();
    TSPSolution *train_best_solution = new TSPSolution();
    bool is_train_best_solution_null = true;
    float train_best_solution_cost = std::numeric_limits<float>::max();
    LocalOptimaOutputs local_optima_storage;

    instance.normalize = normalize;

    if (in_test_phase == 1){
        instance.in_test_phase = true;
    }
    else
    {
        instance.in_test_phase = false;
    }

    // CREATE FURTHEST SOLUTION USING RANDOM FURTHEST NEIGHBOR HEURISTIC
    cout << "Computing initial solution" << endl;
    initial_furthest_solution = furthest_heuristics[0]->run(instance, randomization);
    float initial_furthest_cost = initial_furthest_solution->compute_cost(instance);
    cout << "Furthest cost: " << initial_furthest_cost << endl;
    // THIS COST OF THIS FURTHEST SOLUTION WILL BE USED TO NORMALIZE THE ROUTE-TIME COST OF THE OTHER SOLUTIONS GENERATED IN THIS GRASP
    instance.worst_initial_cost = initial_furthest_cost;

    //CREATE ZONE_TIME AND ZONE_DISTANCE PENALIZATION MATRICES
    TSPZoneMatrixSolution zones_info = create_zones_matrix(instance);

    //DETERMINE NUMBER OF GRASP ITERATIONS THAT WILL BE PERFORMED, ACCORDING TO NUMBER OF CUSTOMERS AND NUMBER OF ZONES IN THE ROUTE
    //int calc_iterations = instance.n_cust * log(instance.n_cust/(instance.zones_time_matrix_df.size()-1))/ sqrt(instance.zones_time_matrix_df.size()-1) ;
    //int number_of_zones = instance.zones_time_matrix_df.size()-1;
    //int total_iterations = max(calc_iterations,  number_of_zones);

    int total_iterations = omp_get_max_threads();
    cout << "number_of_grasp_iteration: " << total_iterations << endl;

    string local_optima_direction = local_optima_direc + "solution_" + splitting_result[0];
    int local_optima_counter = 0;
#pragma omp parallel
    {
        int seeds = omp_get_thread_num();

        //ASSIGN RANDOM_SEED_STATE TO EACH THREAD IN THE METHOD TO CREATE INITIAL SOLUTIONS WITH RANDOM NEAREST NEIGHBOR
        giant_tour_heuristics = {new TSPGiantTour_Prob_RNN(seeds)};
        bool is_best_solution_null = true;
        float best_solution_cost = std::numeric_limits<float>::max();

        //START GRASP ITERATIONS
#pragma omp for
        for (int it = 0; it < total_iterations; it++) {
            TSPSolution *giant_tour_solution;

            //CREATE INITIAL SOLUTION WITH CLUSTERED RANDOM NEAREST NEIGHBOR
            giant_tour_solution = giant_tour_heuristics[0]->run(instance, randomization);
            giant_tour_solution->cost = giant_tour_solution->compute_prob_cost(instance);

            float local_optima_cost;

            TSPSolution *local_optima;
            //local_optima = run_prob_tsp_vnd(*giant_tour_solution, instance, neighborhood_combinations_sliced[0], true);
            local_optima = run_vnd_generator(*giant_tour_solution, instance,  neighborhood_combinations_sliced[0], local_optima_direction, local_optima_counter, true);
            local_optima_cost = local_optima->cost;

            if (is_best_solution_null || local_optima_cost < best_solution_cost) {
                if (!is_best_solution_null) {
                    //delete best_solution;
                }
                //best_tsp_solution = giant_tour_solution;
                best_initial_solution = giant_tour_solution;
                best_solution = local_optima;

                best_solution_cost = local_optima_cost;
                is_best_solution_null = false;
            } else {
                //delete local_optima;
            }

#pragma omp critical
            {

                if (is_train_best_solution_null || best_solution_cost < train_best_solution_cost) {
                    train_best_solution = best_solution;
                    train_best_initial_solution = best_initial_solution;

                    train_best_solution_cost = best_solution_cost;
                    cout << train_best_solution_cost << endl;
                    is_train_best_solution_null = false;
                }
            }
        }

    }

    cout << "Final Cost: " << train_best_solution->cost << " Time Cost: " << train_best_solution->time_cost <<" Heading Cost: " << train_best_solution->heading_penalization_cost << " Angle cost: " << train_best_solution->angle_penalization_cost  <<endl;// << train_best_solution->time_window_cost << endl;
    cout << " Zone_time cost: " <<  train_best_solution->zone_time_penalization_cost << " Zone_distance cost: " <<  train_best_solution->zone_distance_penalization_cost  << " Time_window cost: " <<  train_best_solution->time_window_cost <<  endl; //best_solution->time_window_cost << endl;

    //global_output << train_best_solution->TSP_output_string_zones(instance).str() << endl;  //OUTPUT CSV WITH ZONES_IDS ACCORDING TO BEST SOLUTION FOUND
    ofstream global_output(global_results);
    global_output << train_best_solution->TSP_output_string_solutions(instance).str() << endl;
    // global_output << "Solution-ids: " << endl << train_best_solution->TSP_output_number_solutions().str() << endl;

    global_output.close();
}

int main(int argc, char* argv[])
{
    string route_number_str = argv[1];
    size_t pos;
    string n_randomization = argv[2];
    size_t pos9;
    int randomization = stoi(n_randomization, &pos9);
    int route = stoi(route_number_str, &pos) - 1; // Shell/bash/slurm job arrays start at 1, but routes start at 0
    string n_type = argv[3];
    size_t pos2;
    int in_test_phase = stoi(n_type, &pos2);
    int seed = 1;// = stoi(n_seed, &pos2);
    //int route=0;
    /*
    string p_beta = argv[5];
    size_t pos3;
    float penalization_beta = stof(p_beta, &pos3);
    */

    bool normalize = true;
    bool first_improvement = false;

    string instance_name = "route_"+to_string(route)+".txt";
    vector<TSPNeighborhood*> neighborhoods = {};

    int indexes[] = { 0, 1, 2, 3 };
    vector<string> neighborhoods_names = { "r", "s", "R", "S" };

    neighborhoods = {new TSPRelocateNeighborhood(first_improvement),
                     new TSPSwapNeighborhood(first_improvement),
                     new TSP_ProbZoneRelocateNeighborhood(first_improvement),
                     new TSP_ProbZoneSwapNeighborhood(first_improvement)
    };

    vector<vector<TSPNeighborhood*>> neighborhood_combinations;
    int until = 10;
    vector<vector<TSPNeighborhood*>> neighborhood_combinations_sliced;

    vector<vector<string>> neighborhood_names_combinations;
    vector<vector<string>> neighborhood_names_combinations_sliced;

    do {
        vector<TSPNeighborhood*> combination;
        vector<string> names_combination;
        for(unsigned int i = 0; i < neighborhoods_names.size(); i++) {
            combination.push_back(neighborhoods[indexes[i]]);
            names_combination.push_back(neighborhoods_names[indexes[i]]);
        }
        neighborhood_combinations.push_back(combination);
        neighborhood_names_combinations.push_back(names_combination);
    } while(std::next_permutation(begin(indexes), end(indexes)));

    for(unsigned int i = 0; i < neighborhood_combinations.size(); i += 1) {
        if (i == 0){
            neighborhood_combinations_sliced.push_back(neighborhood_combinations[i]);
            neighborhood_names_combinations_sliced.push_back(neighborhood_names_combinations[i]);
        }
    }

    cout << "In test phase type "<< in_test_phase << endl;

    run_search(randomization, seed, instance_name, in_test_phase,  neighborhood_combinations_sliced, first_improvement, normalize);

    cout << "Sali de run_search" << endl;
}


#ifndef _APPLICATIONS_LDA_IO_HPP_
#define _APPLICATIONS_LDA_IO_HPP_

#include <memory>

#include <Eigen/Core>

#include "ldaplusplus/Parameters.hpp"

using namespace ldaplusplus;

namespace io {

/**
  * Parse the input data from a file and save them to two Eigen matrixes, one
  * for the input data and one for their corresponding class labels.
  *
  * @param data_path The file to read the input data from
  * @param X An Eigen matix containing the input data
  * @param y An Eigen matrix containing the class labels of the input data
  */
void parse_input_data(
    std::string data_path,
    Eigen::MatrixXi &X,
    Eigen::MatrixXi &y
);

/**
  * Parse the input data from a file and save them to one Eigen matrix that
  * corresponds to the input data.
  *
  * @param data_path The file to read the input data from
  * @param X An Eigen matix containing the input data
  */
void parse_input_data(
    std::string data_path,
    Eigen::MatrixXi &X
);

/**
  * Save a set of model parameters in a file defined by the model_path input
  * argument, according to the NumpyFormat.
  *
  * @param model_path The file to save the set of the input parameters
  * @param parameters The set of input parameters to be saved
  */
void save_lda(
    std::string model_path,
    std::shared_ptr<parameters::Parameters> parameters
);

/**
  * Read a set of model parameters saved in NumpyInput from a file
  *
  * @param model_path The file to read a set of model parameters from
  * @return The model parameters
  */
std::shared_ptr<parameters::SupervisedModelParameters<double> > load_lda(
    std::string model_path
);

}  // namespace io

#endif  // _APPLICATIONS_LDA_IO_HPP_


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
//
#ifndef BOOST_GRAPH_MST_PRIM_HPP
#define BOOST_GRAPH_MST_PRIM_HPP

#include <functional>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

namespace boost {

  namespace detail {
    // this should be somewhere else in boost...
    template <class U, class V> struct _project2nd {
      V operator()(U, V v) const { return v; }
    };
  }

  namespace detail {

    // This is Prim's algorithm to calculate the Minimum Spanning Tree
    // for an undirected graph with weighted edges.

    template <class Graph, class P, class T, class R, class Weight>
    inline void
    prim_mst_impl(const Graph& G,
                  typename graph_traits<Graph>::vertex_descriptor s,
                  const bgl_named_params<P,T,R>& params,
                  Weight)
    {
      typedef typename property_traits<Weight>::value_type W;
      std::less<W> compare;
      detail::_project2nd<W,W> combine;
      dijkstra_shortest_paths(G, s, params.distance_compare(compare).
                              distance_combine(combine));
    }
  } // namespace detail

  template <class VertexListGraph, class DijkstraVisitor,
            class PredecessorMap, class DistanceMap,
            class WeightMap, class IndexMap>
  inline void
  prim_minimum_spanning_tree
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
     IndexMap index_map,
     DijkstraVisitor vis)
  {
    typedef typename property_traits<WeightMap>::value_type W;
    std::less<W> compare;
    detail::_project2nd<W,W> combine;
    dijkstra_shortest_paths(g, s, predecessor, distance, weight, index_map,
                            compare, combine, std::numeric_limits<W>::max(), 0,
                            vis);
  }

  template <class VertexListGraph, class PredecessorMap,
            class P, class T, class R>
  inline void prim_minimum_spanning_tree
    (const VertexListGraph& g,
     PredecessorMap p_map,
     const bgl_named_params<P,T,R>& params)
  {
    detail::prim_mst_impl
      (g,
       choose_param(get_param(params, root_vertex_t()), *vertices(g).first),
       params.predecessor_map(p_map),
       choose_const_pmap(get_param(params, edge_weight), g, edge_weight));
  }

  template <class VertexListGraph, class PredecessorMap>
  inline void prim_minimum_spanning_tree
    (const VertexListGraph& g, PredecessorMap p_map)
  {
    detail::prim_mst_impl
      (g, *vertices(g).first, predecessor_map(p_map).
       weight_map(get(edge_weight, g)),
       get(edge_weight, g));
  }

} // namespace boost

#endif // BOOST_GRAPH_MST_PRIM_HPP


/*
 [auto_generated]
 boost/numeric/odeint/integrate/integrate_const.hpp

 [begin_description]
 Constant integration of ODEs, meaning that the state of the ODE is observed on constant time intervals.
 The routines makes full use of adaptive and dense-output methods.
 [end_description]

 Copyright 2011-2013 Karsten Ahnert
 Copyright 2011-2015 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_CONST_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_CONST_HPP_INCLUDED

#include <boost/type_traits/is_same.hpp>

#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/integrate/null_observer.hpp>
#include <boost/numeric/odeint/integrate/check_adapter.hpp>
#include <boost/numeric/odeint/integrate/detail/integrate_const.hpp>
#include <boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp>

namespace boost {
namespace numeric {
namespace odeint {

/*
 * Integrates with constant time step dt.
 */
template<class Stepper, class System, class State, class Time, class Observer, class StepOverflowChecker>
size_t integrate_const(
        Stepper stepper, System system, State &start_state,
        Time start_time, Time end_time, Time dt,
        Observer observer, StepOverflowChecker checker
) {
    typedef typename odeint::unwrap_reference<Stepper>::type::stepper_category stepper_category;
    // we want to get as fast as possible to the end
    // no overflow checks needed
    if (boost::is_same<null_observer, Observer>::value) {
        return detail::integrate_adaptive(
                stepper, system, start_state,
                start_time, end_time, dt,
                observer, stepper_category());
    }
    else {
        // unwrap references
        typedef typename odeint::unwrap_reference< Stepper >::type stepper_type;
        typedef typename odeint::unwrap_reference< Observer >::type observer_type;
        typedef typename odeint::unwrap_reference< StepOverflowChecker >::type checker_type;

        return detail::integrate_const(checked_stepper<stepper_type, checker_type>(stepper, checker),
                                       system, start_state,
                                       start_time, end_time, dt,
                                       checked_observer<observer_type, checker_type>(observer, checker),
                                       stepper_category());
    }
}

/**
* \brief Second version to solve the forwarding problem,
* can be called with Boost.Range as start_state.
*/
template<class Stepper, class System, class State, class Time, class Observer, class StepOverflowChecker >
size_t integrate_const(
        Stepper stepper, System system, const State &start_state,
        Time start_time, Time end_time, Time dt,
        Observer observer, StepOverflowChecker checker
) {
    typedef typename odeint::unwrap_reference<Stepper>::type::stepper_category stepper_category;
    // we want to get as fast as possible to the end

    if (boost::is_same<null_observer, Observer>::value) {
        return detail::integrate_adaptive(
                stepper, system, start_state,
                start_time, end_time, dt,
                observer, stepper_category());
    }
    else {
        typedef typename odeint::unwrap_reference< Stepper >::type stepper_type;
        typedef typename odeint::unwrap_reference< Observer >::type observer_type;
        typedef typename odeint::unwrap_reference< StepOverflowChecker >::type checker_type;

        return detail::integrate_const(checked_stepper<stepper_type, checker_type>(stepper, checker),
                                       system, start_state,
                                       start_time, end_time, dt,
                                       checked_observer<observer_type, checker_type>(observer, checker),
                                       stepper_category());
    }
}

/**
* \brief integrate_const without step overflow checker
*/
template<class Stepper, class System, class State, class Time, class Observer>
size_t integrate_const(
        Stepper stepper, System system, State &start_state,
        Time start_time, Time end_time, Time dt, Observer observer)
{
    typedef typename odeint::unwrap_reference<Stepper>::type::stepper_category stepper_category;
    return detail::integrate_const(stepper, system, start_state,
                                   start_time, end_time, dt, observer, stepper_category());
}

/**
* \brief Second version to solve the forwarding problem,
* can be called with Boost.Range as start_state.
*/
template<class Stepper, class System, class State, class Time, class Observer>
size_t integrate_const(
        Stepper stepper, System system, const State &start_state,
        Time start_time, Time end_time, Time dt, Observer observer
) {
    typedef typename odeint::unwrap_reference<Stepper>::type::stepper_category stepper_category;
    return detail::integrate_const(stepper, system, start_state,
                                   start_time, end_time, dt, observer, stepper_category());
}

/**
* \brief integrate_const without observer calls
*/
template<class Stepper, class System, class State, class Time>
size_t integrate_const(
        Stepper stepper, System system, State &start_state,
        Time start_time, Time end_time, Time dt
) {
    return integrate_const(stepper, system, start_state, start_time, end_time, dt, null_observer());
}

/**
* \brief Second version to solve the forwarding problem,
* can be called with Boost.Range as start_state.
*/
template<class Stepper, class System, class State, class Time>
size_t integrate_const(
        Stepper stepper, System system, const State &start_state,
        Time start_time, Time end_time, Time dt
) {
    return integrate_const(stepper, system, start_state, start_time, end_time, dt, null_observer());
}

/********* DOXYGEN *********/
/**
 * \fn integrate_const( Stepper stepper , System system , State &start_state , Time start_time ,
 *                      Time end_time , Time dt , Observer observer , StepOverflowChecker checker )
 * \brief Integrates the ODE with constant step size.
 *
 * Integrates the ODE defined by system using the given stepper.
 * This method ensures that the observer is called at constant intervals dt.
 * If the Stepper is a normal stepper without step size control, dt is also
 * used for the numerical scheme. If a ControlledStepper is provided, the
 * algorithm might reduce the step size to meet the error bounds, but it is
 * ensured that the observer is always called at equidistant time points
 * t0 + n*dt. If a DenseOutputStepper is used, the step size also may vary
 * and the dense output is used to call the observer at equidistant time
 * points.
 * If a max_step_checker is provided as StepOverflowChecker, a
 * no_progress_error is thrown if too many steps (default: 500) are performed
 * without progress, i.e. in between observer calls. If no checker is provided,
 * no such overflow check is performed.
 *
 * \param stepper The stepper to be used for numerical integration.
 * \param system Function/Functor defining the rhs of the ODE.
 * \param start_state The initial condition x0.
 * \param start_time The initial time t0.
 * \param end_time The final integration time tend.
 * \param dt The time step between observer calls, _not_ necessarily the
 * time step of the integration.
 * \param observer [optional] Function/Functor called at equidistant time intervals.
 * \param checker [optional] Functor to check for step count overflows, if no
 * checker is provided, no exception is thrown.
 * \return The number of steps performed.
 */

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_CONST_HPP_INCLUDED


/**
 *    \file include/geombd/io/parser.cpp
 *    \author Keny Ordaz
 *    \version 1.0
 *    \date 2021
 *
 *    Class to parse a multibody system from URDF
 *    Copyright (c) 2021 Cinvestav
 *    This library is distributed under the MIT License.
 */

/*#include <iostream>
#include <iomanip>
#include <vector>
#include <fstream>
#include <sstream>
#include <optional>
#include <memory>
#include <unordered_map>
#include <chrono>
#include <Eigen/Dense>*/
#include "pugixml.hpp"

#include "parser.hpp"

using namespace Eigen;

// support {{{1
RowVectorXd string_to_vector(const std::string& vec, int size)
{
   RowVectorXd res(size);
   std::istringstream ss(vec);
   for (auto i=0; i < size; i++) {
       ss >> res[i];
   }
   return res;
}

/*
class Pose
{
public:
    Pose(): xyz(0, 0, 0), rpy(0, 0, 0) {}

    void init_rpy(double roll, double pitch, double yaw)
    {
        auto phi = roll / 2.0;
        auto the = pitch / 2.0;
        auto psi = yaw / 2.0;
        auto sphi = sin(phi);  auto cphi = cos(phi);
        auto sthe = sin(the);  auto cthe = cos(the);
        auto spsi = sin(psi);  auto cpsi = cos(psi);

        this->x = sphi * cthe * cpsi - cphi * sthe * spsi;
        this->y = cphi * sthe * cpsi + sphi * cthe * spsi;
        this->z = cphi * cthe * spsi - sphi * sthe * cpsi;
        this->w = cphi * cthe * cpsi + sphi * sthe * spsi;

        this->normalize();
    };
    void normalize()
    {
        double s = sqrt(this->x * this->x +
                this->y * this->y +
                this->z * this->z +
                this->w * this->w);
        if (s == 0.0) {
            this->x = 0.0;
            this->y = 0.0;
            this->z = 0.0;
            this->w = 1.0;
        } else {
            this->x /= s;
            this->y /= s;
            this->z /= s;
            this->w /= s;
        }
    };
    double x, y, z, w;
    RowVector3d xyz;
    RowVector3d rpy;
};

class Axis
{
public:
    Axis(): xyz(1.0, 0., 0.) {}

    RowVector3d xyz;
};

class Inertia { // optional zero inertia
public:
    Inertia(double ixx=0.0, double ixy=0.0, double ixz=0.0, double iyy=0.0, double iyz=0.0, double izz=0.0)
        : ixx(ixx), ixy(ixy), ixz(ixz), iyy(iyy), iyz(iyz), izz(izz)
        { setMatrix(ixx, ixy, ixz, iyy, iyz, izz); }
    void setMatrix(double ixx=0.0, double ixy=0.0, double ixz=0.0, double iyy=0.0, double iyz=0.0, double izz=0.0)
        { value << ixx, ixy, ixz, ixy, iyy, iyz, ixz, iyz, izz; }

    double ixx;
    double ixy;
    double ixz;
    double iyy;
    double iyz;
    double izz;
    Matrix3d value;
};
*/

Inertial::Inertial(const pugi::xml_node & node): Inertial() {

    auto node_o = node.child("origin"); //(optional: defaults to identity if not specified)
    if (!node_o.empty()) { // TODO: Should default to identity if it is not indicated in the URDF
        auto xyz = string_to_vector(node_o.attribute("xyz").value());
        auto rpy = string_to_vector(node_o.attribute("rpy").value());
        origin.xyz = xyz;
        origin.rpy = rpy;
    }
    mass = node.child("mass").attribute("value").as_double(); //default 0.0
    auto node_i = node.child("inertia");
    if (!node_i.empty()) {
        inertia.setMatrix(node_i.attribute("ixx").as_double(),
                node_i.attribute("ixy").as_double(),
                node_i.attribute("ixz").as_double(),
                node_i.attribute("iyy").as_double(),
                node_i.attribute("iyz").as_double(),
                node_i.attribute("izz").as_double());
    }

}

/*

    Inertial(): origin(), mass(0.0), inertia() {}

    std::string Inertial::to_string() const {
        static IOFormat CommaInitFmt(StreamPrecision, DontAlignCols, ", ", ", ", "", "", " << ", ";");
        static IOFormat OctaveFmt(StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
        std::ostringstream out;
        out << "mass (" << mass << "),\n\t origin[xyz] (" << origin.xyz.format(CommaInitFmt);
        out << "), origin[rpy] (" << origin.rpy.format(CommaInitFmt) << "); inertia tensor:\n";
        out << inertia.value.format(OctaveFmt);
        return out.str();

    }

*/

Body::Body(const std::string& name, int id)
    : parent(-1), has_inertial(false), Element(name, id)
{
}

std::string Body::to_string() const
{
    std::ostringstream out;
    out << "body " << name << ", id " << id;
    if (parent_links.size() > 0)
        out << "; parent (";
    for (auto && parent : parent_links) {
        out << parent << " ";
    }
    out << ")";
    if (child_links.size() > 0) {
        out << "; children ( ";
        for (auto && child : child_links) {
            out << child << " ";
        }
        out << ")";
    }
    if (has_inertial) {
        out << " has inertial properties: ";
        out << inertial_properties.to_string();
    }
    //out << "\n";
    return out.str();
}

void Body::addChild(int child_id) { child_links.push_back(child_id); }
void Body::addParent(int parent_id) { parent_links.push_back(parent_id); }
void Body::addJoint(int joint_id) { parent_joints.push_back(joint_id); }
void Body::setInertialProperties(const Inertial& value)
{
    inertial_properties = value; has_inertial = true;
}

/*
class Joint : public Element // {{{1
{
    // MAke enum for joint types
public:
    enum joint_type {
        revolute = 'r',  // The type of the joint is revolute.
        prismatic = 'p', // The type of the joint is prismatic.
        fixed = 'f',      // The type of the joint is glue.
        notype = 'n'      // There is no information about the type.
    };

    Joint(const std::string& name="unassigned", int id=-1): nq(0), Element(name, id) {}
    Joint(const std::string& name, unsigned int id,
            const std::string& parent, const std::string& child, const std::string& type)
        : predecessor(parent), successor(child)
        , axis(1.0, 0, 0), Element(name, id) {
            if (type == "revolute") {
                this->type = Joint::joint_type::revolute;
            } else if (type == "prismatic") {
                this->type = Joint::joint_type::prismatic;
            } else if (type == "fixed") {
                this->type = Joint::joint_type::fixed;
            } else {
                this->type = Joint::joint_type::notype;

            }

            switch(this->type) {
            case joint_type::revolute:
            case joint_type::prismatic:
                // TODO: check limits
                setLimit();
                nq = 1;
                break;
            default:
                nq = 0;// Do nothing?;
            }
        }
    //~Joint() { std::cout << __PRETTY_FUNCTION__ << "\n"; }

    std::string to_string() {
        std::ostringstream out;
        out << "joint " << name << ", id " << id << ", type " << std::string(1, type);
        out << ", axis " << axis;
        if (type == joint_type::revolute || type == joint_type::prismatic) {
            out << ", limits(" << upper << ", " << lower << ", " << effort << ", " << velocity << ") ";
        }
        out << joint_axis_indicator();
        out << "\t/" << successor << "/" << predecessor << "/ ";
        return out.str();
    }

    std::string joint_axis_indicator() {
        std::ostringstream out;
        out << "[";
        switch(type) {
        case Joint::joint_type::revolute:
            out << "⚙";
            break;
        case Joint::joint_type::prismatic:
            out << "↕";
            break;
        case Joint::joint_type::fixed:
            out << "⚓";
            break;
        case Joint::joint_type::notype:
            out << "⦻";
        }
        if (type == joint_type::revolute || type == joint_type::prismatic) {
            if (axis == RowVector3d(1.0, 0.0, 0.0))
                out << "+X";
            else if (axis == RowVector3d(0.0, 1.0, 0.0))
                out << "+Y";
            else if (axis == RowVector3d(-1.0, 0.0, 0.0))
                out << "-X";
            else if (axis == RowVector3d(0.0, -1.0, 0.0))
                out << "-Y";
            else if (axis == RowVector3d(0.0, 0.0, 1.0))
                out << "+Z";
            else if (axis == RowVector3d(0.0, 0.0, -1.0))
                out << "-Z";
            else
                out << "⦻";
        }

        out << "]";
        return out.str();
    }

    void setLimit(double upper=0.0, double lower=0.0, double effort=0.0, double velocity=0.0) {
        this->upper = upper; this->lower = lower; this->effort = effort; this->velocity = velocity;
    }

    void setOrigin(const RowVector3d& xyz, const RowVector3d& rpy) {
        origin.xyz = xyz;
        origin.rpy = rpy;
    }

    void setDampingAndFriction(double damping=0.0, double friction=0.0) {} //FIXME: If required
    std::string predecessor;
    std::string successor;
    joint_type type;
    RowVector3d axis;
    Pose origin;
    double upper, lower, effort, velocity;
    double damping, friction;
    int nq;
};  // }}}1
*/

Robot::Robot(const std::string& name): name(name), nq(0), joint_p(), joint_s(), body_l(), body_m(), _count(0) {}

void Robot::addJoint(const std::string& name, unsigned int id,
        const std::string& parent, const std::string& child, const std::string& type) {
    auto joint = std::make_shared<Joint>(name, id, parent, child, type);
    joints.push_back(joint);
    joint_dico[name] = id;
    joint_pc_dico[parent + ":" + child] = id;
    //auto parent_id = getBodyID(parent);
    //auto child_id = getBodyID(child);
    auto parent_id = link_dico[parent];
    auto child_id = link_dico[child];

    bodies[parent_id]->addChild(child_id);
    bodies[child_id]->addParent(parent_id);
    bodies[child_id]->addJoint(id);
    this->nq += joints[id]->nq;
    //std::cout << __FUNCTION__ << " dofs: " << this->nq << "\n";
}

void Robot::addBody(const std::string& name, unsigned int id){

    bodies.push_back(std::make_shared<Body>(name, id));
    link_dico[name] = id;
}

void Robot::dump(std::ostream & os) {
    os << "robot name is: " << name << " with " << nq << "dof\n";
    os << "root link: " << bodies[getRootBodyID()]->name << "\n";
    for (auto && body : bodies) {
        os << body->to_string() << "\n";
    }
    for (auto && joint : joints) {
        os << joint->to_string() << "\n";
    }
    // λμ 11 daniel. 12 alvaro, 1 carla y viernes con Yang 12
    os << "\n";
}

void Robot::print_featherstone(std::ostream & os) {
    auto n = joint_p.size();
    auto ii = Eigen::RowVectorXi::LinSpaced(n,0,n-1);
    Eigen::MatrixXi feather(4, n);
    //feather.row(0) = ii;

    Map<RowVectorXi> p_(joint_p.data(), n);
    Map<RowVectorXi> s_(joint_s.data(), n);

    feather << ii ,  p_ , s_, ii;

    feather.row(3) = feather.block(1,0, 2, n).colwise().minCoeff();
    os << feather<< std::endl;
    os << "p(i): ";
    for (auto && i : joint_p) {
        os << " " << i;
    }
    os << "\ns(i): ";
    for (auto && i : joint_s) {
        os << " " << i;
    }
    os << "\nλ(i): ";
    for (auto && i : body_l) {
        os << " " << i;
    }
    os << "\nlambda: " << this->j_tree_ids;
    os << "\nmap: " << this->tree_ids;
    os << "\n";
}
void Robot::print_tree(std::ostream & os, const std::string & prefix, int id, int parent_id) {
    static const std::string spc_pref{"   "};
    auto root = bodies[id];
    auto nb_children = root->child_links.size();
    os << prefix;
    if (parent_id >= 0) {
        auto pre = bodies[parent_id]->name;
        auto curr = root->name;
        auto j_id = this->joint_pc_dico[pre + ":" + curr];
        os << "[" << j_id << "|" << joints[j_id]->tree_id << "|" << joints[j_id]->name << "] ";
    }
    os << "/" << root->tree_id << "/" << root->name << "/" ;
    if (nb_children == 1) {
        os << " has " <<  nb_children << " child\n";
    } else if (nb_children > 1) {
        os << " has " << nb_children << " children\n";
    } else
        os << " \n";

    for (auto && child : root->child_links) {
        print_tree(os, spc_pref + prefix, child, id);
    }

}
void Robot::print_tree(std::ostream & os) {
    auto root_id = getRootBodyID();
    print_tree(os, "", root_id);
}

int Robot::getBodyID(const std::string& frame_name) const
{
    auto id = this->link_dico.find(frame_name);
    if (id == this->link_dico.end()) {
        return -1;
    } else {
        auto [key, value] = (*id);
        //std::cout << "Found at " << key << " wth id: " << value << std::endl;
        return value;
    }
}

int Robot::getJointID(const std::string& name) const
{
    auto id = this->joint_dico.find(name);
    if (id == this->joint_dico.end()) {
        return -1;
    } else {
        auto [key, value] = (*id);
        //std::cout << "Found at " << key << " with id: " << value << std::endl;
        return value;
    }
}

int Robot::getRootBodyID() const {
    auto root = -1;
    for (auto && body : bodies) {
        if (body->parent_links.size() == 0) {
            root = body->id;
            break;
        }
    }
    return root;
}

        void Robot::add_pre(std::shared_ptr<Body> body, std::shared_ptr<Body> body_p) {
          if (body_p->id != 0) body->Pre.insert(body->Pre.begin(), body_p->id-1);  // -1 for zero init and 0 for one init

          auto parent_v = body_p->parent_links;
          for (auto && parent_i : parent_v) {
              add_pre(body, bodies[parent_i]);
            }
        }

        void Robot::add_suc(std::shared_ptr<Body> body, std::shared_ptr<Body> body_p) {
          body->Suc.push_back(body_p->id-1);  // -1 for zero init and 0 for one init
          auto child_v = body_p->child_links;
          for (auto && child_i : child_v) {
              add_suc(body, bodies[child_i]);
            }
        }

        void Robot::build_PSvector( ) {
          for (auto && body = bodies.begin()+1; body != bodies.end(); body++) {
              add_pre(*body, *body);
              add_suc(*body, *body);
            }
        }

        void Robot::build_spatial_inertias( ) {
          for (auto && body = bodies.begin()+1; body != bodies.end(); body++) {
              if ((*body)->has_inertial) {
                  double mass = (*body)->inertial_properties.mass;
                  auto com = (*body)->inertial_properties.origin.xyz;
                  Matrix3d skew_com;
                  skew_com <<       0, -com(2),  com(1),
                               com(2),       0, -com(0),
                              -com(1),  com(0),       0;
                  (*body)->inertial_properties.spatial << mass*Matrix3d::Identity(),
                      -mass*skew_com,
                      mass*skew_com,
                      (*body)->inertial_properties.inertia.value - mass*skew_com*skew_com;
                }
            }
        }

bool Robot::load_from_urdf(const pugi::xml_node & node) {
    auto id = 0;
    for (pugi::xml_node link: node.children("link"))  // http://wiki.ros.org/urdf/XML/link
    {
        auto name =  link.attribute("name").value();
        this->addBody(name, id);
        auto inertial = link.child("inertial");
        if (!inertial.empty()) { // optional
            Inertial inertial_properties{inertial};
            bodies[id]->setInertialProperties(inertial_properties);  // modify body [id]
        }
        id++;
    }
    id = 0;
    for (pugi::xml_node joint: node.children("joint")) // http://wiki.ros.org/urdf/XML/joint
    {
        auto name =  joint.attribute("name").value();
        auto type =  joint.attribute("type").value();
        auto predecessor = joint.child("parent").attribute("link").value();
        auto successor = joint.child("child").attribute("link").value();
        this->addJoint(name, id, predecessor, successor, type);
        auto axis = joint.child("axis");
        if (!axis.empty()) joints[id]->axis = string_to_vector(axis.attribute("xyz").value()); //optional: defaults to (1,0,0
        auto origin = joint.child("origin");
        if (!origin.empty()) { // TODO: Should default to identity if it is not indicated in the URDF
            auto xyz = string_to_vector(origin.attribute("xyz").value());
            auto rpy = string_to_vector(origin.attribute("rpy").value());

            joints[id]->setOrigin(xyz, rpy);  // modify joint (id)
        }
        auto limit = joint.child("limit");
        if (!limit.empty()) { // required for revolute and prismatic
            auto upper = limit.attribute("upper").as_double(); // optional, 0.0
            auto lower = limit.attribute("lower").as_double(); // optional, defaults to 0
            auto effort = limit.attribute("effort").as_double();
            auto velocity = limit.attribute("velocity").as_double();

            joints[id]->setLimit(upper, lower, effort, velocity);
        }
        // TODO: Comment if no dynamics properties are considered.
        /*auto dynamics = joint.child("dynamics");
          if (!dynamics.empty()) {
          auto damping = dynamics.attribute("damping").as_double(); // optional, defaults to 0
          auto friction = dynamics.attribute("friction").as_double(); // optional, defaults to 0

          joints[id]->setDampingAndFriction(damping, friction);
          }*/
        id++;
    }

    this->build_graph();
    return true;
}

void Robot::visit_spanning_tree(int root_id) {
    auto root_name = bodies[root_id]->name;
    for (auto && i : bodies[root_id]->child_links){
        auto curr_name = bodies[i]->name;
        auto & joint_ref = joints[joint_pc_dico[root_name + ":" + curr_name]]->tree_id;
        auto & ref = bodies[i]->tree_id;
        if (ref == -1) {
            ref = ++_count;
            //if (joint_ref == -1)
            //    joint_ref = _count;
        }
        visit_spanning_tree(i);
    }
}

void Robot::build_graph() {
    auto root_id = getRootBodyID();
    if (root_id != 0) {
        std::cerr << "Warning: first body is not root.\n";
    }
    auto nb_joints = joints.size();
    auto nb_links = bodies.size();

    if (nb_joints > nb_links - 1) {
        std::cerr << "Warning: there maybe cycles (closed kinematic chains).\n"; // kinematic trees and closed-loop systems.
    }

    this->_count = 0;
    bodies[root_id]->tree_id = 0;
    visit_spanning_tree(root_id);

    //j_tree_ids.resize(nb_joints + 1);
    //j_tree_ids[0] = -1;
    j_tree_ids.setConstant(nb_joints + 1, -1);
    joint_p.resize(nb_joints);
    joint_s.resize(nb_joints);
    auto i = 0;
    for (auto && joint : joints) {
        joint_p[i] = link_dico[joint->predecessor];
        joint_s[i] = link_dico[joint->successor];
        i++;
    }

    tree_ids.resize(nb_links);
    for (auto && link : bodies) {
        tree_ids[link->tree_id] = link->id;
    }
    /*for (auto i = 1; i < nb_links; i++) {
      auto idx = bodies[tree_ids[i]]->parent_joints[0];
      j_tree_ids[i] = idx;
      }*/

    for (auto && joint : joints) {
        //std::cout << "j id: (" << joint->id << ";" << joint->tree_id << ")  ";
        //j_tree_ids[joint->tree_id] = joint->id;
    }
    //std::set<int> joint_list{};

    //std::cout << "Regular numbering:\n" << tree_ids << "\n";

    body_l.resize(nb_links);
    body_m.resize(nb_links); //TODO: validate
    tree_ids.resize(nb_links);
    i = 0;
    auto j=0;
    for (auto && link : bodies) {
        if (link->parent_links.size() == 0) {
            body_l[i] = -1;
        } else {
            body_l[i] = std::min(joint_p[j], joint_s[j]);
            j++;
        }
        //std::cout << i << " < λ(i)= " << body_l[i];
        i++;

    }

}

/**
 *
 *
 * https://github.com/ros/urdfdom/blob/master/xsd/urdf.xsd
 */
std::optional<std::shared_ptr<Robot>> Robot::build_model(const std::string& urdf_file)
{
    pugi::xml_document doc;
    pugi::xml_parse_result result = doc.load_file(urdf_file.c_str());
    if (!result)
        return {};
    auto node = doc.child("robot");
    auto name = node.attribute("name").value(); // http://wiki.ros.org/urdf/XML/robot
    auto robot = std::make_shared<Robot>(name);
    robot->load_from_urdf(node); // TODO: modify to send false for unknown joint types
    robot->build_PSvector( );
    robot->build_spatial_inertias( );
    return {robot};

}


//
//=======================================================================
// Copyright 2007 Stanford University
// Authors: David Gleich
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//
#ifndef BOOST_GRAPH_CORE_NUMBERS_HPP
#define BOOST_GRAPH_CORE_NUMBERS_HPP

#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/iterator/reverse_iterator.hpp>
#include <boost/concept/assert.hpp>

/*
 * core_numbers
 *
 * Requirement: IncidenceGraph
 */

// History
//
// 30 July 2007
// Added visitors to the implementation
//
// 8 February 2008
// Fixed headers and missing typename

namespace boost {

    // A linear time O(m) algorithm to compute the indegree core number
    // of a graph for unweighted graphs.
    //
    // and a O((n+m) log n) algorithm to compute the in-edge-weight core
    // numbers of a weighted graph.
    //
    // The linear algorithm comes from:
    // Vladimir Batagelj and Matjaz Zaversnik, "An O(m) Algorithm for Cores
    // Decomposition of Networks."  Sept. 1 2002.

    template <typename Visitor, typename Graph>
    struct CoreNumbersVisitorConcept {
        void constraints()
        {
            BOOST_CONCEPT_ASSERT(( CopyConstructibleConcept<Visitor> ));
            vis.examine_vertex(u,g);
            vis.finish_vertex(u,g);
            vis.examine_edge(e,g);
        }
        Visitor vis;
        Graph g;
        typename graph_traits<Graph>::vertex_descriptor u;
        typename graph_traits<Graph>::edge_descriptor e;
    };

    template <class Visitors = null_visitor>
    class core_numbers_visitor : public bfs_visitor<Visitors> {
        public:
        core_numbers_visitor() {}
        core_numbers_visitor(Visitors vis)
            : bfs_visitor<Visitors>(vis) {}

        private:
        template <class Vertex, class Graph>
        void initialize_vertex(Vertex, Graph&) {}

        template <class Vertex, class Graph>
        void discover_vertex(Vertex , Graph&) {}

        template <class Vertex, class Graph>
        void gray_target(Vertex, Graph&) {}

        template <class Vertex, class Graph>
        void black_target(Vertex, Graph&) {}

        template <class Edge, class Graph>
        void tree_edge(Edge, Graph&) {}

        template <class Edge, class Graph>
        void non_tree_edge(Edge, Graph&) {}
    };

    template <class Visitors>
    core_numbers_visitor<Visitors> make_core_numbers_visitor(Visitors vis)
    { return core_numbers_visitor<Visitors>(vis); }

    typedef core_numbers_visitor<> default_core_numbers_visitor;

    namespace detail {

        // implement a constant_property_map to simplify compute_in_degree
        // for the weighted and unweighted case
        // this is based on dummy property map
        template <typename ValueType>
        class constant_value_property_map
          : public boost::put_get_helper<ValueType,
              constant_value_property_map<ValueType>  >
        {
        public:
            typedef void key_type;
            typedef ValueType value_type;
            typedef const ValueType& reference;
            typedef boost::readable_property_map_tag category;
            inline constant_value_property_map(ValueType cc) : c(cc) { }
            inline constant_value_property_map(const constant_value_property_map<ValueType>& x)
              : c(x.c) { }
            template <class Vertex>
            inline reference operator[](Vertex) const { return c; }
        protected:
            ValueType c;
        };

        // the core numbers start as the indegree or inweight.  This function
        // will initialize these values
        template <typename Graph, typename CoreMap, typename EdgeWeightMap>
        void compute_in_degree_map(Graph& g, CoreMap d, EdgeWeightMap wm)
        {
            typename graph_traits<Graph>::vertex_iterator vi,vi_end;
            typename graph_traits<Graph>::out_edge_iterator ei,ei_end;
            for (boost::tie(vi,vi_end) = vertices(g); vi!=vi_end; ++vi) {
                put(d,*vi,0);
            }
            for (boost::tie(vi,vi_end) = vertices(g); vi!=vi_end; ++vi) {
                for (boost::tie(ei,ei_end) = out_edges(*vi,g); ei!=ei_end; ++ei) {
                    put(d,target(*ei,g),get(d,target(*ei,g))+get(wm,*ei));
                }
            }
        }

        // the version for weighted graphs is a little different
        template <typename Graph, typename CoreMap,
            typename EdgeWeightMap, typename MutableQueue,
            typename Visitor>
        typename property_traits<CoreMap>::value_type
        core_numbers_impl(Graph& g, CoreMap c, EdgeWeightMap wm,
            MutableQueue& Q, Visitor vis)
        {
            typename property_traits<CoreMap>::value_type v_cn = 0;
            typedef typename graph_traits<Graph>::vertex_descriptor vertex;
            while (!Q.empty())
            {
                // remove v from the Q, and then decrease the core numbers
                // of its successors
                vertex v = Q.top();
                vis.examine_vertex(v,g);
                Q.pop();
                v_cn = get(c,v);
                typename graph_traits<Graph>::out_edge_iterator oi,oi_end;
                for (boost::tie(oi,oi_end) = out_edges(v,g); oi!=oi_end; ++oi) {
                    vis.examine_edge(*oi,g);
                    vertex u = target(*oi,g);
                    // if c[u] > c[v], then u is still in the graph,
                    if (get(c,u) > v_cn) {
                        // remove the edge
                        put(c,u,get(c,u)-get(wm,*oi));
                        if (Q.contains(u))
                          Q.update(u);
                    }
                }
                vis.finish_vertex(v,g);
            }
            return (v_cn);
        }

        template <typename Graph, typename CoreMap, typename EdgeWeightMap,
            typename IndexMap, typename CoreNumVisitor>
        typename property_traits<CoreMap>::value_type
        core_numbers_dispatch(Graph&g, CoreMap c, EdgeWeightMap wm,
            IndexMap im, CoreNumVisitor vis)
        {
            typedef typename property_traits<CoreMap>::value_type D;
            typedef std::less<D> Cmp;
            // build the mutable queue
            typedef typename graph_traits<Graph>::vertex_descriptor vertex;
            std::vector<std::size_t> index_in_heap_data(num_vertices(g));
            typedef iterator_property_map<std::vector<std::size_t>::iterator, IndexMap>
              index_in_heap_map_type;
            index_in_heap_map_type index_in_heap_map(index_in_heap_data.begin(), im);
            typedef d_ary_heap_indirect<vertex, 4, index_in_heap_map_type, CoreMap, Cmp> MutableQueue;
            MutableQueue Q(c, index_in_heap_map, Cmp());
            typename graph_traits<Graph>::vertex_iterator vi,vi_end;
            for (boost::tie(vi,vi_end) = vertices(g); vi!=vi_end; ++vi) {
                Q.push(*vi);
            }
            return core_numbers_impl(g, c, wm, Q, vis);
        }

        // the version for the unweighted case
        // for this functions CoreMap must be initialized
        // with the in degree of each vertex
        template <typename Graph, typename CoreMap, typename PositionMap,
            typename Visitor>
        typename property_traits<CoreMap>::value_type
        core_numbers_impl(Graph& g, CoreMap c, PositionMap pos, Visitor vis)
        {
            typedef typename graph_traits<Graph>::vertices_size_type size_type;
            typedef typename graph_traits<Graph>::degree_size_type degree_type;
            typedef typename graph_traits<Graph>::vertex_descriptor vertex;
            typename graph_traits<Graph>::vertex_iterator vi,vi_end;

            // store the vertex core numbers
            typename property_traits<CoreMap>::value_type v_cn = 0;

            // compute the maximum degree (degrees are in the coremap)
            typename graph_traits<Graph>::degree_size_type max_deg = 0;
            for (boost::tie(vi,vi_end) = vertices(g); vi!=vi_end; ++vi) {
                max_deg = (std::max<typename graph_traits<Graph>::degree_size_type>)(max_deg, get(c,*vi));
            }

            // store the vertices in bins by their degree
            // allocate two extra locations to ease boundary cases
            std::vector<size_type> bin(max_deg+2);
            for (boost::tie(vi,vi_end) = vertices(g); vi!=vi_end; ++vi) {
                ++bin[get(c,*vi)];
            }

            // this loop sets bin[d] to the starting position of vertices
            // with degree d in the vert array for the bucket sort
            size_type cur_pos = 0;
            for (degree_type cur_deg = 0; cur_deg < max_deg+2; ++cur_deg) {
                degree_type tmp = bin[cur_deg];
                bin[cur_deg] = cur_pos;
                cur_pos += tmp;
            }

            // perform the bucket sort with pos and vert so that
            // pos[0] is the vertex of smallest degree
            std::vector<vertex> vert(num_vertices(g));
            for (boost::tie(vi,vi_end) = vertices(g); vi!=vi_end; ++vi) {
                vertex v=*vi;
                size_type p=bin[get(c,v)];
                put(pos,v,p);
                vert[p]=v;
                ++bin[get(c,v)];
            }
            // we ``abused'' bin while placing the vertices, now,
            // we need to restore it
            std::copy(boost::make_reverse_iterator(bin.end()-2),
                boost::make_reverse_iterator(bin.begin()),
                boost::make_reverse_iterator(bin.end()-1));
            // now simulate removing the vertices
            for (size_type i=0; i < num_vertices(g); ++i) {
                vertex v = vert[i];
                vis.examine_vertex(v,g);
                v_cn = get(c,v);
                typename graph_traits<Graph>::out_edge_iterator oi,oi_end;
                for (boost::tie(oi,oi_end) = out_edges(v,g); oi!=oi_end; ++oi) {
                    vis.examine_edge(*oi,g);
                    vertex u = target(*oi,g);
                    // if c[u] > c[v], then u is still in the graph,
                    if (get(c,u) > v_cn) {
                        degree_type deg_u = get(c,u);
                        degree_type pos_u = get(pos,u);
                        // w is the first vertex with the same degree as u
                        // (this is the resort operation!)
                        degree_type pos_w = bin[deg_u];
                        vertex w = vert[pos_w];
                        if (u!=v) {
                            // swap u and w
                            put(pos,u,pos_w);
                            put(pos,w,pos_u);
                            vert[pos_w] = u;
                            vert[pos_u] = w;
                        }
                        // now, the vertices array is sorted assuming
                        // we perform the following step
                        // start the set of vertices with degree of u
                        // one into the future (this now points at vertex
                        // w which we swapped with u).
                        ++bin[deg_u];
                        // we are removing v from the graph, so u's degree
                        // decreases
                        put(c,u,get(c,u)-1);
                    }
                }
                vis.finish_vertex(v,g);
            }
            return v_cn;
        }

    } // namespace detail

    // non-named parameter version for the unweighted case
    template <typename Graph, typename CoreMap, typename CoreNumVisitor>
    typename property_traits<CoreMap>::value_type
    core_numbers(Graph& g, CoreMap c, CoreNumVisitor vis)
    {
        typedef typename graph_traits<Graph>::vertices_size_type size_type;
        detail::compute_in_degree_map(g,c,
            detail::constant_value_property_map<
                typename property_traits<CoreMap>::value_type>(1) );
        return detail::core_numbers_impl(g,c,
            make_iterator_property_map(
                std::vector<size_type>(num_vertices(g)).begin(),get(vertex_index, g)),
            vis
        );
    }

    // non-named paramter version for the unweighted case
    template <typename Graph, typename CoreMap>
    typename property_traits<CoreMap>::value_type
    core_numbers(Graph& g, CoreMap c)
    {
        return core_numbers(g, c, make_core_numbers_visitor(null_visitor()));
    }

    // non-named parameter version for the weighted case
    template <typename Graph, typename CoreMap, typename EdgeWeightMap,
        typename VertexIndexMap, typename CoreNumVisitor>
    typename property_traits<CoreMap>::value_type
    core_numbers(Graph& g, CoreMap c, EdgeWeightMap wm, VertexIndexMap vim,
        CoreNumVisitor vis)
    {
        typedef typename graph_traits<Graph>::vertices_size_type size_type;
        detail::compute_in_degree_map(g,c,wm);
        return detail::core_numbers_dispatch(g,c,wm,vim,vis);
    }

    // non-named parameter version for the weighted case
//    template <typename Graph, typename CoreMap, typename EdgeWeightMap>
//    typename property_traits<CoreMap>::value_type
//    core_numbers(Graph& g, CoreMap c, EdgeWeightMap wm)
//    {
//        typedef typename graph_traits<Graph>::vertices_size_type size_type;
//        detail::compute_in_degree_map(g,c,wm);
//        return detail::core_numbers_dispatch(g,c,wm,get(vertex_index,g),
//            make_core_numbers_visitor(null_visitor()));
//    }

    template <typename Graph, typename CoreMap>
    typename property_traits<CoreMap>::value_type
    weighted_core_numbers(Graph& g, CoreMap c)
    {
        return weighted_core_numbers(
            g,c, make_core_numbers_visitor(null_visitor())
        );
    }

    template <typename Graph, typename CoreMap, typename CoreNumVisitor>
    typename property_traits<CoreMap>::value_type
    weighted_core_numbers(Graph& g, CoreMap c, CoreNumVisitor vis)
    { return core_numbers(g,c,get(edge_weight,g),get(vertex_index,g),vis); }

} // namespace boost

#endif // BOOST_GRAPH_CORE_NUMBERS_HPP


/*
Copyright 2018 Rui Miguel Horta Pimentel de Figueiredo

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

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

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

/*!
    \author Rui Figueiredo : ruipimentelfigueiredo
*/

#include <ctime>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
#include "sphere_fitting_hough.h"
#include "helpers.h"
#include <chrono>
using namespace std;
using namespace std::chrono;

int main (int argc, char** argv)
{
	/* TODO - Process options */
	if (argc < 6)
	{
		std::cout << "invalid number of arguments: dataset_dir heights radii noise clutter"<< std::endl;
		exit(-1);
	}

	std::string dataset_dir = std::string(argv[1]);
	std::cout << "dataset_dir: " << dataset_dir << std::endl;
	std::string ground_truth_dir=dataset_dir+"ground_truth/";
	std::string point_clouds_dir=dataset_dir+"point_clouds/";
	std::string output_dir=dataset_dir+"results/";

	// HOUGH PARAMETERS
	unsigned int radius_bins=atoi(argv[2]);
	std::cout << "radius_bins: " << radius_bins<< std::endl;

	unsigned int position_bins=atoi(argv[3]);
	std::cout << "position_bins: " << position_bins<< std::endl;

	// OUR HOUGH IMPLEMENTATION PARAMETERS
	unsigned int orientation_accumulators_num=atoi(argv[4]);
	std::cout << "orientation_accumulators_num: " << orientation_accumulators_num<< std::endl;

	unsigned int gaussian_sphere_points_num=atoi(argv[5]);
	std::cout << "gaussian_sphere_points_num: " << gaussian_sphere_points_num<< std::endl;

	float accumulator_peak_threshold=atof(argv[6]);
	std::cout << "accumulator_peak_threshold: " << accumulator_peak_threshold<< std::endl;

	float min_radius=atof(argv[7]);
	std::cout << "min_radius: " << min_radius<< std::endl;

	float max_radius=atof(argv[8]);
	std::cout << "max_radius: " << max_radius<< std::endl;

	// RANSAC PARAMETERS
	float normal_distance_weight=atof(argv[9]);
	std::cout << "normal_distance_weight: " << normal_distance_weight<< std::endl;

	float distance_threshold=atof(argv[10]);
	std::cout << "distance_threshold: " << distance_threshold << std::endl;

	bool do_refine=atoi(argv[11]);
	std::cout << "do_refine: " << do_refine << std::endl;

	bool visualize=atoi(argv[12]);
	std::cout << "visualize: " << visualize << std::endl;

	boost::shared_ptr<VisualizeFittingData> visualizer;

	if(visualize)
		visualizer=boost::shared_ptr<VisualizeFittingData>(new VisualizeFittingData());

	// Gaussian Sphere Uniform
	std::vector<double> weights;
	std::vector<Eigen::Matrix<double, 3 ,1> > means;
	std::vector<Eigen::Matrix<double, 3 ,1> > std_devs;
	weights.push_back(1.0);
	Eigen::Matrix<double, 3 ,1> mean_eigen(0,0,0);
	means.push_back(mean_eigen);
	Eigen::Matrix<double, 3 ,1> std_dev_eigen(0.5,0.5,0.5);
	std_devs.push_back(std_dev_eigen);

	GaussianMixtureModel gmm(weights, means, std_devs);
	GaussianSphere gaussian_sphere(gmm,gaussian_sphere_points_num,orientation_accumulators_num);

	// Gaussian Sphere Top Biased
	std::vector<double> weights_biased;
	std::vector<Eigen::Matrix<double, 3 ,1> > means_biased;
	std::vector<Eigen::Matrix<double, 3 ,1> > std_devs_biased;
	weights_biased.push_back(1.0);
	Eigen::Matrix<double, 3 ,1> mean_eigen_biased(0,0,1.0);
	means_biased.push_back(mean_eigen_biased);
	Eigen::Matrix<double, 3 ,1> std_dev_eigen_biased(0.1,0.1,0.1);
	std_devs_biased.push_back(std_dev_eigen_biased);

	GaussianMixtureModel gmm_biased(weights_biased, means_biased, std_devs_biased);
	GaussianSphere gaussian_sphere_biased(gmm_biased,gaussian_sphere_points_num,orientation_accumulators_num);

	std::vector<boost::shared_ptr<SphereFittingHough> > sphere_segmentators;

	// HOUGH NORMAL
	sphere_segmentators.push_back(boost::shared_ptr<SphereFittingHough> (new SphereFittingHough(gaussian_sphere,(unsigned int)position_bins,(unsigned int)radius_bins,(float)min_radius, (float)max_radius,(float)accumulator_peak_threshold, false, false)));

	// HOUGH NORMAL (Soft-Voting)
	sphere_segmentators.push_back(boost::shared_ptr<SphereFittingHough> (new SphereFittingHough(gaussian_sphere,(unsigned int)position_bins,(unsigned int)radius_bins,(float)min_radius, (float)max_radius,(float)accumulator_peak_threshold, false, true)));

	// HOUGH HYBRID BIASED
	//sphere_segmentators.push_back(boost::shared_ptr<SphereFittingHough> (new SphereFittingHough(gaussian_sphere_biased,(unsigned int)position_bins,(unsigned int)radius_bins,(float)min_radius, (float)max_radius,(float)accumulator_peak_threshold,false, false)));

	// HOUGH HYBRID BIASED (Soft-Voting)
	//sphere_segmentators.push_back(boost::shared_ptr<SphereFittingHough> (new SphereFittingHough(gaussian_sphere_biased,(unsigned int)position_bins,(unsigned int)radius_bins,(float)min_radius, (float)max_radius,(float)accumulator_peak_threshold,false, true)));

	// Get ground truths
	GroundTruth ground_truths(ground_truth_dir);
	std::cout << "Number of ground truths:" << ground_truths.ground_truth.size() << std::endl;

	// Get point clouds
	PointClouds point_clouds(point_clouds_dir);
	std::cout << "Number of point_clouds:" << point_clouds.point_clouds.size() << std::endl;

	std::string detections_frame_id="world";
	std::string marker_detections_namespace_="detections";
	// Segment cylinder and store results
	std::vector<Eigen::VectorXf > detections;
	std::vector<float> position_errors;

	createDirectory(output_dir);

	for (unsigned int d=0;d < sphere_segmentators.size();++d)
	{
		std::fstream fs_radius;
		std::fstream fs_position;
		std::fstream fs_time;

		fs_radius.open (output_dir+"radius_noise_"           + std::to_string(d)+".txt", std::fstream::in | std::fstream::out | std::fstream::app);
		fs_position.open (output_dir+"position_noise_"       + std::to_string(d)+".txt", std::fstream::in | std::fstream::out | std::fstream::app);
		fs_time.open (output_dir+"time_noise_"               + std::to_string(d)+".txt", std::fstream::in | std::fstream::out | std::fstream::app);

		for(unsigned int i=0;i<point_clouds.point_clouds.size();++i)
		{
			// Ground truth iterations
			unsigned int iteration=i % ground_truths.ground_truth.size();

			Cylinder ground_truth=ground_truths.ground_truth[iteration];

			pcl::PointCloud<PointT>::Ptr point_cloud(new pcl::PointCloud<PointT>());
			*point_cloud=*point_clouds.point_clouds[i];

			/* FIT */
    			high_resolution_clock::time_point t1 = high_resolution_clock::now();
			FittingData model_params=sphere_segmentators[d]->fit(point_clouds.point_clouds[i]);
    			high_resolution_clock::time_point t2 = high_resolution_clock::now();
			/* END FIT */

			/* STORE RESULTS */
			float radius_error=fabs(model_params.parameters[3]-ground_truth.radius);
			fs_radius << radius_error << " " << "\n";

			float position_error=(Eigen::Vector3f(model_params.parameters[0],model_params.parameters[1],model_params.parameters[2])-Eigen::Vector3f(ground_truth.position[0], ground_truth.position[1],ground_truth.position[2])).norm();
			fs_position << position_error << " " << "\n";

    			auto duration = duration_cast<milliseconds>( t2 - t1 ).count();
			fs_time << duration << " " << "\n";
			/* END STORE RESULTS */

			/* VISUALIZE */
			model_params.visualize(point_cloud,visualizer);
			/* END VISUALIZE */

			std::cout << "iteration " << i << " of " << point_clouds.point_clouds.size() << " total time: " << duration << " ms"<<  std::endl;
		}

		fs_radius.close();
		fs_position.close();
		fs_time.close();
	}

	return (0);
}