// This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.

/* discriminative_trainer.cc
   Jeremy Barnes, 9 November 2009
   Copyright (c) 2009 Jeremy Barnes.  All rights reserved.

   Discriminative training for neural network architectures via backprop.
*/

#include "discriminative_trainer.h"
#include "mldb/base/parallel.h"
#include "mldb/base/thread_pool.h"
#include "mldb/base/scope.h"
#include <boost/progress.hpp>

#include "mldb/arch/threads.h"
#include "mldb/arch/timers.h"
#include "mldb/plugins/jml/stats/auc.h"

using namespace std;

namespace ML {

/*****************************************************************************/
/* DISCRIMINATIVE_TRAINER                                                    */
/*****************************************************************************/

pair<double, double>
Discriminative_Trainer::
train_example(const distribution<float> & data,
              Label label,
              Parameters_Copy<double> & updates,
              const Output_Encoder & encoder,
              float weight) const
{
    distribution<float> label_dist = encoder.target(label);
    return train_example(&data[0], &label_dist[0], updates, weight);
}

pair<double, double>
Discriminative_Trainer::
train_example(const float * data,
              Label label,
              Parameters_Copy<double> & updates,
              const Output_Encoder & encoder,
              float weight) const
{
    distribution<float> label_dist = encoder.target(label);
    return train_example(data, &label_dist[0], updates, weight);
}

pair<double, double>
Discriminative_Trainer::
train_example(const distribution<float> & data,
              const distribution<float> & label,
              Parameters_Copy<double> & updates,
              float weight) const
{
    if (data.size() != layer->inputs())
        throw Exception("data had wrong length");
    if (label.size() != layer->outputs())
        throw Exception("layer had wrong length");

    return train_example(&data[0], &label[0], updates, weight);
}

pair<double, double>
Discriminative_Trainer::
train_example(const float * data,
              const float * label,
              Parameters_Copy<double> & updates,
              float weight) const
{
    /* fprop */

    size_t temp_space_required
        = layer->fprop_temporary_space_required();

    float temp_space[temp_space_required];

    int no = layer->outputs();

    distribution<float> outputs(no);
    layer->fprop(&data[0], temp_space, temp_space_required, &outputs[0]);

    /* error */

    distribution<float> labelv(label, label + no);

    distribution<float> errors = (labelv - outputs);
    double error = errors.dotprod(errors);

    if (weight != 0.0) {
        // TODO: get the loss function to do this...
        distribution<float> derrors = -2.0 * errors;

        /* bprop */
        layer->bprop(&data[0],
                     &outputs[0],
                     temp_space, temp_space_required,
                     &derrors[0],
                     0 /* don't calculate input errors */,
                     updates,
                     weight);
    }

    if (false) {
        distribution<float> in(data, data + layer->inputs());
        cerr << "in " << in << endl;
        cerr << "outputs " << outputs << endl;
        cerr << "labelv " << labelv << endl;
        cerr << "errors " << errors << endl;

        Parameters_Copy<float> params = layer->parameters();

        cerr << "params " << params.values << endl;
        cerr << "updates " << updates.values << endl;
    }

    return make_pair(sqrt(error), outputs[0]);
}

namespace {

struct Train_Examples_Job {

    const Discriminative_Trainer & trainer;
    const std::vector<const float *> & data;
    const std::vector<Label> & labels;
    const std::vector<float> & weights;
    const Output_Encoder & output_encoder;
    Thread_Context & thread_context;
    const vector<int> & examples;
    int first;
    int last;
    Parameters_Copy<double> & updates;
    vector<float> & outputs;
    double & total_rmse;
    Lock & updates_lock;
    boost::progress_display * progress;
    int verbosity;

    Train_Examples_Job(const Discriminative_Trainer & trainer,
                       const std::vector<const float *> & data,
                       const std::vector<Label> & labels,
                       const std::vector<float> & weights,
                       const Output_Encoder & output_encoder,
                       Thread_Context & thread_context,
                       const vector<int> & examples,
                       int first, int last,
                       Parameters_Copy<double> & updates,
                       vector<float> & outputs,
                       double & total_rmse,
                       Lock & updates_lock,
                       boost::progress_display * progress,
                       int verbosity)
        : trainer(trainer), data(data), labels(labels), weights(weights),
          output_encoder(output_encoder),
          thread_context(thread_context), examples(examples),
          first(first), last(last), updates(updates), outputs(outputs),
          total_rmse(total_rmse), updates_lock(updates_lock),
          progress(progress), verbosity(verbosity)
    {
    }

