/*
Points to note regarding variable names:
total_loss and prev_loss actually refer not to loss, but the metric (usually BLEU)
*/
#include <sstream>
#include <iostream>
#include <vector>
#include <cassert>
#include <cmath>

//boost libraries
#include <boost/shared_ptr.hpp>
#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

//cdec libraries
#include "config.h"
#include "hg_sampler.h"
#include "sentence_metadata.h"
#include "scorer.h"
#include "verbose.h"
#include "viterbi.h"
#include "hg.h"
#include "prob.h"
#include "kbest.h"
#include "ff_register.h"
#include "decoder.h"
#include "filelib.h"
#include "fdict.h"
#include "weights.h"
#include "sparse_vector.h"
#include "sampler.h"

using namespace std;
namespace po = boost::program_options;

bool invert_score;
boost::shared_ptr<MT19937> rng; //random seed ptr

void RandomPermutation(int len, vector<int>* p_ids) {
  vector<int>& ids = *p_ids;
  ids.resize(len);
  for (int i = 0; i < len; ++i) ids[i] = i;
  for (int i = len; i > 0; --i) {
    int j = rng->next() * i;
    if (j == i) i--;
    swap(ids[i-1], ids[j]);
  }
}

bool InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("weights,w",po::value<string>(),"[REQD] Input feature weights file")
        ("input,i",po::value<string>(),"[REQD] Input source file for development set")
        ("passes,p", po::value<int>()->default_value(15), "Number of passes through the training data")
        ("weights_write_interval,n", po::value<int>()->default_value(1000), "Number of lines between writing out weights")
        ("reference,r",po::value<vector<string> >(), "[REQD] Reference translation(s) (tokenized text file)")
        ("mt_metric,m",po::value<string>()->default_value("ibm_bleu"), "Scoring metric (ibm_bleu, nist_bleu, koehn_bleu, ter, combi)")
        ("regularizer_strength,C", po::value<double>()->default_value(0.01), "regularization strength")
        ("mt_metric_scale,s", po::value<double>()->default_value(1.0), "Cost function is -mt_metric_scale*BLEU")
        ("costaug_log_bleu,l", "Flag converts BLEU to log space. Cost function is thus -mt_metric_scale*log(BLEU). Not on by default")
        ("average,A", "Average the weights (this is a weighted average due to the scaling factor)")
        ("mu,u", po::value<double>()->default_value(0.0), "weight (between 0 and 1) to scale model score by for oracle selection")
        ("stepsize_param,a", po::value<double>()->default_value(0.01), "Stepsize parameter, during optimization")
        ("stepsize_reduce,t", "Divide step size by sqrt(number of examples seen so far), as per Ratliff et al., 2007")
	("metric_threshold,T", po::value<double>()->default_value(0.0), "Threshold for diff between oracle BLEU and cost-aug BLEU for updating the weights")
	("check_positive,P", "Check that the loss is positive before updating")
        ("k_best_size,k", po::value<int>()->default_value(250), "Size of hypothesis list to search for oracles")
        ("best_ever,b", "Keep track of the best hypothesis we've ever seen (metric score), and use that as the reference")
        ("random_seed,S", po::value<uint32_t>(), "Random seed (if not specified, /dev/random will be used)")
        ("decoder_config,c",po::value<string>(),"Decoder configuration file");
  po::options_description clo("Command line options");
  clo.add_options()
        ("config", po::value<string>(), "Configuration file")
        ("help,h", "Print this help message and exit");
  po::options_description dconfig_options, dcmdline_options;
  dconfig_options.add(opts);
  dcmdline_options.add(opts).add(clo);

  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("config")) {
    ifstream config((*conf)["config"].as<string>().c_str());
    po::store(po::parse_config_file(config, dconfig_options), *conf);
  }
  po::notify(*conf);

  if (conf->count("help") || !conf->count("weights") || !conf->count("input") || !conf->count("decoder_config") || !conf->count("reference")) {
    cerr << dcmdline_options << endl;
    return false;
  }
  return true;
}

double scaling_trick = 1; // see http://blog.smola.org/post/940672544/fast-quadratic-regularization-for-online-learning
/*computes and returns cost augmented score for negative example selection*/
double cost_augmented_score(const LogVal<double> model_score, const double mt_metric_score, const double mt_metric_scale, const bool logbleu) {
  if(logbleu) {
    if(mt_metric_score != 0)
      // NOTE: log(model_score) is just the model score feature weights * features
      return log(model_score) * scaling_trick + (- mt_metric_scale * log(mt_metric_score));
    else
      return -1000000;
  }
  // NOTE: log(model_score) is just the model score feature weights * features
  return log(model_score) * scaling_trick + (- mt_metric_scale * mt_metric_score);
}

/*computes and returns mu score, for oracle selection*/
double muscore(const vector<weight_t>& feature_weights, const SparseVector<double>& feature_values, const double mt_metric_score, const double mu, const bool logbleu) {
  if(logbleu) {
    if(mt_metric_score != 0)
      return feature_values.dot(feature_weights) * mu + (1 - mu) * log(mt_metric_score);
    else
      return feature_values.dot(feature_weights) * mu + (1 - mu) * (-1000000);  // log(0) is -inf
  }
  return feature_values.dot(feature_weights) * mu + (1 - mu) * mt_metric_score;
}

static const double kMINUS_EPSILON = -1e-6;

struct HypothesisInfo {
  SparseVector<double> features;
  double mt_metric_score;
  // The model score changes when the feature weights change, so it is not stored here
  // It must be recomputed every time
};

struct GoodOracle {
  boost::shared_ptr<HypothesisInfo> good;
};

struct TrainingObserver : public DecoderObserver {
  TrainingObserver(const int k,
                   const DocScorer& d,
                   vector<GoodOracle>* o,
                   const vector<weight_t>& feat_weights,
                   const double metric_scale,
                   const double Mu,
                   const bool bestever,
                   const bool LogBleu) : ds(d), feature_weights(feat_weights), oracles(*o), kbest_size(k), mt_metric_scale(metric_scale), mu(Mu), best_ever(bestever), log_bleu(LogBleu) {}
  const DocScorer& ds;
  const vector<weight_t>& feature_weights;
  vector<GoodOracle>& oracles;
  boost::shared_ptr<HypothesisInfo> cur_best;
  boost::shared_ptr<HypothesisInfo> cur_costaug_best;
  boost::shared_ptr<HypothesisInfo> cur_ref;
  const int kbest_size;
  const double mt_metric_scale;
  const double mu;
  const bool best_ever;
  const bool log_bleu;

  const HypothesisInfo& GetCurrentBestHypothesis() const {
    return *cur_best;
  }

  const HypothesisInfo& GetCurrentCostAugmentedHypothesis() const {
    return *cur_costaug_best;
  }

  const HypothesisInfo& GetCurrentReference() const {
    return *cur_ref;
  }

  virtual void NotifyTranslationForest(const SentenceMetadata& smeta, Hypergraph* hg) {
    UpdateOracles(smeta.GetSentenceID(), *hg);
  }

  boost::shared_ptr<HypothesisInfo> MakeHypothesisInfo(const SparseVector<double>& feats, const double metric) {
    boost::shared_ptr<HypothesisInfo> h(new HypothesisInfo);
    h->features = feats;
    h->mt_metric_score = metric;
    return h;
  }

  void UpdateOracles(int sent_id, const Hypergraph& forest) {
    //shared_ptr<HypothesisInfo>& cur_ref = oracles[sent_id].good;
    cur_ref = oracles[sent_id].good;
    if(!best_ever)
      cur_ref.reset();

    KBest::KBestDerivations<vector<WordID>, ESentenceTraversal> kbest(forest, kbest_size);
    double costaug_best_score = 0;

    for (int i = 0; i < kbest_size; ++i) {
      const KBest::KBestDerivations<vector<WordID>, ESentenceTraversal>::Derivation* d =
        kbest.LazyKthBest(forest.nodes_.size() - 1, i);
      if (!d) break;
      double mt_metric_score = ds[sent_id]->ScoreCandidate(d->yield)->ComputeScore(); //this might need to change!!
      const SparseVector<double>& feature_vals = d->feature_values;
      double costaugmented_score = cost_augmented_score(d->score, mt_metric_score, mt_metric_scale, log_bleu); //note that d->score, i.e., model score, is passed in
      if (i == 0) { //i.e., setting up cur_best to be model score highest, and initializing costaug_best
        cur_best = MakeHypothesisInfo(feature_vals, mt_metric_score);
        cur_costaug_best = cur_best;
        costaug_best_score = costaugmented_score;
      }
      if (costaugmented_score > costaug_best_score) {   // kbest_mira's cur_bad, i.e., "fear" derivation
        cur_costaug_best = MakeHypothesisInfo(feature_vals, mt_metric_score);
        costaug_best_score = costaugmented_score;
      }
      double cur_muscore = mt_metric_score;
      if (!cur_ref)   // kbest_mira's cur_good, i.e., "hope" derivation
        cur_ref =  MakeHypothesisInfo(feature_vals, cur_muscore);
      else {
          double cur_ref_muscore = cur_ref->mt_metric_score;
          if(mu > 0) { //select oracle with mixture of model score and BLEU
              cur_ref_muscore =  muscore(feature_weights, cur_ref->features, cur_ref->mt_metric_score, mu, log_bleu);
              cur_muscore = muscore(feature_weights, d->feature_values, mt_metric_score, mu, log_bleu);
          }
          if (cur_muscore > cur_ref_muscore) //replace oracle
            cur_ref = MakeHypothesisInfo(feature_vals, mt_metric_score);
      }
    }
  }
};

void ReadTrainingCorpus(const string& fname, vector<string>* c) {
  ReadFile rf(fname);
  istream& in = *rf.stream();
  string line;
  while(in) {
    getline(in, line);
    if (!in) break;
    c->push_back(line);
  }
}

bool ApproxEqual(double a, double b) {
  if (a == b) return true;
  return (fabs(a-b)/fabs(b)) < 0.000001;
}

