// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRICES_INCLUDE
#define MTL_MATRICES_INCLUDE

#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/sparse_banded.hpp>
#include <boost/numeric/mtl/matrix/multi_vector.hpp>
#include <boost/numeric/mtl/matrix/multi_vector_range.hpp>
#include <boost/numeric/mtl/matrix/element_matrix.hpp>
#include <boost/numeric/mtl/matrix/element.hpp>
#include <boost/numeric/mtl/matrix/implicit_dense.hpp>
#include <boost/numeric/mtl/matrix/block_diagonal2D.hpp>
#include <boost/numeric/mtl/matrix/ell_matrix.hpp>

#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/matrix/shifted_inserter.hpp>
#include <boost/numeric/mtl/matrix/mapped_inserter.hpp>

#include <boost/numeric/mtl/matrix/map_view.hpp>
#include <boost/numeric/mtl/matrix/transposed_view.hpp>
#include <boost/numeric/mtl/matrix/hermitian_view.hpp>
#include <boost/numeric/mtl/matrix/banded_view.hpp>
#include <boost/numeric/mtl/matrix/indirect.hpp>

#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/matrix/laplacian_setup.hpp>
#include <boost/numeric/mtl/matrix/hessian_setup.hpp>
#include <boost/numeric/mtl/matrix/poisson2D_dirichlet.hpp>
#include <boost/numeric/mtl/matrix/identity2D.hpp>

#include <boost/numeric/mtl/recursion/predefined_masks.hpp>

#endif // MTL_MATRICES_INCLUDE


#pragma once

//Types such as GeneVec, Individual, GameplayResults, etc.
#include "EvoAlg/Types.hpp"

#include <string>
#include <armadillo> // http://arma.sourceforge.net/docs.html v10.2.0
#include <vector>
#include <thread> /* std::this_thread::sleep_for */
#include <mutex>

namespace EvoAlg
{

    class EAController;

    class EvolutionaryAlgorithm
    {
    public:
        EvolutionaryAlgorithm(EAController &eaController);
        ~EvolutionaryAlgorithm();
        void startAlgorithm();
        void saveBestIndvParamsCSV();
        void initializePop(int);
        void normalizeFitness(double *);
        void evaluatePop();
        void crossover(int, arma::rowvec, arma::rowvec);
        void elitism();
        void tournament();
        void roulette();
        void mutate(int);
        void selectionAndMutation();
        void createCSV(std::string);
        std::string formatParamsString(int);
        void saveGenerationData(int, const std::string&);
        void increaseMutation();
        void predationOfOne();
        void partIncrease();
        void oneRemainingPopReset();
        void endEABatch();
        bool eventHappens(int);
        void checkEvents();
        void startEventTriggerList();
        void evoAlg(int,const std::string&);
        void fillInitialsWithBests(int);
        void semiFinalsTournment(int);
        void finalTournment();
        void scriptLoop();
        void calcFitness(const std::vector<IndividualRunResult> &);

    private:
        std::thread *scriptThread = nullptr;
        std::mutex mutex;
        std::mutex mutex2;

        arma::rowvec bestIndividual;
        EAController &m_EAController;

        double indvFitness;
        bool eaFinished;
        const double MIN_MULT = 1.0;
        const double MAX_MULT = 1.0;
        const double MUTATE_POSITIVE_MULT = 1.0;
        const double MUTATE_NEGATIVE_MULT = -1.0;

#define NB_PARAMETERS 27 // number of parameters that will be adjusted
#define POPULATION_SIZE 20
#define TOURNMENT_RATE 10

        const long MAX_MUTATION_RATE = 1;
        const double MUTATION_INCREASE_RATIO = 1.1; // mutation increase 10% at a time
        const double INITIAL_MUTATION = 0.05;

        const long BEGIN_DECAY_THRESHOLD = 50; // after how many generations without improvement the mutation starts to decay
        const long APPLY_DECAY_MODULE = 15;    // decay will be applied when the generation index % APPLY_DECAY_MODULE = 0

        const long APPLY_PREDATION_INTERVAL = 10; // interval (in generations) to apply predation

        enum tournments
        {
            SEMI_FINALS,
            FINALS,
            INITIALS
        };

        arma::mat::fixed<TOURNMENT_RATE, NB_PARAMETERS> bestTournmentIndv[2] = {
            arma::mat::fixed<TOURNMENT_RATE, NB_PARAMETERS>(),
            arma::mat::fixed<TOURNMENT_RATE, NB_PARAMETERS>()};

        int eventTriggerModule[6] = {1, 10, 5, 10, 2, 10};

        struct GeneValueBound {
            std::string name;
            double min = 0,max = 1;
        };
        #define EA_CB_3(NAME,MIN,MAX) {NAME,MIN,MAX}
        #define EA_CB_2(NAME,MIN) {NAME,MIN,15*MIN}
        #define EA_CB_1(NAME) {NAME,0,1}
        #define EA_GET_MACRO_NAME_3(arg1, arg2, arg3, macro, ...) macro
        #define EA_CB(...) EA_GET_MACRO_NAME_3(__VA_ARGS__,EA_CB_3,EA_CB_2,EA_CB_1)(__VA_ARGS__)

        std::vector<GeneValueBound> attributesValues = {
            EA_CB("ON_ATTACK_MULTIPLIER",1), //DEFAULT
            EA_CB("POP_INCREASE_TAX",1), //DEFAULT
            EA_CB("POP_PER_CONSTRUCTION",1), //DEFAULT
            EA_CB("INIT_POP_VALUE",100), //DEFAULT
            EA_CB("INIT_STAT_VALUE",50), //DEFAULT
            EA_CB("ON_ATTACK_DECAY_TAX"), //DEFAULT
            EA_CB("NORMAL_DECAY_TAX",1), // NORMAL_DECAY_TAX = ON_ATTACK_DECAY_TAX/NORMAL_DECAY_TAX
            EA_CB("ATTACK_FREQUENCY",5), //DEFAULT
            EA_CB("INIT_RESOURCES_VALUE",25), //DEFAULT
            EA_CB("TAX_REDUCT"), //DEFAULT
            EA_CB("MIN_LOSS_0",1), // MIN_LOSS_0 = MAX_LOSS_0/MIN_LOSS_0
            EA_CB("MIN_LOSS_1",1), // MIN_LOSS_1 = MAX_LOSS_1/MIN_LOSS_1
            EA_CB("MIN_LOSS_2",1), // MIN_LOSS_2 = MAX_LOSS_2/MIN_LOSS_2
            EA_CB("MIN_LOSS_3",1), // MIN_LOSS_3 = MAX_LOSS_3/MIN_LOSS_3
            EA_CB("MAX_LOSS_0"), //DEFAULT
            EA_CB("MAX_LOSS_1"), //DEFAULT
            EA_CB("MAX_LOSS_2"), //DEFAULT
            EA_CB("MAX_LOSS_3"), //DEFAULT
            EA_CB("TOT_ROBOTS_INI",1,25), //DEFAULT
            EA_CB("PROD_COST_INI",5), //DEFAULT
            EA_CB("PROD_COST_INCREASE_TAX"), //DEFAULT
            EA_CB("TIME_STEP",15,60), //DEFAULT
            EA_CB("INIT_TIME_STEP",1), //DEFAULT
            EA_CB("MAX_TIME_STEPS",1), //DEFAULT
            EA_CB("MIN_REWARD",1), //DEFAULT
            EA_CB("REWARD_RANGE",1), //DEFAULT
            EA_CB("FAILURE_TAX") //DEFAULT
        };

        enum selectionMethods
        {
            TOURNAMENT,
            ELITISM,
            ROULETTE
        };
        selectionMethods partsSelectionMethods[20] = {
            TOURNAMENT, TOURNAMENT, TOURNAMENT, TOURNAMENT, TOURNAMENT, TOURNAMENT, TOURNAMENT, TOURNAMENT, TOURNAMENT, TOURNAMENT,
            ELITISM, ELITISM, ELITISM, ELITISM, ELITISM, ELITISM, ELITISM, ELITISM, ELITISM, ELITISM};

        int partPos = 0;
        selectionMethods selectionMethod = partsSelectionMethods[partPos];

        arma::mat::fixed<POPULATION_SIZE, NB_PARAMETERS> population; // arma::mat : armadillo matrix (Mat<double>)

        // Each LINE (ROW) is the individual's parameters
        double fitness[POPULATION_SIZE];
        double bestFitness = 2000000000, worstFitness = -1;
        int bestFitIndex = -1, worstFitIndex = -1;
        double mutationRate = INITIAL_MUTATION;
        int nbNoImprovementGens = 0; // number of generations in a row without any improvement (best indiviNdual remains the same)
        int generationIndex = 0;

        // Choose if population evaluation will be according to max fitness value or min fitness
        enum evalTypes
        {
            BY_MIN,
            BY_MAX
        };
        evalTypes EVAL_TYPE = BY_MIN;

        bool continueBatch;
        void (EvolutionaryAlgorithm::*selectionType[3])() = {
            &EvolutionaryAlgorithm::tournament,
            &EvolutionaryAlgorithm::elitism,
            &EvolutionaryAlgorithm::roulette};

        void (EvolutionaryAlgorithm::*eventTypes[5])() = {
            &EvolutionaryAlgorithm::increaseMutation,
            &EvolutionaryAlgorithm::predationOfOne,
            &EvolutionaryAlgorithm::partIncrease,
            &EvolutionaryAlgorithm::oneRemainingPopReset,
            &EvolutionaryAlgorithm::endEABatch};
        int remainingFitnessToCalc;
    };

} // namespace EvoAlg


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_FUNCTION_MINNUM_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_MINNUM_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing minnum capabilities

     Returns the minimum value, ignoring nans.

    @par Semantic:

    @code
    T r = minnum(x,y);
    @endcode

    is similar to:

    @code
    T r =     T r = (is_nan(x) ? y : (isnan(y) ? x : min(x, y));;
    @endcode

    @par Alias:
    @c fmin

    With the std_ decorator std_(minnum)(x, y) calls the stdlibc++ function std::fmin.

    @par Decorators

    std_ for floating entries

  **/
  const boost::dispatch::functor<tag::minnum_> minnum = {};
} }
#endif

#include <boost/simd/function/scalar/minnum.hpp>
#include <boost/simd/function/simd/minnum.hpp>

#endif


#include <iostream>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include "netsne.h"
#include <armadillo>

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

bool load_data(string infile_X, mat &X, int &num_instances, int &num_features) {

  FILE *fp = fopen(infile_X.c_str(), "rb");
	if (fp == NULL) {
		cout << "Error: could not open data file " << infile_X << endl;
		return false;
	}

  uint64_t ret;
	ret = fread(&num_instances, sizeof(int), 1, fp);
	ret = fread(&num_features, sizeof(int), 1, fp);

  X.set_size(num_features, num_instances);

  uint64_t nelem = (uint64_t)num_instances * num_features;

  size_t batch_size = 1e8;
  double *ptr = X.memptr();
  ret = 0;
  for (uint64_t remaining = nelem; remaining > 0; remaining -= batch_size) {
    if (remaining < batch_size) {
      batch_size = remaining;
    }
    ret += fread(ptr, sizeof(double), batch_size, fp);
    ptr += batch_size;
  }

  if (ret != nelem) {
    cout << "Error: reading input returned incorrect number of elements (" << ret
         << ", expected " << nelem << ")" << endl;
    return false;
  }

	fclose(fp);

	return true;
}

int main(int argc, char **argv) {
  // Declare the supported options
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ("input-X", po::value<string>()->value_name("FILE")->default_value("data.dat"), "name of binary input file containing data feature matrix (see prepare_input.m)")
    ("input-P", po::value<string>()->value_name("FILE")->default_value("P.dat"), "name of binary input file containing P matrix (see ComputeP)")
    ("input-Y", po::value<string>()->value_name("FILE"), "if this option is provided, net-SNE will train to match the provided embedding instead of using the P matrix")
    ("out-dir", po::value<string>()->value_name("DIR")->default_value("out"), "where to create output files; directory will be created if it does not exist")
    ("out-dim", po::value<int>()->value_name("NUM")->default_value(2), "number of output dimensions")
    ("max-iter", po::value<int>()->value_name("NUM")->default_value(1000), "maximum number of iterations")
    ("rand-seed", po::value<int>()->value_name("NUM")->default_value(-1), "seed for random number generator; to use current time as seed set it to -1")
    ("theta", po::value<double>()->value_name("NUM")->default_value(0.5, "0.5"), "a value between 0 and 1 that controls the accuracy-efficiency tradeoff in SPTree for gradient computation; 0 means exact")
    ("learn-rate", po::value<double>()->value_name("NUM")->default_value(0.02, "0.02"), "learning rate for gradient steps")
    ("mom-init", po::value<double>()->value_name("NUM")->default_value(0.5, "0.5"), "initial momentum between 0 and 1")
    ("mom-final", po::value<double>()->value_name("NUM")->default_value(0.8, "0.8"), "final momentum between 0 and 1 (switch point controlled by --mom-switch-iter)")
    ("mom-switch-iter", po::value<int>()->value_name("NUM")->default_value(250), "duration (number of iterations) of initial momentum")
    ("early-exag-iter", po::value<int>()->value_name("NUM")->default_value(250), "duration (number of iterations) of early exaggeration")
    ("num-local-sample", po::value<int>()->value_name("NUM")->default_value(20), "number of local samples for each data point in the mini-batch")
    ("batch-frac", po::value<double>()->value_name("NUM")->default_value(0.1, "0.1"), "fraction of data to sample for mini-batch")
    ("min-sample-Z", po::value<double>()->value_name("NUM")->default_value(0.1, "0.1"), "minimum fraction of data to use for approximating the normalization factor Z in the gradient")
    ("l2-reg", po::value<double>()->value_name("NUM")->default_value(0, "0"), "L2 regularization parameter")
    ("init-model-prefix", po::value<string>()->value_name("STR"), "prefix of model files for initialization")
    ("step-method", po::value<string>()->value_name("STR")->default_value("adam"), "gradient step schedule; 'adam', 'mom' (momentum), 'mom_gain' (momentum with gains), 'fixed'")
    ("num-input-feat", po::value<int>()->value_name("NUM"), "if set, use only the first NUM features for the embedding function")
    ("init-map", po::bool_switch()->default_value(false), "output initial mapping for the entire data")
    ("num-layers", po::value<int>()->value_name("NUM")->default_value(2), "number of layers in the neural network")
    ("num-units", po::value<int>()->value_name("NUM")->default_value(50), "number of units for each layer in the neural network")
    ("act-fn", po::value<string>()->value_name("STR")->default_value("relu"), "activation function of the neural network; 'sigmoid' or 'relu'")
    ("test-model", po::bool_switch()->default_value(false), "if set, use the model provided with --init-model-prefix and visualize the entire data set then terminate without training")
    ("no-target", po::bool_switch()->default_value(false), "if this option is provided (ignored if --test-model is not set), then only the new embedding is printed, without the objective value")
    ("perm-iter", po::value<int>()->value_name("NUM")->default_value(INT_MAX, "INT_MAX"), "After every NUM iterations, permute the ordering of data points for fast mini-batching")
    ("cache-iter", po::value<int>()->value_name("NUM")->default_value(INT_MAX, "INT_MAX"), "After every NUM iterations, write intermediary embeddings and parameters to disk. Final embedding is always reported.")
    ("no-sgd", po::bool_switch()->default_value(false), "if set, do not use SGD acceleration; equivalent to t-SNE with an additional backpropagation step to train a neural network. Effective for small datasets")
    //("batch-norm", po::bool_switch()->default_value(false), "turn on batch normalization")
    //("monte-carlo-pos", po::bool_switch()->default_value(false), "use monte-carlo integration for positive gradient term")
    //("match-pos-neg", po::bool_switch()->default_value(false), "compute negative forces for points sampled for positive force")

  ;

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

  if (vm.count("help")) {
    cout << "Usage: RunBhtsne [options]" << endl;
    cout << desc << "\n";
    return 1;
  }

  string infile_P = vm["input-P"].as<string>();
  string infile_X = vm["input-X"].as<string>();
  string outdir = vm["out-dir"].as<string>();

  fsys::path dir(outdir);
  if (fsys::is_directory(dir)) {
    cout << "Error: Output directory (" << outdir << ") already exists" << endl;
    return 1;
  }
  if (fsys::create_directory(dir)) {
    cout << "Output directory created: " << outdir << endl;
  }

  fsys::path paramfile = dir;
  paramfile /= "param.txt";
  ofstream ofs(paramfile.string().c_str());

  NETSNE* netsne = new NETSNE();

  bool use_known_Y = vm.count("input-Y");
  string infile_Y;

  netsne->TEST_RUN = vm["test-model"].as<bool>();
  netsne->NO_TARGET = netsne->TEST_RUN && vm["no-target"].as<bool>();

  if (!netsne->NO_TARGET) {
    if (use_known_Y) {
      infile_Y = vm["input-Y"].as<string>();
      ofs << "input-Y: " << infile_Y << endl;
      cout << "Learning to match the provided embedding: " << infile_Y << endl;
    } else {
      ofs << "input-P: " << infile_P << endl;
    }
  }
  ofs << "input-X: " << infile_X << endl;
  ofs << "out-dir: " << fsys::canonical(dir).string() << endl;

  netsne->BATCH_FRAC = vm["batch-frac"].as<double>(); ofs << "batch-frac: " << netsne->BATCH_FRAC << endl;
  netsne->N_SAMPLE_LOCAL = vm["num-local-sample"].as<int>(); ofs << "num-local-sample: " << netsne->N_SAMPLE_LOCAL << endl;
  netsne->MIN_SAMPLE_Z = vm["min-sample-Z"].as<double>(); ofs << "min-sample-Z: " << netsne->MIN_SAMPLE_Z << endl;
  netsne->STOP_LYING = vm["early-exag-iter"].as<int>(); ofs << "early-exag-iter: " << netsne->STOP_LYING << endl;
  netsne->STEP_METHOD = vm["step-method"].as<string>(); ofs << "step-method: " << netsne->STEP_METHOD << endl;
  netsne->LEARN_RATE = vm["learn-rate"].as<double>(); ofs << "learn-rate: " <<  netsne->LEARN_RATE << endl;
  netsne->L2_REG = vm["l2-reg"].as<double>(); ofs << "l2-reg: " <<  netsne->L2_REG << endl;
  netsne->SGD_FLAG = !vm["no-sgd"].as<bool>(); ofs << "sgd: " << netsne->SGD_FLAG << endl;
  netsne->PERM_ITER = vm["perm-iter"].as<int>(); ofs << "perm-iter: " << netsne->PERM_ITER << endl;
  netsne->CACHE_ITER = vm["cache-iter"].as<int>(); ofs << "cache-iter: " << netsne->CACHE_ITER << endl;
  //netsne->BATCH_NORM = vm["batch-norm"].as<bool>(); ofs << "batch-norm: " << netsne->BATCH_NORM << endl;
  //netsne->MONTE_CARLO_POS = vm["monte-carlo-pos"].as<bool>(); ofs << "monte-carlo-pos: " << netsne->MONTE_CARLO_POS << endl;
  //netsne->MATCH_POS_NEG = vm["match-pos-neg"].as<bool>(); ofs << "match-pos-neg: " << netsne->MATCH_POS_NEG << endl;

  netsne->MODEL_PREFIX_FLAG = vm.count("init-model-prefix");
  if (netsne->MODEL_PREFIX_FLAG) {
    netsne->MODEL_PREFIX = vm["init-model-prefix"].as<string>();
  }
  ofs << "init-model-prefix: " << netsne->MODEL_PREFIX << endl;

  if (netsne->TEST_RUN) {
    netsne->COMPUTE_INIT = true;

    if (!netsne->MODEL_PREFIX_FLAG) {
      cout << "Error: if --test-model is set, then --init-model-prefix must be provided; see --help" << endl;
      return 1;
    }
  } else {
    netsne->COMPUTE_INIT = vm["init-map"].as<bool>();
  }
  ofs << "init-map: " << netsne->COMPUTE_INIT << endl;

  if (!netsne->MODEL_PREFIX_FLAG) {
    netsne->NUM_LAYERS = vm["num-layers"].as<int>(); ofs << "num-layers: " << netsne->NUM_LAYERS << endl;
    netsne->NUM_UNITS = vm["num-units"].as<int>(); ofs << "num-units: " << netsne->NUM_UNITS << endl;
    netsne->ACT_FN = vm["act-fn"].as<string>(); ofs << "act-fn: " << netsne->ACT_FN << endl;
  }

  if (netsne->STEP_METHOD == "mom" || netsne->STEP_METHOD == "mom_gain") {
    netsne->MOM_SWITCH_ITER = vm["mom-switch-iter"].as<int>(); ofs << "mom-switch-iter: " << netsne->MOM_SWITCH_ITER << endl;
    netsne->MOM_INIT = vm["mom-init"].as<double>(); ofs << "mom-init: " << netsne->MOM_INIT << endl;
    netsne->MOM_FINAL = vm["mom-final"].as<double>(); ofs << "mom-final: " << netsne->MOM_FINAL << endl;
  }

  int no_dims = vm["out-dim"].as<int>(); ofs << "out-dim: " << no_dims << endl;
  double theta = vm["theta"].as<double>(); ofs << "theta: " << theta << endl;
  int rand_seed = vm["rand-seed"].as<int>(); ofs << "rand-seed: " << rand_seed << endl;
  int max_iter = vm["max-iter"].as<int>(); ofs << "max-iter: " << max_iter << endl;

  ofs.close();

  if (vm.count("help")) {
    cout << "Usage: RunBhtsne [options]" << endl;
    cout << desc << "\n";
    return 1;
  }

  if (netsne->STEP_METHOD != "adam" && netsne->STEP_METHOD != "mom" && netsne->STEP_METHOD != "mom_gain"
      && netsne->STEP_METHOD != "fixed") {
    cout << "Error: Unrecognized --step-method argument " << netsne->STEP_METHOD << "; see --help" << endl;
    return 1;
  }

  if (netsne->ACT_FN != "sigmoid" && netsne->ACT_FN != "relu") {
    cout << "Error: Unrecognized --act-fn argument " << netsne->ACT_FN << "; see --help" << endl;
    return 1;
  }

  int num_input_feat;
  if (vm.count("num-input-feat")) {
    num_input_feat = vm["num-input-feat"].as<int>();
  } else {
    num_input_feat = INT_MAX;
  }

  cout << "Loading input features ... ";
  mat X;
  int num_instances;
  int num_features;
	if (!load_data(infile_X, X, num_instances, num_features)) {
    return 1;
  }
  cout << endl;

  if (X.n_rows > num_input_feat) {
    cout << "Truncating to top " << num_input_feat << " features" << endl;
    X = X.head_rows(num_input_feat);
  }

  cout << "Data feature matrix is " << X.n_rows << " by " << X.n_cols << endl;

  int N;
  unsigned int *row_P = NULL;
  unsigned int *col_P = NULL;
  double *val_P = NULL;
  mat target_Y;

  if (netsne->NO_TARGET) {

    cout << "No target provided" << endl;
    N = X.n_cols;

  } else if (use_known_Y) {

    cout << "Loading target Y ... ";
    target_Y.load(infile_Y, arma_ascii);
    target_Y = target_Y.t();
    cout << "done" << endl;

    N = target_Y.n_cols;

    if (N != X.n_cols) {
      cout << "Error: Y matrix dimensions (" << N << ") do not match with X matrix (" << X.n_cols << ")" << endl;
      return 1;
    }

  } else {

    cout << "Loading input similarities ... ";
    if (!netsne->load_P(infile_P, N, &row_P, &col_P, &val_P)) {
      cout << "Error: failed to load P from " << infile_P << endl;
      return 1;
    }
    cout << "done" << endl;

    if (N != X.n_cols) {
      cout << "Error: P matrix dimensions (" << N << ") do not match with X matrix (" << X.n_cols << ")" << endl;
      return 1;
    }

  }

  mat Y(no_dims, N);

  if (!netsne->run(N, row_P, col_P, val_P, target_Y, X, Y, no_dims, theta, rand_seed,
           max_iter, dir)) {
    return 1;
  }

  free(row_P);
  free(col_P);
  free(val_P);

  delete(netsne);

  cout << "Done" << endl;
}


//
// Created by liu on 17-12-24.
//

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include "../src/shape/Circle.h"
#include "../src/shape/Rectangle.h"
#include "../src/shape/HalfSpace.h"
#include "../src/shape/IntersectShape.h"

using namespace boost;
using namespace boost::unit_test;

BOOST_AUTO_TEST_SUITE(test_cases)

    BOOST_AUTO_TEST_CASE(circle) {
        Circle circle = Circle(0.5, 0.5, 0.1, 2.0);
        Radial line = Radial(Point(0.0, 0.0), std::sqrt(2) / 2, std::sqrt(2) / 2);
        auto list = circle.intersect(line);
        BOOST_CHECK_EQUAL((++list.begin())->x, 0.5 - std::sqrt(2.0) / 20);
    }

    BOOST_AUTO_TEST_CASE(rectangle) {
        Rectangle rectangle = Rectangle(0.5, 0.5, 0.1, 0.1, 0.0, 2.0);
        Radial line = Radial(Point(0.5, 1.0), 0.0, -1.0);
        auto list = rectangle.intersect(line);
        BOOST_CHECK_EQUAL((++list.begin())->x, 0.5);
    }

    BOOST_AUTO_TEST_CASE(rectangle_no_insersect) {
        Rectangle rectangle = Rectangle(0.5, 0.5, 0.1, 0.1, 0.0, 2.0);
        Radial line = Radial(Point(0.5, 1.0), std::sqrt(2) / 2, std::sqrt(2) / 2);
        auto list = rectangle.intersect(line);
        BOOST_CHECK_EQUAL(list.size(), 1);
    }

    BOOST_AUTO_TEST_CASE(halfspace) {
        HalfSpace space = HalfSpace(0.5, 0.5, 0.1, 0.0, 2.0);
        Radial line = Radial(Point(0.0, 0.5), 1.0, 1.0);
        auto list = space.intersect(line);
        BOOST_CHECK_EQUAL((++list.begin())->x, 0.5);
    }

    BOOST_AUTO_TEST_CASE(halfspace_no_insersect) {
        HalfSpace space = HalfSpace(0.5, 0.5, 0.0, 0.1, 2.0);
        Radial line = Radial(Point(0.0, 0.0), 0.5, 0.0);
        auto list = space.intersect(line);
        BOOST_CHECK_EQUAL(list.size(), 1);
    }

    BOOST_AUTO_TEST_CASE(initersect_shape) {
        HalfSpace halfSpace = HalfSpace(0.5, 0.5, 0.0, 0.1, 1.0);
        Circle circle = Circle(0.5, 0.5, 0.1, 2.0);
        IntersectShape intersect = IntersectShape(&halfSpace, &circle);
        Radial line = Radial(Point(0.0, 0.0), 0.25, 0.25);
        auto list = intersect.intersect(line);
        BOOST_CHECK_EQUAL((++intersect.intersect(line).begin())->x, 0.5);
    }

BOOST_AUTO_TEST_SUITE_END()


// MIT License
//
// Copyright (c) 2021 Aditya Shridhar Hegde
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "mean_shift.hpp"

#include <NTL/ZZ.h>

#include <algorithm>
#include <numeric>
#include <random>
#include <stdexcept>

MeanShift::MeanShift(Scheme &scheme, long logn, long logq, long logq_boot,
                     long logp, long logt)
    : scheme(scheme),
      logn(logn),
      logq(logq),
      logq_boot(logq_boot),
      logp(logp),
      logt(logt) {
  slots = 1 << logn;
  refreshSeed();
}

MeanShift::MeanShift(Scheme &scheme, long logn, long logq, long logq_boot,
                     long logp, long logt, long seed)
    : scheme(scheme),
      logn(logn),
      logq(logq),
      logq_boot(logq_boot),
      logp(logp),
      logt(logt),
      seed(seed) {
  slots = 1 << logn;
}

void MeanShift::refreshSeed(long s) { seed = s; }

void MeanShift::refreshSeed() { seed = std::rand(); }

void MeanShift::leftRotateAndEqual(Ciphertext &x, long r) {
  while (r & (-r)) {
    scheme.leftRotateFastAndEqual(x, r & (-r));
    r -= (r & (-r));
  }
}

void MeanShift::rightRotateAndEqual(Ciphertext &x, long r) {
  while (r & (-r)) {
    scheme.rightRotateFastAndEqual(x, r & (-r));
    r -= (r & (-r));
  }
}

// x.logq := x.logq
void MeanShift::repeatSlotRight(Ciphertext &x, long rep, long step) {
  Ciphertext orig;
  Ciphertext temp;
  orig.copy(x);

  long rot = 1;
  for (long i = NumBits(rep) - 2; i >= 0; --i) {
    temp.copy(x);
    rightRotateAndEqual(temp, step * rot);
    scheme.addAndEqual(x, temp);
    rot *= 2;

    if (bit(rep, i)) {
      temp.copy(orig);
      rightRotateAndEqual(temp, rot * step);
      scheme.addAndEqual(x, temp);
      rot += 1;
    }
  }
}

// x.logq := x.logq
void MeanShift::sumSlotsLeft(Ciphertext &x, long num, long step) {
  Ciphertext orig;
  Ciphertext temp;
  orig.copy(x);

  long rot = 1;
  for (long i = NumBits(num) - 2; i >= 0; --i) {
    temp.copy(x);
    leftRotateAndEqual(temp, rot * step);
    scheme.addAndEqual(x, temp);
    rot *= 2;

    if (bit(num, i)) {
      temp.copy(orig);
      leftRotateAndEqual(temp, rot * step);
      scheme.addAndEqual(x, temp);
      rot += 1;
    }
  }
}

// x.logq := x.logq - logp
void MeanShift::clearSlotsRight(Ciphertext &x, long ret_size, long skip,
                                long max_slot) {
  std::vector<complex<double>> selector(slots, complex<double>{0, 0});

  for (long i = 0; i + ret_size <= max_slot; i += skip) {
    std::fill(selector.begin() + i, selector.begin() + i + ret_size,
              complex<double>{1, 0});
  }

  scheme.multByConstVecAndEqual(x, selector.data(), logp);
  scheme.reScaleByAndEqual(x, logp);
}

// res.logq = x.logq - (steps + 2) * logp
void MeanShift::inverse(Ciphertext &res, Ciphertext &x, double m, long steps) {
  Ciphertext scaled;
  scheme.multByConst(scaled, x, 2.0 / m, logp);
  scheme.reScaleByAndEqual(scaled, logp);

  SchemeAlgo schemeAlgo(scheme);
  schemeAlgo.inverse(res, scaled, scaled.logp, steps);

  scheme.multByConstAndEqual(res, 2.0 / m, res.logp);
  scheme.reScaleByAndEqual(res, logp);
}

// res[i].logq = x[i].logq - (inv_steps + t + 3) * logp
void MeanShift::minIdx(std::vector<Ciphertext> &res, std::vector<Ciphertext> &x,
                       long t, long inv_steps) {
  Ciphertext sum;
  res.resize(x.size());

  for (size_t i = 0; i < x.size(); ++i) {
    // res[i] := 1 - x[i]
    scheme.negate(res[i], x[i]);
    scheme.addConstAndEqual(res[i], 1.0, logp);

    // res[i] := res[i]^(2^t)
    for (long j = 0; j < t; ++j) {
      scheme.squareAndEqual(res[i]);
      scheme.reScaleByAndEqual(res[i], logp);
    }

    // sum := sum + res[i]
    if (i == 0)
      sum.copy(res[0]);
    else {
      scheme.addAndEqual(sum, res[i]);
    }
  }

  // inv := 1/sum
  Ciphertext inv;
  inverse(inv, sum, x.size(), inv_steps);

  // res[i] := res[i] / sum
  for (size_t i = 0; i < res.size(); ++i) {
    scheme.modDownToAndEqual(res[i], inv.logq);
    scheme.multAndEqual(res[i], inv);
    scheme.reScaleByAndEqual(res[i], logp);
  }
}

// res.logq := x.logq - (inv_steps + t + 4) logp
void MeanShift::minIdxSIMD(Ciphertext &res, Ciphertext &x, long dim,
                           long npoints, long t, long inv_steps,
                           long batchsize) {
  scheme.negate(res, x);
  scheme.addConstAndEqual(res, 1.0, logp);

  for (long j = 0; j < t; ++j) {
    scheme.squareAndEqual(res);
    scheme.reScaleByAndEqual(res, logp);
  }

  Ciphertext sum;
  sum.copy(res);
  sumSlotsLeft(sum, batchsize, npoints * dim);
  clearSlotsRight(sum, npoints * dim, slots, slots);
  repeatSlotRight(sum, batchsize, npoints * dim);

  Ciphertext inv;
  inverse(inv, sum, batchsize, inv_steps);

  scheme.modDownToAndEqual(res, inv.logq);
  scheme.multAndEqual(res, inv);
  scheme.reScaleByAndEqual(res, logp);
}

// res.logq = x.logq - logp
void MeanShift::l2NormSquared(Ciphertext &res, Ciphertext &x) {
  Ciphertext rot;
  scheme.square(res, x);
  scheme.reScaleByAndEqual(res, logp);
  rot.copy(res);
  for (long i = 1; i < slots; ++i) {
    scheme.leftRotateFastAndEqual(rot, 1);
    scheme.addAndEqual(res, rot);
  }
}

// res.logq := x.logq - 2 logp
void MeanShift::l2NormSquaredSIMD(Ciphertext &res, Ciphertext &x, long dim,
                                  long num_points) {
  scheme.square(res, x);
  scheme.reScaleByAndEqual(res, logp);
  sumSlotsLeft(res, dim, 1);
  clearSlotsRight(res, 1, dim, num_points * dim);
  repeatSlotRight(res, dim, 1);
}

// res.logq := x.logq - (degree + 1) * logp
void MeanShift::kernel(Ciphertext &res, Ciphertext &x, Ciphertext &y,
                       long degree) {
  // diff = (x - y)^2
  Ciphertext diff;
  scheme.sub(diff, x, y);

  l2NormSquared(res, diff);

  // res = (1 - res)^(2^degree)
  scheme.negateAndEqual(res);
  scheme.addConstAndEqual(res, 1.0, logp);
  for (long i = 0; i < degree; ++i) {
    scheme.squareAndEqual(res);
    scheme.reScaleByAndEqual(res, logp);
  }
}

// res.logp := x.logp - (degree + 2) logp
void MeanShift::kernelSIMD(Ciphertext &res, Ciphertext &x, Ciphertext &y,
                           long dim, long num_points, long degree) {
  // diff = (x - y)^2
  Ciphertext diff;
  scheme.sub(diff, x, y);

  l2NormSquaredSIMD(res, diff, dim, num_points);

  // res = (1 - res)^(2^degree)
  scheme.negateAndEqual(res);
  scheme.addConstAndEqual(res, 1.0, logp);
  for (long i = 0; i < degree; ++i) {
    scheme.squareAndEqual(res);
    scheme.reScaleByAndEqual(res, logp);
  }
}

void MeanShift::modeSeeking(std::vector<Ciphertext> &res,
                            std::vector<Ciphertext> &points, long d, long steps,
                            long kdegree, long inv_steps) {
  res.resize(d);

  // Choose dusts
  std::mt19937 gen(seed);
  std::uniform_int_distribution<> distrib(0, points.size() - 1);
  for (long i = 0; i < d; ++i) {
    res[i].copy(points[distrib(gen)]);
  }

  // Gradient ascent
  for (long iter = 0; iter < steps; ++iter) {
    // dust.logq := dust.logq - (kdegree + inv_steps + 4) * logp
    for (auto &dust : res) {
      // Bootstrap if modulus not large enough for one more iteration
      if (dust.logq < (kdegree + inv_steps + 4) * logp + 1) {
        scheme.bootstrapAndEqual(dust, logq_boot, logQ, logt);
      }

      Ciphertext sum, A, a, tpoint, temp;

      for (size_t k = 0; k < points.size(); ++k) {
        if (points[k].logq != dust.logq)
          scheme.modDownTo(tpoint, points[k], dust.logq);
        else
          tpoint.copy(points[k]);

        kernel(a, tpoint, dust, kdegree);

        scheme.modDownTo(temp, tpoint, a.logq);
        scheme.multAndEqual(temp, a);
        scheme.reScaleByAndEqual(temp, logp);

        if (k == 0) {
          sum.copy(a);
          A.copy(temp);
        } else {
          scheme.addAndEqual(sum, a);
          scheme.addAndEqual(A, temp);
        }
      }

      Ciphertext inv;
      inverse(inv, sum, points.size(), inv_steps);

      scheme.modDownToAndEqual(A, inv.logq);
      scheme.mult(dust, inv, A);
      scheme.reScaleByAndEqual(dust, logp);
    }
  }
}

