#include <iostream>
#include <memory>
#include <string>
#include <vector>
#include <unordered_map>
#include <functional>
#include <Eigen/Dense>
#include "../include/sample_network.h"
#include "../include/numerical_gradient.h"

namespace MyDL{

    using namespace Eigen;
    using std::shared_ptr;
    using std::make_shared;
    using std::string;
    using std::vector;
    using std::unordered_map;
    using std::cout;
    using std::endl;

    TwoLayerNet::TwoLayerNet(int input_size, int hidden_size, int output_size, double weight_init_std)
    {
        // 内部保持パラメータ
        _input_size = input_size;
        _hidden_size = hidden_size;
        _output_size = output_size;
        _weight_init_std = weight_init_std;

        // Affine Layer用のパラメータ -> スマートポインタで保持し、それをパラメータのリストに格納
        auto W1 = make_shared<MatrixXd>(input_size, hidden_size);
        auto W2 = make_shared<MatrixXd>(hidden_size, output_size);
        auto b1 = make_shared<MatrixXd>(1, hidden_size);
        auto b2 = make_shared<MatrixXd>(1, output_size);

        *W1 = weight_init_std * MatrixXd::Random(input_size, hidden_size);
        *W2 = weight_init_std * MatrixXd::Random(hidden_size, output_size);
        *b1 = MatrixXd::Zero(1, hidden_size);
        *b2 = MatrixXd::Zero(1, output_size);

        // Layer作成 -> スマートポインタで実装(コンストラクタを抜けた時に、実体が消されないようにするため)
        // 左辺の型はautoにしてはいけない(BaseLayerで統一し、コンテナに格納する ※ポリモーフィズムの実現)
        shared_ptr<BaseLayer> affine1    = make_shared<MyDL::Affine>(W1, b1);
        shared_ptr<BaseLayer> affine2    = make_shared<MyDL::Affine>(W2, b2);
        shared_ptr<BaseLayer> relu1      = make_shared<ReLU>();
        shared_ptr<BaseLayer> last_layer = make_shared<SoftmaxWithLoss>(); // predictではaffine2の出力、lossではlastlayerの出力を使うので分ける

        // 生のポインタを格納すると実体がスコープ外となり解放されてしまうので、shared_ptrで対処
        _layers["Affine1"] = affine1;
        _layers["ReLU1"] = relu1;
        _layers["Affine2"] = affine2;
        _last_layer = last_layer;

        // 各パラメータへのポインタを格納
        if (auto cast_affine1 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine1"]))
        {
            params["W1"] = cast_affine1->pW;
            params["b1"] = cast_affine1->pb;
        }
        if (auto cast_affine2 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine2"]))
        {
            params["W2"] = cast_affine2->pW;
            params["b2"] = cast_affine2->pb;
        }

        // unordered_mapでは追加順が保存されないので、別途順番通り名称を格納したコンテナを用意
        _layer_list.push_back("Affine1");
        _layer_list.push_back("BatchNorm"); // for batchnorm debug 21/03/21追加
        _layer_list.push_back("ReLU1");
        _layer_list.push_back("Affine2");

        // ------------------------------------------------
        // for batchnorm debug 21/03/21追加
        // ------------------------------------------------
        auto gamma = make_shared<MatrixXd>(1, hidden_size);
        auto beta = make_shared<MatrixXd>(1, hidden_size);
        *gamma = weight_init_std * MatrixXd::Random(1, hidden_size);
        *beta = MatrixXd::Zero(1, hidden_size);
        shared_ptr<BaseLayer> batch_norm = make_shared<BatchNorm>(gamma, beta);
        _layers["BatchNorm"] = batch_norm;
        if (auto cast_batchnorm = std::dynamic_pointer_cast<BatchNorm>(_layers["BatchNorm"]))
        {
            params["gamma"] = cast_batchnorm->pgamma;
            params["beta"]  = cast_batchnorm->pbeta;
        }
    }

    vector<MatrixXd> TwoLayerNet::predict(vector<MatrixXd> inputs)
    {
        // inputのバリデーションをしておくか？
        vector<MatrixXd> X = inputs;// 入力もvectorなので、そのまま受ければOK
        vector<MatrixXd> tmp_X;

        // mapのrange-forは内部的にstd::pairが返される
        for(auto layer : _layer_list)
        {
            // cout << layer << endl;
            tmp_X = _layers[layer]->forward(X);
            X.swap(tmp_X); // 中身入れ替え
        }
        return X;
    }

    vector<MatrixXd> TwoLayerNet::loss(vector<MatrixXd> inputs)
    {
        vector<MatrixXd> pred_input, pred_out, loss_inputs, loss_output;
        pred_input.push_back(inputs[0]);
        pred_out = predict(pred_input);

        loss_inputs = inputs;
        loss_inputs[0] = pred_out[0];

        loss_output = _last_layer->forward(loss_inputs);
        return loss_output;
    }

    double TwoLayerNet::accuracy(vector<MatrixXd> inputs)
    {
        vector<MatrixXd> pred_input, pred_out;
        pred_input.push_back(inputs[0]);
        pred_out = predict(pred_input);

        MatrixXd y, t;
        y = pred_out[0];
        t = inputs[1];
        double batch_size = t.rows();
        double accuracy = 0;

        MatrixXd::Index y_row, y_col, t_row, t_col;
        for (int i = 0; i < batch_size; i++)
        {
            y.row(i).maxCoeff(&y_row, &y_col);
            t.row(i).maxCoeff(&t_row, &t_col);

            accuracy += (double)(y_col == t_col);
        }

        return accuracy / batch_size;
    }

    unordered_map<string, MatrixXd> TwoLayerNet::gradient(vector<MatrixXd> inputs)
    {
        // Forward
        vector<MatrixXd> output;
        output = loss(inputs); // forward -> 逆伝播計算に必要な情報を各レイヤにキャッシュ

        // Backward
        vector<MatrixXd> dout, tmp_dout;
        dout.push_back(MatrixXd::Ones(1,1));

        dout = _last_layer->backward(dout);

        // 逆順ループ → Boostライブラリのboost::adaptors::reverse()を使う方がEasyではある
        for (auto it = _layer_list.rbegin(); it != _layer_list.rend(); it++)
        {
            string layer = *it;
            tmp_dout = _layers[layer]->backward(dout);
            dout.swap(tmp_dout);
        }

        unordered_map<string, MatrixXd> grads;
        // _layersに格納している変数はBaseLayerにアップキャストしているので、ダウンキャストが必要 -> nullptrのときは実行しないようにする
        if(shared_ptr<MyDL::Affine> affine1 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine1"]))
        {
            grads["W1"] = affine1->dW;
            grads["b1"] = affine1->db;
        }
        if(shared_ptr<MyDL::Affine> affine2 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine2"]))
        {
            grads["W2"] = affine2->dW;
            grads["b2"] = affine2->db;
        }

        // for batchnorm debug 21/03/21追加
        if(auto batchnorm = std::dynamic_pointer_cast<BatchNorm>(_layers["BatchNorm"]))
        {
            grads["gamma"] = batchnorm->dgamma;
            grads["beta"]  = batchnorm->dbeta;
        }

        return grads;
    }

    unordered_map<string, MatrixXd> TwoLayerNet::numerical_gradient(vector<MatrixXd> inputs)
    {
        // [&]は、スコープ外の変数を参照するというキャプチャー(ここではthisポインタを使うために指定)
        std::function<vector<MatrixXd>(MatrixXd)> loss_W = [this, &inputs](MatrixXd W) -> vector<MatrixXd> { return this->loss(inputs); };
        std::function<vector<MatrixXd>(VectorXd)> loss_W2 = [this, &inputs](VectorXd W) -> vector<MatrixXd> { return this->loss(inputs); };
        unordered_map<string, MatrixXd> grads;

        MatrixXd dW1, dW2, db1, db2;

        // 直接内部のレイヤのパラメータにアクセスするので、ダウンキャストが必要
        if (shared_ptr<MyDL::Affine> affine1 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine1"]))
        {
            dW1 = MyDL::numerical_gradient(loss_W, affine1->_W);
            db1 = MyDL::numerical_gradient(loss_W2, affine1->_b);
        }
        if (shared_ptr<MyDL::Affine> affine2 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine2"]))
        {
            dW2 = MyDL::numerical_gradient(loss_W, affine2->_W);
            db2 = MyDL::numerical_gradient(loss_W2, affine2->_b);
        }

        grads["dW1"] = dW1;
        grads["dW2"] = dW2;
        grads["db1"] = db1;
        grads["db2"] = db2;

        return grads;
    }

    // -----------------------------------------------------------
    // MultiLayerNet: Weight Decay の検証用
    // 【21/04/06】
    // とりあえず動作することを目指すので、最初はreluで構成
    // 後ほどsigmoid含めて動くように変更。初期値も XavierとHeの両方を選択できるようにする。
    // -----------------------------------------------------------

    MultiLayerNet::MultiLayerNet(const int input_size,
                                 const vector<int> hidden_size,
                                 const int output_size,
                                 const double weight_decay_lambda)
    {
        _input_size = input_size;
        _hidden_size_list = hidden_size;
        _output_size = output_size;
        _weight_decay_lambda = weight_decay_lambda;

        // 各パラメータの初期化

        _layers["Affine1"] = make_shared<MyDL::Affine>(_input_size, _hidden_size_list[0]);
        _layers["ReLU1"] = make_shared<ReLU>();
        _layer_list.push_back("Affine1");
        _layer_list.push_back("ReLU1");
        for (int i=0; i<_hidden_size_list.size()-1; i++)
        {
            string tmp_num_str = std::to_string(i+2);
            _layers["Affine" + tmp_num_str] = make_shared<MyDL::Affine>(_hidden_size_list[i], _hidden_size_list[i + 1]);
            _layers["ReLU" + tmp_num_str] = make_shared<ReLU>();
            _layer_list.push_back("Affine" + tmp_num_str);
            _layer_list.push_back("ReLU" + tmp_num_str);
        }
        string tmp_num_str = std::to_string(_hidden_size_list.size()+1);
        _layers["Affine" + tmp_num_str] = make_shared<MyDL::Affine>(_hidden_size_list[_hidden_size_list.size() - 1], _output_size);
        _layers["ReLU" + tmp_num_str] = make_shared<ReLU>();
        _layer_list.push_back("Affine" + tmp_num_str);
        _layer_list.push_back("ReLU" + tmp_num_str);

        _last_layer = make_shared<SoftmaxWithLoss>(); // Loss Layer

        for (int layer_num = 1; layer_num <= _hidden_size_list.size()+1; layer_num++)
        {
            if (auto cast_affine = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine" + std::to_string(layer_num)]))
            {
                params["W" + std::to_string(layer_num)] = cast_affine->pW;
                params["b" + std::to_string(layer_num)] = cast_affine->pb;
            }
        }

    }

    vector<MatrixXd> MultiLayerNet::predict(vector<MatrixXd> inputs)
    {
        vector<MatrixXd> X = inputs;
        vector<MatrixXd> tmp_X;

        for (auto layer : _layer_list)
        {
            tmp_X = _layers[layer]->forward(X);
            X.swap(tmp_X);
        }
        return X;
    }

    vector<MatrixXd> MultiLayerNet::loss(vector<MatrixXd> inputs, MatrixXd& t)
    {
        vector<MatrixXd> pred_input, pred_out, loss_inputs, loss_output;
        pred_input.push_back(inputs[0]);
        pred_out = predict(pred_input);

        loss_inputs.push_back(pred_out[0]);
        loss_inputs.push_back(t);

        loss_output = _last_layer->forward(loss_inputs);

        // あとは Weight Decay の項も計算してLossに加える
        double weight_decay = 0;
        for (auto param: params)
        {
            weight_decay += 0.5 * _weight_decay_lambda * (*(param.second)).sum();
        }

        loss_output[0](0) = loss_output[0](0) + weight_decay;

        return loss_output;
    }

    double MultiLayerNet::accuracy(vector<MatrixXd> inputs, MatrixXd& t)
    {
        vector<MatrixXd> pred_out;
        pred_out = predict(inputs);

        MatrixXd y;
        y = pred_out[0];
        double batch_size = t.rows();
        double accuracy = 0;

        MatrixXd::Index y_row, y_col, t_row, t_col;
        for (int i = 0; i < batch_size; i++)
        {
            y.row(i).maxCoeff(&y_row, &y_col);
            t.row(i).maxCoeff(&t_row, &t_col);

            accuracy += (double)(y_col == t_col);
        }

        return accuracy / batch_size;
    }

    unordered_map<string, MatrixXd> MultiLayerNet::gradient(vector<MatrixXd> inputs, MatrixXd& t)
    {
        // Forward
        vector<MatrixXd> output;
        output = loss(inputs, t);

        // Backward
        vector<MatrixXd> dout, tmp_dout;
        dout.push_back(MatrixXd::Ones(1,1));

        dout = _last_layer->backward(dout);

        for (auto it = _layer_list.rbegin(); it != _layer_list.rend(); it++)
        {
            string layer = *it;
            tmp_dout = _layers[layer]->backward(dout);
            dout.swap(tmp_dout);
        }

        unordered_map<string, MatrixXd> grads;

        for (int i = 1; i <= _hidden_size_list.size()+1; i++)
        {
            if(auto tmp_affine = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine" + std::to_string(i)]))
            {
                grads["W" + std::to_string(i)] = tmp_affine->dW + _weight_decay_lambda * (*(tmp_affine->pW));
                grads["b" + std::to_string(i)] = tmp_affine->db;
            }
        }

        return grads;
    }

    // -----------------------------------------------------------
    // AffineLayer デバッグ用クラス → 参照先の更新確認
    // -----------------------------------------------------------
    DebugAffine::DebugAffine(int input_size, int output_size, double weight_init_std)
    {
        _input_size = input_size;
        _output_size = output_size;

        MatrixXd W = weight_init_std * MatrixXd::Random(input_size, output_size);
        MatrixXd b = MatrixXd::Zero(1, output_size);

        shared_ptr<BaseLayer> affine = std::make_shared<MyDL::Affine>(W, b);
        _layers["Affine"] = affine;

        if(shared_ptr<MyDL::Affine> tmp_affine = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine"]))
        {
            params["W"] = tmp_affine->_W; // ここではコピーコンストラクタが走る
            params["b"] = tmp_affine->_b;
        }
    }

    void DebugAffine::PrintLayerParams(void)
    {
        if (shared_ptr<MyDL::Affine> tmp_affine = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine"]))
        {
            cout << "--- parameter W ---" << endl;
            cout << tmp_affine->_W << endl;
            cout << "--- parameter b ---" << endl;
            cout << tmp_affine->_b << endl;
        }
    }

}


#include <utils.h>
#include <cmath>
#include <vector>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <boost/lexical_cast.hpp>
#include <iomanip>
#include <ros/package.h>

#define DEG2RAD 0.01745329
palette GetPalette(palette::palettetypes pal)
{
  palette ret;

  int i, r, g, b;
  float f;

  switch (pal)
  {
    case palette::Linear_red_palettes:
      /*
       * Linear red palettes.
       */
      for (i = 0; i < 256; i++)
      {
        ret.colors[i].rgbBlue = 0;
        ret.colors[i].rgbGreen = 0;
        ret.colors[i].rgbRed = i;
      }
      break;
    case palette::GammaLog_red_palettes:
      /*
       * GammaLog red palettes.
       */
      for (i = 0; i < 256; i++)
      {
        f = log10(pow((i / 255.0), 1.0) * 9.0 + 1.0) * 255.0;
        ret.colors[i].rgbBlue = 0;
        ret.colors[i].rgbGreen = 0;
        ret.colors[i].rgbRed = f;
      }
      break;
    case palette::Inversion_red_palette:
      /*
       * Inversion red palette.
       */
      for (i = 0; i < 256; i++)
      {
        ret.colors[i].rgbBlue = 0;
        ret.colors[i].rgbGreen = 0;
        ret.colors[i].rgbRed = 255 - i;
      }
      break;
    case palette::Linear_palettes:
      /*
       * Linear palettes.
       */
      for (i = 0; i < 256; i++)
      {
        ret.colors[i].rgbBlue = ret.colors[i].rgbGreen = ret.colors[i].rgbRed = i;
      }
      break;
    case palette::GammaLog_palettes:
      /*
       * GammaLog palettes.
       */
      for (i = 0; i < 256; i++)
      {
        f = log10(pow((i / 255.0), 1.0) * 9.0 + 1.0) * 255.0;
        ret.colors[i].rgbBlue = ret.colors[i].rgbGreen = ret.colors[i].rgbRed = f;
      }
      break;
    case palette::Inversion_palette:
      /*
       * Inversion palette.
       */
      for (i = 0; i < 256; i++)
      {
        ret.colors[i].rgbBlue = ret.colors[i].rgbGreen = ret.colors[i].rgbRed = 255 - i;
      }
      break;
    case palette::False_color_palette1:
      /*
       * False color palette #1.
       */
      for (i = 0; i < 256; i++)
      {
        r = (sin((i / 255.0 * 360.0 - 120.0 > 0 ? i / 255.0 * 360.0 - 120.0 : 0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        g = (sin((i / 255.0 * 360.0 + 60.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        b = (sin((i / 255.0 * 360.0 + 140.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        ret.colors[i].rgbBlue = b;
        ret.colors[i].rgbGreen = g;
        ret.colors[i].rgbRed = r;
      }
      break;
    case palette::False_color_palette2:
      /*
       * False color palette #2.
       */
      for (i = 0; i < 256; i++)
      {
        r = (sin((i / 255.0 * 360.0 + 120.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        g = (sin((i / 255.0 * 360.0 + 240.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        b = (sin((i / 255.0 * 360.0 + 0.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        ret.colors[i].rgbBlue = b;
        ret.colors[i].rgbGreen = g;
        ret.colors[i].rgbRed = r;
      }
      break;
    case palette::False_color_palette3:
      /*
       * False color palette #3.
       */
      for (i = 0; i < 256; i++)
      {
        r = (sin((i / 255.0 * 360.0 + 240.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        g = (sin((i / 255.0 * 360.0 + 0.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        b = (sin((i / 255.0 * 360.0 + 120.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        ret.colors[i].rgbBlue = b;
        ret.colors[i].rgbGreen = g;
        ret.colors[i].rgbRed = r;
      }
      break;

    case palette::False_color_palette4:
      /*
       * False color palette #4. Matlab JET
       */

      enum
      {
        nsep = 64, nvals = 192, n = 256
      };

      std::vector<double> vals;
      vals.resize(nvals, 0);

      int idx = 0;
      for (int i = 0; i < nsep; ++i)
      {
        vals.at(idx++) = (i / (double)nsep);
      }

      for (int i = 0; i < nsep; ++i){
        vals.at(idx + i) = 1.;
      }

      idx += nsep;
      for (int i = nsep - 1; i >= 0; --i)
      {
        vals.at(idx++) = i / (double)nsep;
      }

      std::vector<int> r;
      r.resize(nvals);
      std::vector<int> g;
      g.resize(nvals);
      std::vector<int> b;
      b.resize(nvals);
      for (std::size_t i = 0; i < nvals; ++i)
      {
        g.at(i) = ceil(nsep / 2) - 1 + i;
        r.at(i) = g.at(i) + nsep;
        b.at(i) = g.at(i) - nsep;
      }

      int idxr = 0;
      int idxg = 0;

      for (int i = 0; i < nvals; ++i)
      {
        if (r.at(i) >= 0 && r.at(i) < n)
          ret.colors[r.at(i)].rgbRed = vals.at(idxr++) * 255.;

        if (g.at(i) >= 0 && g.at(i) < n)
          ret.colors[g.at(i)].rgbGreen = vals.at(idxg++) * 255.;
      }

      int idxb = 0;
      int cntblue = 0;
      for (int i = 0; i < nvals; ++i)
      {
        if (b.at(i) >= 0 && b.at(i) < n)
          cntblue++;
      }

      for (int i = 0; i < nvals; ++i)
      {
        if (b.at(i) >= 0 && b.at(i) < n)
          ret.colors[b.at(i)].rgbBlue = vals.at(nvals - 1 - cntblue + idxb++) * 255.;
      }
      break;
  }
  return ret;
}

palette GetPalette(const std::string &pal_choice)
{
  palette ret;

  int i, r, g, b;
  float f;

  if(pal_choice == "Linear_red_palettes")
  {
    /*
      * Linear red palettes.
      */
    for (i = 0; i < 256; i++)
    {
      ret.colors[i].rgbBlue = 0;
      ret.colors[i].rgbGreen = 0;
      ret.colors[i].rgbRed = i;
    }
  }
  else if(pal_choice == "GammaLog_red_palettes")
  {
    /*
      * GammaLog red palettes.
      */
    for (i = 0; i < 256; i++)
    {
      f = log10(pow((i / 255.0), 1.0) * 9.0 + 1.0) * 255.0;
      ret.colors[i].rgbBlue = 0;
      ret.colors[i].rgbGreen = 0;
      ret.colors[i].rgbRed = f;
    }
  }
  else if(pal_choice == "Inversion_red_palette")
  {
    /*
      * Inversion red palette.
      */
    for (i = 0; i < 256; i++)
    {
      ret.colors[i].rgbBlue = 0;
      ret.colors[i].rgbGreen = 0;
      ret.colors[i].rgbRed = 255 - i;
    }
  }
  else if(pal_choice == "Linear_palettes")
  {
    /*
      * Linear palettes.
      */
    for (i = 0; i < 256; i++)
    {
      ret.colors[i].rgbBlue = ret.colors[i].rgbGreen = ret.colors[i].rgbRed = i;
    }
  }
  else if(pal_choice == "GammaLog_palettes")
  {
    /*
      * GammaLog palettes.
      */
    for (i = 0; i < 256; i++)
    {
      f = log10(pow((i / 255.0), 1.0) * 9.0 + 1.0) * 255.0;
      ret.colors[i].rgbBlue = ret.colors[i].rgbGreen = ret.colors[i].rgbRed = f;
    }
  }
  else if(pal_choice == "Inversion_palette")
  {
    /*
      * Inversion palette.
      */
    for (i = 0; i < 256; i++)
    {
      ret.colors[i].rgbBlue = ret.colors[i].rgbGreen = ret.colors[i].rgbRed = 255 - i;
    }
  }
  else if(pal_choice == "False_color_palette1")
  {
    /*
      * False color palette #1.
      */
    for (i = 0; i < 256; i++)
    {
      r = (sin((i / 255.0 * 360.0 - 120.0 > 0 ? i / 255.0 * 360.0 - 120.0 : 0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      g = (sin((i / 255.0 * 360.0 + 60.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      b = (sin((i / 255.0 * 360.0 + 140.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      ret.colors[i].rgbBlue = b;
      ret.colors[i].rgbGreen = g;
      ret.colors[i].rgbRed = r;
    }
  }
  else if(pal_choice == "False_color_palette2")
  {
    /*
      * False color palette #2.
      */
    for (i = 0; i < 256; i++)
    {
      r = (sin((i / 255.0 * 360.0 + 120.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      g = (sin((i / 255.0 * 360.0 + 240.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      b = (sin((i / 255.0 * 360.0 + 0.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      ret.colors[i].rgbBlue = b;
      ret.colors[i].rgbGreen = g;
      ret.colors[i].rgbRed = r;
    }
  }
  else if(pal_choice == "False_color_palette3")
  {
    /*
      * False color palette #3.
      */
    for (i = 0; i < 256; i++)
    {
      r = (sin((i / 255.0 * 360.0 + 240.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      g = (sin((i / 255.0 * 360.0 + 0.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      b = (sin((i / 255.0 * 360.0 + 120.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      ret.colors[i].rgbBlue = b;
      ret.colors[i].rgbGreen = g;
      ret.colors[i].rgbRed = r;
    }
  }
  else if(pal_choice == "False_color_palette4")
  {
      /*
       * False color palette #4. Matlab JET
       */

      enum
      {
        nsep = 64, nvals = 192, n = 256
      };

      std::vector<double> vals;
      vals.resize(nvals, 0);

      int idx = 0;
      for (int i = 0; i < nsep; ++i)
      {
        vals.at(idx++) = (i / (double)nsep);
      }

      for (int i = 0; i < nsep; ++i){
        vals.at(idx + i) = 1.;
      }

      idx += nsep;
      for (int i = nsep - 1; i >= 0; --i)
      {
        vals.at(idx++) = i / (double)nsep;
      }

      std::vector<int> r;
      r.resize(nvals);
      std::vector<int> g;
      g.resize(nvals);
      std::vector<int> b;
      b.resize(nvals);
      for (std::size_t i = 0; i < nvals; ++i)
      {
        g.at(i) = ceil(nsep / 2) - 1 + i;
        r.at(i) = g.at(i) + nsep;
        b.at(i) = g.at(i) - nsep;
      }

      int idxr = 0;
      int idxg = 0;

      for (int i = 0; i < nvals; ++i)
      {
        if (r.at(i) >= 0 && r.at(i) < n)
          ret.colors[r.at(i)].rgbRed = vals.at(idxr++) * 255.;

        if (g.at(i) >= 0 && g.at(i) < n)
          ret.colors[g.at(i)].rgbGreen = vals.at(idxg++) * 255.;
      }

      int idxb = 0;
      int cntblue = 0;
      for (int i = 0; i < nvals; ++i)
      {
        if (b.at(i) >= 0 && b.at(i) < n)
          cntblue++;
      }

      for (int i = 0; i < nvals; ++i)
      {
        if (b.at(i) >= 0 && b.at(i) < n)
          ret.colors[b.at(i)].rgbBlue = vals.at(nvals - 1 - cntblue + idxb++) * 255.;
      }
  }
  return ret;
}
#undef DEG2RAD

void convertFalseColor(const cv::Mat& srcmat, cv::Mat& dstmat, const palette &pal, bool drawlegend, double mintemp, double maxtemp)
{
  dstmat.create(srcmat.rows, srcmat.cols, CV_8UC3);

  cv::Size sz = srcmat.size();
  const unsigned char* src = srcmat.data;
  unsigned char* dst = dstmat.data;

  if (srcmat.isContinuous() && dstmat.isContinuous())
  {
    sz.width *= sz.height;
    sz.height = 1;
  }

  for (int i = 0; i < sz.width; ++i)
  {
    for (int j = 0; j < sz.height; ++j)
    {
      int idx = j * sz.width + i;
      uint8_t val = src[idx];
      dst[idx * dstmat.channels() + 0] = pal.colors[val].rgbBlue;
      dst[idx * dstmat.channels() + 1] = pal.colors[val].rgbGreen;
      dst[idx * dstmat.channels() + 2] = pal.colors[val].rgbRed;
    }
  }

  //draw a legend if true Temperatures
  if(drawlegend){

    //get min max to scale the legend
    double max_val;
    double min_val;
    cv::minMaxIdx(srcmat, &min_val, &max_val);

    enum{
      legenddiscretization = 5,
      legendnumbers = 5,
      legendwidth = 5,
      x_0 = 20,
      y_0 = 10,
    };
    double stepsize;

    //    std::cout<<"mintemp "<<mintemp<<" maxtemp "<<maxtemp<<std::endl;

    //draw legend color bar
    for(int i = y_0 ; i < dstmat.rows - y_0 ; ++i){
      int py = dstmat.rows - i;
      int val = (i - y_0) / (double)(dstmat.rows - y_0 * 2) * 255.;
      cv::rectangle(dstmat, cv::Point(x_0, py), cv::Point(x_0 + legendwidth, py + 1),
                    CV_RGB(pal.colors[val].rgbRed, pal.colors[val].rgbGreen, pal.colors[val].rgbBlue ), -1);
    }

    //draw temp tick labels
    stepsize = (dstmat.rows - y_0 * 2) / (double)legendnumbers;
    for(int i = 0 ; i <= legendnumbers ; ++i){
      int py = y_0 + (legendnumbers - i) * stepsize + 5; //bottom up
      double tempval = (mintemp - 273.15) + i * (maxtemp - mintemp) / (double)legendnumbers;
      std::stringstream ss;
      ss<<std::setprecision(2)<<tempval<<" C";
      cv::putText(dstmat, ss.str(), cv::Point(x_0 + 20, py), CV_FONT_HERSHEY_SIMPLEX, 0.4, CV_RGB(255,255,255), 1);
    }

    //draw ticks into legends
    stepsize = (dstmat.rows - y_0 * 2) / (double)legenddiscretization;
    for(int i = 0 ; i <= legenddiscretization ; ++i){
      int py = y_0 + (legenddiscretization - i) * stepsize; //bottom up
      cv::line(dstmat, cv::Point(x_0 - 2, py), cv::Point(x_0 + legendwidth + 2, py), CV_RGB(255,255,255), 1);
    }
  }

}

converter_16_8::converter_16_8()
{
  min_ = std::numeric_limits<uint16_t>::max();
  max_ = 0;
  firstframe_ = true;
}

converter_16_8::~converter_16_8()
{
  delete inst_;
  inst_ = NULL;
}

double converter_16_8::getMin(){
  return min_;
}
double converter_16_8::getMax(){
  return max_;
}
// void converter_16_8::toneMapping(const cv::Mat& img16, cv::Mat& img8){
//   if(!retina_){
//     retina_.reset(new cv::Retina(img16.size(), false));
//     retina_->setup(ros::package::getPath("brisk") + "/include/flir/retina_params");
//   }
//   retina_->run(img16);
//   retina_->getParvo(img8);
// }

//adjustment -3 for slightly wrong fit
double powerToK4(double power){
  double slope = 2.58357167114001779457e-07;
  double y_0 = 2.26799217314804718626e+03;
return sqrt(sqrt(((double)power - y_0) / slope)) - 3;
}

void converter_16_8::convert_to8bit(const cv::Mat& img16, cv::Mat& img8, bool doTempConversion)
{
  if(img8.empty()){ //make an image if the user has provided nothing
    img8.create(cvSize(img16.cols, img16.rows), CV_8UC1);
  }

  double min = std::numeric_limits<uint16_t>::max();
  double max = 0;

  //make a histogram of intensities
  typedef std::map<double, int> hist_t;
  hist_t hist;

  double bucketwidth = 2.; //bucketwidth in degrees K

  for (int i = 0; i < img16.cols; ++i)
  {
    for (int j = 0; j < img16.rows; ++j)
    {
      double power = img16.at<uint16_t>(j, i);
      double temp;
      if(doTempConversion){
        temp = powerToK4(power);
      }else{
        temp = power;
      }
      temp = round(temp / bucketwidth) * bucketwidth;
      hist[temp]++;
    }
  }

  //find the main section of the histogram
  for (hist_t::const_iterator it = hist.begin(); it != hist.end(); ++it)
  {
    if (it->second > histminmembersperbucket)
    {
      if (it->first > max)
      {
        max = it->first;
      }
      if (it->first < min)
      {
        min = it->first;
      }
    }
  }

  if (firstframe_)
  {
    min_ = min;
    max_ = max;
  }

  //  std::cout<<"min: "<<min-273.15<<" max: "<<max-273.15<<" sm: min: "<<min_-273.15<<" max: "<<max_-273.15<<std::endl;

  //exp smoothing
  double expsm = 0.95;
  min_ = expsm * min_ + (1. - expsm) * min;
  max_ = expsm * max_ + (1. - expsm) * max;

  for (int i = 0; i < img16.cols; ++i)
  {
    for (int j = 0; j < img16.rows; ++j)
    {
      double temp;
      if(doTempConversion){
        temp = powerToK4(img16.at<uint16_t>(j, i));
      }else{
        temp = (double)(img16.at<uint16_t>(j, i));
      }

      int val = (((temp - min_) / (max_ - min_)) * 255);

      val = val > std::numeric_limits<uint8_t>::max() ? std::numeric_limits<uint8_t>::max() : val < 0 ? 0 : val; //saturate
      img8.at<uint8_t>(j, i) = (uint8_t)val;
    }
  }

  firstframe_ = false;
}

converter_16_8* converter_16_8::inst_ = NULL;


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file lp_row_generation.hpp
 * @brief
 * @author Piotr Godlewski, Robert Rosołek
 * @version 1.0
 * @date 2013-06-04
 */
#ifndef PAAL_LP_ROW_GENERATION_HPP
#define PAAL_LP_ROW_GENERATION_HPP

#include "paal/lp/lp_base.hpp"
#include "paal/lp/problem_type.hpp"
#include "paal/utils/rotate.hpp"

#include <boost/range/counting_range.hpp>

namespace paal {
namespace lp {

/**
 * Finds an extreme point solution to the LP using row generation:
 * solves the initial LP and then ask the separation oracle if the found
 * solution is a feasible solution to the complete problem. If not,
 * adds a new row (generated by the oracle) to the LP and re-optimizes it.
 * This procedure is iterated until a feasible solution to the full LP
 * is found.
 */
template <class TryAddViolated, class SolveLp>
    problem_type row_generation(TryAddViolated try_add_violated, SolveLp solve_lp)
    {
        problem_type res;
        do res = solve_lp(); while (res == OPTIMAL && try_add_violated());
        return res;
    }

/**
 * @brief functor for adding maximum violated constraint
 *
 * @tparam GetCandidates
 * @tparam HowViolated
 * @tparam AddViolated
 * @tparam CompareHow
 */
template<
    class GetCandidates,
    class HowViolated,
    class AddViolated,
    class CompareHow
>
class add_max_violated {
   GetCandidates m_get_candidates;
   HowViolated m_how_violated;
   AddViolated m_add_violated;
   CompareHow m_cmp;

   public:
      ///contructor
      add_max_violated(GetCandidates get_candidates,
            HowViolated how_violated, AddViolated add_violated, CompareHow cmp)
         : m_get_candidates(get_candidates), m_how_violated(how_violated),
            m_add_violated(add_violated), m_cmp(cmp) {}

      ///operator()
      bool operator()() {
         auto&& cands = m_get_candidates();
         using how_violated_t = puretype(m_how_violated(*std::begin(cands)));
         using cand_it_t = puretype(std::begin(cands));
         boost::optional<std::pair<how_violated_t, cand_it_t>> most;
         for (auto cand : boost::counting_range(cands)) {
            auto const how = m_how_violated(*cand);
            if (!how) continue;
            if (!most || m_cmp(most->first, how))
               most = std::make_pair(std::move(how), cand);
         }
         if (!most) return false;
         m_add_violated(*most->second);
         return true;
      }
};

///functor computing add_max_violated
struct max_violated_separation_oracle {
   template <
      class GetCandidates,
      class HowViolated,
      class AddViolated,
      class CompareHow = utils::less
   >
   ///operator()
   auto operator()(
      GetCandidates get_candidates,
      HowViolated is_violated,
      AddViolated add_violated,
      CompareHow compare_how = CompareHow{}
   ) const {
      return add_max_violated<GetCandidates, HowViolated, AddViolated,
         CompareHow>(get_candidates, is_violated, add_violated, compare_how);
   }
};

///functor
template <class GetCandidates,
          class HowViolated,
          class AddViolated,
          class ReorderCandidates>
class add_first_violated {
   GetCandidates m_get_candidates;
   HowViolated m_how_violated;
   AddViolated m_add_violated;
   ReorderCandidates m_reorder_candidates;

   public:
      ///constructor
      add_first_violated(
         GetCandidates get_candidates,
         HowViolated how_violated,
         AddViolated add_violated,
         ReorderCandidates reorder_candidates
      ) : m_get_candidates(get_candidates),
         m_how_violated(how_violated),
         m_add_violated(add_violated),
         m_reorder_candidates(std::move(reorder_candidates)) {}

      ///operator()
      bool operator()() {
         auto&& cands = m_get_candidates();
         auto reordered =
            m_reorder_candidates(std::forward<decltype(cands)>(cands));
         for (auto c : boost::counting_range(reordered)) {
            if (m_how_violated(*c)) {
                m_add_violated(*c);
                return true;
            }
         }
         return false;
      }
};

///functor computing add_first_violated
struct first_violated_separation_oracle {
   template <
      class GetCandidates,
      class HowViolated,
      class AddViolated,
      class ReorderCandidates = utils::identity_functor
   >
   ///operator()
   auto operator() (
      GetCandidates get_candidates,
      HowViolated how_violated,
      AddViolated add_violated,
      ReorderCandidates reorder_candidates = ReorderCandidates{}
   ) const {
      return add_first_violated<GetCandidates, HowViolated, AddViolated,
         ReorderCandidates>(get_candidates, how_violated, add_violated,
            reorder_candidates);
   }
};

namespace detail {
template <class URNG>
class random_rotate {
   URNG m_g;
   public:
      random_rotate(URNG&& g)
         : m_g(std::forward<URNG>(g)) {}
      template <class ForwardRange>
      auto operator()(const ForwardRange& rng)
      {
         auto const len = boost::distance(rng);
         std::uniform_int_distribution<decltype(len)> d(0, len);
         return utils::rotate(rng, d(m_g));
      }
};

template <class URNG = std::default_random_engine>
auto make_random_rotate(URNG&& g = URNG{})
{
   return random_rotate<URNG>(std::forward<URNG>(g));
}
} //! detail

///functor returning add_first_violated
///Separation oracle for the row generation,
///using the random violated strategy.
struct random_violated_separation_oracle {
   template <
      class GetCandidates,
      class HowViolated,
      class AddViolated,
      class URNG = std::default_random_engine
   >
   ///operator()
   auto operator() (
      GetCandidates get_candidates,
      HowViolated how_violated,
      AddViolated add_violated,
      URNG&& g = URNG{}
   ) const {
      return first_violated_separation_oracle{}(get_candidates,
            how_violated, add_violated, detail::make_random_rotate(std::forward<URNG>(g)));
   }
};

} // lp
} // paal

#endif // PAAL_LP_ROW_GENERATION_HPP


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

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>

namespace nt2
{
  namespace tag
  {
   /*!
     @brief Deginrad generic tag

     Represents the Deginrad constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Deginrad, double
                                , 0, 0x3c8efa35
                                , 0x3f91df46a2529d39ll
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Deginrad, Site> dispatching_Deginrad(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Deginrad, Site>();
   }
   template<class... Args>
   struct impl_Deginrad;
  }
  /*!
    Constant radian in Degree multiplier, \f$\frac{180}\pi\f$.

    @par Semantic:

    For type T0:

    @code
    T0 r = Deginrad<T0>();
    @endcode

    is similar to:

    @code
    T0 r = _180<T0>()/Pi<T0>() ;
    @endcode

    @see  @funcref{inrad}, @funcref{indeg}, @funcref{Radindeg}, @funcref{Radindegr}
    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Deginrad, Deginrad);

}

#endif


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

#ifndef AUTODIF_HPP
#define AUTODIF_HPP

// Automatic Differentiation by bottom up algorithm

#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <cmath>

#include <kv/convert.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T> class autodif;
template <class C, class T> struct convertible<C, autodif<T> > {
	static const bool value = convertible<C, T>::value || boost::is_same<C, autodif<T> >::value;
};
template <class C, class T> struct acceptable_n<C, autodif<T> > {
	static const bool value = convertible<C, T>::value;
};

template <class T> class autodif {
	public:
	T v;
	ub::vector<T> d;

	typedef T base_type;

	autodif() {
		v = 0.;
		d.resize(0);
	}

	template <class C> explicit autodif(const C& x, typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value >::type* =0) {
		v = x;
		d.resize(0);
	}

	template <class C> typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif& >::type operator=(const C& x) {
		v = x;
		d.resize(0);
		return *this;
	}

	friend autodif operator+(const autodif& a, const autodif& b) {
		autodif r;

		r.v = a.v + b.v;

		if (a.d.size() == 0) {
			r.d = b.d;
		} else if (b.d.size() == 0) {
			r.d = a.d;
		} else {
			r.d = a.d + b.d;
		}

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator+(const autodif& a, const C& b) {
		autodif r;

		r.v = a.v + b;
		r.d = a.d;

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator+(const C& a, const autodif& b) {
		autodif r;

		r.v = a + b.v;
		r.d = b.d;

		return r;
	}

	friend autodif& operator+=(autodif& a, const autodif& b) {
		a = a + b;
		return a;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif& >::type operator+=(autodif& a, const C& b) {
		a.v += b;
		return a;
	}

	friend autodif operator-(const autodif& a, const autodif& b) {
		autodif r;

		r.v = a.v - b.v;

		if (a.d.size() == 0) {
			r.d = - b.d;
		} else if (b.d.size() == 0) {
			r.d = a.d;
		} else {
			r.d = a.d - b.d;
		}

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator-(const autodif& a, const C& b) {
		autodif r;

		r.v = a.v - b;
		r.d = a.d;

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator-(const C& a, const autodif& b) {
		autodif r;

		r.v = a - b.v;
		r.d = - b.d;

		return r;
	}

	friend autodif& operator-=(autodif& a, const autodif& b) {
		a = a - b;
		return a;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif& >::type operator-=(autodif& a, const C& b) {
		a.v -= b;
		return a;
	}

	friend autodif operator-(const autodif& a) {
		autodif r;

		r.v = - a.v;
		r.d = - a.d;

		return r;
	}

	friend autodif operator*(const autodif& a, const autodif& b) {
		autodif r;

		r.v = a.v * b.v;

		if (a.d.size() == 0) {
			r.d = a.v * b.d;
		} else if (b.d.size() == 0) {
			r.d = b.v * a.d;
		} else {
			r.d = b.v * a.d + a.v * b.d;
		}

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator*(const autodif& a, const C& b) {
		autodif r;

		r.v = a.v * b;
		// r.d = b * a.d;
		r.d = T(b) * a.d; // assist for VC++

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator*(const C& a, const autodif& b) {
		autodif r;

		r.v = a * b.v;
		// r.d = a * b.d;
		r.d = T(a) * b.d; // assist for VC++

		return r;
	}

	friend autodif& operator*=(autodif& a, const autodif& b) {
		a = a * b;
		return a;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif& >::type operator*=(autodif& a, const C& b) {
		a.v *= b;
		// a.d *= b;
		a.d *= T(b); // assist for VC++
		return a;
	}

	friend autodif operator/(const autodif& a, const autodif& b) {
		autodif r;

		r.v = a.v / b.v;

		if (a.d.size() == 0) {
			r.d = b.d * (-a.v/(b.v*b.v));
		} else if (b.d.size() == 0) {
			r.d = a.d / b.v;
		} else {
			r.d = a.d / b.v + b.d * (-a.v/(b.v*b.v));
		}

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator/(const autodif& a, const C& b) {
		autodif r;

		r.v = a.v / b;
		// r.d = a.d / b;
		r.d = a.d / T(b); // assist for VC++

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator/(const C& a, const autodif& b) {
		autodif r;

		r.v = a / b.v;
		r.d = b.d * (-a/(b.v*b.v));

		return r;
	}

	friend autodif& operator/=(autodif& a, const autodif& b) {
		a = a / b;
		return a;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif& >::type operator/=(autodif& a, const C& b) {
		a.v /= b;
		// a.d /= b;
		a.d /= T(b); // assist for VC++
		return a;
	}

	friend std::ostream& operator<<(std::ostream& s, const autodif& x) {
		int i;
		int n = x.d.size();
		s << x.v;
		s << '<';
		for (i=0; i<n; i++) {
			s << x.d(i);
			if (i != n-1) {
				s << ',';
			}
		}
		s << '>';
		return s;
	}

	friend autodif pow(const autodif& x, int y) {
		autodif r;

		using std::pow;
		r.v = pow(x.v, y);
		if (y == 0) {
			r.d = T(0.) * x.d;
		} else {
			r.d = (y * pow(x.v, y - 1)) * x.d;
		}
		return r;
	}

	friend autodif pow(const autodif& x, const autodif& y) {
		return exp(y * log(x));
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value && ! boost::is_integral<C>::value, autodif >::type pow(const autodif& a, const C& b) {
		return pow(a, autodif(b));
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type pow(const C& a, const autodif& b) {
		return pow(autodif(a), b);
	}

	friend autodif exp (const autodif& x) {
		autodif r;

		using std::exp;
		r.v = exp(x.v);
		// r.d = exp(x.v) * x.d;
		r.d = r.v * x.d;

		return r;
	}

	friend autodif log (const autodif& x) {
		autodif r;

		using std::log;
		r.v = log(x.v);
		r.d = x.d / x.v;

		return r;
	}

	friend autodif sqrt (const autodif& x) {
		autodif r;

		using std::sqrt;
		r.v = sqrt(x.v);
		// r.d = 1./(2. * sqrt(x.v)) * x.d;
		r.d = x.d / (2. * r.v);

		return r;
	}

	friend autodif sin (const autodif& x) {
		autodif r;

		using std::sin;
		using std::cos;
		r.v = sin(x.v);
		r.d = cos(x.v) * x.d;

		return r;
	}

	friend autodif cos (const autodif& x) {
		autodif r;

		using std::sin;
		using std::cos;
		r.v = cos(x.v);
		r.d = -sin(x.v) * x.d;

		return r;
	}

	friend autodif tan (const autodif& x) {
		autodif r;
		T tmp;

		using std::tan;
		using std::cos;
		r.v = tan(x.v);
		tmp = cos(x.v);
		tmp = 1. / (tmp * tmp);
		r.d = tmp * x.d;

		return r;
	}

	friend autodif asin (const autodif& x) {
		autodif r, tmp;

		using std::asin;
		using std::sqrt;
		r.v = asin(x.v);
		r.d = (1. / sqrt(1. - x.v * x.v)) * x.d;

		return r;
	}

	friend autodif acos (const autodif& x) {
		autodif r;

		using std::acos;
		using std::sqrt;
		r.v = acos(x.v);
		r.d = (-1. / sqrt(1. - x.v * x.v)) * x.d;

		return r;
	}

	friend autodif atan (const autodif& x) {
		autodif r;

		using std::atan;
		r.v = atan(x.v);
		r.d = (1. / (1. + x.v * x.v)) * x.d;

		return r;
	}

	friend autodif sinh (const autodif& x) {
		autodif r;

		using std::sinh;
		using std::cosh;
		r.v = sinh(x.v);
		r.d = cosh(x.v) * x.d;

		return r;
	}

	friend autodif cosh (const autodif& x) {
		autodif r;

		using std::sinh;
		using std::cosh;
		r.v = cosh(x.v);
		r.d = sinh(x.v) * x.d;

		return r;
	}

	friend autodif tanh (const autodif& x) {
		autodif r;
		T tmp;

		using std::tanh;
		using std::cosh;
		r.v = tanh(x.v);
		tmp = cosh(x.v);
		tmp = 1. / (tmp * tmp);
		r.d = tmp * x.d;

		return r;
	}

	friend autodif asinh (const autodif& x) {
		autodif r, tmp;

		// using std::asinh;
		using std::sqrt;
		r.v = asinh(x.v);
		r.d = (1. / sqrt(x.v * x.v + 1.)) * x.d;

		return r;
	}

	friend autodif acosh (const autodif& x) {
		autodif r;

		// using std::acosh;
		using std::sqrt;
		r.v = acosh(x.v);
		r.d = (1. / sqrt(x.v * x.v - 1.)) * x.d;

		return r;
	}

	friend autodif atanh (const autodif& x) {
		autodif r;

		// using std::atanh;
		r.v = atanh(x.v);
		r.d = (1. / (1. - x.v * x.v)) * x.d;

		return r;
	}

	// n-dimensional version
	static ub::vector<autodif> init (const ub::vector<T>& in) {
		int i, j;
		int n = in.size();
		ub::vector<autodif> out(n);

		for (i=0; i<n; i++) {
			out(i).v = in(i);
			out(i).d.resize(n);
			for (j=0; j<n; j++) {
				out(i).d(j) = (i==j) ? 1. : 0.;
			}
		}

		return out;
	}

	// 1-dimensional version
	static autodif init (const T& in) {
		int j;
		autodif out;

		out.v = in;
		out.d.resize(1);
		out.d(0) = 1.;

		return out;
	}

	// for functions R^n -> R^m
	static void split (const ub::vector<autodif>& in, ub::vector<T>& v, ub::matrix<T>& d) {
		int i, j, n, tmp;
		int m = in.size();

		if (in.size() == 0) return;
		n = in(0).d.size();
		for (i=1; i<m; i++) {
			tmp = in(i).d.size();
			if (tmp > n) n = tmp;
		}

		v.resize(m);
		d.resize(m, n);
		for (i=0; i<m; i++) {
			v(i) = in(i).v;
			tmp = in(i).d.size();
			for (j=0; j<tmp; j++) {
				d(i, j) = in(i).d(j);
			}
			for (j=tmp; j<n; j++) {
				d(i, j) = 0.;
			}
		}
	}

	// for functions R^n -> R
	static void split (const autodif& in, T& v, ub::vector<T>& d) {
		int j, n;

		n = in.d.size();
		d.resize(n);

		v = in.v;
		for (j=0; j<n; j++) {
			d(j) = in.d(j);
		}
	}

	// for functions R -> R^m
	static void split (const ub::vector<autodif>& in, ub::vector<T>& v, ub::vector<T>& d) {
		int i;
		int m = in.size();

		if (in.size() == 0) return;

		v.resize(m);
		d.resize(m);
		for (i=0; i<m; i++) {
			v(i) = in(i).v;
			d(i) = in(i).d(0);
		}
	}

	// for functions R -> R
	static void split (const autodif& in, T& v, T& d) {
		v = in.v;
		d = in.d(0);
	}

	static ub::vector<autodif>
	compress (const ub::vector<autodif>& in, ub::matrix<T>& save) {
		ub::vector<autodif> out;
		int i, j, m, tmp;
		int n = in.size();

		out.resize(n);

		m = in(0).d.size();
		for (i=1; i<n; i++) {
			tmp = in(i).d.size();
			if (tmp > m) m = tmp;
		}

		save.resize(n, m);

		for (i=0; i<n; i++) {
			out(i).v = in(i).v;
			tmp = in(i).d.size();
			for (j=0; j<tmp; j++) {
				save(i, j) = in(i).d(j);
			}
			for (j=tmp; j<m; j++) {
				save(i, j) = 0.;
			}
			out(i).d.resize(n);
			for (j=0; j<n; j++) {
				out(i).d(j) = (i==j) ? 1 : 0;
			}
		}

		return out;
	}

	static autodif
	expand (const autodif& in, const ub::matrix<T>& save) {
		autodif out;

		out.v = in.v;
		out.d = prod(in.d, save);

		return out;
	}

	static ub::vector<autodif>
	expand (const ub::vector<autodif>& in, const ub::matrix<T>& save) {
		ub::vector<autodif> out;
		int i;
		int s = in.size();

		out.resize(s);

		for (i=0; i<s; i++) {
			out(i) = expand(in(i), save);
		}

		return out;
	}
};

} // namespace kv

#endif //AUTODIF_HPP


#ifndef N_BODY_DATA_HPP
#define N_BODY_DATA_HPP

#include <array>
#include <boost/serialization/access.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp>
#include <cmath>
#include <cstddef>
#include <memory>
#include <tuple>
#include <vector>

namespace n_body::data {

template <typename T, std::size_t Dimension>
using Vector = std::array<T, Dimension>;

template <typename T> using Scalar = T;

template <typename T, std::size_t Dimension> struct Body {
  using vector_type = Vector<T, Dimension>;
  using scalar_type = Scalar<T>;

  vector_type position;
  vector_type velocity;
  scalar_type mass;

private:
  /* serialization */
  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive &ar, const unsigned int /* version */) {
    ar &BOOST_SERIALIZATION_NVP(position);
    ar &BOOST_SERIALIZATION_NVP(velocity);
    ar &BOOST_SERIALIZATION_NVP(mass);
  }
};

template <typename T, std::size_t Dimension>
using Bodies = std::vector<Body<T, Dimension>>;

template <typename T, std::size_t Dimension> struct Space {
  using vector_type = Vector<T, Dimension>;

  vector_type min;
  vector_type max;
  vector_type center;

private:
  /* serialization */
  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive &ar, const unsigned int /* version */) {
    ar &BOOST_SERIALIZATION_NVP(min);
    ar &BOOST_SERIALIZATION_NVP(max);
    ar &BOOST_SERIALIZATION_NVP(center);
  }
};

template <typename T, std::size_t Dimension>
Vector<T, Dimension> operator+(const Vector<T, Dimension> &v1,
                               const Vector<T, Dimension> &v2) {
  Vector<T, Dimension> result;
  for (std::size_t d = 0; d < Dimension; ++d) {
    result[d] = v1[d] + v2[d];
  }
  return result;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> &operator+=(Vector<T, Dimension> &v1,
                                 const Vector<T, Dimension> &v2) {
  for (std::size_t d = 0; d < Dimension; ++d) {
    v1[d] += v2[d];
  }
  return v1;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> operator-(const Vector<T, Dimension> &v1,
                               const Vector<T, Dimension> &v2) {
  Vector<T, Dimension> result;
  for (std::size_t d = 0; d < Dimension; ++d) {
    result[d] = v1[d] - v2[d];
  }
  return result;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> operator-(const Vector<T, Dimension> &v) {
  Vector<T, Dimension> result;
  for (std::size_t d = 0; d < Dimension; ++d) {
    result[d] = -v[d];
  }
  return result;
}

template <typename T, std::size_t Dimension>
Scalar<T> module_of(const Vector<T, Dimension> &v) {
  T sum = 0;
  for (auto x : v) {
    sum += x * x;
  }
  return std::sqrt(sum);
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> &operator-=(Vector<T, Dimension> &v1,
                                 const Vector<T, Dimension> &v2) {
  for (std::size_t d = 0; d < Dimension; ++d) {
    v1[d] -= v2[d];
  }
  return v1;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> operator*(const Scalar<T> &s,
                               const Vector<T, Dimension> &v) {
  Vector<T, Dimension> result{};
  for (std::size_t d = 0; d < Dimension; ++d) {
    result[d] = v[d] * s;
  }
  return result;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> operator*(const Vector<T, Dimension> &v,
                               const Scalar<T> &s) {
  return s * v;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> &operator*=(Vector<T, Dimension> &v, const Scalar<T> &s) {
  for (std::size_t d = 0; d < Dimension; ++d) {
    v[d] *= s;
  }
  return v;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> operator/(const Vector<T, Dimension> &v,
                               const Scalar<T> &s) {
  Vector<T, Dimension> result;
  for (std::size_t d = 0; d < Dimension; ++d) {
    result[d] = v[d] / s;
  }
  return result;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> &operator/=(Vector<T, Dimension> &v, const Scalar<T> &s) {
  for (std::size_t d = 0; d < Dimension; ++d) {
    v[d] /= s;
  }
  return v;
}

template <typename T, std::size_t Dimension>
std::tuple<Vector<T, Dimension>, Scalar<T>> average_position_by_mass(
    const Vector<T, Dimension> &position1, const Scalar<T> &mass1,
    const Vector<T, Dimension> &position2, const Scalar<T> &mass2) {
  auto sum = mass1 * position1 + mass2 * position2;
  auto sum_mass = mass1 + mass2;
  return {
      sum / sum_mass,
      sum_mass,
  };
}

template <typename T, std::size_t Dimension>
void average_position_by_mass_in_place(Vector<T, Dimension> &position1,
                                       Scalar<T> &mass1,
                                       const Vector<T, Dimension> &position2,
                                       const Scalar<T> &mass2) {
  position1 *= mass1;
  position1 += position2 * mass2;
  mass1 += mass2;
  position1 /= mass1;
}

} // namespace n_body::data

#endif


/******************************************************************************
Copyright (c) 2021, Farbod Farshidian. All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/

#include "ocs2_core/loopshaping/LoopshapingPropertyTree.h"

#include <boost/property_tree/info_parser.hpp>
#include "ocs2_core/misc/LoadData.h"

namespace ocs2 {
namespace loopshaping_property_tree {
Filter readSISOFilter(const boost::property_tree::ptree& pt, std::string filterName, bool invert) {
  // Get Sizes
  const auto numRepeats = pt.get<size_t>(filterName + ".numRepeats", 1);
  const auto numPoles = pt.get<size_t>(filterName + ".numPoles", 0);
  const auto numZeros = pt.get<size_t>(filterName + ".numZeros", 0);
  const auto scaling = pt.get<scalar_t>(filterName + ".scaling", 1.0);
  const size_t numStates = invert ? numZeros : numPoles;
  const size_t numInputs = 1;

  // Setup Filter, convention a0*s^n + a1*s^(n-1) + ... + an
  vector_t numerator(numZeros + 1);
  numerator.setZero();
  numerator(0) = 1.0;
  for (size_t z = 0; z < numZeros; z++) {
    auto zero = pt.get<scalar_t>(filterName + ".zeros." + "(" + std::to_string(z) + ")");
    numerator.segment(1, z + 1) -= zero * numerator.segment(0, z + 1).eval();
  }
  numerator *= scaling;  // Apply scale to numerator

  vector_t denominator(numPoles + 1);
  denominator.setZero();
  denominator(0) = 1.0;
  for (size_t p = 0; p < numPoles; p++) {
    auto pole = pt.get<scalar_t>(filterName + ".poles." + "(" + std::to_string(p) + ")");
    denominator.segment(1, p + 1) -= pole * denominator.segment(0, p + 1).eval();
  }

  // Print frequency domain information
  std::cerr << "Read filter " << filterName << (invert ? " (before inversion)" : "") << ", convention: a0*s^n + a1*s^(n-1) + ... + an \n";
  std::cerr << "\tnumerator: [" << numerator.transpose() << "]\n";
  std::cerr << "\tdenominator: [" << denominator.transpose() << "]\n";
  std::cerr << "\tDC gain: " << numerator(numZeros) / denominator(numPoles) << "\n";
  std::cerr << "\tInf gain: ";
  if (numZeros > numPoles) {
    std::cerr << "Inf\n";
  }
  if (numZeros < numPoles) {
    std::cerr << 0.0 << "\n";
  }
  if (numZeros == numPoles) {
    std::cerr << numerator(0) / denominator(0) << "\n";
  }

  if (invert) {
    vector_t temp;
    temp = numerator;
    numerator = denominator;
    denominator = temp;
  }

  // Convert to state space
  matrix_t a, b, c, d;
  ocs2::tf2ss(numerator, denominator, a, b, c, d);

  matrix_t A = matrix_t::Zero(numRepeats * numStates, numRepeats * numStates);
  matrix_t B = matrix_t::Zero(numRepeats * numStates, numRepeats * numInputs);
  matrix_t C = matrix_t::Zero(numRepeats * numInputs, numRepeats * numStates);
  matrix_t D = matrix_t::Zero(numRepeats * numInputs, numRepeats * numInputs);
  for (size_t r = 0; r < numRepeats; r++) {
    A.block(r * numStates, r * numStates, numStates, numStates) = a;
    B.block(r * numStates, r * numInputs, numStates, numInputs) = b;
    C.block(r * numInputs, r * numStates, numInputs, numStates) = c;
    D.block(r * numInputs, r * numInputs, numInputs, numInputs) = d;
  }

  return Filter(A, B, C, D);
}

Filter readMIMOFilter(const boost::property_tree::ptree& pt, std::string filterName, bool invert) {
  const auto numFilters = pt.get<size_t>(filterName + ".numFilters", 0);
  if (numFilters > 0) {
    // Read the sisoFilters
    std::vector<Filter> sisoFilters;
    size_t numStates(0), numInputs(0), numOutputs(0);
    for (size_t i = 0; i < numFilters; ++i) {
      // Read filter
      std::string sisoFilterName = filterName + ".Filter" + std::to_string(i);
      sisoFilters.emplace_back(readSISOFilter(pt, sisoFilterName, invert));

      // Track sizes
      numStates += sisoFilters.back().getNumStates();
      numInputs += sisoFilters.back().getNumInputs();
      numOutputs += sisoFilters.back().getNumOutputs();
    }

    // Concatenate siso matrices into one MIMO filter
    matrix_t A = matrix_t::Zero(numStates, numStates);
    matrix_t B = matrix_t::Zero(numStates, numInputs);
    matrix_t C = matrix_t::Zero(numOutputs, numStates);
    matrix_t D = matrix_t::Zero(numOutputs, numInputs);
    size_t statecount(0), inputcount(0), outputcount(0);
    for (const auto& filt : sisoFilters) {
      A.block(statecount, statecount, filt.getNumStates(), filt.getNumStates()) = filt.getA();
      B.block(statecount, inputcount, filt.getNumStates(), filt.getNumInputs()) = filt.getB();
      C.block(outputcount, statecount, filt.getNumOutputs(), filt.getNumStates()) = filt.getC();
      D.block(outputcount, inputcount, filt.getNumOutputs(), filt.getNumInputs()) = filt.getD();
      statecount += filt.getNumStates();
      inputcount += filt.getNumInputs();
      outputcount += filt.getNumOutputs();
    }
    return Filter(A, B, C, D);
  } else {
    return Filter();
  }
}

std::shared_ptr<LoopshapingDefinition> load(const std::string& settingsFile) {
  // Read from settings File
  boost::property_tree::ptree pt;
  boost::property_tree::read_info(settingsFile, pt);
  Filter r_filter = loopshaping_property_tree::readMIMOFilter(pt, "r_filter");
  Filter s_filter = loopshaping_property_tree::readMIMOFilter(pt, "s_inv_filter", /*invert=*/true);

  if (r_filter.getNumOutputs() > 0 && s_filter.getNumOutputs() > 0) {
    throw std::runtime_error("[LoopshapingDefinition] using both r and s filter not implemented");
  }

  if (r_filter.getNumOutputs() > 0) {
    return std::make_shared<LoopshapingDefinition>(LoopshapingType::outputpattern, r_filter);
  }
  if (s_filter.getNumOutputs() > 0) {
    return std::make_shared<LoopshapingDefinition>(LoopshapingType::eliminatepattern, s_filter);
  }

  throw std::runtime_error("[LoopshapingDefinition] error loading loopshaping definition, no valid filter found");
}

}  // namespace loopshaping_property_tree
}  // namespace ocs2


/***
 *  $Id$
 **
 *  File: graph_dijkstra.hpp
 *  Created: May 9, 2012
 *
 *  Author: Olga Wodo, Baskar Ganapathysubramanian
 *  Copyright (c) 2012 Olga Wodo, Baskar Ganapathysubramanian
 *  See accompanying LICENSE.
 *
 *  This file is part of GraSPI.
 */

#ifndef GRAPH_DIJKSTRA_HPP
#define GRAPH_DIJKSTRA_HPP

#include "graspi_types.hpp"
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/filtered_graph.hpp>

namespace graspi {
    /// This function determines the shortest paths in the fitered graph.
    ///
    /// This function determines the shortest paths in the filtered graph.
    /// The shortest paths and the lengths (distances) are computed from the source vertex to all vertices in the filtered graph.
    /// The filtered graph is determined using predicate Pred.
    /// @tparam Pred is the predicate used to filter the graph
    /// @param G is the input graph
    /// @param W is the map storing the weights of the edges
    /// @param source is the source vertex with respect to which distances are to be determine_shortest_distanced
    /// @param pred is the predicate used to filter the graph
    /// @param d is the vector of distance to be determined as a result of this function
    template<typename Pred>
    inline void determine_shortest_distances(graph_t*G,
                                             const edge_weights_t& W,
                                             vertex_t source,
                                             const Pred& pred,
                                             std::vector<float>& d){
        boost::filtered_graph<graph_t, Pred> FG(*G,pred);
        unsigned int n = boost::num_vertices(*G);
        std::vector<vertex_t> p(n);
        std::fill(d.begin(), d.end(), 0.0);
        for (unsigned int i = 0; i < n; ++i) p[i] = i;

        boost::dijkstra_shortest_paths(FG, source,
                                       boost::predecessor_map(&p[0])
                                       .
                                       distance_map(&d[0]).weight_map(W));
    }
}//graspi-namespace

#endif


#include <iostream>
#include <fstream>
#include <string.h>
#include <math.h>
#include <Eigen/Dense>
#include <algorithm>

#include "matrix.h"
#include "kdtree_nano.h"

using namespace std;

/**
 * @brief SparsifyData Sparsify a point cloud
 * @param M Input Point cloud
 * @param M_num Number of points in M
 * @param dim Dimemsion of points
 * @param idx_sparse Output binary masks indicating which point should be preserved
 * @param idx_size Output number of points to be preserved
 * @param min_dist Minimum distance for sparsifying
 * @param out_dist Distance for removing outliers
 * @param idx_start Start sparsifying from the specific point
 */
void SparsifyData (double *M,int32_t M_num,int32_t dim,int32_t* idx_sparse,int32_t &idx_size,double min_dist,double out_dist,int32_t idx_start) {

  // copy model data to kdtree
  PointCloud<double> cloud;
  for (int32_t m=0; m<M_num; m++) {
      cloud.pts.push_back(Point<double>((double)M[m*dim], (double)M[m*dim+1], (double)M[m*dim+2]));  }

  // build a kd tree from the model point cloud
  KDtree<double>* tree = new KDtree<double>(cloud);

  // for all data points do
  for (int32_t i=0; i<M_num; i++) {

    if (i>=idx_start) {

      vector<size_t> result;
      vector<double> dist;
      tree->radiusSearch(cloud.pts[i], min_dist, result, dist);

      bool neighbor_exists = false;
      for (int32_t j=0; j<result.size(); j++)
        neighbor_exists |= (bool)idx_sparse[result[j]];

      if (!neighbor_exists) {
        idx_sparse[i] = 1;
        idx_size++;
      }

    // simply add
    } else {
      idx_sparse[i] = 1;
      idx_size++;
    }
  }

  // remove outliers
  if (out_dist>0) {
    for (int32_t i=idx_start; i<M_num; i++) {

      if (idx_sparse[i] == 1) {

        vector<size_t> result;
        vector<double> dist;
        tree->radiusSearch(cloud.pts[i], out_dist, result, dist);

        int32_t num_neighbors = 0;
        for (int32_t j=1; j<result.size(); j++) {
          if (idx_sparse[result[j]]==1)
            num_neighbors++;
        }

        if (num_neighbors==0) {
          idx_sparse[i] = 0;
          idx_size--;
        }
      }
    }
  }

  // release memory of kd tree
  delete tree;
}

/**
 * @brief ExtractCols Extract columns of a matrix given indices
 * @param input Input matrix
 * @param output Output matrix
 * @param idx_sparse Binary masks indicate which columns should be preserved
 * @param idx_size Number of columns to be preserved
 */
void ExtractCols(Eigen::MatrixXd &input, Eigen::MatrixXd &output, int32_t *idx_sparse, int32_t idx_size){

    output.resize(input.rows(), idx_size);

    int32_t k=0;
    for (int i=0; i<input.cols(); i++){
        if (idx_sparse[i]){
            for (int c=0; c<input.rows(); c++){
                output(c,k) = input(c, i);
            }
            k++;
        }
    }
}

/**
 * @brief ExtractCols Extract columns of a matrix given indices
 * @param input Input matrix
 * @param output Output matrix
 * @param idx Indices of columns to extract
 */
void ExtractCols(Eigen::MatrixXd &input, Eigen::MatrixXd &output, vector<int> idx){

    int dim=input.rows();
    output.resize(dim, idx.size());
    for (int i=0; i<idx.size(); i++){
        for (int c=0; c<dim; c++){
            output(c, i) = input(c, idx[i]);
        }
    }
}

/**
 * @brief ExtractCols Extract columns of a matrix given indices
 * @param input Input matrix
 * @param output Output matrix
 * @param idx Indices of rows to extract
 */
void ExtractRows(Eigen::MatrixXd &input, Eigen::MatrixXd &output, vector<int> idx){

    int dim=input.cols();
    output.resize(idx.size(), dim);
    for (int i=0; i<idx.size(); i++){
        for (int c=0; c<dim; c++){
            output(i, c) = input(idx[i], c);
        }
    }
}

/**
 *@brief RemoveBlindSpot Remove the points within a sector
 *@param matIn Input point cloud
 *@param matOut Output point cloud
 *@param blind_splot_angle Sector angle
 */
void RemoveBlindSpot(Eigen::MatrixXd &matIn, Eigen::MatrixXd &matOut, float blind_splot_angle){
    if (blind_splot_angle<=0){
        matOut = matIn;
        return;
    }
    Eigen::MatrixXd matInSub = matIn.block(0,0,matIn.rows(),2);
    Eigen::MatrixXd matNorm = matInSub.rowwise().norm();

    matInSub = matIn.block(0,0,matIn.rows(),1);
    Eigen::ArrayXd v = - matInSub.array() / matNorm.array();

    float angle = cos(blind_splot_angle/2);

    int k=0;
    matOut.resize(matIn.rows(), matIn.cols());
    for (int i=0; i<matIn.rows(); i++){
        if (v(i, 0)<=angle){
            matOut.block(k,0,1,matIn.cols()) = matIn.block(i,0,1,matIn.cols());
            k++;
        }
    }
    Eigen::MatrixXd matrixSlice = matOut.block(0,0,k,matIn.cols());
    matOut = matrixSlice;
}

/**
 *@brief CropVelodyneData Crop velodyne data given max and min distance
 *@param matIn Input point cloud
 *@param matOut Output point cloud
 *@param minDist Minimum distance to crop data
 *@param maxDist Maximum distance to crop data
 */
void CropVelodyneData (Eigen::MatrixXd &matIn, Eigen::MatrixXd &matOut, float minDist, float maxDist){

    Eigen::MatrixXd matNorm = matIn.rowwise().norm();

    int k = 0;
    matOut.resize(matIn.rows(), matIn.cols());
    for (int i=0; i<matIn.rows(); i++){
        if (matNorm(i,0) > minDist && matNorm(i,0) < maxDist){
            matOut.block(k,0,1,matIn.cols()) = matIn.block(i,0,1,matIn.cols());
            k++;
        }
    }
    Eigen::MatrixXd matrixSlice = matOut.block(0,0,k,matIn.cols());
    matOut = matrixSlice;

}

/**
 *@brief CropVelodyneData Crop velodyne data given max and min distance
 *@param matIn Input point cloud
 *@param matOut Output indices of preserved points
 *@param minDist Minimum distance to crop data
 *@param maxDist Maximum distance to crop data
 */
void CropVelodyneData (Eigen::MatrixXd &matIn, vector<int> &idxOut, float minDist, float maxDist){

    Eigen::MatrixXd matNorm = matIn.rowwise().norm();

    idxOut.clear();
    for (int i=0; i<matIn.rows(); i++){
        if (matNorm(i,0) > minDist && matNorm(i,0) < maxDist){
            idxOut.push_back(i);
        }
    }
}

/**
 *@brief CurlVelodyneData Transform velodyne data given curl parameters
 *@param velo_in Input velodyne data
 *@param velo_out Output velodyne data
 *@param r Rotation matrix for curl
 *@param t Translation vector for curl
 */
void CurlVelodyneData (Eigen::MatrixXd &velo_in, Eigen::MatrixXd &velo_out, Eigen::Vector3d r, Eigen::Vector3d t){

  // for all points do
  int dim = velo_in.cols(); // 3
  int pt_num = velo_in.rows();
  velo_out.resize(pt_num, dim);
  for (int32_t i=0; i<pt_num; i++) {

    double vx = velo_in(i, 0);
    double vy = velo_in(i, 1);
    double vz = velo_in(i, 2);

    double s = 0.5*atan2(vy,vx)/M_PI;

    double rx = s*r(0);
    double ry = s*r(1);
    double rz = s*r(2);

    double tx = s*t(0);
    double ty = s*t(1);
    double tz = s*t(2);

    double theta = sqrt(rx*rx+ry*ry+rz*rz);

    if (theta>1e-10) {

      double kx = rx/theta;
      double ky = ry/theta;
      double kz = rz/theta;

      double ct = cos(theta);
      double st = sin(theta);

      double kv = kx*vx+ky*vy+kz*vz;

      velo_out(i, 0) = vx*ct + (ky*vz-kz*vy)*st + kx*kv*(1-ct) + tx;
      velo_out(i, 1) = vy*ct + (kz*vx-kx*vz)*st + ky*kv*(1-ct) + ty;
      velo_out(i, 2) = vz*ct + (kx*vy-ky*vx)*st + kz*kv*(1-ct) + tz;

    } else {

      velo_out(i, 0) = vx + tx;
      velo_out(i, 1) = vy + ty;
      velo_out(i, 2) = vz + tz;

    }

    // intensity
    // velo_out[i*4+3] = velo_in[i*4+3];
  }
}

/**
 *@brief GetHalfPoints Get either forward or backward points
 *@param matIn Input matrix
 *@param matOut Output matrix
 *@param direction Bool value indicating forward (true) or backward (false)
 */
void GetHalfPoints(Eigen::MatrixXd &matIn, Eigen::MatrixXd &matOut, bool direction){

    int k = 0;
    matOut.resize(matIn.rows(), matIn.cols());
    for (int i=0; i<matIn.cols(); i++){
        if ((direction && matIn(0,i)>0) || (!direction && matIn(0,i)<0)){
            matOut.block(0,k,matIn.rows(),1) = matIn.block(0,i,matIn.rows(),1);
            k++;
        }
    }
    Eigen::MatrixXd matrixSlice = matOut.block(0,0,matIn.rows(),k);
    matOut = matrixSlice;
}

typedef struct {
    double r,g,b;
} COLOUR;

/**
 *@brief GetColour Turn a double into a color vector
 *@param v Input value
 *@param vmin Min value for coloring
 *@param vmax Max value for coloring
 */
COLOUR GetColour(double v,double vmin,double vmax)
{
   COLOUR c = {1.0,1.0,1.0}; // white
   double dv;

   if (v < vmin)
      v = vmin;
   if (v > vmax)
      v = vmax;
   dv = vmax - vmin;

   if (v < (vmin + 0.25 * dv)) {
      c.r = 0;
      c.g = 4 * (v - vmin) / dv;
   } else if (v < (vmin + 0.5 * dv)) {
      c.r = 0;
      c.b = 1 + 4 * (vmin + 0.25 * dv - v) / dv;
   } else if (v < (vmin + 0.75 * dv)) {
      c.r = 4 * (v - vmin - 0.5 * dv) / dv;
      c.b = 0;
   } else {
      c.g = 1 + 4 * (vmin + 0.75 * dv - v) / dv;
      c.b = 0;
   }

   return(c);
}

/**
 *@brief GetColorToHeight Assign a color to each point according to its height
 *@param pose Input point cloud
 *@param color Output color
 */
void GetColorToHeight(Eigen::MatrixXd &pose, Eigen::MatrixXd &color){
    // get height from poses
    Eigen::VectorXd height = pose.block(0,2,pose.rows(),1);

    // sort and get the quantiles
    sort(height.data(),height.data()+height.size());
    int idxL = (int)(ceil((double)height.size()*0.05));
    int idxH = (int)(floor((double)height.size()*0.95));
    double heightL = height(idxL);
    double heightH = height(idxH);

    for (int i=0; i<height.size(); i++){
        COLOUR c = GetColour(pose(i, 2), heightL, heightH);
        color(i, 0) = c.r;
        color(i, 1) = c.g;
        color(i, 2) = c.b;
    }
}


#ifndef ALEPH_GEOMETRY_HEAT_KERNEL_HH__
#define ALEPH_GEOMETRY_HEAT_KERNEL_HH__

#include <aleph/config/Eigen.hh>

#ifdef ALEPH_WITH_EIGEN
  #include <Eigen/Core>
  #include <Eigen/Eigenvalues>
#endif

#include <aleph/math/KahanSummation.hh>

#include <algorithm>
#include <unordered_map>
#include <stdexcept>
#include <string>
#include <vector>

#include <cmath>

#define THROW_EIGEN_REQUIRED_ERROR()\
{\
  auto message =  std::string( __FILE__ )           \
                + std::string( ":" )                \
                + std::to_string( __LINE__ )        \
                + std::string( " in " )             \
                + std::string( __PRETTY_FUNCTION__ )\
                + std::string( ":" )                \
                + std::string( " Eigen is required for this function to work properly" );\
  \
  throw std::runtime_error( message );\
}

namespace aleph
{

namespace geometry
{

#ifdef ALEPH_WITH_EIGEN

/**
  Extracts a weighted adjacency matrix from a simplicial complex. At
  present, this function only supports adjacencies between edges, so
  the resulting matrix is a graph adjacency matrix.

  @param K Simplicial complex

  @returns Weighted adjacency matrix. The indices of rows and columns
           follow the order of the vertices in the complex.
*/

template <class SimplicialComplex> auto weightedAdjacencyMatrix( const SimplicialComplex& K ) -> Eigen::Matrix<typename SimplicialComplex::ValueType::DataType, Eigen::Dynamic, Eigen::Dynamic>
{
  using Simplex    = typename SimplicialComplex::ValueType;
  using VertexType = typename Simplex::VertexType;
  using DataType   = typename Simplex::DataType;
  using Matrix     = Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic>;

#if EIGEN_VERSION_AT_LEAST(3,3,0)
  using IndexType  = Eigen::Index;
#else
  using IndexType  = typename Matrix::Index;
#endif

  // Prepare map from vertex to index ----------------------------------

  std::unordered_map<VertexType, IndexType> vertex_to_index;
  IndexType n = IndexType();

  {
    std::vector<VertexType> vertices;
    K.vertices( std::back_inserter( vertices ) );

    IndexType index = IndexType();

    for( auto&& vertex : vertices )
      vertex_to_index[vertex] = index++;

    n = static_cast<IndexType>( vertices.size() );
  }

  // Prepare matrix ----------------------------------------------------

  Matrix W = Matrix::Zero( n, n );

  for(auto&& s : K )
  {
    if( s.dimension() != 1 )
      continue;

    auto&& u = s[0];
    auto&& v = s[1];
    auto&& i = vertex_to_index.at( u );
    auto&& j = vertex_to_index.at( v );

    W(i,j)   = s.data();
    W(j,i)   = W(i,j);
  }

  return W;
}

/**
  Calculates the weighted Laplacian matrix of a given simplicial
  complex and returns it.

  @param K Simplicial complex

  @returns Weighted Laplacian matrix. The indices of rows and columns
           follow the order of the vertices in the complex.
*/

template <class SimplicialComplex> auto weightedLaplacianMatrix( const SimplicialComplex& K ) -> Eigen::Matrix<typename SimplicialComplex::ValueType::DataType, Eigen::Dynamic, Eigen::Dynamic>
{
  auto W          = weightedAdjacencyMatrix( K );
  using Matrix    = decltype(W);
  using IndexType = typename Matrix::Index;

  Matrix L = Matrix::Zero( W.rows(), W.cols() );

  auto V = W.rowwise().sum();

  for( IndexType i = 0; i < V.size(); i++ )
    L(i,i) = V(i);

  return L - W;
}

/**
  Calculates the Moore--Penrose pseudo-inverse of the weighted Laplacian
  matrix of a given simplicial complex and returns it.

  @param K Simplicial complex

  @returns Moore--Penrose pseudo-inverse matrix. The indices of rows and
           columns follow the order of the vertices in the complex.
*/

template <class SimplicialComplex> auto pinvWeightedLaplacianMatrix( const SimplicialComplex& K ) -> Eigen::Matrix<typename SimplicialComplex::ValueType::DataType, Eigen::Dynamic, Eigen::Dynamic>
{
  auto L = weightedLaplacianMatrix( K );
  auto n = L.rows();

  using Matrix    = decltype(L);
  using Simplex   = typename SimplicialComplex::ValueType;
  using DataType  = typename Simplex::DataType;

  Matrix M = Matrix::Constant( L.rows(), L.cols(), static_cast<DataType>( 1/ n ) );

  return (M+L).inverse() - M;
}

#endif

/**
  @class HeatKernel
  @brief Calculates the heat kernel for simplicial complexes

  This class acts as a query functor for the heat kernel values of
  vertices in a weighted simplicial complex. It will pre-calculate
  the heat matrix and permit queries about the progression of heat
  values for *all* vertices for some time \f$t\f$.
*/

class HeatKernel
{
public:

  using T = double;

#ifdef ALEPH_WITH_EIGEN
  using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
  using Vector = Eigen::Matrix<T, 1, Eigen::Dynamic>;

#if EIGEN_VERSION_AT_LEAST(3,3,0)
  using IndexType  = Eigen::Index;
#else
  using IndexType  = typename Matrix::Index;
#endif

#else
  // This declares a fallback index type in case Eigen is not available,
  // making sure that the interface of the class remains the same.
  using IndexType = unsigned;
#endif

  /**
    Constructs a heat kernel from a given simplicial complex. Afterwards,
    the functor will be ready for queries.

    @param K Simplicial complex
  */

  template <class SimplicialComplex> HeatKernel( const SimplicialComplex& K )
  {
#ifdef ALEPH_WITH_EIGEN

    auto L = weightedLaplacianMatrix( K );

    Eigen::SelfAdjointEigenSolver< decltype(L) > solver;
    solver.compute( L );

    auto&& eigenvalues  = solver.eigenvalues(). template cast<T>();
    auto&& eigenvectors = solver.eigenvectors().template cast<T>();

    _eigenvalues.reserve( std::size_t( eigenvalues.size() ) );
    _eigenvectors.reserve( std::size_t( eigenvectors.size() ) );

    using IndexType_ = typename decltype(L)::Index;

    // If configured, skip both the first eigenvector and the first
    // eigenvalue because they do not contribute anything later on.
    for( IndexType_ i = _skip ? 1 : 0; i < eigenvalues.size(); i++ )
      _eigenvalues.push_back( eigenvalues(i) );

    for( IndexType_ i = _skip ? 1 : 0; i < eigenvectors.cols(); i++ )
      _eigenvectors.push_back( eigenvectors.col(i) );

#else
  (void) K;

  THROW_EIGEN_REQUIRED_ERROR();
#endif

  }

  /**
    Evaluates the heat kernel for *all* vertices at a given time \f$t\f$
    and returns the resulting values. This function is guaranteed to be
    more efficient than calling the per-element functions repeatedly.
  */

  std::vector<T> operator()( T t )
  {
#ifdef ALEPH_WITH_EIGEN

    Vector result = Vector();

    for( std::size_t k = 0; k < _eigenvalues.size(); k++ )
    {
      auto&& lk  = std::exp( -t * _eigenvalues[k] );
      auto&& uk = _eigenvectors[k];

      result += lk * uk * uk.transpose();
    }

    return std::vector<T>( result.data(), result.data() + result.size() );

#else
    (void) t;

    THROW_EIGEN_REQUIRED_ERROR();
#endif
  }

  /**
    Evaluates the heat kernel for two vertices \f$i\f$ and \f$j\f$ at
    a given time \f$t\f$ and returns the result.
  */

  T operator()( IndexType i, IndexType j, T t )
  {
#ifdef ALEPH_WITH_EIGEN

    aleph::math::KahanSummation<T> result = T();

    for( std::size_t k = 0; k < _eigenvalues.size(); k++ )
    {
      auto&& lk  = std::exp( -t * _eigenvalues[k] );
      auto&& uik = _eigenvectors[k](i);
      auto&& ujk = _eigenvectors[k](j);

      result += lk * uik * ujk;
    }

    return result;

#else
  (void) i;
  (void) j;
  (void) t;

  THROW_EIGEN_REQUIRED_ERROR();
#endif
  }

  /**
    Calculates the auto-diffusion for a given vertex \f$i\f$ and a given
    time \f$t\f$ and returns it.
  */

  T operator()( IndexType i, T t )
  {
#ifdef ALEPH_WITH_EIGEN

    // Note that this function could have been implemented in terms of
    // operator(i,j,t), but this implementation is a *little* bit more
    // efficient as it defines the multiplication explicitly.

    aleph::math::KahanSummation<T> result = T();

    for( std::size_t k = 0; k < _eigenvalues.size(); k++ )
    {
      auto&& lk  = std::exp( -t * _eigenvalues[k] );
      auto&& uik = _eigenvectors[k](i);

      result += lk * uik * uik;
    }

    return result;

#else
  (void) i;
  (void) t;

  THROW_EIGEN_REQUIRED_ERROR();
#endif
  }

  /**
    Calculates the *trace* of the heat kernel for a given time \f$t\f$
    and returns it.
  */

  T trace( T t ) const
  {
    aleph::math::KahanSummation<T> result = T();

    for( auto&& eigenvalue : _eigenvalues )
      result += std::exp( -t * eigenvalue );

    return result;
  }

  /**
    Calculates the *determinant* of the heat kernel for a given time
    \f$t\f$ and returns it.
  */

  T determinant( T t) const
  {
    T result = T();

    for( auto&& eigenvalue : _eigenvalues )
      result = result * std::exp( -t * eigenvalue );

    return result;
  }

  // Sampling intervals ------------------------------------------------

  /**
    Uses a heuristic to determine a sampling interval for the time
    parameter \f$t\f$ of the heat kernel. This heuristic was first
    described by Sun et al. in their paper *A Concise and Provably
    Informative Multi-Scale Signature Based on Heat Diffusion*.

    @param n Number of sampling points
    @returns Vector of sampling points
  */

  std::vector<T> logarithmicSamplingInterval( unsigned n ) const
  {
    auto t_min  = 4 * std::log( 10 ) / _eigenvalues.back();
    auto t_max  = 4 * std::log( 10 ) / ( _skip ? _eigenvalues.front() : *( _eigenvalues.begin() + 1 ) );
    auto offset = ( std::log( t_max ) - std::log( t_min ) ) / ( n - 1 );

    std::vector<T> samples;
    samples.reserve( n );

    for( unsigned i = 0; i < n; i++ )
      samples.push_back( std::log( t_min ) + i * offset );

    std::transform( samples.begin(), samples.end(),
                    samples.begin(),
                    [] ( const T x )
                    {
                      return std::pow( std::exp(1), x );
                    } );

    return samples;
  }

  // Configuration -----------------------------------------------------

  void setSkip( bool value = true ) { _skip = value; }
  bool skip() const noexcept        { return _skip;  }

private:

  /** If set, skips the first eigenvector and eigenvalue */
  bool _skip = false;

  /**
    Stores the eigenvalues of the heat matrix, or, more precisely, the
    eigenvalues of the Laplacian. They will be used for the evaluation
    of the heat kernel.
  */

  std::vector<T> _eigenvalues;

#ifdef ALEPH_WITH_EIGEN

  /** Stores the eigenvectors of the heat matrix */
  std::vector<Vector> _eigenvectors;

  /**
    Heat matrix; will be created automatically upon constructing this
    functor class.
  */

  Matrix _H;
#endif

};

} // namespace geometry

} // namespace aleph

#endif


/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <string>
#include <ros/ros.h>

#include <sensor_msgs/JointState.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Twist.h>

#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/frames.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/jntarrayvel.hpp>

#include <Eigen/Dense>
#include <kdl_conversions/kdl_msg.h>

class DebugEvaluateJointStates
{
    ros::NodeHandle nh_;
    ros::Subscriber jointstate_sub_;
    ros::Publisher manipulability_pub_;
    ros::Publisher twist_current_pub_;

    std::string chain_base_link_;
    std::string chain_tip_link_;

    KDL::Chain chain_;
    KDL::ChainFkSolverVel_recursive* p_fksolver_vel_;
    KDL::ChainJntToJacSolver* p_jnt2jac_;

public:
    int init()
    {
        if (!nh_.getParam("chain_base_link", this->chain_base_link_))
        {
            ROS_ERROR("Failed to get parameter \"chain_base_link\".");
            return -1;
        }

        if (!nh_.getParam("chain_tip_link", this->chain_tip_link_))
        {
            ROS_ERROR("Failed to get parameter \"chain_tip_link\".");
            return -2;
        }

        /// parse robot_description and generate KDL chains
        KDL::Tree my_tree;
        if (!kdl_parser::treeFromParam("/robot_description", my_tree))
        {
            ROS_ERROR("Failed to construct kdl tree");
            return -3;
        }

        my_tree.getChain(this->chain_base_link_, this->chain_tip_link_, chain_);
        if (chain_.getNrOfJoints() == 0)
        {
            ROS_ERROR("Failed to initialize kinematic chain");
            return -4;
        }

        p_fksolver_vel_ = new KDL::ChainFkSolverVel_recursive(chain_);
        p_jnt2jac_ = new KDL::ChainJntToJacSolver(chain_);

        /// initialize ROS interfaces
        jointstate_sub_ = nh_.subscribe("joint_states", 1, &DebugEvaluateJointStates::jointstateCallback, this);
        manipulability_pub_ = nh_.advertise<std_msgs::Float64> ("debug/manipulability", 1);
        twist_current_pub_ = nh_.advertise<geometry_msgs::Twist> ("debug/twist_current", 1);

        return 0;
    }

    void jointstateCallback(const sensor_msgs::JointState::ConstPtr& msg)
    {
        KDL::JntArray q = KDL::JntArray(chain_.getNrOfJoints());
        KDL::JntArray q_dot = KDL::JntArray(chain_.getNrOfJoints());

        for (unsigned int i = 0; i < msg->name.size(); i++)
        {
            q(i) = msg->position[i];
            q_dot(i) = msg->velocity[i];
        }

        /// compute current twist
        KDL::FrameVel FrameVel;
        KDL::JntArrayVel jntArrayVel = KDL::JntArrayVel(q, q_dot);
        if (p_fksolver_vel_->JntToCart(jntArrayVel, FrameVel, -1) >= 0)
        {
            geometry_msgs::Twist twist_msg;
            tf::twistKDLToMsg(FrameVel.GetTwist(), twist_msg);
            twist_current_pub_.publish(twist_msg);
        }

        /// compute manipulability
        KDL::Jacobian jac(chain_.getNrOfJoints());
        p_jnt2jac_->JntToJac(q, jac);
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> prod = jac.data * jac.data.transpose();
        double d = prod.determinant();
        double kappa = std::sqrt(std::abs(d));
        std_msgs::Float64 manipulability_msg;
        manipulability_msg.data = kappa;
        manipulability_pub_.publish(manipulability_msg);
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "debug_evaluate_jointstates_node");

    DebugEvaluateJointStates dejs;
    if (dejs.init() != 0)
    {
        ROS_ERROR("Failed to initialize DebugEvaluateJointStates.");
        return -1;
    }

    ros::spin();
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/constant/expx2c1.hpp>
#include <boost/simd/detail/constant/expx2c2.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/detail/constant/maxlog.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/signnz.hpp>
#include <boost/simd/function/sqr.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/function/is_equal.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF( expx2_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(const A0& a0) const
      {
        A0 x =  bs::abs(a0);
        // Represent x as an exact multiple of 1/32 plus a residual.
        A0 m = Expx2c1<A0>()*bs::floor(fma(Expx2c2<A0>(), x, bs::Half<A0>()));
        A0 f =  x-m;
        // x**2 = m**2 + 2mf + f**2
        A0 u = bs::sqr(m);
        A0 u1 = fma(m+m,f,sqr(f));
        // u is exact, u1 is small.
        A0 r = bs::if_else(bs::is_greater(u+u1, bs::Maxlog<A0>()),
                            bs::Inf<A0>(),
                            bs::exp(u)*bs::exp(u1));
        #ifndef BOOST_SIMD_NO_INFINITIES
        r =  bs::if_else(is_equal(x, Inf<A0>()), x, r);
        #endif
        return r;
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF( expx2_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(const A0& a0,  const A0 & s) const
      {
        A0 sgn =  signnz(s);
        A0 x =  a0*sgn;
        // Represent x as an exact multiple of 1/32 plus a residual.
        A0 m = Expx2c1<A0>()*bs::floor(fma(Expx2c2<A0>(), x, bs::Half<A0>()));
        A0 f =  x-m;
        // x**2 = m**2 + 2mf + f**2
        A0 u = sgn*bs::sqr(m);
        A0 u1 = sgn*fma(m+m,f,sqr(f));
        // u is exact, u1 is small.
        A0 r = bs::if_else(bs::is_greater(u+u1, bs::Maxlog<A0>()),
                            bs::Inf<A0>(),
                            bs::exp(u)*bs::exp(u1));
        #ifndef BOOST_SIMD_NO_INFINITIES
        r =  bs::if_else(is_equal(x, Inf<A0>()), x, r);
        #endif
        return r;
      }
   };

} } }

#endif