int main(int argc, char** argv) {
  register_feature_functions();
  SetSilent(true);  // turn off verbose decoder output

  po::variables_map conf;
  if (!InitCommandLine(argc, argv, &conf)) return 1;

  if (conf.count("random_seed"))
    rng.reset(new MT19937(conf["random_seed"].as<uint32_t>()));
  else
    rng.reset(new MT19937);

  const bool best_ever = conf.count("best_ever") > 0;
  vector<string> corpus;
  ReadTrainingCorpus(conf["input"].as<string>(), &corpus);

  const string metric_name = conf["mt_metric"].as<string>(); //set up scoring; this may need to be changed!!

  ScoreType type = ScoreTypeFromString(metric_name);
  if (type == TER) {
    invert_score = true;
  } else {
    invert_score = false;
  }
  DocScorer ds(type, conf["reference"].as<vector<string> >(), "");
  cerr << "Loaded " << ds.size() << " references for scoring with " << metric_name << endl;
  if (ds.size() != corpus.size()) {
    cerr << "Mismatched number of references (" << ds.size() << ") and sources (" << corpus.size() << ")\n";
    return 1;
  }

  ReadFile ini_rf(conf["decoder_config"].as<string>());
  Decoder decoder(ini_rf.stream());

  // load initial weights
  vector<weight_t>& decoder_weights = decoder.CurrentWeightVector(); //equivalent to "dense_weights" vector in kbest_mira.cc
  SparseVector<weight_t> sparse_weights; //equivaelnt to  kbest_mira.cc "lambdas"
  Weights::InitFromFile(conf["weights"].as<string>(), &decoder_weights);
  Weights::InitSparseVector(decoder_weights, &sparse_weights);

  //initializing other algorithm and output parameters
  const double c = conf["regularizer_strength"].as<double>();
  const int weights_write_interval = conf["weights_write_interval"].as<int>();
  const double mt_metric_scale = conf["mt_metric_scale"].as<double>();
  const double mu = conf["mu"].as<double>();
  const double metric_threshold = conf["metric_threshold"].as<double>();
  const double stepsize_param = conf["stepsize_param"].as<double>(); //step size in structured SGD optimization step
  const bool stepsize_reduce = conf.count("stepsize_reduce") > 0;
  const bool costaug_log_bleu = conf.count("costaug_log_bleu") > 0;
  const bool average = conf.count("average") > 0;
  const bool checkpositive = conf.count("check_positive") > 0;

  assert(corpus.size() > 0);
  vector<GoodOracle> oracles(corpus.size());
  TrainingObserver observer(conf["k_best_size"].as<int>(),  // kbest size
                            ds,                             // doc scorer
                            &oracles,
                            decoder_weights,
                            mt_metric_scale,
                            mu,
                            best_ever,
                            costaug_log_bleu);
  int cur_sent = 0;
  int line_count = 0;
  int normalizer = 0;
  double total_loss = 0;
  double prev_loss = 0;
  int dots = 0;             // progess bar
  int cur_pass = 0;
  SparseVector<double> tot;
  tot += sparse_weights; //add initial weights to total
  normalizer++; //add 1 to normalizer
  int max_iteration = conf["passes"].as<int>();
  string msg = "# LatentSVM tuned weights";
  vector<int> order;
  int interval_counter = 0;
  RandomPermutation(corpus.size(), &order); //shuffle corpus
  while (line_count <= max_iteration * corpus.size()) { //loop over all (passes * num sentences) examples
    //if ((interval_counter * 40 / weights_write_interval) > dots) { ++dots; cerr << '.'; } //check this
    if ((cur_sent * 40 / corpus.size()) > dots) { ++dots; cerr << '.';}
    if (interval_counter == weights_write_interval) { //i.e., we need to write out weights
      sparse_weights *= scaling_trick;
      tot *= scaling_trick;
      scaling_trick = 1;
      cerr << " [SENTENCE NUMBER= " << cur_sent << "\n";
      cerr << " [AVG METRIC LAST INTERVAL =" << ((total_loss - prev_loss) / weights_write_interval) << "]\n";
      cerr << " [AVG METRIC THIS PASS THUS FAR =" << (total_loss / cur_sent) << "]\n";
      cerr << " [TOTAL LOSS: =" << total_loss << "\n";
      Weights::ShowLargestFeatures(decoder_weights);
      //dots = 0;
      interval_counter = 0;
      prev_loss = total_loss;
      if (average){
	SparseVector<double> x = tot;
	x /= normalizer;
	ostringstream sa;
	sa << "weights.latentsvm-" << line_count/weights_write_interval << "-avg.gz";
	x.init_vector(&decoder_weights);
	Weights::WriteToFile(sa.str(), decoder_weights, true, &msg);
      }
      else {
	ostringstream os;
	os << "weights.latentsvm-" << line_count/weights_write_interval << ".gz";
	sparse_weights.init_vector(&decoder_weights);
	Weights::WriteToFile(os.str(), decoder_weights, true, &msg);
      }
    }
    if (corpus.size() == cur_sent) { //i.e., finished a pass
      //cerr << " [AVG METRIC LAST PASS=" << (document_metric_score / corpus.size()) << "]\n";
      cerr << " [AVG METRIC LAST PASS=" << (total_loss / corpus.size()) << "]\n";
      cerr << " TOTAL LOSS: " << total_loss << "\n";
      Weights::ShowLargestFeatures(decoder_weights);
      cur_sent = 0;
      total_loss = 0;
      dots = 0;
      if(average) {
        SparseVector<double> x = tot;
        x /= normalizer;
        ostringstream sa;
        sa << "weights.latentsvm-pass" << (cur_pass < 10 ? "0" : "") << cur_pass << "-avg.gz";
        x.init_vector(&decoder_weights);
        Weights::WriteToFile(sa.str(), decoder_weights, true, &msg);
      }
      else {
	ostringstream os;
	os << "weights.latentsvm-pass" << (cur_pass < 10 ? "0" : "") << cur_pass << ".gz";
	Weights::WriteToFile(os.str(), decoder_weights, true, &msg);
      }
      cur_pass++;
      RandomPermutation(corpus.size(), &order);
    }
    if (cur_sent == 0) { //i.e., starting a new pass
      cerr << "PASS " << (line_count / corpus.size() + 1) << endl;
    }
    sparse_weights.init_vector(&decoder_weights);   // copy sparse_weights to the decoder weights
    decoder.SetId(order[cur_sent]); //assign current sentence
    decoder.Decode(corpus[order[cur_sent]], &observer);  // decode/update oracles

    const HypothesisInfo& cur_best = observer.GetCurrentBestHypothesis(); //model score best
    const HypothesisInfo& cur_costaug = observer.GetCurrentCostAugmentedHypothesis(); //(model + cost) best; cost = -metric_scale*log(BLEU) or -metric_scale*BLEU
    //const HypothesisInfo& cur_ref = *oracles[order[cur_sent]].good; //this oracle-best line only picks based on BLEU
    const HypothesisInfo& cur_ref = observer.GetCurrentReference();  //if mu > 0, this mu-mixed oracle will be picked; otherwise, only on BLEU
    total_loss += cur_best.mt_metric_score;

    double step_size = stepsize_param;
    if (stepsize_reduce){       // w_{t+1} = w_t - stepsize_t * grad(Loss)
        step_size  /= (sqrt(cur_sent+1.0));
    }
    //actual update step - compute gradient, and modify sparse_weights
    if(cur_ref.mt_metric_score - cur_costaug.mt_metric_score > metric_threshold) {
      const double loss = (cur_costaug.features.dot(decoder_weights) - cur_ref.features.dot(decoder_weights)) * scaling_trick + mt_metric_scale * (cur_ref.mt_metric_score - cur_costaug.mt_metric_score);
      if (!checkpositive || loss > 0.0) { //can update either all the time if check positive is off, or only when loss > 0 if it's on
	sparse_weights -= cur_costaug.features * step_size / ((1.0-2.0*step_size*c)*scaling_trick);    // cost augmented hyp orig -
	sparse_weights += cur_ref.features * step_size / ((1.0-2.0*step_size*c)*scaling_trick);        // ref orig +
      }
    }
    scaling_trick *= (1.0 - 2.0 * step_size * c);

    tot += sparse_weights; //for averaging purposes
    normalizer++; //for averaging purposes
    line_count++;
    interval_counter++;
    cur_sent++;
  }
  cerr << endl;
  if(average) {
    tot /= normalizer;
    tot.init_vector(decoder_weights);
    msg = "# Latent SSVM tuned weights (averaged vector)";
    Weights::WriteToFile("weights.latentsvm-final-avg.gz", decoder_weights, true, &msg);
    cerr << "Optimization complete.\n" << "AVERAGED WEIGHTS: weights.latentsvm-final-avg.gz\n";
  } else {
    Weights::WriteToFile("weights.latentsvm-final.gz", decoder_weights, true, &msg);
    cerr << "Optimization complete.\n";
  }
  return 0;
}


#ifndef YOLO_DETECTOR_H
#define YOLO_DETECTOR_H

#include <opencv2/dnn.hpp>
#include <opencv2/dnn/shape_utils.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <fstream>
#include <iostream>
#include <Eigen/Core>
#include "../models/phd_gaussian_mixture.hpp"

using namespace std;
using namespace cv;
using namespace cv::dnn;
using namespace Eigen;

class YOLODetector{
    public:
        YOLODetector();
        YOLODetector(string model_cfg, string model_binary, string class_names, float min_confidence);
        bool is_initialized();
        vector<Rect> detect(Mat frame);
        void draw(Mat frame, Scalar color = Scalar(0, 255, 0));
        VectorXd weights;

    private:
        bool initialized;
        vector<Rect> detections;
        vector<String> classes;
        Net net;
        float min_confidence;
        vector<String> class_names_vec;
        vector<String> getOutputsNames(const Net& net);
};

#endif


/**
 * @file descartes_tesseract_kinematics.h
 * @brief Implememntatino of a wrapper around tesseract kinematics for the descartes_light kinematics interface
 *
 * @author Matthew Powelson
 * @author Levi Armstrong
 * @date September 17, 2019
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2019, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * 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
 * @par
 * 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.
 */
#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_TESSERACT_KINEMATICS_HPP
#define TESSERACT_MOTION_PLANNERS_DESCARTES_TESSERACT_KINEMATICS_HPP

#include <tesseract_motion_planners/descartes/descartes_tesseract_kinematics.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <Eigen/Eigen>
#include <console_bridge/console.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

namespace tesseract_motion_planners
{
template <typename FloatType>
bool DescartesTesseractKinematics<FloatType>::ik(const Eigen::Transform<FloatType, 3, Eigen::Isometry>& p,
                                                 std::vector<FloatType>& solution_set) const
{
  return ik(p, is_valid_fn_, redundant_sol_fn_, solution_set);
}

template <typename FloatType>
bool DescartesTesseractKinematics<FloatType>::ik(
    const Eigen::Transform<FloatType, 3, Eigen::Isometry>& p,
    const descartes_light::IsValidFn<FloatType>& is_valid_fn,
    const descartes_light::GetRedundantSolutionsFn<FloatType>& redundant_sol_fn,
    std::vector<FloatType>& solution_set) const
{
  auto dof = static_cast<int>(tesseract_ik_->numJoints());

  // Convert to appropriate Eigen types
  Eigen::Isometry3d p_double;
  p_double = p.template cast<double>();
  Eigen::VectorXd solution_eigen;

  // Solve IK
  if (!tesseract_ik_->calcInvKin(solution_eigen, p_double, ik_seed_))
    return false;

  // Convert back to a float array
  Eigen::Matrix<FloatType, Eigen::Dynamic, 1> solution_float_type;
  solution_float_type = solution_eigen.template cast<FloatType>();
  FloatType* sol = solution_float_type.data();

  // Apply is_valid_fn and redundant_sol_fn
  if (is_valid_fn && redundant_sol_fn)
  {
    if (is_valid_fn_(sol))
      solution_set.insert(end(solution_set), sol, sol + dof);  // If good then add to solution set

    std::vector<FloatType> redundant_sols = redundant_sol_fn(sol);
    if (!redundant_sols.empty())
    {
      int num_sol = static_cast<int>(redundant_sols.size()) / dof;
      for (int s = 0; s < num_sol; ++s)
      {
        FloatType* redundant_sol = redundant_sols.data() + dof * s;
        if (is_valid_fn_(redundant_sol))
          solution_set.insert(end(solution_set), redundant_sol, redundant_sol + dof);  // If good then add to solution
                                                                                // set
      }
    }
  }
  else if (is_valid_fn && !redundant_sol_fn)
  {
    if (is_valid_fn(sol))
      solution_set.insert(end(solution_set), sol, sol + dof);  // If good then add to solution set
    else
    {
      // If it failed the is_valid_fn get solution that is +/-pi and retry
      descartes_light::harmonizeTowardZero<FloatType>(sol, dof);
      if (is_valid_fn(sol))
        solution_set.insert(end(solution_set), sol, sol + dof);
    }
  }
  else if (!is_valid_fn && redundant_sol_fn)
  {
    solution_set.insert(end(solution_set), sol, sol + dof);  // If good then add to solution set

    std::vector<FloatType> redundant_sols = redundant_sol_fn(sol);
    if (!redundant_sols.empty())
    {
      long num_sol = static_cast<long>(redundant_sols.size()) / dof;
      for (long s = 0; s < num_sol; ++s)
      {
        FloatType* redundant_sol = redundant_sols.data() + dof * s;
        solution_set.insert(end(solution_set), redundant_sol, redundant_sol + dof);  // If good then add to solution
                                                                                // set
      }
    }
  }
  else
  {
    solution_set.insert(end(solution_set), sol, sol + dof);
  }
  return !solution_set.empty();
}

template <typename FloatType>
bool DescartesTesseractKinematics<FloatType>::fk(const FloatType* pose,
                                                 Eigen::Transform<FloatType, 3, Eigen::Isometry>& solution) const
{
  assert(pose);

  // Convert the Array to an Eigen VectorXd
  Eigen::VectorXd joints(tesseract_fk_->numJoints());
  for (int i = 0; static_cast<unsigned int>(i) < tesseract_fk_->numJoints(); i++)
    joints(i, 0) = pose[i];

  // Get the solution from the Tesseract Kinematics
  Eigen::Isometry3d solution_double;
  bool success = tesseract_fk_->calcFwdKin(solution_double, joints);

  // Cast from double to FloatType
  solution = solution_double.cast<FloatType>();
  return success;
}

template <typename FloatType>
int DescartesTesseractKinematics<FloatType>::dof() const
{
  assert(tesseract_fk_->numJoints() < std::numeric_limits<int>::max());
  return static_cast<int>(tesseract_fk_->numJoints());
}

template <typename FloatType>
void DescartesTesseractKinematics<FloatType>::analyzeIK(const Eigen::Transform<FloatType, 3, Eigen::Isometry>& p) const
{
  Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "AnalyzeIK: ", ";");

  std::stringstream ss;
  ss << p.matrix().format(CommaInitFmt);
  CONSOLE_BRIDGE_logInform(ss.str().c_str());

  std::string valid_fn_defined = "\tIs Valid Function: " + std::string(is_valid_fn_ ? "True" : "False");
  CONSOLE_BRIDGE_logInform(valid_fn_defined.c_str());
  std::string redundant_fn_defined = "\tIs Valid Function: " + std::string(is_valid_fn_ ? "True" : "False");
  CONSOLE_BRIDGE_logInform(redundant_fn_defined.c_str());

  std::vector<FloatType> solution_set;
  ik(p, nullptr, nullptr, solution_set);
  ss.clear();
  ss << "\tSampling without functions, found solutions: " << solution_set.size() / 8;
  CONSOLE_BRIDGE_logInform(ss.str().c_str());

  solution_set.clear();
  ik(p, is_valid_fn_, nullptr, solution_set);
  ss.clear();
  ss << "\tSampling with only IsValid functions, found solutions: " << solution_set.size() / 8;
  CONSOLE_BRIDGE_logInform(ss.str().c_str());

  solution_set.clear();
  ik(p, nullptr, redundant_sol_fn_, solution_set);
  ss.clear();
  ss << "\tSampling with only Redundant Solutions functions, found solutions: " << solution_set.size() / 8;
  CONSOLE_BRIDGE_logInform(ss.str().c_str());

  solution_set.clear();
  ik(p, is_valid_fn_, redundant_sol_fn_, solution_set);
  ss.clear();
  ss << "\tSampling with both functions, found solutions: " << solution_set.size() / 8;
  CONSOLE_BRIDGE_logInform(ss.str().c_str());
}

template <typename FloatType>
void DescartesTesseractKinematics<FloatType>::setIKSeed(
    const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1> >& seed)
{
  assert(static_cast<int>(seed.size()) == dof());
  ik_seed_ = seed.template cast<double>();
}

template <typename FloatType>
void DescartesTesseractKinematics<FloatType>::setIKSeed(const std::vector<FloatType>& seed)
{
  assert(static_cast<int>(seed.size()) == dof());
  std::vector<double> seed_copy;
  seed_copy.reserve(seed.size());
  for (auto& i : seed)
    seed_copy.push_back(static_cast<double>(i));
  ik_seed_ = Eigen::Map<Eigen::VectorXd>(seed_copy.data(), static_cast<long>(seed_copy.size()));
}
}  // namespace tesseract_motion_planners
#endif


/******************************************************************************
 * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved.
 *
 * 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 "iba_helper.h"
#include <IBA_config.h>  // for CFG_STEREO
#include <Eigen/Core>
#include <Eigen/Dense>

#include "../Geometry/Rotation.h"

namespace XP {
IBA::Intrinsic to_iba_intrinsic(const DuoCalibParam::Camera_t& cam,
                                const int lr) {
  IBA::Intrinsic K;
  K.fx = cam.cameraK_lr[lr](0, 0);
  K.fy = cam.cameraK_lr[lr](1, 1);
  K.cx = cam.cameraK_lr[lr](0, 2);
  K.cy = cam.cameraK_lr[lr](1, 2);
  for (int i = 0; i < 8; ++i) {
    if (i < cam.cv_dist_coeff_lr[lr].rows) {
      K.ds[i] = cam.cv_dist_coeff_lr[lr].at<float>(i);
    } else {
      K.ds[i] = 0.f;
    }
  }
  return K;
}

IBA::Calibration to_iba_calibration(const DuoCalibParam& calib) {
  IBA::Calibration iba_calib;
  iba_calib.w = calib.Camera.img_size.width;
  iba_calib.h = calib.Camera.img_size.height;
  iba_calib.fishEye = false;
  Eigen::Matrix4f Cl_T_I = calib.Camera.D_T_C_lr[0].inverse() * calib.Imu.D_T_I;
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 4; ++j) {
      iba_calib.Tu[i][j] = Cl_T_I(i, j);
    }
  }
  Rotation3D R;
  R.Set(iba_calib.Tu[0], iba_calib.Tu[1], iba_calib.Tu[2]);
  R.MakeOrthogonal();
  R.Get(iba_calib.Tu[0], iba_calib.Tu[1], iba_calib.Tu[2]);
  iba_calib.K = to_iba_intrinsic(calib.Camera, 0);

#ifdef CFG_STEREO
  Eigen::Matrix4f Cl_T_Cr =
      calib.Camera.D_T_C_lr[0].inverse() * calib.Camera.D_T_C_lr[1];
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 4; ++j) {
      iba_calib.Tr[i][j] = Cl_T_Cr(i, j);
    }
  }
  R.Set(iba_calib.Tr[0], iba_calib.Tr[1], iba_calib.Tr[2]);
  R.MakeOrthogonal();
  R.Get(iba_calib.Tr[0], iba_calib.Tr[1], iba_calib.Tr[2]);
  iba_calib.Kr = to_iba_intrinsic(calib.Camera, 1);
#endif

  for (int i = 0; i < 3; ++i) {
    iba_calib.ba[i] = calib.Imu.accel_bias(i);
    iba_calib.bw[i] = calib.Imu.gyro_bias(i);
  }
  return iba_calib;
}

IBA::IMUMeasurement to_iba_imu(const ImuData& imu_in) {
  IBA::IMUMeasurement imu_out;
  imu_out.t = imu_in.time_stamp;
  for (size_t i = 0; i < 3; ++i) {
    imu_out.a[i] = imu_in.accel(i);
    imu_out.w[i] = imu_in.ang_v(i);
  }
  return imu_out;
}

/*

IBA::CameraPose vio_to_iba_pose(const Eigen::Matrix4f& T) {
  IBA::CameraPose pose;
  for (size_t i = 0; i < 3; ++i) {
    for (size_t j = 0; j < 3; ++j) {
      pose.R[i][j] = T(i, j);
    }
    pose.p[i] = T(i, 3);
  }
  return pose;
}

IBA::CameraIMUState vio_to_iba_state(const Eigen::Matrix4f& W_T_C,
                                     const VIO::CalibParam& calib) {
  // TODO(mingyu): Need to save ground-truth velocity from synthetic data
  IBA::CameraIMUState state;
  for (int i = 0; i < 3; ++i) {
    // Check IBA_datatype.h for the CameraPose notation:
    // R is R_CW, p is the camera position in W
    state.C.p[i] = W_T_C(i, 3);
    for (int j = 0; j < 3; ++j) {
      state.C.R[i][j] = W_T_C(j, i);  // Transpose here!
    }
    state.v[i] = 0.f;  // WRONG velocity for now
    state.ba[i] = calib.Imu.accel_bias(i);
    state.bw[i] = calib.Imu.gyro_bias(i);
  }
  return state;
}
*/

}  // namespace XP


#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <splineread.hh>
#include <TF1.h>
#include <TGraph.h>

splineread::splineread(std::string xmlfilename)
{
    boost::property_tree::ptree pt;
    boost::property_tree::xml_parser::read_xml(xmlfilename, pt);
    const auto genie_xsec_spline_list = pt.get_child("genie_xsec_spline_list");
    version = genie_xsec_spline_list.get<std::string>("<xmlattr>.version");
    uselog = genie_xsec_spline_list.get<bool>("<xmlattr>.uselog");
    const auto genie_tune = genie_xsec_spline_list.get_child("genie_tune");
    genie_tune_name = genie_tune.get<std::string>("<xmlattr>.name");
    for (const auto &spline : genie_tune)
    {
        if (spline.first == std::string("spline"))
        {
            const auto spline_name = spline.second.get<std::string>("<xmlattr>.name");
            namelist.push_back(spline_name);
            const auto spline_nknots = spline.second.get<double>("<xmlattr>.nknots");
            // next version of CERN ROOT will provide TGraph::AddPoint(), in that case
            // we could emit those std::vector containers.
            std::vector<double> Elist, xsec_list;
            Elist.reserve(spline_nknots);
            xsec_list.reserve(spline_nknots);
            for (const auto &knot : spline.second)
            {
                if (knot.first == std::string("knot"))
                {
                    Elist.push_back(knot.second.get<double>("E"));
                    xsec_list.push_back(knot.second.get<double>("xsec"));
                }
            }
            TGraph gh(spline_nknots, &Elist[0], &xsec_list[0]);
            // TGraph::Eval can sometimes used to do envaluing, while
            // TF1 can do more things, initialize a TF1 instance here
            // by lambda, TGraph instance is copied in. Life cycle is
            // managed by TF1. Hope TF1 won't introduce memory leaks.
            splinelist.insert({spline_name, new TF1(
                                                spline_name.c_str(),
                                                [gh](double *x, double *) {
                                                    return gh.Eval(x[0]);
                                                },
                                                0, 1000, 0)});
        }
    }
}

splineread::splineread(const splineread &rhs) : version(rhs.version),
                                          uselog(rhs.uselog),
                                          genie_tune_name(rhs.genie_tune_name),
                                          namelist(rhs.namelist)
{
    for (const auto &i : rhs.splinelist)
    {
        splinelist[i.first] = (TF1 *)i.second->Clone();
    }
}

splineread::splineread(splineread &&rhs) : version(rhs.version),
                                           uselog(rhs.uselog),
                                           genie_tune_name(rhs.genie_tune_name),
                                           namelist(rhs.namelist)
{
    splinelist = rhs.splinelist;
    rhs.splinelist.clear();
}

splineread::~splineread()
{
    for (const auto &i : splinelist)
    {
        delete i.second;
    }
}

TF1 *splineread::get_spline(const std::string splinename) const
{
    auto f = splinelist.find(splinename);
    if (f == splinelist.end())
    {
        return nullptr;
    }
    else
    {
        return f->second;
    }
}


// Boost.Geometry

// Copyright (c) 2021-2022, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGY_GEOGRAPHIC_ENVELOPE_RANGE_HPP
#define BOOST_GEOMETRY_STRATEGY_GEOGRAPHIC_ENVELOPE_RANGE_HPP

#include <boost/geometry/strategy/geographic/envelope_segment.hpp>
#include <boost/geometry/strategy/geographic/expand_segment.hpp>
#include <boost/geometry/strategy/spherical/envelope_range.hpp>

// Get rid of this dependency?
#include <boost/geometry/strategies/spherical/point_in_poly_winding.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace envelope
{

template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = geometry::srs::spheroid<double>,
    typename CalculationType = void
>
class geographic_linestring
{
public:
    using model_type = Spheroid;

    geographic_linestring()
        : m_spheroid()
    {}

    explicit geographic_linestring(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename Range, typename Box>
    void apply(Range const& range, Box& mbr) const
    {
        auto const envelope_s = envelope::geographic_segment
            <
                FormulaPolicy, Spheroid, CalculationType
            >(m_spheroid);
        auto const expand_s = expand::geographic_segment
            <
                FormulaPolicy, Spheroid, CalculationType
            >(m_spheroid);
        detail::spheroidal_linestring(range, mbr, envelope_s, expand_s);
    }

    Spheroid model() const
    {
        return m_spheroid;
    }

private:
    Spheroid m_spheroid;
};

template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = geometry::srs::spheroid<double>,
    typename CalculationType = void
>
class geographic_ring
{
public:
    using model_type = Spheroid;

    geographic_ring()
        : m_spheroid()
    {}

    explicit geographic_ring(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename Range, typename Box>
    void apply(Range const& range, Box& mbr) const
    {
        auto const envelope_s = envelope::geographic_segment
            <
                FormulaPolicy, Spheroid, CalculationType
            >(m_spheroid);
        auto const expand_s = expand::geographic_segment
            <
                FormulaPolicy, Spheroid, CalculationType
            >(m_spheroid);
        auto const within_s = within::detail::spherical_winding_base
            <
                envelope::detail::side_of_pole<CalculationType>, CalculationType
            >();
        detail::spheroidal_ring(range, mbr, envelope_s, expand_s, within_s);
    }

    Spheroid model() const
    {
        return m_spheroid;
    }

private:
    Spheroid m_spheroid;
};

}} // namespace strategy::envelope

}} //namepsace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_GEOGRAPHIC_ENVELOPE_RANGE_HPP


#include <scitbx/array_family/boost_python/shared_wrapper.h>
#include <cctbx/miller/randomize_amplitude_and_phase.h>
#include <boost/python/def.hpp>

namespace cctbx { namespace miller { namespace boost_python {

  template <typename ComplexType, typename FloatType>
  struct randomize_amplitude_and_phase_wrapper
  {
    static void wrap() {
      using namespace boost::python;
      return_value_policy<return_by_value> rbv;

      def("randomize_amplitude_and_phase",
        (af::shared<ComplexType>(*)
          (af::const_ref<ComplexType> const&,
           af::const_ref<bool> const&,
           FloatType const&,
           FloatType const&,
           int))
             randomize_amplitude_and_phase, (
               arg("data"),
               arg("selection"),
               arg("amplitude_error"),
               arg("phase_error_deg"),
               arg("random_seed")))
      ;
    }
  };

  void wrap_randomize_amplitude_and_phase() {
    randomize_amplitude_and_phase_wrapper<std::complex<double>, double>::wrap();
  }
}}} // cctbx::miller::boostpython


#include <benchmark/benchmark.h>
#include <sstream>
#include <random>
#include <cmath>
#include <charconv>
#include <absl/strings/numbers.h>
#include <absl/strings/str_format.h>
#include <boost/lexical_cast.hpp>
#include <google/protobuf/stubs/strutil.h>

uint64_t tag = 1234567890123456;
float tagf = M_PI;

template<typename S, typename T>
void NumberToString(T &t, std::string &str) {
    thread_local static S oss;
    oss.str("");
    oss << t;
    str = oss.str();
}

template<typename S, typename T>
static void BenchStreamStringFrom(benchmark::State &state) {
    std::string r;
    for (auto _ : state) {
        NumberToString<S, T>(tag, r);
        benchmark::DoNotOptimize(r);
    }
}

template<typename S, typename T>
static void BenchStreamStringFromF(benchmark::State &state) {
    std::string r;
    for (auto _ : state) {
        NumberToString<S, T>(tagf, r);
        benchmark::DoNotOptimize(r);
    }
}

BENCHMARK_TEMPLATE(BenchStreamStringFrom, std::stringstream, uint64_t);
BENCHMARK_TEMPLATE(BenchStreamStringFrom, std::ostringstream, uint64_t);

BENCHMARK_TEMPLATE(BenchStreamStringFromF, std::stringstream, float);
BENCHMARK_TEMPLATE(BenchStreamStringFromF, std::ostringstream, float);

template<typename T>
static void BenchItoa(benchmark::State &state) {
    for (auto _ : state) {
        char buf[64]{};
        auto r = itoa(tag, buf, 10);
        benchmark::DoNotOptimize(r);
    }
}
BENCHMARK_TEMPLATE(BenchItoa, uint64_t);

template<typename T>
static void BenchGcvt(benchmark::State &state) {
    for (auto _ : state) {
        char buf[64]{};
        auto r = gcvt(tagf, 11, buf);
        benchmark::DoNotOptimize(r);
    }
}
BENCHMARK_TEMPLATE(BenchGcvt, float);

template<typename T>
static void BenchToString(benchmark::State &state) {
    for (auto _ : state) {
        auto r = std::to_string(tag);
        benchmark::DoNotOptimize(r);
    }
}
BENCHMARK_TEMPLATE(BenchToString, uint64_t);

template<typename T>
static void BenchToStringF(benchmark::State &state) {
    for (auto _ : state) {
        auto r = std::to_string(tagf);
        benchmark::DoNotOptimize(r);
    }
}
BENCHMARK_TEMPLATE(BenchToStringF, float);

template<typename T>
static void BenchStdToChars(benchmark::State &state) {
    for (auto _ : state) {
        char buf[64]{};
        auto r = std::to_chars(buf, buf + 64, tag, 10);
        benchmark::DoNotOptimize(r);
    }
}

BENCHMARK_TEMPLATE(BenchStdToChars, uint64_t);

template<typename T>
static void BenchStdToCharsF(benchmark::State &state) {
    for (auto _ : state) {
        char buf[64]{};
        auto r = std::to_chars(buf, buf + 64, tagf);
        benchmark::DoNotOptimize(r);
    }
}

BENCHMARK_TEMPLATE(BenchStdToCharsF, float);

template<typename T>
static void BenchBoostStringFrom(benchmark::State &state) {
    for (auto _ : state) {
        T r = boost::lexical_cast<T>(tag);
        benchmark::DoNotOptimize(r);
    }
}
template<typename T>
static void BenchBoostStringTof(benchmark::State &state) {
    for (auto _ : state) {
        T r = boost::lexical_cast<T>(tagf);
        benchmark::DoNotOptimize(r);
    }
}

BENCHMARK_TEMPLATE(BenchBoostStringFrom, std::string);

BENCHMARK_TEMPLATE(BenchBoostStringTof, std::string);

template<typename T>
static void BenchAbFastIntToBuffer(benchmark::State &state) {
    T r = 0;
    for (auto _ : state) {
        char buf[64]{};
        if (absl::numbers_internal::FastIntToBuffer(tag, buf)) {
        }
        benchmark::DoNotOptimize(r);
    }
}
BENCHMARK_TEMPLATE(BenchAbFastIntToBuffer, uint64_t);

template<typename T>
static void BenchAbSixDigitsToBuffer(benchmark::State &state) {
    T r = 0;
    for (auto _ : state) {
        char buf[64]{};
        if (absl::numbers_internal::SixDigitsToBuffer(tagf, buf)) {

        }
        benchmark::DoNotOptimize(r);
    }
}
BENCHMARK_TEMPLATE(BenchAbSixDigitsToBuffer, float);

template<typename T>
static void BenchAbStrFormat(benchmark::State &state) {
    for (auto _ : state) {
        auto r = absl::StrFormat("%d", tag);
        benchmark::DoNotOptimize(r);
    }
}
BENCHMARK_TEMPLATE(BenchAbStrFormat, uint64_t);

template<typename T>
static void BenchAbStrFormatF(benchmark::State &state) {
    for (auto _ : state) {
        auto r = absl::StrFormat("%f", tagf);
        benchmark::DoNotOptimize(r);
    }
}
BENCHMARK_TEMPLATE(BenchAbStrFormatF, float);

template<typename T>
static void BenchPBSimpleItoa(benchmark::State &state) {
    T r = 0;
    for (auto _ : state) {
        char buf[64]{};
        auto r = google::protobuf::FastUInt64ToBuffer(tag, buf);
        benchmark::DoNotOptimize(r);
    }
}
BENCHMARK_TEMPLATE(BenchPBSimpleItoa, uint64_t);

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

    tag = std::mt19937_64{std::random_device{}()}();
    tagf = std::mt19937_64{std::random_device{}()}();
    benchmark::Initialize(&argc, argv);
    benchmark::RunSpecifiedBenchmarks();
    benchmark::Shutdown();
    return 0;
}


//==============================================================================
//         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 BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_SATURATE_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_SATURATE_HPP_INCLUDED

#include <boost/simd/toolbox/ieee/functions/saturate.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/simd/include/functions/simd/insert.hpp>
#include <boost/simd/include/functions/simd/extract.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/valmin.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::saturate_, tag::cpu_
                                    , (A0)(T)(X)
                                    , ((simd_< arithmetic_<A0>,X>))
                                      (target_< generic_<arithmetic_<T> > >)
                                    )
  {
    typedef A0 result_type;
    typedef typename meta::scalar_of<T>::type target_t;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, T const& ) const
    {
      result_type that;
      for(std::size_t i=0; i!=result_type::static_size; ++i)
        that[i] = saturate<target_t>(a0[i]);
      return that;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::saturate_, tag::cpu_
                                    , (A0)(T)(X)
                                    , ((simd_<int_<A0>,X>))
                                      (target_< generic_<arithmetic_<T> > >)
                                    )
  {
    typedef A0 result_type;
    typedef typename meta::scalar_of<T>::type target_t;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, T const& ) const
    {
      const A0 vma = splat<A0>(Valmax<target_t>());
      const A0 vmi = splat<A0>(Valmin<target_t>());
      return select(gt(a0, vma), vma, select(lt(a0, vmi), vmi, a0));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::saturate_, tag::cpu_
                                    , (A0)(T)(X)
                                    , ((simd_<uint_<A0>,X>))
                                      (target_< generic_<integer_<T> > >)
                                    )
  {
    typedef A0 result_type;
    typedef typename meta::scalar_of<T>::type target_t;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, T const& ) const
    {
      const A0 vma = splat<A0>(Valmax<target_t>());
      return select(gt(a0, vma), vma, a0);
    }
  };
} } }

#endif


/*
 * Copyright (c) 2015 Cryptonomex, Inc., and contributors.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Any modified source or binaries are used only with the BitShares network.
 *
 * 2. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 3. 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.
 *
 * 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 <boost/test/unit_test.hpp>

#include <graphene/chain/database.hpp>
#include <graphene/chain/protocol/protocol.hpp>
#include <graphene/chain/exceptions.hpp>

#include <graphene/chain/account_object.hpp>
#include <graphene/chain/asset_object.hpp>
#include <graphene/chain/committee_member_object.hpp>
#include <graphene/chain/proposal_object.hpp>

#include <graphene/db/simple_index.hpp>

#include <fc/crypto/digest.hpp>
#include "../common/database_fixture.hpp"

using namespace graphene::chain;

BOOST_FIXTURE_TEST_SUITE( confidential_tests, database_fixture )
BOOST_AUTO_TEST_CASE( confidential_test )
{ try {
   ACTORS( (dan)(nathan) )
   const asset_object& core = asset_id_type()(db);

   transfer(account_id_type()(db), dan, core.amount(1000000));

   transfer_to_blind_operation to_blind;
   to_blind.amount = core.amount(1000);
   to_blind.from   = dan.id;

   auto owner1_key = fc::ecc::private_key::generate();
   auto owner1_pub = owner1_key.get_public_key();
   auto owner2_key = fc::ecc::private_key::generate();
   auto owner2_pub = owner2_key.get_public_key();

   blind_output out1, out2;
   out1.owner = authority( 1, public_key_type(owner1_pub), 1 );
   out2.owner = authority( 1, public_key_type(owner2_pub), 1 );

   auto InB1  = fc::sha256::hash("InB1");
   auto InB2  = fc::sha256::hash("InB2");
   auto OutB  = fc::sha256::hash("InB2");
   auto nonce1 = fc::sha256::hash("nonce");
   auto nonce2 = fc::sha256::hash("nonce2");

   out1.commitment  = fc::ecc::blind(InB1,250);
   out1.range_proof = fc::ecc::range_proof_sign( 0, out1.commitment, InB1, nonce1, 0, 0, 250 );

   out2.commitment = fc::ecc::blind(InB2,750);
   out2.range_proof = fc::ecc::range_proof_sign( 0, out2.commitment, InB1, nonce2, 0, 0, 750 );

   to_blind.blinding_factor = fc::ecc::blind_sum( {InB1,InB2}, 2 );
   to_blind.outputs = {out2,out1};

   trx.operations = {to_blind};
   sign( trx,  dan_private_key  );
   db.push_transaction(trx);
   trx.signatures.clear();

   BOOST_TEST_MESSAGE( "Transfering from blind to blind with change address" );
   auto Out3B  = fc::sha256::hash("Out3B");
   auto Out4B  = fc::ecc::blind_sum( {InB2,Out3B}, 1 ); // add InB2 - Out3b
   blind_output out3, out4;
   out3.commitment = fc::ecc::blind(Out3B,300);
   out3.range_proof = fc::ecc::range_proof_sign( 0, out3.commitment, InB1, nonce1, 0, 0, 300 );
   out4.commitment = fc::ecc::blind(Out4B,750-300-10);
   out4.range_proof = fc::ecc::range_proof_sign( 0, out3.commitment, InB1, nonce1, 0, 0, 750-300-10 );

   blind_transfer_operation blind_tr;
   blind_tr.fee = core.amount(10);
   blind_tr.inputs.push_back( {out2.commitment, out2.owner} );
   blind_tr.outputs = {out3,out4};
   blind_tr.validate();
   trx.operations = {blind_tr};
   sign( trx,  owner2_key  );
   db.push_transaction(trx);

   BOOST_TEST_MESSAGE( "Attempting to double spend the same commitments" );
   blind_tr.fee = core.amount(11);

   Out4B  = fc::ecc::blind_sum( {InB2,Out3B}, 1 ); // add InB2 - Out3b
   out4.commitment = fc::ecc::blind(Out4B,750-300-11);
   auto out4_amount = 750-300-10;
   out4.range_proof = fc::ecc::range_proof_sign( 0, out3.commitment, InB1, nonce1, 0, 0, 750-300-11 );
   blind_tr.outputs = {out4,out3};
   trx.operations = {blind_tr};
   BOOST_REQUIRE_THROW( db.push_transaction(trx, ~0), graphene::chain::blind_transfer_unknown_commitment );

   BOOST_TEST_MESSAGE( "Transfering from blind to nathan public" );
   out4.commitment = fc::ecc::blind(Out4B,750-300-10);

   transfer_from_blind_operation from_blind;
   from_blind.fee = core.amount(10);
   from_blind.to  = nathan.id;
   from_blind.amount = core.amount( out4_amount - 10 );
   from_blind.blinding_factor = Out4B;
   from_blind.inputs.push_back( {out4.commitment, out4.owner} );
   trx.operations = {from_blind};
   trx.signatures.clear();
   db.push_transaction(trx);

   BOOST_REQUIRE_EQUAL( get_balance( nathan, core ), 750-300-10-10 );

} FC_LOG_AND_RETHROW() }

BOOST_AUTO_TEST_SUITE_END()


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#include <libpanning/LoudspeakerArray.h>
#include <libefl/basic_vector.hpp>
#include <librcl/listener_compensation.hpp>
#include <librcl/position_decoder.hpp>
#include <librcl/udp_receiver.hpp>

#include <libvisr/signal_flow_context.hpp>

// #include <libpml/message_queue.hpp>
#include <libpml/listener_position.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp> // case-insensitive string compare

#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <stdexcept>
#include <sstream>
#include <string>

namespace visr
{
namespace rcl
{
namespace test
{

#if 0
class Flow: public AudioSignalFlow
{
public:
  explicit Flow( std::size_t period, SamplingFrequencyType samplingFrequency )
    : AudioSignalFlow( period, samplingFrequency )
  {
  }

  /*virtual*/ void setup()
  {
  }
  /*virtual*/ void process()
  {
  }
};

BOOST_AUTO_TEST_CASE(testKinectReceiver)
{

  Flow flow( 256, 48000 );

  rcl::UdpReceiver kinect( flow, "Receiver");
  rcl::PositionDecoder decoder( flow, "Decoder" );
  rcl::ListenerCompensation listenerComp( flow, "ListenerCompensation");

  pml::MessageQueue<pml::StringParameter> posMessage; //output position for the Kinect
  const std::size_t kinectNetworkPort=8888;

  std::size_t const numSpeakers = 2;

  boost::filesystem::path const basePath(CMAKE_SOURCE_DIR);
  boost::filesystem::path const arrayPath = basePath / boost::filesystem::path("config/generic/stereo.xml");
  BOOST_CHECK_MESSAGE( exists(arrayPath), "The loudspeaker array file does not exist.");
  std::string const arrayConfigFileName = arrayPath.string();
  panning::LoudspeakerArray loudspeakerArray;
  // As long as we have two different config formats, we decide based on the file extention.
  std::string::size_type lastDotIdx = arrayConfigFileName.rfind( '.' );
  std::string const configfileExtension = lastDotIdx == std::string::npos ? std::string( ) : arrayConfigFileName.substr( lastDotIdx + 1 );
  if( boost::iequals( configfileExtension, std::string( "xml" ) ) )
  {
    loudspeakerArray.loadXmlFile( arrayConfigFileName );
  }
  else
  {
    FILE* hFile = fopen( arrayConfigFileName.c_str( ), "r" );
    if( loudspeakerArray.load( hFile ) < 0 )
    {
      throw std::invalid_argument( "Error while parsing the loudspeaker array configuration file \""
        + arrayConfigFileName + "\"." );
    }
  }

  pml::ListenerPosition pos;
  efl::BasicVector<rcl::ListenerCompensation::SampleType> gains(numSpeakers, cVectorAlignmentSamples );
  efl::BasicVector<rcl::ListenerCompensation::SampleType> delays(numSpeakers, cVectorAlignmentSamples );

  kinect.setup( kinectNetworkPort, rcl::UdpReceiver::Mode::Synchronous ); //setup to listen from th, how to set up mode?
  decoder.setup( panning::XYZ( +2.08f, 0.0f, 0.0f ) );
  listenerComp.setup( loudspeakerArray );

  while( true )
  {
    kinect.process( posMessage );
    decoder.process( posMessage, pos );
    std::cout << "Decoded position message: " << pos << std::endl;
    listenerComp.process( pos, gains, delays );
    std::cout << "Gains: " << gains[0] << "Delays: " << delays[0] << std::endl;
    std::cout << "Gains: " << gains[1] << "Delays: " << delays[1] << std::endl;
    //std::cout << "Gains: " << gains[2] << "Delays: " << delays[2] << std::endl;
    //std::cout << "Gains: " << gains[3] << "Delays: " << delays[3] << std::endl;
    //std::cout << "Gains: " << gains[4] << "Delays: " << delays[4] << std::endl;
    //std::cout << "Gains: " << gains[5] << "Delays: " << delays[5] << std::endl;
    //std::cout << "Gains: " << gains[6] << "Delays: " << delays[6] << std::endl;
    //std::cout << "Gains: " << gains[7] << "Delays: " << delays[7] << std::endl;
    //std::cout << "Gains: " << gains[8] << "Delays: " << delays[8] << std::endl;
    //std::cout << "Gains: " << gains[9] << "Delays: " << delays[9] << std::endl;
    //std::cout << "Gains: " << gains[10] << "Delays: " << delays[10] << std::endl;
    //std::cout << "Gains: " << gains[11] << "Delays: " << delays[11] << std::endl;
    //std::cout << "Gains: " << gains[12] << "Delays: " << delays[12] << std::endl;
    //std::cout << "Gains: " << gains[13] << "Delays: " << delays[13] << std::endl;
    //std::cout << "Gains: " << gains[14] << "Delays: " << delays[14] << std::endl;
    //std::cout << "Gains: " << gains[15] << "Delays: " << delays[15] << std::endl;
    //std::cout << "Gains: " << gains[16] << "Delays: " << delays[16] << std::endl;
    //std::cout << "Gains: " << gains[17] << "Delays: " << delays[17] << std::endl;
  }
}
#endif

} // namespace test
} // namespace rcl
} // namespce visr


#include <tf/transform_datatypes.h>

#include <urdf_parser/urdf_parser.h>

#include <boost/assign.hpp>
#include <angles/angles.h>
#include <control_toolbox/filters.h>
#include <position_effort_controller/position_effort_controller.h>

namespace position_effort_controller{

PositionEffortController::PositionEffortController()
 : next_state_update_(0), controller_state_period_(0.5), command_timeout_(0.5)
{
}

bool PositionEffortController::init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
{
  std::string joint_name;

  urdf::Model urdf;
  if (!urdf.initParam("robot_description")) {
    ROS_ERROR("Failed to parse urdf file");
    return false;
  }

  controller_nh.param("command_timeout", command_timeout_, command_timeout_);

  if (!controller_nh.getParam("joint", joint_name)) {
    ROS_ERROR("No joint given (namespace: %s)", controller_nh.getNamespace().c_str());
    return false;
  }

  joint_ = hw->getHandle(joint_name);
  boost::shared_ptr<const urdf::Joint> joint_urdf =  urdf.getJoint(joint_name);
  if (!joint_urdf) {
    ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
    return false;
  }

  if (!pid_controller_.init(ros::NodeHandle(controller_nh, "pid")))
    return false;

  controller_state_publisher_.reset(new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(controller_nh, "state", 1));

  command_sub_ = controller_nh.subscribe<position_effort_controller::PositionEffortCommand>("command", 1, &PositionEffortController::setCommandCallback, this);
}

void PositionEffortController::setCommandCallback(const position_effort_controller::PositionEffortCommandConstPtr& command) {
  position_effort_controller::PositionEffortCommandStamped command_stamped;
  command_stamped.header.stamp = ros::Time::now();
  command_stamped.command = *command;
  command_buffer_.writeFromNonRT(command_stamped);
}

void PositionEffortController::starting(const ros::Time& time) {
  position_effort_controller::PositionEffortCommandStamped command;
  command.header.stamp = ros::Time(0);
  command.command.mode = position_effort_controller::PositionEffortCommand::HOLD_POSITION;
  command_buffer_.initRT(command);
}

void PositionEffortController::stopping(const ros::Time& time) {
  joint_.setCommand(0.0); // set joint effort to zero
}

void PositionEffortController::update(const ros::Time& time, const ros::Duration& period) {
  double current_position = joint_.getPosition();
  position_effort_controller::PositionEffortCommandStamped command_stamped = *(command_buffer_.readFromRT());

  const double dt = (time - command_stamped.header.stamp).toSec();
  double command_effort;
  double command_position;
  double error;

  position_effort_controller::PositionEffortCommand command = command_stamped.command;
  if (dt > command_timeout_) {
    // timeout
    command.mode = position_effort_controller::PositionEffortCommand::DISABLED;
  }

  if(command.mode == position_effort_controller::PositionEffortCommand::POSITION ||
     command.mode == position_effort_controller::PositionEffortCommand::HOLD_POSITION) {
    // switching to position mode so reset the pid controller
    if(last_command_.mode != position_effort_controller::PositionEffortCommand::POSITION &&
       last_command_.mode != position_effort_controller::PositionEffortCommand::HOLD_POSITION) {
      pid_controller_.reset();
    }

    if(command.mode == position_effort_controller::PositionEffortCommand::HOLD_POSITION) {
      // Hold the position that was held last time
      if(last_command_.mode == position_effort_controller::PositionEffortCommand::HOLD_POSITION) {
	command.set_point = last_command_.set_point;
      }
      else {
	command.set_point = current_position;
      }
    }

    command_position = filters::clamp(command.set_point, -M_PI, M_PI);
    error = angles::shortest_angular_distance(current_position, command_position);
    command_effort = pid_controller_.computeCommand(error, period);
  }
  else if(command.mode == position_effort_controller::PositionEffortCommand::EFFORT) {
    command_position = std::numeric_limits<double>::quiet_NaN();
    error = std::numeric_limits<double>::quiet_NaN();
    command_effort = command.set_point;
  }
  else { // disabled
    command_position = std::numeric_limits<double>::quiet_NaN();
    error = std::numeric_limits<double>::quiet_NaN();
    command_effort = 0.0;
  }
  last_command_ = command;
  joint_.setCommand(command_effort);

  // publish state
  if (time > next_state_update_) {
    if(controller_state_publisher_ && controller_state_publisher_->trylock()) {
      controller_state_publisher_->msg_.header.stamp = time;
      controller_state_publisher_->msg_.set_point = command_position;
      controller_state_publisher_->msg_.process_value = current_position;
      controller_state_publisher_->msg_.process_value_dot = joint_.getVelocity();
      controller_state_publisher_->msg_.error = error;
      controller_state_publisher_->msg_.time_step = period.toSec();
      controller_state_publisher_->msg_.command = command_effort;

      double dummy;
      pid_controller_.getGains(controller_state_publisher_->msg_.p,
			       controller_state_publisher_->msg_.i,
			       controller_state_publisher_->msg_.d,
			       controller_state_publisher_->msg_.i_clamp,
			       dummy);
      controller_state_publisher_->unlockAndPublish();
    }
    next_state_update_ = time + controller_state_period_;
  }
}

} // namespace position_effort_controller


#ifndef KMCL_PARTICLE_FILTER_HPP_
#define KMCL_PARTICLE_FILTER_HPP_

#include <vector>
#include <limits>
#include <iostream>
#include <boost/ptr_container/ptr_vector.hpp>

#include <boost/shared_ptr.hpp>
#include "rng.hpp"
#include "particle.hpp"

#include <pronto_utils/pronto_vis.hpp>

///////////////////////////////////////////////////////////////
class ParticleFilter{
  public:

    ParticleFilter(lcm_t* publish_lcm, long N_p,
          Eigen::Isometry3d init_pose,
          std::vector<double> initial_var,
          int rng_seed,
          double resample_threshold_);

    ~ParticleFilter(){
      delete pRng;

      //for(int i = 0; i < N_p ; ++i) {
      // particleset[i].~Particle();
      //}
      //  particleset.clear();

      delete [] dRSWeights;
      delete [] uRSCount;
      delete [] uRSIndices;
    }

    // Restart a fraction of particle filter centered on this location:
    void ReinitializeFraction(Eigen::Isometry3d reinit_pose,
        std::vector<double> reinit_var, double fraction);
    // Restart the entire particle filter centered on this location:
    void ReinitializeComplete(Eigen::Isometry3d reinit_pose,std::vector<double> reinit_var);

    // Move the particles
    // mode: 0: typical propogation with full delta pose value
    //       1: use velocity to propogate pose (when vo fails)
    void MoveParticles(pf_state odom_diff, std::vector<double> propogate_var, double dtime,int mode);

    // Specifically set these 3 dimensions used for VO
    // depreciated: use SetDimension below
    void SetHeightPitchRoll(std::vector<double> height_pitch_roll);

    // Apply a manifold constraint to a plane
    // @input: xyzypr - the relative offset for each specific dimension
    // @input: set_xyzypr - a binary vector of which dimensions to project
    // @input: plane_pose - the plane onto which to project. TODO: use something more useful
    void applyPlaneConstraint(std::vector<double> xyzrpy, std::vector<bool> set_xyzrpy,
                  Eigen::Isometry3d plane_pose);

    void LogLikelihoodParticles(std::vector<float> loglikelihoods);

    void SendParticlesLCM(int64_t time_stamp,int vo_estimate_status);
    double ConsiderResample();
    void Resample();
    ///Calculates and Returns the Effective Sample Size.
    double GetESS(void) ;

    // Find the weight mean of the weight particle set:
    pf_state Integrate();
    // Return the position and attitude:
    Eigen::Isometry3d IntegratePose(){
      pf_state mean_state = Integrate();
      return mean_state.pose;
    }
    pf_state IntegrateVelocity();

    // Find the mean of the weight particle set:
    // assuming that the yaw/heading in near to wrapping.
    // this is only useful for 2D. a better solution would be to find the
    // mean of a set of weighted quaternions in a better manner
    pf_state IntegrateWrapSafe();

    // Find the particle with largest weight:
    pf_state MaxWeight();

    void SetParticleState(int i, pf_state particle_in_){
      particleset[i].SetState(particle_in_);
    }

    pf_state GetParticleState(int i){ return particleset[i].GetState(); }
    double GetParticleLogWeight(int i){ return particleset[i].GetLogWeight(); }
    double GetParticleWeight(int i){ return particleset[i].GetWeight(); }

    typedef boost::shared_ptr<ParticleFilter> Ptr;
    typedef boost::shared_ptr<const ParticleFilter> ConstPtr;
  private:
    lcm_t* publish_lcm;
    long N_p; // number of particles

    pronto_vis* pc_vis_;

    rng* pRng;
    boost::ptr_vector<Particle>    particleset;

    ///A flag which tracks whether the ensemble was resampled during this iteration
    int nResampled;
    ///The effective sample size at which resampling should be used.
    double dResampleThreshold;
    /// normalized version:
    double resample_threshold_; // dResampleThreshold = resample_threshold_ *n_particles

    ///Structures used internally for resampling.
    double* dRSWeights;
    unsigned int* uRSCount;
    unsigned int* uRSIndices;

    //Most recent lhoods [local copy]:
    std::vector <float> loglikelihoods_;

};

#endif


//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_joint_revolute_unaligned_hpp__
#define __pinocchio_joint_revolute_unaligned_hpp__

#include "pinocchio/fwd.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/math/matrix.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options=0> struct MotionRevoluteUnalignedTpl;
  typedef MotionRevoluteUnalignedTpl<double> MotionRevoluteUnaligned;

  template<typename Scalar, int Options>
  struct SE3GroupAction< MotionRevoluteUnalignedTpl<Scalar,Options> >
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

  template<typename Scalar, int Options, typename MotionDerived>
  struct MotionAlgebraAction< MotionRevoluteUnalignedTpl<Scalar,Options>, MotionDerived>
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

  template<typename _Scalar, int _Options>
  struct traits< MotionRevoluteUnalignedTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
    typedef Vector3 AngularType;
    typedef Vector3 LinearType;
    typedef const Vector3 ConstAngularType;
    typedef const Vector3 ConstLinearType;
    typedef Matrix6 ActionMatrixType;
    typedef MotionTpl<Scalar,Options> MotionPlain;
    typedef MotionPlain PlainReturnType;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
  }; // traits MotionRevoluteUnalignedTpl

  template<typename _Scalar, int _Options>
  struct MotionRevoluteUnalignedTpl
  : MotionBase< MotionRevoluteUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    MOTION_TYPEDEF_TPL(MotionRevoluteUnalignedTpl);

    MotionRevoluteUnalignedTpl() {}

    template<typename Vector3Like, typename OtherScalar>
    MotionRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
                               const OtherScalar & w)
    : m_axis(axis)
    , m_w(w)
    {}

    inline PlainReturnType plain() const
    {
      return PlainReturnType(PlainReturnType::Vector3::Zero(),
                             m_axis*m_w);
    }

    template<typename OtherScalar>
    MotionRevoluteUnalignedTpl __mult__(const OtherScalar & alpha) const
    {
      return MotionRevoluteUnalignedTpl(m_axis,alpha*m_w);
    }

    template<typename MotionDerived>
    inline void addTo(MotionDense<MotionDerived> & v) const
    {
      v.angular() += m_axis*m_w;
    }

    template<typename Derived>
    void setTo(MotionDense<Derived> & other) const
    {
      other.linear().setZero();
      other.angular().noalias() = m_axis*m_w;
    }

    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      // Angular
      v.angular().noalias() = m_w * m.rotation() * m_axis;

      // Linear
      v.linear().noalias() = m.translation().cross(v.angular());
    }

    template<typename S2, int O2>
    MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3Action_impl(m,res);
      return res;
    }

    template<typename S2, int O2, typename D2>
    void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      // Linear
      // TODO: use v.angular() as temporary variable
      Vector3 v3_tmp;
      v3_tmp.noalias() = m_axis.cross(m.translation());
      v3_tmp *= m_w;
      v.linear().noalias() = m.rotation().transpose() * v3_tmp;

      // Angular
      v.angular().noalias() = m.rotation().transpose() * m_axis;
      v.angular() *= m_w;
    }

    template<typename S2, int O2>
    MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3ActionInverse_impl(m,res);
      return res;
    }

    template<typename M1, typename M2>
    void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
    {
      // Linear
      mout.linear().noalias() = v.linear().cross(m_axis);
      mout.linear() *= m_w;

      // Angular
      mout.angular().noalias() = v.angular().cross(m_axis);
      mout.angular() *= m_w;
    }

    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }

    bool isEqual_impl(const MotionRevoluteUnalignedTpl & other) const
    {
      return m_axis == other.m_axis && m_w == other.m_w;
    }

    const Scalar & angularRate() const { return m_w; }
    Scalar & angularRate() { return m_w; }

    const Vector3 & axis() const { return m_axis; }
    Vector3 & axis() { return m_axis; }

  protected:
    Vector3 m_axis;
    Scalar m_w;

  }; // struct MotionRevoluteUnalignedTpl

  template<typename S1, int O1, typename MotionDerived>
  inline typename MotionDerived::MotionPlain
  operator+(const MotionRevoluteUnalignedTpl<S1,O1> & m1,
            const MotionDense<MotionDerived> & m2)
  {
    typename MotionDerived::MotionPlain res(m2);
    res += m1;
    return res;
  }

  template<typename MotionDerived, typename S2, int O2>
  inline typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1,
            const MotionRevoluteUnalignedTpl<S2,O2> & m2)
  {
    return m2.motionAction(m1);
  }

  template<typename Scalar, int Options> struct ConstraintRevoluteUnalignedTpl;

  template<typename _Scalar, int _Options>
  struct traits< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef MotionRevoluteUnalignedTpl<Scalar,Options> JointMotion;
    typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;

    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
  }; // traits ConstraintRevoluteUnalignedTpl

  template<typename Scalar, int Options>
  struct SE3GroupAction< ConstraintRevoluteUnalignedTpl<Scalar,Options> >
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };

  template<typename Scalar, int Options, typename MotionDerived>
  struct MotionAlgebraAction< ConstraintRevoluteUnalignedTpl<Scalar,Options>, MotionDerived >
  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };

  template<typename Scalar, int Options, typename ForceDerived>
  struct ConstraintForceOp< ConstraintRevoluteUnalignedTpl<Scalar,Options>, ForceDerived>
  {
    typedef typename traits< ConstraintRevoluteUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
    typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
  };

  template<typename Scalar, int Options, typename ForceSet>
  struct ConstraintForceSetOp< ConstraintRevoluteUnalignedTpl<Scalar,Options>, ForceSet>
  {
    typedef typename traits< ConstraintRevoluteUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
    typedef typename MatrixMatrixProduct<Eigen::Transpose<const Vector3>,
    typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
    >::type ReturnType;
  };

  template<typename _Scalar, int _Options>
  struct ConstraintRevoluteUnalignedTpl
  : ConstraintBase< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintRevoluteUnalignedTpl)

    enum { NV = 1 };

    typedef typename traits<ConstraintRevoluteUnalignedTpl>::Vector3 Vector3;

    ConstraintRevoluteUnalignedTpl() {}

    template<typename Vector3Like>
    ConstraintRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
    : m_axis(axis)
    {}

    template<typename Vector1Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
      return JointMotion(m_axis,v[0]);
    }

    template<typename S1, int O1>
    typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType
    se3Action(const SE3Tpl<S1,O1> & m) const
    {
      typedef typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType ReturnType;

      /* X*S = [ R pxR ; 0 R ] [ 0 ; a ] = [ px(Ra) ; Ra ] */
      ReturnType res;
      res.template segment<3>(ANGULAR).noalias() = m.rotation() * m_axis;
      res.template segment<3>(LINEAR).noalias() = m.translation().cross(res.template segment<3>(ANGULAR));
      return res;
    }

    template<typename S1, int O1>
    typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType
    se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      typedef typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType ReturnType;

      ReturnType res;
      res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * m_axis;
      res.template segment<3>(LINEAR).noalias() = -m.rotation().transpose() * m.translation().cross(m_axis);
      return res;
    }

    int nv_impl() const { return NV; }

    struct TransposeConst
    {
      const ConstraintRevoluteUnalignedTpl & ref;
      TransposeConst(const ConstraintRevoluteUnalignedTpl & ref) : ref(ref) {}

      template<typename ForceDerived>
      typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType
      operator*(const ForceDense<ForceDerived> & f) const
      {
        typedef typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType ReturnType;
        ReturnType res;
        res[0] = ref.axis().dot(f.angular());
        return res;
      }

      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
      template<typename ForceSet>
      typename ConstraintForceSetOp<ConstraintRevoluteUnalignedTpl,ForceSet>::ReturnType
      operator*(const Eigen::MatrixBase<ForceSet> & F)
      {
        EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
        /* Return ax.T * F[3:end,:] */
        return ref.axis().transpose() * F.template middleRows<3>(ANGULAR);
      }

    };

    TransposeConst transpose() const { return TransposeConst(*this); }

    /* CRBA joint operators
     *   - ForceSet::Block = ForceSet
     *   - ForceSet operator* (Inertia Y,Constraint S)
     *   - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
     *   - SE3::act(ForceSet::Block)
     */
    DenseBase matrix_impl() const
    {
      DenseBase S;
      S.template segment<3>(LINEAR).setZero();
      S.template segment<3>(ANGULAR) = m_axis;
      return S;
    }

    template<typename MotionDerived>
    typename MotionAlgebraAction<ConstraintRevoluteUnalignedTpl,MotionDerived>::ReturnType
    motionAction(const MotionDense<MotionDerived> & m) const
    {
      const typename MotionDerived::ConstLinearType v = m.linear();
      const typename MotionDerived::ConstAngularType w = m.angular();

      DenseBase res;
      res.template segment<3>(LINEAR).noalias() = v.cross(m_axis);
      res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis);

      return res;
    }

    const Vector3 & axis() const { return m_axis; }
    Vector3 & axis() { return m_axis; }

    bool isEqual(const ConstraintRevoluteUnalignedTpl & other) const
    {
      return m_axis == other.m_axis;
    }

  protected:

    Vector3 m_axis;

  }; // struct ConstraintRevoluteUnalignedTpl

  template<typename S1, int O1,typename S2, int O2>
  struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteUnalignedTpl<S2,O2> >
  {
    typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
  };

  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  namespace impl
  {
    template<typename S1, int O1, typename S2, int O2>
    struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteUnalignedTpl<S2,O2> >
    {
      typedef InertiaTpl<S1,O1> Inertia;
      typedef ConstraintRevoluteUnalignedTpl<S2,O2> Constraint;
      typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
      static inline ReturnType run(const Inertia & Y,
                                   const Constraint & cru)
      {
        ReturnType res;

        /* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
        const typename Inertia::Scalar & m       = Y.mass();
        const typename Inertia::Vector3 & c      = Y.lever();
        const typename Inertia::Symmetric3 & I   = Y.inertia();

        res.template segment<3>(Inertia::LINEAR) = -m*c.cross(cru.axis());
        res.template segment<3>(Inertia::ANGULAR).noalias() = I*cru.axis();
        res.template segment<3>(Inertia::ANGULAR) += c.cross(res.template segment<3>(Inertia::LINEAR));

        return res;
      }
    };
  } // namespace impl

  template<typename M6Like, typename Scalar, int Options>
  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteUnalignedTpl<Scalar,Options> >
  {
    typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols;
    typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;

    typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint;
    typedef typename Constraint::Vector3 Vector3;
    typedef const typename MatrixMatrixProduct<M6LikeColsNonConst,Vector3>::type ReturnType;
  };

  /* [ABA] operator* (Inertia Y,Constraint S) */
  namespace impl
  {
    template<typename M6Like, typename Scalar, int Options>
    struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteUnalignedTpl<Scalar,Options> >
    {
      typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint;
      typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;

      static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
                                   const Constraint & cru)
      {
        EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
        return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis();
      }
    };
  } // namespace impl

  template<typename Scalar, int Options> struct JointRevoluteUnalignedTpl;

  template<typename _Scalar, int _Options>
  struct traits< JointRevoluteUnalignedTpl<_Scalar,_Options> >
  {
    enum {
      NQ = 1,
      NV = 1
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataRevoluteUnalignedTpl<Scalar,Options> JointDataDerived;
    typedef JointModelRevoluteUnalignedTpl<Scalar,Options> JointModelDerived;
    typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef MotionRevoluteUnalignedTpl<Scalar,Options> Motion_t;
    typedef MotionZeroTpl<Scalar,Options> Bias_t;

    // [ABA]
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;

    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE

    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;

  };

  template<typename Scalar, int Options>
  struct traits< JointDataRevoluteUnalignedTpl<Scalar,Options> >
  { typedef JointRevoluteUnalignedTpl<Scalar,Options> JointDerived; };

  template<typename Scalar, int Options>
  struct traits <JointModelRevoluteUnalignedTpl<Scalar,Options> >
  { typedef JointRevoluteUnalignedTpl<Scalar,Options> JointDerived; };

  template<typename _Scalar, int _Options>
  struct JointDataRevoluteUnalignedTpl
  : public JointDataBase< JointDataRevoluteUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointRevoluteUnalignedTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR

    Transformation_t M;
    Constraint_t S;
    Motion_t v;
    Bias_t c;

    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

    JointDataRevoluteUnalignedTpl()
    : M(Transformation_t::Identity())
    , S(Constraint_t::Vector3::Zero())
    , v(Constraint_t::Vector3::Zero(),(Scalar)0)
    , U(U_t::Zero())
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Zero())
    {}

    template<typename Vector3Like>
    JointDataRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
    : M(Transformation_t::Identity())
    , S(axis)
    , v(axis,(Scalar)NAN)
    , U(U_t::Zero())
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Zero())
    {}

    static std::string classname() { return std::string("JointDataRevoluteUnaligned"); }
    std::string shortname() const { return classname(); }

  }; // struct JointDataRevoluteUnalignedTpl

  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnalignedTpl);
  template<typename _Scalar, int _Options>
  struct JointModelRevoluteUnalignedTpl
  : public JointModelBase< JointModelRevoluteUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointRevoluteUnalignedTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
    typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;

    typedef JointModelBase<JointModelRevoluteUnalignedTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;

    JointModelRevoluteUnalignedTpl() {}

    JointModelRevoluteUnalignedTpl(const Scalar & x,
                                   const Scalar & y,
                                   const Scalar & z)
    : axis(x,y,z)
    {
      axis.normalize();
      assert(isUnitary(axis) && "Rotation axis is not unitary");
    }

    template<typename Vector3Like>
    JointModelRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
    : axis(axis)
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
      assert(isUnitary(axis) && "Rotation axis is not unitary");
    }

    JointDataDerived createData() const { return JointDataDerived(axis); }

    using Base::isEqual;
    bool isEqual(const JointModelRevoluteUnalignedTpl & other) const
    {
      return Base::isEqual(other) && axis == other.axis;
    }

    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      typedef typename ConfigVector::Scalar OtherScalar;
      typedef Eigen::AngleAxis<Scalar> AngleAxis;

      const OtherScalar & q = qs[idx_q()];

      data.M.rotation(AngleAxis(q,axis).toRotationMatrix());
    }

    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
      calc(data,qs.derived());

      data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
    }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
    {
      data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
      data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR));
      data.UDinv.noalias() = data.U * data.Dinv;

      if (update_I)
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
    }

    static std::string classname() { return std::string("JointModelRevoluteUnaligned"); }
    std::string shortname() const { return classname(); }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelRevoluteUnalignedTpl<NewScalar,Options> cast() const
    {
      typedef JointModelRevoluteUnalignedTpl<NewScalar,Options> ReturnType;
      ReturnType res(axis.template cast<NewScalar>());
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }

    // data

    ///
    /// \brief 3d main axis of the joint.
    ///
    Vector3 axis;
  }; // struct JointModelRevoluteUnalignedTpl

} //namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};
}

#endif // ifndef __pinocchio_joint_revolute_unaligned_hpp__