void MeanShift::refreshDusts(Ciphertext &dusts, long dim, long npoints,
                             long d) {
  Ciphertext temp1, temp2;

  // The cost of bootstrapping is proportional to the number of underlying
  // plaintext slots. Bootstrapping constitutes one of the most expensive
  // operations in the algorithm.
  //
  // Currently dusts are of the form (d1, d1, ..., d1, d2, d2, ..., dd).
  // We want convert it to (d1, d2, d3, ..., dd) to reduce the number of
  // plaintext slots.
  std::vector<complex<double>> selector(slots, complex<double>{0, 0});
  for (long i = 0; i < d; ++i) {
    std::fill(selector.begin() + i * npoints * dim,
              selector.begin() + i * npoints * dim + dim,
              complex<double>{1, 0});
    scheme.multByConstVec(temp1, dusts, selector.data(), logp);
    scheme.reScaleByAndEqual(temp1, logp);
    std::fill(selector.begin() + i * npoints * dim,
              selector.begin() + i * npoints * dim + dim,
              complex<double>{0, 0});

    if (i == 0) {
      temp2.copy(temp1);
    } else {
      leftRotateAndEqual(temp1, i * dim * (npoints - 1));
      scheme.addAndEqual(temp2, temp1);
    }
  }

  long step = 1 << ((long)std::ceil(std::log2(dim * d)));
  repeatSlotRight(temp2, slots / step, step);
  auto temp = temp2.n;
  temp2.n = step;

  scheme.bootstrapAndEqual(temp2, logq_boot, logQ, logt);
  temp2.n = temp;

  for (long i = 0; i < d; ++i) {
    std::fill(selector.begin() + i * dim, selector.begin() + (i + 1) * dim,
              complex<double>{1, 0});
    scheme.multByConstVec(temp1, temp2, selector.data(), logp);
    scheme.reScaleByAndEqual(temp1, logp);
    std::fill(selector.begin() + i * dim, selector.begin() + (i + 1) * dim,
              complex<double>{0, 0});

    if (i == 0) {
      dusts.copy(temp1);
    } else {
      rightRotateAndEqual(temp1, i * dim * (npoints - 1));
      scheme.addAndEqual(dusts, temp1);
    }
  }
}

void MeanShift::modeSeekingSIMD(Ciphertext &dusts,
                                std::vector<Ciphertext> &points, long dim,
                                long npoints, long d, long steps, long kdegree,
                                long inv_steps) {
  Ciphertext temp;

  // Choose dusts
  std::mt19937 gen(seed);
  // Can also choose a ciphertext at random and then a point in the ciphertext
  // at random. However, this approach allows easy verification when computed
  // in plaintext (check plaintext_clustering).
  std::uniform_int_distribution<> distrib(0, npoints * points.size() - 1);
  std::vector<complex<double>> selector(slots, complex<double>{0, 0});

  // Assume d < slots / (dim * npoints)
  for (long i = 0; i < d; ++i) {
    long r = distrib(gen);
    long pi = r % npoints;
    long ci = r / npoints;

    std::fill(selector.begin() + pi * dim, selector.begin() + (pi + 1) * dim,
              complex<double>{1, 0});
    scheme.multByConstVec(temp, points[ci], selector.data(), logp);
    scheme.reScaleByAndEqual(temp, logp);
    std::fill(selector.begin() + pi * dim, selector.begin() + (pi + 1) * dim,
              complex<double>{0, 0});

    long beg = i * npoints;
    if (beg < pi) {
      leftRotateAndEqual(temp, (pi - beg) * dim);
    } else if (beg > pi) {
      rightRotateAndEqual(temp, (beg - pi) * dim);
    }

    if (i == 0)
      dusts.copy(temp);
    else
      scheme.addAndEqual(dusts, temp);
  }

  // dusts will be repeated in loop below. The current dust format is expected
  // at the start of each iteration.

  // Gradient ascent
  // Now, dusts.logq = points.logq - logp

  Ciphertext grad, sum, A, temp2, tpoint;

  for (long iter = 0; iter < steps; ++iter) {
    // Bootstrap if modulus not large enough for one more iteration
    if (dusts.logq - (kdegree + inv_steps + 6) * logp < logq_boot) {
      refreshDusts(dusts, dim, npoints, d);
    }

    if (dusts.logq - (kdegree + inv_steps + 6) * logp < logq_boot) {
      throw std::runtime_error(
          "Modulus not large enough for mode seeking after bootstrap.");
    }

    // In the beginning of each iteration, only the first `dim` slots of dusts
    // have the right value and everything else has 0.
    repeatSlotRight(dusts, npoints, dim);

    for (size_t i = 0; i < points.size(); ++i) {
      scheme.modDownTo(tpoint, points[i], dusts.logq);
      kernelSIMD(grad, dusts, tpoint, dim, npoints * d, kdegree);

      scheme.modDownTo(temp2, tpoint, grad.logq);
      scheme.mult(temp, grad, temp2);
      scheme.reScaleByAndEqual(temp, logp);
      sumSlotsLeft(temp, npoints, dim);
      if (i == 0) {
        A.copy(temp);
      } else {
        scheme.addAndEqual(A, temp);
      }

      sumSlotsLeft(grad, npoints, dim);
      if (i == 0) {
        sum.copy(grad);
      } else {
        scheme.addAndEqual(sum, grad);
      }
    }

    Ciphertext inv;
    inverse(inv, sum, npoints * points.size(), inv_steps);

    scheme.modDownToAndEqual(A, inv.logq);
    scheme.mult(dusts, inv, A);
    scheme.reScaleByAndEqual(dusts, logp);
    clearSlotsRight(dusts, dim, dim * npoints, d * npoints * dim);
  }

  repeatSlotRight(dusts, npoints, dim);
}

// z := max(kdegree + 2, inv_steps + minidx_t + 5)
// res.logq = dusts.logq - z * logp
void MeanShift::pointLabeling(std::vector<std::vector<Ciphertext>> &res,
                              std::vector<Ciphertext> &dusts,
                              std::vector<Ciphertext> &points, long kdegree,
                              long inv_steps, long minidx_t) {
  std::vector<Ciphertext> nbhd(dusts.size());
  Ciphertext temp;

  for (size_t i = 0; i < dusts.size(); ++i) {
    for (size_t j = 0; j < dusts.size(); ++j) {
      kernel(temp, dusts[i], dusts[j], kdegree);

      if (j == 0) {
        nbhd[i].copy(temp);
      } else {
        scheme.addAndEqual(nbhd[i], temp);
      }
    }
  }

  std::vector<Ciphertext> Cbar;
  std::vector<Ciphertext> norms(dusts.size());

  res.resize(points.size());

  for (size_t i = 0; i < points.size(); ++i) {
    for (size_t j = 0; j < dusts.size(); ++j) {
      scheme.sub(temp, points[i], dusts[j]);
      l2NormSquared(norms[j], temp);
    }

    minIdx(Cbar, norms, minidx_t, inv_steps);
    res[i].resize(dusts.size());

    for (size_t j = 0; j < dusts.size(); ++j) {
      // make compatible for multiplication
      if (Cbar[j].logq < nbhd[j].logq) {
        // this will be run once for all points
        scheme.modDownToAndEqual(nbhd[j], Cbar[j].logq);
      } else if (Cbar[j].logq > nbhd[j].logq) {
        scheme.modDownToAndEqual(Cbar[j], nbhd[j].logq);
      }

      scheme.mult(res[i][j], Cbar[j], nbhd[j]);
      scheme.reScaleByAndEqual(res[i][j], logp);
    }
  }
}

// res.logq := dusts.logq - max(kdegree + 5, inv_steps + minidx_t + 7) logp
void MeanShift::pointLabelingSIMD(std::vector<Ciphertext> &res,
                                  Ciphertext &dusts,
                                  std::vector<Ciphertext> &points, long dim,
                                  long npoints, long d, long kdegree,
                                  long inv_steps, long minidx_t) {
  Ciphertext temp1, temp2, nbhd;

  // Currently dusts are of the form (d1, d1, ..., d1, d2, d2, ..., dd).
  // We want to now convert it to
  // (d1, d2, d3, ..., dd, 0, .., 0, d1, d2, d3, ... (repeated d times), dd, 0,
  // .., 0) so that we can easily compute pairwise kernel on the dusts.
  std::vector<complex<double>> selector(slots, complex<double>{0, 0});
  for (long i = 0; i < d; ++i) {
    std::fill(selector.begin() + i * npoints * dim,
              selector.begin() + i * npoints * dim + dim,
              complex<double>{1, 0});
    scheme.multByConstVec(temp1, dusts, selector.data(), logp);
    scheme.reScaleByAndEqual(temp1, logp);
    std::fill(selector.begin() + i * npoints * dim,
              selector.begin() + i * npoints * dim + dim,
              complex<double>{0, 0});

    if (i == 0) {
      temp2.copy(temp1);
    } else {
      leftRotateAndEqual(temp1, i * dim * (npoints - 1));
      scheme.addAndEqual(temp2, temp1);
    }
  }

  // `temp2` will now contain the dusts in the required format.
  repeatSlotRight(temp2, d, npoints * dim);

  // Compute `nbhd` which is the sum of the kernel value of a dust to every
  // other dust. (Refer paper for details)
  scheme.modDownBy(temp1, dusts, logp);
  kernelSIMD(nbhd, temp2, temp1, dim, npoints * d, kdegree);
  sumSlotsLeft(nbhd, d, dim);
  clearSlotsRight(nbhd, dim, npoints * dim, d * npoints * dim);
  repeatSlotRight(nbhd, npoints, dim);

  // Compute labels for each point ciphertext and store result in `res`.
  res.resize(points.size());
  for (size_t i = 0; i < points.size(); ++i) {
    scheme.sub(temp1, dusts, points[i]);
    l2NormSquaredSIMD(temp2, temp1, dim, npoints * d);
    minIdxSIMD(temp1, temp2, dim, npoints, minidx_t, inv_steps, d);

    if (nbhd.logq < temp1.logq)
      scheme.modDownToAndEqual(temp1, nbhd.logq);
    else if (nbhd.logq > temp1.logq)
      scheme.modDownToAndEqual(nbhd, temp1.logq);

    scheme.mult(res[i], temp1, nbhd);
    scheme.reScaleByAndEqual(res[i], logp);
  }
}

void MeanShift::clusterSIMD(std::vector<Ciphertext> &res,
                            std::vector<Ciphertext> &points, long dim,
                            long npoints, long num_dusts, long iterations,
                            long mode_kdegree, long label_kdegree,
                            long mode_inv_steps, long label_inv_steps,
                            long minidx_t) {
  Ciphertext dusts;
  modeSeekingSIMD(dusts, points, dim, npoints, num_dusts, iterations,
                  mode_kdegree, mode_inv_steps);

  // `dusts` has a lower modulus
  // Check if modulus suffices for point labeling
  if (dusts.logq < logq_boot)
    throw std::runtime_error(
        "Modulus lower than logl after mode seeking. Cannot bootstrap.");

  if (dusts.logq <
      std::max(label_kdegree + 5, label_inv_steps + minidx_t + 7) * logp) {
    refreshDusts(dusts, dim, npoints, num_dusts);
    repeatSlotRight(dusts, npoints, dim);
  }

  if (dusts.logq <
      std::max(label_kdegree + 5, label_inv_steps + minidx_t + 7) * logp)
    throw std::runtime_error(
        "Modulus not large enough for point labelling after bootstrap.");

  if (dusts.logq <
      std::max(label_kdegree + 5, label_inv_steps + minidx_t + 7) * logp)
    throw std::runtime_error(
        "Modulus not large enough for point labelling after bootstrap.");

  // `pointLabelingSIMD` expects `dusts` and `points` to have same modulus.
  std::vector<Ciphertext> mod_points(points.size());
  for (size_t i = 0; i < points.size(); ++i) {
    scheme.modDownTo(mod_points[i], points[i], dusts.logq);
  }

  pointLabelingSIMD(res, dusts, mod_points, dim, npoints, num_dusts,
                    label_kdegree, label_inv_steps, minidx_t);
}


#ifndef REALTIME_MF_HPP_INCLUDED
#define REALTIME_MF_HPP_INCLUDED
#include <root_includes.hpp>
#include <defines.h>
#include <signal.h>
#include <string>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <boost/thread.hpp>

#include <Eigen/Dense>

// CUDA runtime
#include <cuda_runtime.h>

// Utilities and system includes
#include <nvidia/helper_functions.h>
#include <nvidia/helper_cuda.h>

#include <cuda_pc_helpers.h>
#include <convolutionSeparable_common.h>
#include <convolutionSeparable_common_small.h>
#include <optimizationSO3.hpp>
#include <optimizationSO3_approx.hpp>
#include <cv_helpers.hpp>
#include <pcl_helpers.hpp>

//#include <normalExtractCUDA.hpp>
#include <cudaPcl/timer.hpp>
#include <cudaPcl/timerLog.hpp>

using namespace Eigen;

class RealtimeMF
{
  public:
    RealtimeMF(std::string mode, uint32_t w, uint32_t h);
    ~RealtimeMF();

    /* process a depth image of size w*h and return rotation estimate
     * mfRc
     */
    Matrix3f depth_cb(const uint16_t *data, int w,int h);

    void getAxisAssignments();

    void visualizePc();

    void run();

    double dtNormals_;
    double dtPrep_;
    double dtCG_;
    double dtTotal_;
    double residual_, D_KL_;
    uint32_t nCGIter_;

    TimerLog tLog_;

  protected:

    void init(int w, int h);

    uint32_t w_, h_;
    boost::mutex updateModelMutex;

    NormalExtractGpu<float> normalExtractor_;

    ofstream fout_;
    bool update, updateRGB_;
    std::string mode_;

    Matrix3f kRw_;

    OptSO3 *optSO3_;
    pcl::PointCloud<pcl::PointXYZRGB> nDisp_;
    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr nDisp_cp_;
    pcl::PointCloud<pcl::PointXYZRGB> n_;
    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr n_cp_;

    pcl::PointCloud<pcl::PointXYZ> pc_;
    pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc_cp_;

    cv::Mat rgb_;

    Vector3d vicon_t_;
    Quaterniond vicon_q_;
    Matrix3d kRv;
    Vector3d dt0;

    virtual void run_impl() = 0;
    virtual void run_cleanup_impl() = 0;
};

#endif


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    struct nbexponentbits_ : boost::dispatch::constant_value_<nbexponentbits_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,nbexponentbits_,boost::dispatch::constant_value_<nbexponentbits_>);
      struct value_map
      {
        template<typename X>
        static std::integral_constant<X,0> value(boost::dispatch::integer_<X> const&);

        template<typename X>
        static std::integral_constant<std::int32_t,8> value(boost::dispatch::single_<X> const&);

        template<typename X>
       static std::integral_constant<std::int64_t,11> value(boost::dispatch::double_<X> const&);
      };
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,nbexponentbits_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::nbexponentbits_,nbexponentbits);
  }

  template<typename T> BOOST_FORCEINLINE auto Nbexponentbits()
  BOOST_NOEXCEPT_DECLTYPE(detail::nbexponentbits( boost::dispatch::as_<T>{}))
  {
    return detail::nbexponentbits( boost::dispatch::as_<T>{} );
  }
} }

#endif


/* standardize_swc_plugin.cpp
 * This is a test plugin, you can use it as a demo.
 * 2016-1-28 : by Zhi Zhou
 */

#include "v3d_message.h"
#include <vector>
#include "standardize_swc_plugin.h"
#include <iostream>
#include <boost/lexical_cast.hpp>
#include "../../../released_plugins/v3d_plugins/resample_swc/resampling.h"
#include "../../../released_plugins/v3d_plugins/sort_neuron_swc/sort_swc.h"
#include <climits>

using namespace std;
#define getParent(n,nt) ((nt).listNeuron.at(n).pn<0)?(1000000000):((nt).hashNeuron.value((nt).listNeuron.at(n).pn))
template <class T> T pow2(T a)
{
    return a*a;

}
Q_EXPORT_PLUGIN2(standardize_swc, standardize);

bool export_list2file_retype(QList<NeuronSWC> & lN, QString fileSaveName, int soma_ID, int new_type)
{
    QFile file(fileSaveName);
    if (!file.open(QIODevice::WriteOnly|QIODevice::Text))
        return false;
    QTextStream myfile(&file);
    myfile << "#n,type,x,y,z,radius,parent"<< "\n";
    for (V3DLONG i=0;i<lN.size();i++)
    {
        if(i == soma_ID){
            myfile << lN.at(i).n <<" " << "1" << " "<< lN.at(i).x <<" "<<lN.at(i).y << " "<< lN.at(i).z << " "<< lN.at(i).r << " " <<lN.at(i).pn << "\n";
        }
        else{
            //retype types
            int t = new_type;
            if ( new_type == -1) //use the original type
                 t = lN.at(i).type;
            myfile << lN.at(i).n <<" " << t << " "<< lN.at(i).x <<" "<<lN.at(i).y << " "<< lN.at(i).z << " "<< lN.at(i).r << " " <<lN.at(i).pn << "\n";
          }
    }
    file.close();
    cout<<"swc file "<<fileSaveName.toStdString()<<" has been generated, size: "<<lN.size()<<endl;
    return true;
};

QStringList standardize::menulist() const
{
	return QStringList()
        <<tr("standardize")
		<<tr("about");
}

QStringList standardize::funclist() const
{
	return QStringList()
        <<tr("standardize")
		<<tr("help");
}

void standardize::domenu(const QString &menu_name, V3DPluginCallback2 &callback, QWidget *parent)
{
    if (menu_name == tr("standardize"))
	{
        v3d_msg("Only works on dofunc for now.");
	}
	else
	{
        v3d_msg(tr("Developed by Xiaoxiao Liu, 2016-5-28"));
	}
}

bool standardize::dofunc(const QString & func_name, const V3DPluginArgList & input, V3DPluginArgList & output, V3DPluginCallback2 & callback,  QWidget * parent)
{
	vector<char*> infiles, inparas, outfiles;
	if(input.size() >= 1) infiles = *((vector<char*> *)input.at(0).p);
	if(input.size() >= 2) inparas = *((vector<char*> *)input.at(1).p);
	if(output.size() >= 1) outfiles = *((vector<char*> *)output.at(0).p);

    if (func_name == tr("standardize"))
	{
        cout<<"This is standardize_swc plugin"<<endl;
        if(infiles.size() <2)
        {
            cerr<<"Need  both the reference swc file ( to seed the soma) and the input swc file."<<endl;
            cerr<<"You can use the input swc file as the reference swc file, if no particular soma location is required." <<endl;
            return false;
        }

        QString  ref_swc_file =  infiles[0];
        QString  inputswc_file = infiles[1];
        if(inputswc_file.isEmpty() || ref_swc_file.isEmpty())
        {
            cerr<<"please check both input swc files"<<endl;
            return false;
        }

        if (inparas.size() <1)
        {
            printf("Please specify two parameters - 1)the gap size for bridging during the sorting step; 2) the type to be assgined.\n");
            return false;
        }

        double dis_th = atof(inparas.at(0));

        int type = -1;//use the old type
        if (inparas.size() ==2)
           type =  atof(inparas.at(1));

        double sort_th = dis_th ; //the parameter in sort_neuron_swc plugin, specifying the length threshold to bridge the gap

        QString  outswc_file =  outfiles[0];

        cout<<"ref_swc_file = "<<ref_swc_file.toStdString().c_str()<<endl;
        cout<<"inswc_file = "<<inputswc_file.toStdString().c_str()<<endl;
        cout<<"gap threshold = "<<dis_th<<endl;
        cout<<"outswc_file = "<<outswc_file.toStdString().c_str()<<endl;
        cout<<"new type code = "<<type<<endl;

        NeuronTree nt_input = readSWC_file(inputswc_file);
//        cout<<" Standardize: 1) resample"<<endl;
//        NeuronTree nt_input_rs = resample(nt_input, dis_th);

        NeuronTree nt_ref = readSWC_file(ref_swc_file);
        double soma_x, soma_y, soma_z, soma_r;
        for (V3DLONG i = 0; i < nt_ref.listNeuron.size(); i++)
        {
            if(nt_ref.listNeuron[i].pn<0)
            {
                soma_x = nt_ref.listNeuron.at(i).x;
                soma_y = nt_ref.listNeuron.at(i).y;
                soma_z = nt_ref.listNeuron.at(i).z;
                soma_r = nt_ref.listNeuron.at(i).r;
                break;
            }
        }

        V3DLONG neuronNum = nt_input.listNeuron.size();
        QVector<QVector<V3DLONG> > children_list = QVector< QVector<V3DLONG> >(neuronNum, QVector<V3DLONG>() );

        for (V3DLONG i = 0; i < neuronNum; i++)
        {
            V3DLONG parent = nt_input.listNeuron[i].pn;
            if (parent < 0)
                continue;
            children_list[nt_input.hashNeuron.value(parent)].push_back(i);
        }

        double Dist = INT_MAX;
        double Dist_inrange = INT_MAX;
        V3DLONG soma_ID = -1;
        V3DLONG dist_ID = -1;
        int child_num = 0;

        cout<<" standardize: 1) matching soma roots"<<endl;
       // set the distance threshold to searching for matching soma node

        QList<NeuronSWC> list_neurons = nt_input.listNeuron;
        double search_distance_th = soma_r * 5 ;// choose the bigger value between soma_r*5 and search_distance_th*5
        if (search_distance_th < sort_th *5)
         {
             search_distance_th = sort_th *5;
         }

        for (V3DLONG i=0;i<list_neurons.size();i++)
        {
            NeuronSWC curr = list_neurons.at(i);
            double nodedist = sqrt(pow2(curr.x - soma_x) + pow2(curr.y - soma_y) + pow2(curr.z - soma_z));
            if(nodedist <= search_distance_th && curr.pn <0)
            {
                soma_ID = curr.n;
                child_num = 1;
                break;
            }

            if(nodedist <= search_distance_th  && children_list[i].size() > child_num)
            {
                soma_ID = curr.n;
                child_num = children_list[i].size();
                Dist_inrange = nodedist;
            }

            if(nodedist <= search_distance_th && children_list[i].size() == child_num && nodedist < Dist_inrange)
            {
                soma_ID = curr.n;
                Dist_inrange = nodedist;
            }

            if(nodedist < Dist)
            {
                dist_ID = curr.n;
                Dist = nodedist;
            }
        }

        if(child_num < 1 || soma_ID == -1) soma_ID = dist_ID;

        cout<<" Standardize: 2) sort"<<endl;
        QList<NeuronSWC> result;
        if (!SortSWC(nt_input.listNeuron, result ,soma_ID, sort_th))
        {
            v3d_msg("fail to call swc sorting function.",0);
            return false;
        }

        cout<<" Standardize: 3) only keep the first tree(identified by the soma from the reference."<<endl;

        //skip the first root
        for (V3DLONG i = 1;i<result.size();i++)
        {
            if (result[i].pn == -1)
            {//remove other trees, only keep the first one
                cout<< "remove "<<result.size()-i<<" nodes"<<endl;
                result.erase(result.begin()+i, result.end());

            }
        }

        cout<<" Standardize: 4) reset type for soma and neurites"<<endl;
        //1-soma
        //2-axon
        //3-dendrite
        //4-apical dendrite

        export_list2file_retype(result,outswc_file,0,type);
	}
	else if (func_name == tr("help"))
	{
        cout << "This super-plugin  is used to post-processing auto reconstructions for comparisons. "<<endl;
        cout << "It will identify the soma for each input reconstruction " <<endl;
        cout << "based on the reference SWC file, and sort SWC nodes based on the soma root while bridging" <<endl;
        cout << "all disconneted components when the gap is less then the specified gap_threshold. The search range "<<endl;
        cout << "for the matching soma  is  5*soma_radius." <<endl;
        cout << "Usage : <vaa3d> -x standardize -f standardize -i <reference_swc_file> <inswc_file> -o <outswc_file> -p <gap_threshold>  <new_neurite_type> "<<endl;
	}
    else
        return false;

	return true;
}


#include <cmath>
#include <iomanip>
#include <limits>
#include <stdexcept>
#include <vector>
#include <boost/tr1/array.hpp> // so array works with old and new compilers

#include "coordConv/mathUtils.h"
#include "coordConv/pvtCoord.h"

static const double DeltaT = 0.01;

namespace coordConv {

    PVTCoord::PVTCoord(Coord const &coord, Eigen::Vector3d const &vel, double tai) :
        _coord(coord),
        _vel(vel),
        _tai(tai)
    { };

    PVTCoord::PVTCoord(Coord const &coord0, Coord const &coord1, double tai, double deltaT) {
        _setFromCoordPair(coord0, coord1, tai, deltaT);
    }

    PVTCoord::PVTCoord(PVT const &equatPVT, PVT const &polarPVT, PVT const &distPVT) {
        if (equatPVT.t != polarPVT.t) {
            std::ostringstream os;
            os << "equatPVT.t = " << equatPVT.t << " != " << polarPVT.t << " = polarPVT.t";
            throw std::runtime_error(os.str());
        }
        if (distPVT.isfinite() and (distPVT.t != equatPVT.t)) {
            std::ostringstream os;
            os << "distPVT is finite and distPVT.t = " << distPVT.t << " != " << equatPVT.t << " = equatPVT.t";
            throw std::runtime_error(os.str());
        }
        double const tai = equatPVT.t;
        std::vector<Coord> coordArr;
        for (int i = 0; i < 2; ++i) {
            double evalTAI = tai + (i * DeltaT);
            double parallax = 0;
            if (distPVT.isfinite()) {
                parallax = parallaxFromDistance(distPVT.getPos(evalTAI));
            }
            coordArr.push_back(Coord(equatPVT.getPos(evalTAI), polarPVT.getPos(evalTAI), parallax));
        }
        _setFromCoordPair(coordArr[0], coordArr[1], tai, DeltaT);
    }

    PVTCoord::PVTCoord(PVT const &equatPVT, PVT const &polarPVT, PVT const &distPVT, double equatPM, double polarPM, double radVel) {
        if (equatPVT.t != polarPVT.t) {
            std::ostringstream os;
            os << "equatPVT.t = " << equatPVT.t << " != " << polarPVT.t << " = polarPVT.t";
            throw std::runtime_error(os.str());
        }
        if (distPVT.isfinite() and (distPVT.t != equatPVT.t)) {
            std::ostringstream os;
            os << "distPVT is finite and distPVT.t = " << distPVT.t << " != " << equatPVT.t << " = equatPVT.t";
            throw std::runtime_error(os.str());
        }
        std::vector<Coord> coordArr;
        double const tai = equatPVT.t;
        for (int i = 0; i < 2; ++i) {
            double evalTAI = tai + (i * DeltaT);
            double parallax = 0;
            if (distPVT.isfinite()) {
                parallax = parallaxFromDistance(distPVT.getPos(evalTAI));
            }
            coordArr.push_back(Coord(equatPVT.getPos(evalTAI), polarPVT.getPos(evalTAI), parallax, equatPM, polarPM, radVel));
        }
        _setFromCoordPair(coordArr[0], coordArr[1], tai, DeltaT);
    }

    PVTCoord::PVTCoord() :
        _coord(Coord()),
        _vel(),
        _tai(DoubleNaN)
    { };

    PVTCoord PVTCoord::copy(double tai) const {
        return PVTCoord(getCoord(tai), _vel, tai);
    }

    Coord PVTCoord::getCoord(double tai) const {
        if (tai == _tai) {
            return _coord;
        }
        Eigen::Vector3d pos = _coord.getVecPos() + (_vel * (tai - _tai));
        return Coord(pos, _coord.getVecPM());
    }

    bool PVTCoord::getSphPVT(PVT &equatPVT, PVT &polarPVT) const {
        double equatPos[2], polarPos[2];
        bool atPole = false;
        for (int i = 0; i < 2; ++i) {
            double evalDate = _tai + (i * DeltaT);
            Coord coord = getCoord(evalDate);
            atPole |= coord.getSphPos(equatPos[i], polarPos[i]);
        }
        equatPVT.setFromPair(equatPos, _tai, DeltaT, true);
        polarPVT.setFromPair(polarPos, _tai, DeltaT, false);
        return atPole;
    }

    PVT PVTCoord::getDistance() const {
        double dist[2];
        for (int i = 0; i < 2; ++i) {
            double evalTAI = _tai + (i * DeltaT);
            dist[i] = getCoord(evalTAI).getDistance();
        }
        PVT distPVT;
        distPVT.setFromPair(dist, _tai, DeltaT, false);
        return distPVT;
    }

    bool PVTCoord::isfinite() const {
        return _coord.isfinite()
            && std::isfinite(_vel(0)) && std::isfinite(_vel(1)) && std::isfinite(_vel(2))
            && std::isfinite(_tai);
    }

    PVT PVTCoord::angularSeparation(PVTCoord const &pvtCoord) const {
        double posArr[2];
        for (int i = 0; i < 2; ++i) {
            double evalTAI = _tai + (i * DeltaT);
            Coord thisCoord = getCoord(evalTAI);
            Coord otherCoord = pvtCoord.getCoord(evalTAI);
            posArr[i] = thisCoord.angularSeparation(otherCoord);
        }
        PVT res = PVT();
        res.setFromPair(posArr, _tai, DeltaT, false);
        return res;
    }

    PVT PVTCoord::orientationTo(PVTCoord const &pvtCoord) const {
        double posArr[2];
        for (int i = 0; i < 2; ++i) {
            double evalTAI = _tai + (i * DeltaT);
            Coord thisCoord = getCoord(evalTAI);
            Coord otherCoord = pvtCoord.getCoord(evalTAI);
            posArr[i] = thisCoord.orientationTo(otherCoord);
        }
        PVT res = PVT();
        // if the orientation is only finite at one of the two times
        // then the distance is 0 at the other time and the orientation is fixed
        // (since the relative velocity vector goes through this PVT)
        if (std::isfinite(posArr[0]) && std::isfinite(posArr[1])) {
            res.setFromPair(posArr, _tai, DeltaT, true);
        } else if (std::isfinite(posArr[0])) {
            res.pos = posArr[0];
            res.vel = 0;
            res.t = _tai;
        } else if (std::isfinite(posArr[1])) {
            res.pos = posArr[1];
            res.vel = 0;
            res.t = _tai;
        } else {
            res.pos = DoubleNaN;
            res.vel = DoubleNaN;
            res.t = _tai;
        }
        return res;
    }

    PVTCoord PVTCoord::offset(PVT &toOrient, PVT const &fromOrient, PVT const &dist) const {
        std::vector<Coord> coordArr;
        double toOrientArr[2];
        for (int i = 0; i < 2; ++i) {
            double evalTAI = _tai + (i * DeltaT);
            Coord unoffCoord = getCoord(evalTAI);
            coordArr.push_back(unoffCoord.offset(toOrientArr[i], fromOrient.getPos(evalTAI), dist.getPos(evalTAI)));
        }
        toOrient.setFromPair(toOrientArr, _tai, DeltaT, true);
        return PVTCoord(coordArr[0], coordArr[1], _tai, DeltaT);
    }

    std::string PVTCoord::__repr__() const {
        std::ostringstream os;
        os << *this;
        return os.str();
    }

    void PVTCoord::_setFromCoordPair(Coord const &coord0, Coord const &coord1, double tai, double deltaT) {
        if (deltaT == 0) {
            throw std::runtime_error("deltaT must be nonzero");
        }
        _coord = coord0;
        _tai = tai;
        _vel = (coord1.getVecPos() - coord0.getVecPos()) / deltaT;
    }

    std::ostream &operator<<(std::ostream &os, PVTCoord const &pvtCoord) {
        Coord coord = pvtCoord.getCoord();
        Eigen::Vector3d vel = pvtCoord.getVel();
        double tai = pvtCoord.getTAI();
        std::ios_base::fmtflags oldFlags = os.flags();
        std::streamsize const oldPrecision = os.precision();
        os << std::fixed
            << "PVTCoord(" << coord
            << ", (" << vel(0) << ", " << vel(1) << ", " << vel(2) << "), "
            << std::setprecision(7) << tai
            << ")" << std::setprecision(oldPrecision);
        os.flags(oldFlags);
        return os;
    }

}


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
//#include"TaskSpace.h"
#include"taskspaceoffline.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"

using namespace  std;
using namespace  Eigen;
//data of left foot sensor
double a;
double b;
double c;
double d;

void receiveFootSensor(const std_msgs::Int32MultiArray& msg)
{
    if (msg.data.size()!=4) {
        qDebug("the size of sensor data is in wrong");
        return;
    }

    //ROS_INFO("I heard: [%d  %d %d %d]", (int)msg.data[0],(int)msg.data[1],(int)msg.data[2],(int)msg.data[3]);
    double temp[4];
    int tempInt[4];
    temp[0]=msg.data[0];
    temp[1]=msg.data[1];
    temp[2]=msg.data[2];
    temp[3]=msg.data[3];
    //normalizing data of sensors
    temp[0]=temp[0]*(100.0/105);
    temp[1]=temp[1]*(100.0/99);
    temp[2]=temp[2]*(100.0/116);
    temp[3]=temp[3]*(100.0/116);

    tempInt[0]=temp[0];
    tempInt[1]=temp[1];
    tempInt[2]=temp[2];
    tempInt[3]=temp[3];

    a=tempInt[0];
    b=tempInt[1];
    c=tempInt[2];
    d=tempInt[3];

   // ROS_INFO("I heard: [%d  %d %d %d]", tempInt[0],tempInt[1],tempInt[2],tempInt[3]);
}

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

    QCgenerator QC;
    QElapsedTimer timer;
    double dt;

    double k1;
    double k2;
    double k3;
    double k4;

    bool aState=false;
    bool bState=false;
    bool cState=false;
    bool dState=false;

    double teta_motor_L=0;
    double teta_motor_R=0;
    double phi_motor_L=0;
    double phi_motor_R=0;

    double footSensorSaturation=90;
    //*******************This part of code is for initialization of joints of the robot for walking**********************************
    int count = 0;

    ros::init(argc, argv, "bumpsensorcontroller");

    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);

    ros::Subscriber sub = nh.subscribe("/foot", 1000, receiveFootSensor);

    ros::Rate loop_rate(100);
    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);
    k1=0.00009;
    k2=0.00009;
    k3=0.00001;
    k4=0.00001;

    while (ros::ok())
    {

        if ((a)>=footSensorSaturation && (b)>=footSensorSaturation && (c)>=footSensorSaturation && (d)>=footSensorSaturation){
            qDebug("swing foot landing is successful");
            aState=true;
            bState=true;
            cState=true;
            dState=true;

            //do nothing all sensors are on the ground
            teta_motor_L=teta_motor_L;
            phi_motor_L=phi_motor_L;

            teta_motor_L=teta_motor_R;
            phi_motor_L=phi_motor_R;

            exit(0);

        }
       else {//when the four sensors are not saturated

//            if ((a)>=footSensorthreshold ){aState=true;}else {aState=false;}
//            if ((b)>=footSensorthreshold ){bState=true;}else {bState=false;}
//            if ((c)>=footSensorthreshold ){cState=true;}else {cState=false;}
//            if ((d)>=footSensorthreshold ){dState=true;}else {dState=false;}

//        minimum={-14.0,-55.0,0.0,-50.0,-55.0,-14.0,0.0,-50.0,-23.0,-15.0,-15.0,-23.0};
//        maximum={14.0,26.0,75.0,35.0,26.0,14.0,75.0,35.0,23.0,10.0,10.0,23.0};

        //Pitch left ankle motor
        if (abs(b-a)>=abs(c-d)) {

            if (abs(a-b)<100) {
                teta_motor_L=1*teta_motor_L+k1*(a-b);
            }
            else {
                teta_motor_L=teta_motor_L;
            }

        }

        else {
            if (abs(d-c)<100) {
                teta_motor_L=1*teta_motor_L+k1*(d-c);
            }
            else {
                teta_motor_L=teta_motor_L;
            }

        }

        //Roll left ankle motor
        if (abs(c-b)>=abs(d-a)) {

            if (abs(c-b)<100) {
                phi_motor_L=1*phi_motor_L+k1*(c-b);
            }
            else {
                phi_motor_L=phi_motor_L;
            }

        }

        else {

            if (abs(a-d)<100) {
                phi_motor_L=1*phi_motor_L+k1*(d-a);
            }
            else {
                phi_motor_L=phi_motor_L;
            }

        }
}
//saturation of ankle motors

        if ((abs(phi_motor_L))>0.9) {
            phi_motor_L=0.9;
        }

        if ((abs(teta_motor_L))>0.9) {
            teta_motor_L=0.9;
        }

        if ((abs(phi_motor_R))>0.9) {
            phi_motor_L=0.9;
        }

        if ((abs(teta_motor_R))>0.9) {
            teta_motor_R=0.9;
        }

ROS_INFO("I heard data of sensors : [%f %f %f %f]",a,b,c,d);
//        ROS_INFO("I heard motor pitch: [%f]",teta_motor_L);
//        ROS_INFO("I heard motor roll: [%f]",phi_motor_L);
        msg.data.clear();
        vector<double> cntrl(13);
        cntrl[0]=0.0;
        cntrl[1]=0;
        cntrl[2]=0;
        cntrl[3]=0;
        cntrl[4]=0;
        cntrl[5]=teta_motor_R;//pitch
        cntrl[6]=phi_motor_R;//roll
        cntrl[7]=0;
        cntrl[8]=0;
        cntrl[9]=0;
        cntrl[10]=0;
        cntrl[11]=1*teta_motor_L;
        cntrl[12]=1*phi_motor_L;

        vector<int> qref(12);
        qref=QC.ctrldata2qc(cntrl);

        for(int  i = 0;i < 12;i++)
        {
            msg.data.push_back(qref[i]);

        }

        chatter_pub.publish(msg);
        // ROS_INFO("t={%d} c={%d}",timer.elapsed(),count);

        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }

    return 0;
}


/*
@copyright Louis Dionne 2015
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_HANA_TEST_TEST_AUTO_INTEGRAL_DOMAIN_HPP
#define BOOST_HANA_TEST_TEST_AUTO_INTEGRAL_DOMAIN_HPP

#include <boost/hana/integral_domain.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/comparable.hpp>
#include <boost/hana/core/models.hpp>
#include <boost/hana/ring.hpp>

#include <test/auto/base.hpp>
#include <test/auto/ring.hpp>

namespace boost { namespace hana { namespace test {
    template <typename D>
    auto laws<IntegralDomain, D> = [] {
        static_assert(_models<IntegralDomain, D>{}, "");

        laws<Ring, D>();

        // Instance laws
        {
            for_each(objects<D>, [](auto a) {
                for_each(objects<D>, [=](auto b) {
                    BOOST_HANA_CHECK(equal(
                        mult(a, b),
                        mult(b, a)
                    ));

                    eval_if(equal(b, zero<D>()), [](auto){}, [=](auto _) {

                        BOOST_HANA_CHECK(equal(
                            a,
                            plus(
                                mult(_(quot)(a, b), b),
                                _(rem)(a, b)
                            )
                        ));

                        BOOST_HANA_CHECK(equal(
                            _(rem)(zero<D>(), b),
                            zero<D>()
                        ));
                    });
                });
            });
        }
    };
}}} // end namespace boost::hana::test

#endif // !BOOST_HANA_TEST_TEST_AUTO_INTEGRAL_DOMAIN_HPP


#pragma once

#include <supereight/shared/commons.h>
#include <supereight/voxel_traits.hpp>

#include <Eigen/Dense>

#ifdef SE_CUDA_VF
#include <cuda_runtime.h>
#endif

namespace se {

struct SDF {};

struct sdf_update {
    const float* depth;

    Eigen::Vector2i depthSize;

    float mu;
    int maxweight = 100;

    SE_DEVICE_FUNC
    sdf_update(const float* d, const Eigen::Vector2i& framesize, float m, float,
        float);

    template<typename DataHandlerT>
    SE_DEVICE_FUNC void operator()(DataHandlerT& handler,
        const Eigen::Vector3i&, const Eigen::Vector3f& pos,
        const Eigen::Vector2f& pixel);
};

template<>
struct voxel_traits<SDF> {
#ifdef SE_CUDA_VF
    using value_type = float2;
#else
    using value_type = struct {
        float x;
        float y;
    };
#endif

    using update_func_type = sdf_update;

    static constexpr bool invert_normals = true;

    SE_DEVICE_FUNC
    static value_type empty() { return {1.f, -1.f}; }

    SE_DEVICE_FUNC
    static value_type initValue() { return {1.f, 0.f}; }

    template<typename OctreeT, typename HashType, typename IncF>
    SE_DEVICE_FUNC static void buildAllocationList(HashType* allocation_list,
        int reserved, IncF get_idx, const OctreeT& octree,
        const Eigen::Vector3f& world_vertex, const Eigen::Vector3f& direction,
        const Eigen::Vector3f&, float, float mu);

    template<typename OctreeT>
    SE_DEVICE_FUNC static Eigen::Vector4f raycast(const OctreeT& octree,
        const Eigen::Vector3f& origin, const Eigen::Vector3f& direction,
        const float tnear, const float tfar, const float mu, const float step,
        const float largestep);
};

} // namespace se


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

#include "poses-precomp.h"	// Precompiled headers
//
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/CSchemeArchiveBase.h>
#include <mrpt/serialization/CSerializable.h>

#include <Eigen/Dense>
#include <iomanip>
#include <limits>

using namespace std;
using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::poses;

IMPLEMENTS_SERIALIZABLE(CPose3DQuat, CSerializable, mrpt::poses)

/** Constructor from a CPose3D */
CPose3DQuat::CPose3DQuat(const CPose3D& p)
{
	x() = p.x();
	y() = p.y();
	z() = p.z();
	p.getAsQuaternion(m_quat);
}

/** Constructor from a 4x4 homogeneous transformation matrix.
 */
CPose3DQuat::CPose3DQuat(const CMatrixDouble44& M)
	: m_quat(UNINITIALIZED_QUATERNION)
{
	m_coords[0] = M(0, 3);
	m_coords[1] = M(1, 3);
	m_coords[2] = M(2, 3);
	CPose3D p(M);
	p.getAsQuaternion(m_quat);
}

/** Returns the corresponding 4x4 homogeneous transformation matrix for the
 * point(translation) or pose (translation+orientation).
 * \sa getInverseHomogeneousMatrix
 */
void CPose3DQuat::getHomogeneousMatrix(CMatrixDouble44& out_HM) const
{
	m_quat.rotationMatrixNoResize(out_HM);
	out_HM(0, 3) = m_coords[0];
	out_HM(1, 3) = m_coords[1];
	out_HM(2, 3) = m_coords[2];
	out_HM(3, 0) = out_HM(3, 1) = out_HM(3, 2) = 0;
	out_HM(3, 3) = 1;
}

/** Returns a 1x7 vector with [x y z qr qx qy qz] */
void CPose3DQuat::asVector(vector_t& v) const
{
	v[0] = m_coords[0];
	v[1] = m_coords[1];
	v[2] = m_coords[2];
	v[3] = m_quat[0];
	v[4] = m_quat[1];
	v[5] = m_quat[2];
	v[6] = m_quat[3];
}

/**  Makes "this = A (+) B"; this method is slightly more efficient than "this=
 * A + B;" since it avoids the temporary object.
 *  \note A or B can be "this" without problems.
 */
void CPose3DQuat::composeFrom(const CPose3DQuat& A, const CPose3DQuat& B)
{
	// The 3D point:
	double gx, gy, gz;
	A.m_quat.rotatePoint(
		B.m_coords[0], B.m_coords[1], B.m_coords[2], gx, gy, gz);
	this->m_coords[0] = A.m_coords[0] + gx;
	this->m_coords[1] = A.m_coords[1] + gy;
	this->m_coords[2] = A.m_coords[2] + gz;

	// The 3D rotation:
	this->m_quat.crossProduct(A.m_quat, B.m_quat);
}

/**  Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
 * than "this= A - B;" since it avoids the temporary object.
 *  \note A or B can be "this" without problems.
 * \sa composeFrom
 */
void CPose3DQuat::inverseComposeFrom(const CPose3DQuat& A, const CPose3DQuat& B)
{
	// The 3D point:
	const CQuaternionDouble B_conj(
		B.m_quat.r(), -B.m_quat.x(), -B.m_quat.y(), -B.m_quat.z());
	B_conj.rotatePoint(
		A.m_coords[0] - B.m_coords[0], A.m_coords[1] - B.m_coords[1],
		A.m_coords[2] - B.m_coords[2], this->m_coords[0], this->m_coords[1],
		this->m_coords[2]);
	// The 3D rotation:
	this->m_quat.crossProduct(B_conj, A.m_quat);
}

/**  Computes the 3D point G such as \f$ G = this \oplus L \f$.
 * \sa inverseComposeFrom
 */
void CPose3DQuat::composePoint(
	const double lx, const double ly, const double lz, double& gx, double& gy,
	double& gz, mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint,
	mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose) const
{
	if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
	{
		const double qx2 = square(m_quat.x());
		const double qy2 = square(m_quat.y());
		const double qz2 = square(m_quat.z());

		// Jacob: df/dpoint
		if (out_jacobian_df_dpoint)
		{
			// 3x3:  df_{qr} / da

			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 3] = {
				1 - 2 * (qy2 + qz2),
				2 * (m_quat.x() * m_quat.y() - m_quat.r() * m_quat.z()),
				2 * (m_quat.r() * m_quat.y() + m_quat.x() * m_quat.z()),

				2 * (m_quat.r() * m_quat.z() + m_quat.x() * m_quat.y()),
				1 - 2 * (qx2 + qz2),
				2 * (m_quat.y() * m_quat.z() - m_quat.r() * m_quat.x()),

				2 * (m_quat.x() * m_quat.z() - m_quat.r() * m_quat.y()),
				2 * (m_quat.r() * m_quat.x() + m_quat.y() * m_quat.z()),
				1 - 2 * (qx2 + qy2)};
			out_jacobian_df_dpoint->loadFromArray(vals);
		}

		// Jacob: df/dpose
		if (out_jacobian_df_dpose)
		{
			// 3x7:  df_{qr} / dp
			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals1[3 * 7] = {
				1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0};
			out_jacobian_df_dpose->loadFromArray(vals1);

			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 4] = {
				2 * (-m_quat.z() * ly + m_quat.y() * lz),
				2 * (m_quat.y() * ly + m_quat.z() * lz),
				2 * (-2 * m_quat.y() * lx + m_quat.x() * ly + m_quat.r() * lz),
				2 * (-2 * m_quat.z() * lx - m_quat.r() * ly + m_quat.x() * lz),

				2 * (m_quat.z() * lx - m_quat.x() * lz),
				2 * (m_quat.y() * lx - 2 * m_quat.x() * ly - m_quat.r() * lz),
				2 * (m_quat.x() * lx + m_quat.z() * lz),
				2 * (m_quat.r() * lx - 2 * m_quat.z() * ly + m_quat.y() * lz),

				2 * (-m_quat.y() * lx + m_quat.x() * ly),
				2 * (m_quat.z() * lx + m_quat.r() * ly - 2 * m_quat.x() * lz),
				2 * (-m_quat.r() * lx + m_quat.z() * ly - 2 * m_quat.y() * lz),
				2 * (m_quat.x() * lx + m_quat.y() * ly)};

			CMatrixDouble44 norm_jacob(UNINITIALIZED_MATRIX);
			this->quat().normalizationJacobian(norm_jacob);

			out_jacobian_df_dpose->asEigen().block<3, 4>(0, 3) =
				(CMatrixFixed<double, 3, 4>(vals) * norm_jacob).eval();
		}
	}

	// function itself:
	m_quat.rotatePoint(lx, ly, lz, gx, gy, gz);
	gx += m_coords[0];
	gy += m_coords[1];
	gz += m_coords[2];
}

/**  Computes the 3D point G such as \f$ L = G \ominus this \f$.
 * \sa composeFrom
 */