    void operator () ()
    {
        Parameters_Copy<double> local_updates(*trainer.layer);
        local_updates.fill(0.0);

        double total_rmse_local = 0.0;

        //cerr << "training from " << first << " to " << last << endl;

        for (unsigned ix = first; ix < last;  ++ix) {
            int x = examples[ix];

            double rmse_contribution;
            double output;

            float weight = 1.0;
            if (weights.size())
                weight = weights.at(x);

            //cerr << "x " << x << " weight " << weight << endl;

            std::tie(rmse_contribution, output)
                = trainer.train_example(data[x],
                                        labels[x],
                                        local_updates,
                                        output_encoder,
                                        weight);

            outputs[ix] = output;
            total_rmse_local += rmse_contribution;
        }

        Guard guard(updates_lock);
        total_rmse += total_rmse_local;
        updates.values += local_updates.values;
        if (progress) progress += (last - first);
    }
};

} // file scope

std::pair<double, double>
Discriminative_Trainer::
train_iter(const std::vector<distribution<float> > & data,
           const std::vector<Label> & labels,
           const std::vector<float> & weights,
           const Output_Encoder & output_encoder,
           Thread_Context & thread_context,
           int minibatch_size,
           float learning_rate,
           int verbosity,
           float sample_proportion,
           bool randomize_order) const
{
    vector<const float *> data2(data.size());
    for (unsigned i = 0;  i < data.size();  ++i)
        data2[i] = &data[i][0];

    return train_iter(data2, labels, weights,
                      output_encoder, thread_context, minibatch_size,
                      learning_rate, verbosity, sample_proportion,
                      randomize_order);
}

std::pair<double, double>
Discriminative_Trainer::
train_iter(const std::vector<const float *> & data,
           const std::vector<Label> & labels,
           const std::vector<float> & weights,
           const Output_Encoder & output_encoder,
           Thread_Context & thread_context,
           int minibatch_size, float learning_rate,
           int verbosity,
           float sample_proportion,
           bool randomize_order) const
{
    int nx = data.size();

    int microbatch_size = std::max(minibatch_size / (MLDB::numCpus() * 4), 1);

    Lock update_lock;

    vector<int> examples;
    for (unsigned x = 0;  x < nx;  ++x) {
        // Randomly exclude some samples
        if (thread_context.random01() >= sample_proportion)
            continue;
        examples.push_back(x);
    }

    if (randomize_order) {
        Thread_Context::RNG_Type rng = thread_context.rng();
        std::random_shuffle(examples.begin(), examples.end(), rng);
    }

    int nx2 = examples.size();

    double total_mse = 0.0;
    distribution<float> outputs;
    outputs.resize(nx2);

    std::auto_ptr<boost::progress_display> progress;
    if (verbosity >= 3) progress.reset(new boost::progress_display(nx2, cerr));

    for (unsigned x = 0;  x < nx2;  x += minibatch_size) {

        Parameters_Copy<double> updates(*layer);
        updates.fill(0.0);

        auto onBatch = [&] (size_t x0, size_t x1)
            {
                Train_Examples_Job(*this,
                                   data,
                                   labels,
                                   weights,
                                   output_encoder,
                                   thread_context,
                                   examples,
                                   x0, x1,
                                   updates,
                                   outputs,
                                   total_mse,
                                   update_lock,
                                   progress.get(),
                                   verbosity)();
            };

        MLDB::parallelMapChunked
            (x, std::min<size_t>(nx2, x + minibatch_size),
             microbatch_size, onBatch);

        //cerr << "finished minibatch: updates = " << updates.values
        //     << " learning_rate = " << learning_rate << endl;

        //cerr << "applying minibatch updates" << endl;

        //cerr << "updates.values = " << updates.values << endl;
        //cerr << "learning_rate = " << learning_rate << endl;

        layer->parameters().update(updates, -learning_rate);

        //cerr << "final value = "
        //     << Parameters_Copy<double>(layer->parameters()).values
        //     << endl;
    }

    // TODO: calculate AUC score
    distribution<float> test_labels;
    for (unsigned i = 0;  i < nx2;  ++i)
        test_labels.push_back(labels[examples[i]]);

    float neg = 0.0, pos = 1.0;

    double auc = calc_auc(outputs, test_labels, neg, pos);

    return make_pair(sqrt(total_mse / nx2), auc);
}

std::pair<double, double>
Discriminative_Trainer::
train(const std::vector<distribution<float> > & training_data,
      const std::vector<Label> & training_labels,
      const std::vector<float> & training_weights,
      const std::vector<distribution<float> > & testing_data,
      const std::vector<Label> & testing_labels,
      const std::vector<float> & testing_weights,
      const Configuration & config,
      MLDB::Thread_Context & thread_context) const
{
    double learning_rate = 0.75;
    int minibatch_size = 512;
    int niter = 50;
    int verbosity = 2;

    bool randomize_order = true;
    float sample_proportion = 0.8;
    int test_every = 1;

    Output_Encoder output_encoder;
    output_encoder.configure(config, *layer);

    config.get(learning_rate, "learning_rate");
    config.get(minibatch_size, "minibatch_size");
    config.get(niter, "niter");
    config.get(verbosity, "verbosity");
    config.get(randomize_order, "randomize_order");
    config.get(sample_proportion, "sample_proportion");
    config.get(test_every, "test_every");

    int nx = training_data.size();

    if (training_data.size() != training_labels.size())
        throw Exception("label and example sizes don't match");

    int nxt = testing_data.size();

    if (nx == 0)
        throw Exception("can't train on no data");

    // Learning rate is per-example
    learning_rate /= nx;

    // Compensate for the example proportion
    learning_rate /= sample_proportion;

    if (verbosity == 2)
        cerr << "iter  ---- train ----  ---- test -----\n"
             << "         rmse     auc     rmse     auc\n";

    for (unsigned iter = 0;  iter < niter;  ++iter) {
        if (verbosity >= 3)
            cerr << "iter " << iter << " training on " << nx << " examples"
                 << endl;
        else if (verbosity >= 2)
            cerr << format("%4d", iter) << flush;
        Timer timer;

        double train_error_rmse, train_error_auc;
        std::tie(train_error_rmse, train_error_auc)
            = train_iter(training_data, training_labels, training_weights,
                         output_encoder, thread_context,
                         minibatch_size, learning_rate,
                         verbosity, sample_proportion,
                         randomize_order);

        if (verbosity >= 3) {
            cerr << "error of iteration: rmse " << train_error_rmse
                 << " noisy " << train_error_auc << endl;
            if (verbosity >= 3) cerr << timer.elapsed() << endl;
        }
        else if (verbosity == 2)
            cerr << format("  %7.5f %7.5f",
                           train_error_rmse, train_error_auc)
                 << flush;

        if (iter % test_every == (test_every - 1)
            || iter == niter - 1) {
            timer.restart();
            double test_error_rmse = 0.0, test_error_auc = 0.0;

            if (verbosity >= 3)
                cerr << "testing on " << nxt << " examples"
                     << endl;

            std::tie(test_error_rmse, test_error_auc)
                = test(testing_data, testing_labels, testing_weights,
                       output_encoder, thread_context, verbosity);

            if (verbosity >= 3) {
                cerr << "testing error of iteration: rmse "
                     << test_error_rmse << " auc " << test_error_auc
                     << endl;
                cerr << timer.elapsed() << endl;
            }
            else if (verbosity == 2)
                cerr << format("  %7.5f %7.5f",
                               test_error_rmse, test_error_auc);
        }

        if (verbosity == 2) cerr << endl;
    }

    // TODO: return something
    return make_pair(0.0, 0.0);
}

namespace {

struct Test_Examples_Job {

    const Discriminative_Trainer & trainer;
    const vector<const float *> & data;
    const vector<Label> & labels;
    const vector<float> & weights;
    const Output_Encoder & output_encoder;
    int first;
    int last;
    const Thread_Context & context;
    Lock & update_lock;
    double & error_rmse;
    vector<float> & outputs;
    boost::progress_display * progress;
    int verbosity;

    Test_Examples_Job(const Discriminative_Trainer & trainer,
                      const vector<const float *> & data,
                      const vector<Label> & labels,
                      const vector<float> & weights,
                      const Output_Encoder & output_encoder,
                      int first, int last,
                      const Thread_Context & context,
                      Lock & update_lock,
                      double & error_rmse,
                      vector<float> & outputs,
                      boost::progress_display * progress,
                      int verbosity)
        : trainer(trainer), data(data), labels(labels), weights(weights),
          output_encoder(output_encoder),
          first(first), last(last),
          context(context),
          update_lock(update_lock),
          error_rmse(error_rmse), outputs(outputs),
          progress(progress), verbosity(verbosity)
    {
    }