void CPose3DQuat::inverseComposePoint(
	const double gx, const double gy, const double gz, double& lx, double& ly,
	double& lz, mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint,
	mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose) const
{
	if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
	{
		const double qx2 = square(m_quat.x());
		const double qy2 = square(m_quat.y());
		const double qz2 = square(m_quat.z());

		// Jacob: df/dpoint
		if (out_jacobian_df_dpoint)
		{
			// 3x3:  df_{m_quat.r()} / da
			//		inv_df_da =
			//		[ - 2*qy^2 - 2*qz^2 + 1,     2*qx*qy - 2*qr*qz,     2*qr*qy
			//+
			// 2*qx*qz]
			//		[     2*qr*qz + 2*qx*qy, - 2*qx^2 - 2*qz^2 + 1,     2*qy*qz
			//-
			// 2*qr*qx]
			//		[     2*qx*qz - 2*qr*qy,     2*qr*qx + 2*qy*qz, - 2*qx^2 -
			// 2*qy^2 + 1]
			//

			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 3] = {
				1 - 2 * (qy2 + qz2),
				2 * (m_quat.x() * m_quat.y() + m_quat.r() * m_quat.z()),
				2 * (-m_quat.r() * m_quat.y() + m_quat.x() * m_quat.z()),

				2 * (-m_quat.r() * m_quat.z() + m_quat.x() * m_quat.y()),
				1 - 2 * (qx2 + qz2),
				2 * (m_quat.y() * m_quat.z() + m_quat.r() * m_quat.x()),

				2 * (m_quat.x() * m_quat.z() + m_quat.r() * m_quat.y()),
				2 * (-m_quat.r() * m_quat.x() + m_quat.y() * m_quat.z()),
				1 - 2 * (qx2 + qy2)};
			out_jacobian_df_dpoint->loadFromArray(vals);
		}

		// Jacob: df/dpose
		if (out_jacobian_df_dpose)
		{
			// 3x7:  df_{m_quat.r()} / dp
			//		inv_df_dp =
			//[ 2*qy^2 + 2*qz^2 - 1, - 2*qr*qz - 2*qx*qy,   2*qr*qy - 2*qx*qz,
			// 2*qz*(ay - y) - 2*qy*(az - z),                 2*qy*(ay - y) +
			// 2*qz*(az - z), 2*qx*(ay - y) - 4*qy*(ax - x) - 2*qr*(az - z),
			// 2*qr*(ay - y) - 4*qz*(ax - x) + 2*qx*(az - z)]
			//[   2*qr*qz - 2*qx*qy, 2*qx^2 + 2*qz^2 - 1, - 2*qr*qx - 2*qy*qz,
			// 2*qx*(az - z) - 2*qz*(ax - x), 2*qy*(ax - x) - 4*qx*(ay - y) +
			// 2*qr*(az - z),                 2*qx*(ax - x) + 2*qz*(az - z),
			// 2*qy*(az - z) - 4*qz*(ay - y) - 2*qr*(ax - x)]
			//[ - 2*qr*qy - 2*qx*qz,   2*qr*qx - 2*qy*qz, 2*qx^2 + 2*qy^2 - 1,
			// 2*qy*(ax - x) - 2*qx*(ay - y), 2*qz*(ax - x) - 2*qr*(ay - y) -
			// 4*qx*(az - z), 2*qr*(ax - x) + 2*qz*(ay - y) - 4*qy*(az - z),
			// 2*qx*(ax - x) + 2*qy*(ay - y)]
			//
			const double qr = m_quat.r();
			const double qx = m_quat.x();
			const double qy = m_quat.y();
			const double qz = m_quat.z();

			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals1[3 * 7] = {
				2 * qy2 + 2 * qz2 - 1,
				-2 * qr * qz - 2 * qx * qy,
				2 * qr * qy - 2 * qx * qz,
				0,
				0,
				0,
				0,

				2 * qr * qz - 2 * qx * qy,
				2 * qx2 + 2 * qz2 - 1,
				-2 * qr * qx - 2 * qy * qz,
				0,
				0,
				0,
				0,

				-2 * qr * qy - 2 * qx * qz,
				2 * qr * qx - 2 * qy * qz,
				2 * qx2 + 2 * qy2 - 1,
				0,
				0,
				0,
				0,
			};

			out_jacobian_df_dpose->loadFromArray(vals1);

			const double Ax = 2 * (gx - m_coords[0]);
			const double Ay = 2 * (gy - m_coords[1]);
			const double Az = 2 * (gz - m_coords[2]);

			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 4] = {
				-qy * Az + qz * Ay,
				qy * Ay + qz * Az,
				qx * Ay - 2 * qy * Ax - qr * Az,
				qx * Az + qr * Ay - 2 * qz * Ax,

				qx * Az - qz * Ax,
				qy * Ax - 2 * qx * Ay + qr * Az,
				qx * Ax + qz * Az,
				qy * Az - 2 * qz * Ay - qr * Ax,

				qy * Ax - qx * Ay,
				qz * Ax - qr * Ay - 2 * qx * Az,
				qr * Ax + qz * Ay - 2 * qy * Az,
				qx * Ax + qy * Ay};

			CMatrixDouble44 norm_jacob(UNINITIALIZED_MATRIX);
			this->quat().normalizationJacobian(norm_jacob);

			out_jacobian_df_dpose->insertMatrix(
				0, 3, (CMatrixFixed<double, 3, 4>(vals) * norm_jacob).eval());
		}
	}

	// function itself:
	m_quat.inverseRotatePoint(
		gx - m_coords[0], gy - m_coords[1], gz - m_coords[2], lx, ly, lz);
}

/*---------------------------------------------------------------
	*=
  ---------------------------------------------------------------*/
void CPose3DQuat::operator*=(const double s)
{
	m_coords[0] *= s;
	m_coords[1] *= s;
	m_coords[2] *= s;
	m_quat[0] *= s;
	m_quat[1] *= s;
	m_quat[2] *= s;
	m_quat[3] *= s;
}

uint8_t CPose3DQuat::serializeGetVersion() const { return 0; }
void CPose3DQuat::serializeTo(mrpt::serialization::CArchive& out) const
{
	out << m_coords[0] << m_coords[1] << m_coords[2] << m_quat[0] << m_quat[1]
		<< m_quat[2] << m_quat[3];
}
void CPose3DQuat::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		{
			in >> m_coords[0] >> m_coords[1] >> m_coords[2] >> m_quat[0] >>
				m_quat[1] >> m_quat[2] >> m_quat[3];
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}
/** Serialize CSerializable Object to CSchemeArchiveBase derived object*/
void CPose3DQuat::serializeTo(
	mrpt::serialization::CSchemeArchiveBase& out) const
{
	SCHEMA_SERIALIZE_DATATYPE_VERSION(1);
	CPoint3D point(m_coords[0], m_coords[1], m_coords[2]);
	out["point"] = point;
	out["orientation"]["r"] = m_quat[0];
	out["orientation"]["x"] = m_quat[1];
	out["orientation"]["y"] = m_quat[2];
	out["orientation"]["z"] = m_quat[3];
}
/** Serialize CSchemeArchiveBase derived object to CSerializable Object*/
void CPose3DQuat::serializeFrom(mrpt::serialization::CSchemeArchiveBase& in)
{
	uint8_t version;
	SCHEMA_DESERIALIZE_DATATYPE_VERSION();
	switch (version)
	{
		case 1:
		{
			CPoint3D point;
			in["point"].readTo(point);
			m_coords[0] = point.x();
			m_coords[1] = point.y();
			m_coords[2] = point.z();
			m_quat[0] = static_cast<double>(in["orientation"]["r"]);
			m_quat[1] = static_cast<double>(in["orientation"]["x"]);
			m_quat[2] = static_cast<double>(in["orientation"]["y"]);
			m_quat[3] = static_cast<double>(in["orientation"]["z"]);
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	}
}
/*---------------------------------------------------------------
		sphericalCoordinates
---------------------------------------------------------------*/
void CPose3DQuat::sphericalCoordinates(
	const TPoint3D& point, double& out_range, double& out_yaw,
	double& out_pitch,
	mrpt::math::CMatrixFixed<double, 3, 3>* out_jacob_dryp_dpoint,
	mrpt::math::CMatrixFixed<double, 3, 7>* out_jacob_dryp_dpose) const
{
	const bool comp_jacobs =
		out_jacob_dryp_dpoint != nullptr || out_jacob_dryp_dpose != nullptr;

	// Pass to coordinates as seen from this 6D pose:
	CMatrixFixed<double, 3, 3> jacob_dinv_dpoint,
		*ptr_ja1 = comp_jacobs ? &jacob_dinv_dpoint : nullptr;
	CMatrixFixed<double, 3, 7> jacob_dinv_dpose,
		*ptr_ja2 = comp_jacobs ? &jacob_dinv_dpose : nullptr;

	TPoint3D local;
	this->inverseComposePoint(
		point.x, point.y, point.z, local.x, local.y, local.z, ptr_ja1, ptr_ja2);

	// Range:
	out_range = local.norm();

	// Yaw:
	if (local.y != 0 || local.x != 0) out_yaw = atan2(local.y, local.x);
	else
		out_yaw = 0;

	// Pitch:
	if (out_range != 0) out_pitch = -asin(local.z / out_range);
	else
		out_pitch = 0;

	// Jacobians are:
	//  dryp_dpoint = dryp_dlocalpoint  * dinv_dpoint
	//  dryp_dpose  = dryp_dlocalpoint  * dinv_dpose
	if (comp_jacobs)
	{
		if (out_range == 0)
			THROW_EXCEPTION("Jacobians are undefined for range=0");

		/* MATLAB:
			syms H h_range h_yaw h_pitch real;
			syms xi_ yi_ zi_ real;
			h_range = sqrt(xi_^2+yi_^2+zi_^2);
			h_yaw   = atan(yi_/xi_);
			% h_pitch = -asin(zi_/ sqrt( xi_^2 + yi_^2 + zi_^2 ) );
			h_pitch = -atan(zi_, sqrt( xi_^2 + yi_^2) );
			H=[ h_range ; h_yaw ; h_pitch ];
			jacob_fesf_xyz=jacobian(H,[xi_ yi_ zi_])
		*/
		const double _r = 1.0 / out_range;
		const double x2 = square(local.x);
		const double y2 = square(local.y);

		const double t2 = std::sqrt(x2 + y2);
		const double _K = 1.0 / (t2 * square(out_range));

		double vals[3 * 3] = {
			local.x * _r,
			local.y * _r,
			local.z * _r,
			-local.y / (x2 * (y2 / x2 + 1)),
			1.0 / (local.x * (y2 / x2 + 1)),
			0,
			(local.x * local.z) * _K,
			(local.y * local.z) * _K,
			-t2 / square(out_range)};

		const CMatrixDouble33 dryp_dlocalpoint(vals);
		if (out_jacob_dryp_dpoint)
			*out_jacob_dryp_dpoint = dryp_dlocalpoint * jacob_dinv_dpoint;
		if (out_jacob_dryp_dpose)
			*out_jacob_dryp_dpose = dryp_dlocalpoint * jacob_dinv_dpose;
	}
}

/**  Textual output stream function.
 */
std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose3DQuat& p)
{
	const std::streamsize old_pre = o.precision();
	const ios_base::fmtflags old_flags = o.flags();
	o << "(x,y,z,qr,qx,qy,qz)=(" << std::fixed << std::setprecision(4)
	  << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
	  << p.quat()[0] << "," << p.quat()[1] << "," << p.quat()[2] << ","
	  << p.quat()[3] << ")";
	o.flags(old_flags);
	o.precision(old_pre);
	return o;
}

/** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
 * than a pose with all its arguments multiplied by "-1") */
CPose3DQuat mrpt::poses::operator-(const CPose3DQuat& p)
{
	CPose3DQuat ret = p;
	ret.inverse();
	return ret;
}

/** Convert this pose into its inverse, saving the result in itself. \sa
 * operator- */
void CPose3DQuat::inverse()
{
	// Invert translation:
	this->inverseComposePoint(0, 0, 0, m_coords[0], m_coords[1], m_coords[2]);

	// Invert rotation: [qr qx qy qz] ==> [qr -qx -qy -qz]
	m_quat[1] = -m_quat[1];
	m_quat[2] = -m_quat[2];
	m_quat[3] = -m_quat[3];
}

void CPose3DQuat::setToNaN()
{
	for (int i = 0; i < 3; i++)
		m_coords[i] = std::numeric_limits<double>::quiet_NaN();

	for (int i = 0; i < 4; i++)
		quat()[i] = std::numeric_limits<double>::quiet_NaN();
}

bool mrpt::poses::operator==(const CPose3DQuat& p1, const CPose3DQuat& p2)
{
	return p1.quat() == p2.quat() && p1.x() == p2.x() && p1.y() == p2.y() &&
		p1.z() == p2.z();
}

bool mrpt::poses::operator!=(const CPose3DQuat& p1, const CPose3DQuat& p2)
{
	return !(p1 == p2);
}

CPoint3D mrpt::poses::operator-(const CPoint3D& G, const CPose3DQuat& p)
{
	CPoint3D L;
	p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
	return L;
}

TPoint3D mrpt::poses::operator-(const TPoint3D& G, const CPose3DQuat& p)
{
	mrpt::math::TPoint3D L;
	p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
	return L;
}

TPose3DQuat CPose3DQuat::asTPose() const
{
	return TPose3DQuat(
		x(), y(), z(), m_quat.r(), m_quat.x(), m_quat.y(), m_quat.z());
}

void CPose3DQuat::fromString(const std::string& s)
{
	mrpt::math::CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION_FMT(
			"Malformed expression in ::fromString, s=\"%s\"", s.c_str());
	ASSERTMSG_(m.rows() == 1 && m.cols() == 7, "Expected vector length=7");
	m_coords[0] = m(0, 0);
	m_coords[1] = m(0, 1);
	m_coords[2] = m(0, 2);
	m_quat[0] = m(0, 3);
	m_quat[1] = m(0, 4);
	m_quat[2] = m(0, 5);
	m_quat[3] = m(0, 6);
}

void CPose3DQuat::fromStringRaw(const std::string& s)
{
	this->fromString("[" + s + "]");
}