    void operator () ()
    {
        double local_error_rmse = 0.0;

        for (unsigned x = first;  x < last;  ++x) {
            distribution<float> output(trainer.layer->outputs());
            trainer.layer->apply(data[x], &output[0]);

            outputs[x] = output[0];
            local_error_rmse += pow(labels[x] - output[0], 2);
        }

        Guard guard(update_lock);
        error_rmse += local_error_rmse;
        if (progress && verbosity >= 3) (*progress) += (last - first);
    }
};

} // file scope

pair<double, double>
Discriminative_Trainer::
test(const std::vector<distribution<float> > & data,
     const std::vector<Label> & labels,
     const std::vector<float> & weights,
     const Output_Encoder & output_encoder,
     MLDB::Thread_Context & thread_context,
     int verbosity) const
{
    vector<const float *> data2(data.size());
    for (unsigned i = 0;  i < data.size();  ++i)
        data2[i] = &data[i][0];
    return test(data2, labels, weights, output_encoder,
                thread_context, verbosity);
}

pair<double, double>
Discriminative_Trainer::
test(const std::vector<const float *> & data,
     const std::vector<Label> & labels,
     const std::vector<float> & weights,
     const Output_Encoder & output_encoder,
     MLDB::Thread_Context & thread_context,
     int verbosity) const
{
    Lock update_lock;
    double mse_total = 0.0;

    int nx = data.size();

    std::auto_ptr<boost::progress_display> progress;
    if (verbosity >= 3) progress.reset(new boost::progress_display(nx, cerr));

    distribution<float> outputs;
    outputs.resize(nx);

    // 20 jobs per CPU
    int batch_size = nx / (MLDB::numCpus() * 20);

    auto onBatch = [&] (size_t x0, size_t x1)
        {
            Test_Examples_Job(*this, data, labels, weights,
                              output_encoder,
                              x0, x1,
                              thread_context,
                              update_lock,
                              mse_total, outputs,
                              progress.get(),
                              verbosity)();
        };

    MLDB::parallelMapChunked(0, nx, batch_size, onBatch);

    return make_pair(sqrt(mse_total / nx),
                     output_encoder.calc_auc(outputs, labels));
}

} // namespace ML


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSSE3_ABS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSSE3_ABS_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSSE3_SUPPORT
#include <boost/simd/arithmetic/functions/abs.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/functions/simd/bitwise_notand.hpp>
#include <boost/simd/include/functions/simd/bitwise_xor.hpp>
#include <boost/simd/include/functions/simd/shr.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( abs_, boost::simd::tag::ssse3_
                                    , (A0)
                                    , ((simd_<int32_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return _mm_abs_epi32(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( abs_, boost::simd::tag::ssse3_
                                    , (A0)
                                    , ((simd_<int8_<A0>,boost::simd::tag::sse_>))
                                    )
  {

    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return _mm_abs_epi8(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( abs_, boost::simd::tag::ssse3_
                                    , (A0)
                                    , ((simd_<int16_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
     return _mm_abs_epi16(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( abs_, boost::simd::tag::ssse3_
                                    , (A0)
                                    , ((simd_<floating_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return b_notand(boost::simd::Mzero<A0>(),a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( abs_, boost::simd::tag::ssse3_
                                    , (A0)
                                    , ((simd_<int64_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_integer<A0, signed>::type int_type;
      typedef typename meta::scalar_of<int_type>::type   sint_type;
      A0 const s = shri(a0, 8*sizeof(sint_type)-1);
      return (a0-s)^(-s);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( abs_, boost::simd::tag::ssse3_
                                    , (A0)
                                    , ((simd_<unsigned_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return a0;
    }
  };
} } }

#endif
#endif


/*
 * OBJParser.cpp
 *
 *  Created on: 22.04.2016
 *      Author: flutz
 */

#include "OBJParser.h"
#include <iostream>
#include <fstream>
#include <string>
#include <cassert>
#include <array>
#include <vector>
#include <algorithm>

#include <armadillo>

namespace kinematicTree {
namespace visual {
namespace obj {

namespace {

arma::colvec3 parseVector(std::stringstream& stream) {
	arma::colvec3 vector;
	int i(0);
	while ((not stream.eof()) && (i < 3)) {
		double val;
		stream >> val;
		vector(i) = val;
		++i;
	}
	return vector;
}

std::vector<int> parseFacet(std::stringstream& stream) {
	std::vector<int> retVector;
	std::string line = stream.str().substr(stream.tellg());
	std::replace(line.begin(), line.end(), '/', ' ');
	std::stringstream lineStream(line);
	int idx;
	while (not lineStream.eof()) {
		lineStream >> idx;
		retVector.push_back(idx - 1);
	}
	return retVector;
}

std::string readWord(std::stringstream& stream) {
	std::string word;
	stream >> word;
	return word;
}

}

std::string OBJParser::getMaterialFileName(std::string file)
{
	std::ifstream stlFile(file);
	kinematicTree::visual::mesh::Mesh mesh;
	std::vector<std::string> lines;
	std::string line;
	std::string token;
	std::string mtlFileName = "";
	while (std::getline(stlFile, line, '\n')) {
		std::stringstream lineStream(line);
		token = readWord(lineStream);
		if (token == "mtllib") {
			mtlFileName = readWord(lineStream);
			return mtlFileName;
		}
	}
	return mtlFileName;
}

kinematicTree::visual::mesh::Mesh OBJParser::parse(std::string file)
{
	std::ifstream stlFile(file);
	kinematicTree::visual::mesh::Mesh mesh;
	std::vector<std::string> lines;
	std::string line;
	std::vector<arma::colvec3> vertices;
	std::vector<std::vector<int>> vertexIndexes;
	std::string token;
	while (std::getline(stlFile, line, '\n')) {
		std::stringstream lineStream(line);
		token = readWord(lineStream);
		if (token == "v") {
			vertices.push_back(parseVector(lineStream));
		} else if (token == "f") {
			vertexIndexes.emplace_back(parseFacet(lineStream));
		}
	}

	for (auto const& v : vertexIndexes) {
		int idx = 1;
		int len = v.size();
		while (idx < len - 1) {
			kinematicTree::visual::mesh::Facet facet;
			facet.mVertices.push_back(vertices[v[0]]);
			for (int i = 0; i < 2; ++i) {
				int vertexIdx = v[idx + i ];
				facet.mVertices.push_back(vertices[vertexIdx]);
			}
			mesh.addFacet(facet);
			idx += 1;
		}
	}

	return mesh;
}

} /* namespace obj */
} /* namespace visual */
} /* namespace kinematicTree */


#pragma once

#include <armadillo>
#include "../common/utilities_cuda.cuh"

namespace dDNKG {

	void eigensystem(const arma::vec &mp2, arma::mat &eigenvectors, arma::vec &omegas);

	extern __constant__ double dt_c[8], dt_d[8], mp2[2048], beta;

	completion move(plane2split &splitter, bool &use_split_kernel, const double *mp2_gmem, cudaStream_t stream);

	completion make_linenergies(const double *projection_phi, const double *projection_pi, cudaStream_t stream);

}


//  Copyright (c) 2007-2020 Hartmut Kaiser
//
//  SPDX-License-Identifier: BSL-1.0
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <hpx/config.hpp>
#include <hpx/async_base/launch_policy.hpp>
#include <hpx/modules/string_util.hpp>
#include <hpx/performance_counters/counter_creators.hpp>
#include <hpx/performance_counters/counters.hpp>
#include <hpx/performance_counters/performance_counter.hpp>
#include <hpx/performance_counters/server/arithmetics_counter_extended.hpp>
#include <hpx/runtime/components/derived_component_factory.hpp>
#include <hpx/runtime/runtime_fwd.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>

#include <cstddef>
#include <cstdint>
#include <mutex>
#include <string>
#include <utility>
#include <vector>

///////////////////////////////////////////////////////////////////////////////
namespace hpx { namespace performance_counters { namespace server {
    ///////////////////////////////////////////////////////////////////////////
    template <typename Statistic>
    arithmetics_counter_extended<Statistic>::arithmetics_counter_extended(
        counter_info const& info,
        std::vector<std::string> const& base_counter_names)
      : base_type_holder(info)
      , counters_(base_counter_names)
    {
        if (info.type_ != counter_aggregating)
        {
            HPX_THROW_EXCEPTION(bad_parameter,
                "arithmetics_counter_extended<Statistic>::"
                "arithmetics_counter_extended",
                "unexpected counter type specified");
        }
    }

    namespace detail {
        template <typename Statistic>
        struct statistic_get_value;

        template <>
        struct statistic_get_value<boost::accumulators::tag::mean>
        {
            template <typename Accumulator>
            static double call(Accumulator const& accum)
            {
                return boost::accumulators::mean(accum);
            }
        };

        template <>
        struct statistic_get_value<boost::accumulators::tag::variance>
        {
            template <typename Accumulator>
            static double call(Accumulator const& accum)
            {
                return boost::accumulators::variance(accum);
            }
        };

        template <>
        struct statistic_get_value<boost::accumulators::tag::median>
        {
            template <typename Accumulator>
            static double call(Accumulator const& accum)
            {
                return boost::accumulators::median(accum);
            }
        };

        template <>
        struct statistic_get_value<boost::accumulators::tag::min>
        {
            template <typename Accumulator>
            static double call(Accumulator const& accum)
            {
                return (boost::accumulators::min)(accum);
            }
        };

        template <>
        struct statistic_get_value<boost::accumulators::tag::max>
        {
            template <typename Accumulator>
            static double call(Accumulator const& accum)
            {
                return (boost::accumulators::max)(accum);
            }
        };

        template <>
        struct statistic_get_value<boost::accumulators::tag::count>
        {
            template <typename Accumulator>
            static double call(Accumulator const& accum)
            {
                return static_cast<double>(boost::accumulators::count(accum));
            }
        };
    }    // namespace detail

    template <typename Statistic>
    hpx::performance_counters::counter_value
    arithmetics_counter_extended<Statistic>::get_counter_value(bool /* reset */)
    {
        std::vector<counter_value> base_values =
            counters_.get_counter_values(hpx::launch::sync);

        // apply arithmetic Statistic
        typedef boost::accumulators::accumulator_set<double,
            boost::accumulators::stats<Statistic>>
            accumulator_type;

        accumulator_type accum;
        for (counter_value const& base_value : base_values)
        {
            accum(base_value.get_value<double>());
        }
        double value = detail::statistic_get_value<Statistic>::call(accum);

        if (base_values[0].scale_inverse_ &&
            base_values[0].scaling_ != 1.0)    //-V550
        {
            base_values[0].value_ =
                static_cast<std::int64_t>(value * base_values[0].scaling_);
        }
        else
        {
            base_values[0].value_ =
                static_cast<std::int64_t>(value / base_values[0].scaling_);
        }

        base_values[0].time_ =
            static_cast<std::int64_t>(hpx::get_system_uptime());
        base_values[0].count_ = counters_.get_invocation_count();

        return base_values[0];
    }

    template <typename Statistic>
    bool arithmetics_counter_extended<Statistic>::start()
    {
        return counters_.start(hpx::launch::sync);
    }

    template <typename Statistic>
    bool arithmetics_counter_extended<Statistic>::stop()
    {
        return counters_.stop(hpx::launch::sync);
    }

    template <typename Statistic>
    void arithmetics_counter_extended<Statistic>::reset_counter_value()
    {
        counters_.reset(hpx::launch::sync);
    }
}}}    // namespace hpx::performance_counters::server

///////////////////////////////////////////////////////////////////////////////
template class HPX_EXPORT hpx::performance_counters::server::
    arithmetics_counter_extended<boost::accumulators::tag::mean>;
template class HPX_EXPORT hpx::performance_counters::server::
    arithmetics_counter_extended<boost::accumulators::tag::variance>;
template class HPX_EXPORT hpx::performance_counters::server::
    arithmetics_counter_extended<boost::accumulators::tag::median>;
template class HPX_EXPORT hpx::performance_counters::server::
    arithmetics_counter_extended<boost::accumulators::tag::min>;
template class HPX_EXPORT hpx::performance_counters::server::
    arithmetics_counter_extended<boost::accumulators::tag::max>;
template class HPX_EXPORT hpx::performance_counters::server::
    arithmetics_counter_extended<boost::accumulators::tag::count>;

///////////////////////////////////////////////////////////////////////////////
// /arithmetic/mean
typedef hpx::components::component<hpx::performance_counters::server::
        arithmetics_counter_extended<boost::accumulators::tag::mean>>
    mean_arithmetics_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(mean_arithmetics_counter_type,
    mean_arithmetics_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(mean_arithmetics_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
// /arithmetic/variance
typedef hpx::components::component<hpx::performance_counters::server::
        arithmetics_counter_extended<boost::accumulators::tag::variance>>
    variance_arithmetics_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(variance_arithmetics_counter_type,
    variance_arithmetics_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(variance_arithmetics_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
// /arithmetic/median
typedef hpx::components::component<hpx::performance_counters::server::
        arithmetics_counter_extended<boost::accumulators::tag::median>>
    median_arithmetics_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(median_arithmetics_counter_type,
    median_arithmetics_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(median_arithmetics_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
// /arithmetic/min
typedef hpx::components::component<hpx::performance_counters::server::
        arithmetics_counter_extended<boost::accumulators::tag::min>>
    min_arithmetics_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(min_arithmetics_counter_type,
    min_arithmetics_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(min_arithmetics_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
// /arithmetic/max
typedef hpx::components::component<hpx::performance_counters::server::
        arithmetics_counter_extended<boost::accumulators::tag::max>>
    max_arithmetics_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(max_arithmetics_counter_type,
    max_arithmetics_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(max_arithmetics_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
// /arithmetic/count
typedef hpx::components::component<hpx::performance_counters::server::
        arithmetics_counter_extended<boost::accumulators::tag::count>>
    count_arithmetics_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(count_arithmetics_counter_type,
    count_arithmetics_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(count_arithmetics_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
namespace hpx { namespace performance_counters { namespace detail {
    /// Creation function for aggregating performance counters to be registered
    /// with the counter types.
    naming::gid_type arithmetics_counter_extended_creator(
        counter_info const& info, error_code& ec)
    {
        switch (info.type_)
        {
        case counter_aggregating:
        {
            counter_path_elements paths;
            get_counter_path_elements(info.fullname_, paths, ec);
            if (ec)
                return naming::invalid_gid;

            if (!paths.parameters_.empty())
            {
                // try to interpret the additional parameter as a list of
                // two performance counter names
                std::vector<std::string> names;
                hpx::string_util::split(
                    names, paths.parameters_, hpx::string_util::is_any_of(","));

                if (names.empty())
                {
                    HPX_THROWS_IF(ec, bad_parameter,
                        "arithmetics_counter_extended_creator",
                        "the parameter specification for an arithmetic counter "
                        "has to expand to at least one counter name: " +
                            paths.parameters_);
                    return naming::invalid_gid;
                }

                for (std::string const& name : names)
                {
                    counter_path_elements paths;
                    if (status_valid_data !=
                            get_counter_path_elements(name, paths, ec) ||
                        ec)
                    {
                        HPX_THROWS_IF(ec, bad_parameter,
                            "arithmetics_counter_extended_creator",
                            "the given (expanded) counter name is not "
                            "a validly formed performance counter name: " +
                                name);
                        return naming::invalid_gid;
                    }
                }

                return create_arithmetics_counter_extended(info, names, ec);
            }
            else
            {
                HPX_THROWS_IF(ec, bad_parameter,
                    "arithmetics_counter_extended_creator",
                    "the parameter specification for an arithmetic counter "
                    "has to be a comma separated list of performance "
                    "counter names, none is given: " +
                        remove_counter_prefix(info.fullname_));
            }
        }
        break;

        default:
            HPX_THROWS_IF(ec, bad_parameter,
                "arithmetics_counter_extended_creator",
                "invalid counter type requested");
            break;
        }
        return naming::invalid_gid;
    }
}}}    // namespace hpx::performance_counters::detail


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Garratt Gallagher
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name Garratt Gallagher nor the names of other
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER 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/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition.hpp>
#include <Eigen/StdVector>

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud.h>
#include <mapping_msgs/PolygonalMap.h>
#include <body_msgs/Hands.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl_tools/pcl_utils.h>
#include <nnn/nnn.hpp>
#include <pcl_tools/segfast.hpp>

#include "pcl/io/pcd_io.h"
#include "pcl/point_types.h"
#include <pcl/ModelCoefficients.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/features/normal_3d.h>

#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>

#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

#include <body_msgs/Hands.h>

#include <pcl_conversions/pcl_conversions.h>

float gdist(pcl::PointXYZ pt, const Eigen::Vector4f &v){
   return sqrt((pt.x-v(0))*(pt.x-v(0))+(pt.y-v(1))*(pt.y-v(1))+(pt.z-v(2))*(pt.z-v(2))); //
}

void flipvec(const Eigen::Vector4f &palm, const Eigen::Vector4f &fcentroid,Eigen::Vector4f &dir ){
   if((fcentroid-palm).dot(dir) <0)
      dir=dir*-1.0;

 }

geometry_msgs::Point32 eigenToMsgPoint32(const Eigen::Vector4f &v){
	geometry_msgs::Point32 p;
	p.x=v(0); p.y=v(1); p.z=v(2);
	return p;
}
geometry_msgs::Point eigenToMsgPoint(const Eigen::Vector4f &v){
	geometry_msgs::Point p;
	p.x=v(0); p.y=v(1); p.z=v(2);
	return p;
}

pcl::PointXYZ eigenToPclPoint(const Eigen::Vector4f &v){
   pcl::PointXYZ p;
   p.x=v(0); p.y=v(1); p.z=v(2);
   return p;
}

namespace handdetector{

enum FingerName {THUMB,INDEXF,MIDDLEF,RINGF,PINKY,UNKNOWN};

}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b Finger is mostly an organizational tool. it holds all the information for one finger
 * \author Garratt Gallagher
 */
class Finger{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   pcl::PointCloud<pcl::PointXYZ> cloud;
   handdetector::FingerName fname;
   Eigen::Vector4f centroid, direction;
   Finger(pcl::PointCloud<pcl::PointXYZ> &cluster, Eigen::Vector4f &palmcenter){
      cloud=cluster;
      EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
      EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
      Eigen::Matrix3f cov;
      pcl::compute3DCentroid (cluster, centroid);
      pcl::computeCovarianceMatrixNormalized(cluster,centroid,cov);
      pcl::eigen33 (cov, eigen_vectors, eigen_values);
      direction(0)=eigen_vectors (0, 2);
      direction(1)=eigen_vectors (1, 2);
      direction(2)=eigen_vectors (2, 2);
      flipvec(palmcenter,centroid,direction);
   }

   geometry_msgs::Polygon getNormalPolygon(){
      geometry_msgs::Polygon p;
      p.points.push_back(eigenToMsgPoint32(centroid));
      p.points.push_back(eigenToMsgPoint32(centroid+direction*.1));
      return p;
   }

};

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b HandProcessor does the heavy lifting for finding fingers.
 * \author Garratt Gallagher
 */
class HandProcessor{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    pcl::PointCloud<pcl::PointXYZ> full,digits,palm,digits2;
    std::vector<Finger,Eigen::aligned_allocator<Finger> > fingers;
    body_msgs::Hand handmsg;
    double distfromsensor;
    Eigen::Vector4f centroid,arm;
    int thumb;

//    HandProcessor(pcl::PointCloud<pcl::PointXYZ> &cloud){
//       full=cloud;
//        pcl::compute3DCentroid (full, centroid);
//        distfromsensor=centroid.norm();  //because we are in the sensor's frame
//        thumb=-1;
//        handmsg.thumb=thumb;
//        handmsg.stamp=cloud.header.stamp;
//    }
    //for re-initializing a handProcessor object, so we don't have to re-instantiate
    void Init(pcl::PointCloud<pcl::PointXYZ> &cloud,const Eigen::Vector4f &_arm){
      full=cloud;
        pcl::compute3DCentroid (full, centroid);
        distfromsensor=centroid.norm();  //because we are in the sensor's frame
        thumb=-1;
        handmsg.thumb=thumb;
        handmsg.stamp=ros::Time((double)cloud.header.stamp);
        digits=pcl::PointCloud<pcl::PointXYZ>();
        palm=pcl::PointCloud<pcl::PointXYZ>();
        arm=_arm;
        handmsg.arm=eigenToMsgPoint(arm);

    }

    void Init(const body_msgs::Hand &_handmsg){
       handmsg=_handmsg;

       // PCL_DEPRECATED ("pcl::fromROSMsg is deprecated, please use fromPCLPointCloud2 instead.")
       pcl::fromROSMsg(_handmsg.handcloud,full);
        pcl::compute3DCentroid (full, centroid);
        distfromsensor=centroid.norm();  //because we are in the sensor's frame
        digits=pcl::PointCloud<pcl::PointXYZ>();
        palm=pcl::PointCloud<pcl::PointXYZ>();
        arm(0)=handmsg.arm.x;
        arm(1)=handmsg.arm.y;
        arm(2)=handmsg.arm.z;
    }

    //
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief filter the fingers from the palm.  This is done by performing radius searches to determine the point density.
     * when the density drops off, that's the finger!
      * \param nnthresh number of neighbors we expect to see on the palm in a radius search
      * \param tol the size of the search region when we are doing radius searches
      */
    void radiusFilter(int nnthresh, double tol){
//       //first remove points near the arm:
//       vector<int> tempinds;
//       NNN(full,eigenToPclPoint(arm),tempinds,.1);
//       getSubCloud(full, tempinds, full,false);

      timeval t0=g_tick();
      double t1,t2,t3;
      std::vector<int> inds,inds2,inds3;
      std::vector<int> searchinds;

       SplitCloud2<pcl::PointXYZ> sc2(full,tol);
       inds2.resize(full.points.size(),-1);
       t1=g_tock(t0);t0=g_tick();
       int label;

       //DEBUG:
//       find the number of points near a point at the center in x dist, but at y and z coord of 0 0
//       pcl::PointXYZ testpt;
//       testpt.x=0;
//       testpt.y=0;
//       testpt.z=centroid(2);
//       sc2.NNN(testpt,searchinds,tol);
//       if(searchinds.size()){
//          testpt=full.points[searchinds[0]];
//          sc2.NNN(testpt,searchinds,tol);
//       }
//       printf("%.02f, %.02f, %.02f searchinds.size() = %d \n",centroid(0),centroid(1),centroid(2),(int)searchinds.size());

       for(uint i=0;i<full.points.size();++i){
        if(inds2[i]==0) continue;
          sc2.NNN(full.points[i],searchinds,tol);
          //TODO: this is good for face-on, but not great for tilted hands
          if(searchinds.size()>(530-500*distfromsensor)){
             inds.push_back(i);

             if(searchinds.size()>(570-500*distfromsensor))
                label=0;
             else
                label=1;
             for(uint j=0;j<searchinds.size();++j)
                inds2[searchinds[j]]=label;
          }

       }

       t2=g_tock(t0);t0=g_tick();
       for(uint i=0;i<full.points.size();++i)
          if(inds2[i]==-1)
             inds3.push_back(i);

       getSubCloud(full, inds, palm,true);
       getSubCloud(full,inds3, digits,true);

      t3=g_tock(t0);
//    printf("radius: %05d %.03f, %.03f, %.03f   ",full.points.size(),t1,t2,t3);
       std::cout<<"dist:  "<<distfromsensor<<"  palm: "<<palm.points.size()<<"  digits: "<<digits.points.size()<<" "<<(575-500*distfromsensor)<<std::endl;
    }

    sensor_msgs::PointCloud2 getPalm(){
      sensor_msgs::PointCloud2 cloud;
      pcl::toROSMsg(palm,cloud);
      return cloud;
    }
    sensor_msgs::PointCloud2 getDigits(){
      sensor_msgs::PointCloud2 cloud;
      pcl::toROSMsg(digits,cloud);
      return cloud;
    }
    sensor_msgs::PointCloud2 getFull(){
      sensor_msgs::PointCloud2 cloud;
      pcl::toROSMsg(full,cloud);
      return cloud;
    }

    void addFingerDirs(mapping_msgs::PolygonalMap &pmap){
      for(uint i=0;i<fingers.size();++i)
         pmap.polygons.push_back(fingers[i].getNormalPolygon());

    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief runs a cluster segmentation to differentiate the fingers from each other
      * \param clustertol the max distance between a point on one finger and it's nearest neighbor
      * \param mincluster the fewest number of points allowed in a finger
      */
    void segFingers(double clustertol=.005, int mincluster=50){
       handmsg.palm.translation.x=centroid(0);
       handmsg.palm.translation.y=centroid(1);
       handmsg.palm.translation.z=centroid(2);
      if(digits.size()==0)
         return;
      std::vector< std::vector<int> > indclusts;
       extractEuclideanClustersFast2(digits,indclusts,clustertol,mincluster);
//       cout<<" clusters: "<<indclusts.size()<<endl;
       if(!indclusts.size()) return;
       pcl::PointCloud<pcl::PointXYZ> cluster;
       for(uint i=0;i<indclusts.size();++i){
             getSubCloud(digits,indclusts[i], cluster,true);
             fingers.push_back(Finger(cluster,centroid));
             //if it is actually the wrist, it is easily identified because the largest eigenvalue is perpendicular to the vector from the wrist
             //also, because we flip the 'normal' already, we are guaranteed this is positive:
//             if((fingers.back().centroid-centroid).dot(fingers.back().direction)/(fingers.back().centroid-centroid).norm() < .5 ){//a very conservative value...
            if((fingers.back().centroid-centroid).dot(centroid-arm)/((fingers.back().centroid-centroid).norm() * (centroid-arm).norm()) < 0.0 ){//a very conservative value...
                               fingers.pop_back();
             }
//             TODO: DEBUG
//             else{ //if it is a good finger, add it to digits2:
//                if(fingers.size()==1)
//                   digits2=cluster;
//                else
//                   digits2+=cluster;
//
//             }
//              cout<<indclusts[i].size()<<" ("<<(fingers.back().centroid-centroid).dot(fingers.back().direction)/(fingers.back().centroid-centroid).norm()<<")  ";

       }
//       cout<<endl;
       for(uint i=0;i<fingers.size();++i)
         handmsg.fingers.push_back(eigenToMsgPoint(fingers[i].centroid));

    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief locates the thumb, and then orders the fingers
      */
    void identfyFingers(){
      if(!fingers.size()) return;
      int farthest=0;
      thumb=0;
      if(fingers.size()>2){
         //try to identify the thumb based on distance:
         double biggest_dist=0;
         for(uint i=1;i<fingers.size();++i){
           double dist=10;
           for(uint j=1;j<fingers.size();++j){//find smallest distance to neighbor
             if(i!=j && (fingers[i].centroid-fingers[j].centroid).norm() < dist)
               dist=(fingers[i].centroid-fingers[j].centroid).norm();
           }
           if(i==1 || dist > biggest_dist){
             farthest=i;
//             std::cout<<"farthest = "<<farthest<<std::endl;
             biggest_dist=dist;
           }
         }
         thumb=farthest;
      }
      //add a point beyond the end of the thumb to mark it:
//        pcl::PointXYZ pt;
//        pt.x=fingers[thumb].centroid(0)+.1*fingers[thumb].direction(0);
//        pt.y=fingers[thumb].centroid(1)+.1*fingers[thumb].direction(1);
//        pt.z=fingers[thumb].centroid(2)+.1*fingers[thumb].direction(2);
//        digits.push_back(pt);
//        digits.width++;
        handmsg.thumb=thumb;
//        handmsg.palm.rotation.x=fingers[thumb].direction(0);
//        handmsg.palm.rotation.y=fingers[thumb].direction(1);
//        handmsg.palm.rotation.z=fingers[thumb].direction(2);
//        handmsg.palm.rotation.w=0.0;

//        Eigen::Vector4f minpt,maxpt;
//        pcl::getMinMax3D(digits,minpt,maxpt);
//        cout<<"hand size: "<<(maxpt-minpt).norm()<<" ";

    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief The method to rule them all: when processing a hand, just call this function.
      */
    void Process(){
      timeval t0=g_tick();
      double t1,t2,t3;
      radiusFilter(300,.02);
      t1=g_tock(t0);t0=g_tick();
      segFingers();
      t2=g_tock(t0);t0=g_tick();
       identfyFingers();
      t3=g_tock(t0);
//    printf("process: %.03f, %.03f, %.03f",t1,t2,t3);
//    std::cout<<"process: "<<setw(6)<<t1<<setw(6)<<",  "<<t2<<",  "<<t3;
    }

};

class HandAnalyzer
{

private:
  ros::NodeHandle n_;
  ros::Publisher cloudpub_[2],cloudpub2_[2],pmappub_,handspub_;
  ros::Subscriber sub_;
  mapping_msgs::PolygonalMap pmap;

public:

  HandAnalyzer(int p1=1, double p2=2.0)
  {
   handspub_ = n_.advertise<body_msgs::Hands> ("hands_pros", 1);
   pmappub_ = n_.advertise<mapping_msgs::PolygonalMap> ("finger_norms", 1);
   cloudpub_[0] = n_.advertise<sensor_msgs::PointCloud2> ("hand0_cloud", 1);
   cloudpub_[1] = n_.advertise<sensor_msgs::PointCloud2> ("hand1_cloud", 1);
   cloudpub2_[0] = n_.advertise<sensor_msgs::PointCloud2> ("hand0_cloud2", 1);
   cloudpub2_[1] = n_.advertise<sensor_msgs::PointCloud2> ("hand1_cloud2", 1);
    sub_=n_.subscribe("/hands", 1, &HandAnalyzer::handscb, this);
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Gets the direction of the hand.  Useful for determining where the hand is pointing
    */
  void getEigens(const body_msgs::Hand &h){
     pcl::PointCloud<pcl::PointXYZ> cloud;

     Eigen::Vector4f centroid, direction,armvector;
     pcl::fromROSMsg(h.handcloud,cloud);
     EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
       EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
       Eigen::Matrix3f cov;
       pcl::compute3DCentroid (cloud, centroid);
       pcl::computeCovarianceMatrixNormalized(cloud,centroid,cov);
       pcl::eigen33 (cov, eigen_vectors, eigen_values);
       direction(0)=eigen_vectors (0, 2);
       direction(1)=eigen_vectors (1, 2);
       direction(2)=eigen_vectors (2, 2);
       armvector(0)=h.arm.x; armvector(1)=h.arm.y; armvector(2)=h.arm.z;
       flipvec(armvector,centroid,direction);
       printf("Eigenvalues: %.02f, %.02f \n",eigen_values(0)/eigen_values(1),eigen_values(1)/eigen_values(2));

       //eigen eigen_values(1)/eigen_values(2) < .4 means closed fist, unless you are pointing at the kinect

       //make polygon
       geometry_msgs::Polygon p;
       p.points.push_back(eigenToMsgPoint32(centroid));
       p.points.push_back(eigenToMsgPoint32(centroid+direction));
       pmap.polygons.push_back(p);
       pmap.header=h.handcloud.header;
  }

  void ProcessHand(body_msgs::Hand &hand){
     HandProcessor hp;
     hp.Init(hand);
     hp.Process();

//     hp.radiusFilter(300,.02);
     hp.addFingerDirs(pmap);
     if(hand.left){
        cloudpub_[0].publish(hp.getPalm());
        cloudpub2_[0].publish(hp.getDigits());
     }
     else{
        cloudpub_[1].publish(hp.getPalm());
        cloudpub2_[1].publish(hp.getDigits());
     }
     //update the original message:
     hand=hp.handmsg;
     pmap.header=hand.handcloud.header;

  }

  void handscb(const body_msgs::HandsConstPtr &hands){
     body_msgs::Hands handsout=*hands;
     pmap.polygons.clear();
     pmap.header=hands->header;
     for(uint i=0;i<hands->hands.size();i++){
//        getEigens(hands->hands[i]);
//        if(hands->hands[i].left)
//           cloudpub_[0].publish(hands->hands[i].handcloud);
//        else
//           cloudpub_[1].publish(hands->hands[i].handcloud);
        ProcessHand(handsout.hands[i]);
        handsout.header=hands->hands[0].handcloud.header;

     }
     pmappub_.publish(pmap);
     handspub_.publish(handsout);
  }

} ;

#if 0
int main(int argc, char **argv)
{
  ros::init(argc, argv, "finger_detector");
  ros::NodeHandle n;
  HandAnalyzer detector;
  ros::spin();
  return 0;
}
#endif


/*
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you 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.
 *
 * @file mlp_igd.cpp
 *
 * @brief Multilayer Perceptron functions
 *
 *//* ----------------------------------------------------------------------- */
#include <boost/lexical_cast.hpp>

#include <dbconnector/dbconnector.hpp>
#include <modules/shared/HandleTraits.hpp>

#include "mlp_igd.hpp"

#include "task/mlp.hpp"
#include "task/l2.hpp"
#include "algo/igd.hpp"
#include "algo/loss.hpp"

#include "type/tuple.hpp"
#include "type/model.hpp"
#include "type/state.hpp"

namespace madlib {

namespace modules {

namespace convex {

// These 2 classes contain public static methods that can be called
typedef IGD<MLPIGDState<MutableArrayHandle<double> >, MLPIGDState<ArrayHandle<double> >,
        MLP<MLPModel<MutableArrayHandle<double> >, MLPTuple > > MLPIGDAlgorithm;

typedef IGD<MLPMiniBatchState<MutableArrayHandle<double> >, MLPMiniBatchState<ArrayHandle<double> >,
        MLP<MLPModel<MutableArrayHandle<double> >, MiniBatchTuple > > MLPMiniBatchAlgorithm;

typedef Loss<MLPIGDState<MutableArrayHandle<double> >, MLPIGDState<ArrayHandle<double> >,
        MLP<MLPModel<MutableArrayHandle<double> >, MLPTuple > > MLPLossAlgorithm;

typedef MLP<MLPModel<MutableArrayHandle<double> >,MLPTuple> MLPTask;

typedef MLPModel<MutableArrayHandle<double> > MLPModelType;

/**
 * @brief Perform the multilayer perceptron transition step
 *
 * Called for each tuple.
 */
AnyType
mlp_igd_transition::run(AnyType &args) {
    // For the first tuple: args[0] is nothing more than a marker that
    // indicates that we should do some initial operations.
    // For other tuples: args[0] holds the computation state until last tuple
    MLPIGDState<MutableArrayHandle<double> > state = args[0];

    // initilize the state if first tuple
    if (state.algo.numRows == 0) {
        if (!args[3].isNull()) {
            MLPIGDState<ArrayHandle<double> > previousState = args[3];

            state.allocate(*this, previousState.task.numberOfStages,
                           previousState.task.numbersOfUnits);
            state = previousState;
        } else {
            // configuration parameters and initialization
            // this is run only once (first iteration, first tuple)
            ArrayHandle<double> numbersOfUnits = args[4].getAs<ArrayHandle<double> >();
            uint16_t numberOfStages = static_cast<uint16_t>(numbersOfUnits.size() - 1);

            state.allocate(*this, numberOfStages,
                           reinterpret_cast<const double *>(numbersOfUnits.ptr()));
            state.task.stepsize = args[5].getAs<double>();
            state.task.model.activation = static_cast<double>(args[6].getAs<int>());
            state.task.model.is_classification = static_cast<double>(args[7].getAs<int>());
            // args[8] is for weighting the input row, which is populated later.
            state.task.lambda = args[10].getAs<double>();
            MLPTask::lambda = state.task.lambda;
            state.task.model.momentum = args[11].getAs<double>();
            state.task.model.is_nesterov = static_cast<double>(args[12].getAs<bool>());
            if (!args[9].isNull()){
                // initial coefficients are provided
                MappedColumnVector warm_start_coeff = args[9].getAs<MappedColumnVector>();

                // copy warm start into the task model
                // state.reset() ensures algo.incrModel is copied from task.model
                Index layer_start = 0;
                for (size_t k = 0; k < numberOfStages; ++k){
                    for (Index j=0; j < state.task.model.u[k].cols(); ++j){
                        for (Index i=0; i < state.task.model.u[k].rows(); ++i){
                            state.task.model.u[k](i, j) = warm_start_coeff(
                                layer_start + j * state.task.model.u[k].rows() + i);
                        }
                    }
                    layer_start = state.task.model.u[k].rows() * state.task.model.u[k].cols();
                }
            } else {
                // initialize the model with appropriate coefficients
                state.task.model.initialize(
                    numberOfStages,
                    reinterpret_cast<const double *>(numbersOfUnits.ptr()));
            }
        }
        // resetting in either case
        state.reset();
    }

    MLPTuple tuple;
    try {
        tuple.indVar = args[1].getAs<MappedColumnVector>();;
        tuple.depVar = args[2].getAs<MappedColumnVector>();
    } catch (const ArrayWithNullException &e) {
        return args[0];
    }

    tuple.weight = args[8].getAs<double>();
    MLPIGDAlgorithm::transition(state, tuple);
    // Use the model from the previous iteration to compute the loss (note that
    // it is stored in Task's state, and the Algo's state holds the model from
    // the current iteration.
    MLPLossAlgorithm::transition(state, tuple);
    state.algo.numRows ++;

    return state;
}

/**
 * @brief Perform the perliminary aggregation function: Merge transition states
 */
AnyType
mlp_igd_merge::run(AnyType &args) {
    MLPIGDState<MutableArrayHandle<double> > stateLeft = args[0];
    MLPIGDState<ArrayHandle<double> > stateRight = args[1];

    if (stateLeft.algo.numRows == 0) { return stateRight; }
    else if (stateRight.algo.numRows == 0) { return stateLeft; }

    MLPIGDAlgorithm::merge(stateLeft, stateRight);
    MLPLossAlgorithm::merge(stateLeft, stateRight);

    // The following numRows update cannot be put above, because the model
    // averaging depends on their original values
    stateLeft.algo.numRows += stateRight.algo.numRows;

    return stateLeft;
}
/**
 * @brief Perform the multilayer perceptron final step
 */
AnyType
mlp_igd_final::run(AnyType &args) {
    // We request a mutable object. Depending on the backend, this might perform
    // a deep copy.
    MLPIGDState<MutableArrayHandle<double> > state = args[0];

    if (state.algo.numRows == 0) { return Null(); }

    L2<MLPModelType>::lambda = state.task.lambda;
    state.algo.loss = state.algo.loss/static_cast<double>(state.algo.numRows);
    state.algo.loss += L2<MLPModelType>::loss(state.task.model);
    MLPIGDAlgorithm::final(state);
    return state;
}

/**
 * @brief Perform the multilayer perceptron minibatch transition step
 *
 * Called for each tuple.
 */
AnyType
mlp_minibatch_transition::run(AnyType &args) {
    // For the first tuple: args[0] is nothing more than a marker that
    // indicates that we should do some initial operations.
    // For other tuples: args[0] holds the computation state until last tuple
    MLPMiniBatchState<MutableArrayHandle<double> > state = args[0];

    // initilize the state if first tuple
    if (state.numRows == 0) {
        if (!args[3].isNull()) {
            MLPMiniBatchState<ArrayHandle<double> > previousState = args[3];
            state.allocate(*this, previousState.numberOfStages,
                           previousState.numbersOfUnits);
            state = previousState;
        } else {
            // configuration parameters
            ArrayHandle<double> numbersOfUnits = args[4].getAs<ArrayHandle<double> >();
            uint16_t numberOfStages = static_cast<uint16_t>(numbersOfUnits.size() - 1);
            state.allocate(*this, numberOfStages,
                           reinterpret_cast<const double *>(numbersOfUnits.ptr()));
            state.stepsize = args[5].getAs<double>();
            state.model.activation = static_cast<double>(args[6].getAs<int>());
            state.model.is_classification = static_cast<double>(args[7].getAs<int>());
            // args[8] is for weighting the input row, which is populated later.
            state.model.momentum = args[13].getAs<double>();
            state.model.is_nesterov = static_cast<double>(args[14].getAs<bool>());
            if (!args[9].isNull()){
                // initial coefficients are provided copy warm start into the model
                MappedColumnVector warm_start_coeff = args[9].getAs<MappedColumnVector>();
                Index layer_start = 0;
                for (size_t k = 0; k < numberOfStages; ++k){
                    for (Index j=0; j < state.model.u[k].cols(); ++j){
                        for (Index i=0; i < state.model.u[k].rows(); ++i){
                            state.model.u[k](i, j) = warm_start_coeff(
                                layer_start + j * state.model.u[k].rows() + i);
                        }
                    }
                    layer_start = state.model.u[k].rows() * state.model.u[k].cols();
                }
            } else {
                // initialize the model with appropriate coefficients
                state.model.initialize(
                    numberOfStages,
                    reinterpret_cast<const double *>(numbersOfUnits.ptr()));
            }

            state.lambda = args[10].getAs<double>();
            MLPTask::lambda = state.lambda;
            state.batchSize = static_cast<uint16_t>(args[11].getAs<int>());
            state.nEpochs = static_cast<uint16_t>(args[12].getAs<int>());
        }
        // resetting in either case
        state.reset();
    }

    MiniBatchTuple tuple;
    try {
        // Ideally there should be no NULLs in the pre-processed input data,
        // but keep it in a try block in case the user has modified the
        // pre-processed data in any way.
        // The matrices are by default read as column-major. We will have to
        // transpose it to get back the matrix like how it is in the database.
        tuple.indVar = trans(args[1].getAs<MappedMatrix>());
        tuple.depVar = trans(args[2].getAs<MappedMatrix>());
    } catch (const ArrayWithNullException &e) {
        return args[0];
    }

    tuple.weight = args[8].getAs<double>();

    /*
        Note that the IGD version uses the model in Task (model from the
        previous iteration) to compute the loss.
        Minibatch uses the model from Algo (the model based on current
        iteration) to compute the loss. The difference in loss based on one
        iteration is not too much, hence doing so here. We therefore don't
        need to maintain another copy of the model (from previous iteration)
        in the state. The model for the current iteration, and the loss are
        both computed in one function now.
    */
    MLPMiniBatchAlgorithm::transitionInMiniBatch(state, tuple);
    state.numRows += tuple.indVar.rows();
    return state;
}
/**
 * @brief Perform the perliminary aggregation function: Merge transition states
 */
AnyType
mlp_minibatch_merge::run(AnyType &args) {
    MLPMiniBatchState<MutableArrayHandle<double> > stateLeft = args[0];
    MLPMiniBatchState<ArrayHandle<double> > stateRight = args[1];

    if (stateLeft.numRows == 0) { return stateRight; }
    else if (stateRight.numRows == 0) { return stateLeft; }

    MLPMiniBatchAlgorithm::mergeInPlace(stateLeft, stateRight);

    // The following numRows update, cannot be put above, because the model
    // averaging depends on their original values
    stateLeft.numRows += stateRight.numRows;
    stateLeft.loss += stateRight.loss;

    return stateLeft;
}

/**
 * @brief Perform the multilayer perceptron final step
 */
AnyType
mlp_minibatch_final::run(AnyType &args) {
    // We request a mutable object. Depending on the backend, this might perform
    // a deep copy.
    MLPMiniBatchState<MutableArrayHandle<double> > state = args[0];
    // Aggregates that haven't seen any data just return Null.
    if (state.numRows == 0) { return Null(); }

    L2<MLPModelType>::lambda = state.lambda;
    state.loss = state.loss/static_cast<double>(state.numRows);
    state.loss += L2<MLPModelType>::loss(state.model);
    return state;
}

/**
 * @brief Return the difference in RMSE between two states
 */
AnyType
internal_mlp_igd_distance::run(AnyType &args) {
    MLPIGDState<ArrayHandle<double> > stateLeft = args[0];
    MLPIGDState<ArrayHandle<double> > stateRight = args[1];
    return std::abs(stateLeft.algo.loss - stateRight.algo.loss);
}

AnyType
internal_mlp_minibatch_distance::run(AnyType &args) {
    MLPMiniBatchState<ArrayHandle<double> > stateLeft = args[0];
    MLPMiniBatchState<ArrayHandle<double> > stateRight = args[1];

    return std::abs(stateLeft.loss - stateRight.loss);
}

/**
 * @brief Return the coefficients and diagnostic statistics of the state
 */
AnyType
internal_mlp_igd_result::run(AnyType &args) {
    MLPIGDState<ArrayHandle<double> > state = args[0];
    HandleTraits<ArrayHandle<double> >::ColumnVectorTransparentHandleMap
        flattenU;
    flattenU.rebind(&state.task.model.u[0](0, 0),
                    state.task.model.coeffArraySize(state.task.numberOfStages,
                                               state.task.numbersOfUnits));
    AnyType tuple;
    tuple << flattenU
          << static_cast<double>(state.algo.loss);;
    return tuple;
}

/**
 * @brief Return the coefficients and diagnostic statistics of the state
 */
AnyType
internal_mlp_minibatch_result::run(AnyType &args) {
    MLPMiniBatchState<ArrayHandle<double> > state = args[0];
    HandleTraits<ArrayHandle<double> >::ColumnVectorTransparentHandleMap flattenU;
    flattenU.rebind(&state.model.u[0](0, 0),
                    state.model.coeffArraySize(state.numberOfStages,
                                          state.numbersOfUnits));
    AnyType tuple;
    tuple << flattenU
          << static_cast<double>(state.loss);
    return tuple;
}

AnyType
internal_predict_mlp::run(AnyType &args) {
    MLPModel<MutableArrayHandle<double> > model;
    ColumnVector indVar;
    int is_response = args[5].getAs<int>();
    MappedColumnVector x_means = args[6].getAs<MappedColumnVector>();
    MappedColumnVector x_stds = args[7].getAs<MappedColumnVector>();
    MappedColumnVector coeff = args[0].getAs<MappedColumnVector>();
    MappedColumnVector layerSizes = args[4].getAs<MappedColumnVector>();
    // Input layer doesn't count
    uint16_t numberOfStages = static_cast<uint16_t>(layerSizes.size() - 1);
    double is_classification = args[2].getAs<double>();
    double activation = args[3].getAs<double>();
    int is_dep_var_array_for_classification = args[8].getAs<int>();
    bool is_classification_response = is_classification && is_response;

    // The model rebind function is called by both predict and train functions.
    // Since we have to use the same function, we are passing a dummy value for
    // activation, momentum and nesterov because predict does not care
    // about the actual values for these params.
    const double dummy_value = static_cast<double>(-1);
    model.rebind(&is_classification, &activation, &dummy_value, &dummy_value, &coeff.data()[0],
                 numberOfStages, &layerSizes.data()[0]);
    try {
        indVar = (args[1].getAs<MappedColumnVector>()-x_means).cwiseQuotient(x_stds);
    } catch (const ArrayWithNullException &e) {
        return args[0];
    }
    ColumnVector prediction = MLPTask::predict(model, indVar, is_classification_response,
                                               is_dep_var_array_for_classification);
    return prediction;
}

} // namespace convex

} // namespace modules

} // namespace madlib


/////////////////////////////////////////////////////////////////////////////////////////////
// distribution::survival::models::common::importance_sampling::log_ratio_pdf_generate.hpp //
//                                                                           	   	//
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                      		   //
//  Software License, Version 1.0. (See accompanying file                          		   //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)               		   //
/////////////////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_IMPORTANCE_SAMPLING_LOG_RATIO_PDF_GENERATE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_IMPORTANCE_SAMPLING_LOG_RATIO_PDF_GENERATE_HPP_ER_2009
#include <algorithm>
#include <boost/range.hpp>
#include <boost/statistics/detail/distribution/model/models/likelihood/iid/include.hpp>
#include <boost/statistics/detail/distribution/model/models/posterior/include.hpp>
#include <boost/statistics/detail/distribution/model/key/include.hpp>
#include <boost/statistics/detail/distribution/survival/models/common/importance_sampling/log_ratio_pdf_generator.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace survival{

	template<
    	typename TagPar,
        typename TagLpdf,
        typename ItW,
        typename Pr,
        typename M,
        typename ItD,
        typename ItP
    >
	ItW generate_log_ratio_pdf(
    	ItW it_w,
        const Pr& prior,
        const M& model,
        ItD b_d, // dataset
        ItD e_d,
        ItP b_p, // proposals {(p,log_pdfs)}
        ItP e_p
    )
    {
		typedef boost::iterator_range<ItD> dataset_;

        typedef distribution::model::iid_likelihood<M,dataset_> lik_;
        typedef distribution::model::posterior<Pr,lik_> post_;

		dataset_ dataset(b_d,e_d);

        post_ post = post_((
            distribution::model::keyword::prior = prior,
            distribution::model::keyword::model = model,
            distribution::model::keyword::dataset = dataset
        ));
        return std::transform(
            b_p,
            e_p,
            it_w,
        	make_log_ratio_pdf_generator<TagPar,TagLpdf>(post)
        );
	}

}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif