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

#include "sbacam.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "g2o/stuff/macros.h"
#include "g2o/stuff/misc.h"

namespace g2o {

// initialize an object
SBACam::SBACam() {
  setKcam(1, 1, cst(0.5), cst(0.5), 0);  // unit image projection
}

// set the object pose
SBACam::SBACam(const Quaternion& r_, const Vector3& t_) : SE3Quat(r_, t_) {
  Kcam.setZero();
  setTransform();
  setProjection();
  setDr();
}

SBACam::SBACam(const SE3Quat& p) : SE3Quat(p) {
  Kcam.setZero();
  setTransform();
  setProjection();
  setDr();
}

// update from the linear solution
// defined in se3quat
void SBACam::update(const Vector6& update) {
  // position update
  _t += update.head(3);
  // small quaternion update
  Quaternion qr;
  qr.vec() = update.segment<3>(3);
  qr.w() =
      sqrt(cst(1.0) - qr.vec().squaredNorm());  // should always be positive
  _r *= qr;                                     // post-multiply
  _r.normalize();
  setTransform();
  setProjection();
  setDr();
}

// transforms
void SBACam::transformW2F(Eigen::Matrix<number_t, 3, 4>& m,
                          const Vector3& trans, const Quaternion& qrot) {
  m.block<3, 3>(0, 0) = qrot.toRotationMatrix().transpose();
  m.col(3).setZero();  // make sure there's no translation
  Vector4 tt;
  tt.head(3) = trans;
  tt[3] = 1.0;
  m.col(3) = -m * tt;
}

void SBACam::transformF2W(Eigen::Matrix<number_t, 3, 4>& m,
                          const Vector3& trans, const Quaternion& qrot) {
  m.block<3, 3>(0, 0) = qrot.toRotationMatrix();
  m.col(3) = trans;
}

// set up camera matrix
void SBACam::setKcam(number_t fx, number_t fy, number_t cx, number_t cy,
                     number_t tx) {
  Kcam.setZero();
  Kcam(0, 0) = fx;
  Kcam(1, 1) = fy;
  Kcam(0, 2) = cx;
  Kcam(1, 2) = cy;
  Kcam(2, 2) = cst(1.0);
  baseline = tx;
  setTransform();
  setProjection();
  setDr();
}

// sets angle derivatives
void SBACam::setDr() {
  // inefficient, just for testing
  // use simple multiplications and additions for production code in calculating
  // dRdx,y,z
  Matrix3 dRidx, dRidy, dRidz;
  dRidx << cst(0.0), cst(0.0), cst(0.0), cst(0.0), cst(0.0), cst(2.0), cst(0.0),
      cst(-2.0), cst(0.0);
  dRidy << cst(0.0), cst(0.0), cst(-2.0), cst(0.0), cst(0.0), cst(0.0),
      cst(2.0), cst(0.0), cst(0.0);
  dRidz << cst(0.0), cst(2.0), cst(0.0), cst(-2.0), cst(0.0), cst(0.0),
      cst(0.0), cst(0.0), cst(0.0);

  // for dS'*R', with dS the incremental change
  dRdx = dRidx * w2n.block<3, 3>(0, 0);
  dRdy = dRidy * w2n.block<3, 3>(0, 0);
  dRdz = dRidz * w2n.block<3, 3>(0, 0);
}

// human-readable SBACam object
std::ostream& operator<<(std::ostream& out_str, const SBACam& cam) {
  out_str << cam.translation().transpose() << std::endl;
  out_str << cam.rotation().coeffs().transpose() << std::endl << std::endl;
  out_str << cam.Kcam << std::endl << std::endl;
  out_str << cam.w2n << std::endl << std::endl;
  out_str << cam.w2i << std::endl << std::endl;
  return out_str;
}

}  // namespace g2o


#define BOOST_TEST_MODULE ControlledTemperature_test1

#include <fmi4cpp/fmi2/fmi2.hpp>

#include <boost/test/unit_test.hpp>

#include <string>

using namespace fmi4cpp;

const double step_size = 1E-4;
const fmi2ValueReference vr = 46;

BOOST_AUTO_TEST_CASE(ControlledTemperature_test1)
{

    const std::string fmu_path = "../resources/fmus/2.0/cs/20sim/4.6.4.8004/"
                            "ControlledTemperature/ControlledTemperature.fmu";

    auto fmu = fmi2::fmu(fmu_path).as_cs_fmu();

    size_t numOutputs = 0;
    for (const auto& v : *fmu->get_model_description()->model_variables) {
        if (v.causality == fmi2::causality::output) {
            numOutputs++;
        }
    }
    BOOST_CHECK_EQUAL(2, numOutputs);

    auto slave = fmu->new_instance();
    BOOST_CHECK(slave->setup_experiment());
    BOOST_CHECK(slave->enter_initialization_mode());
    BOOST_CHECK(slave->exit_initialization_mode());

    double ref;
    BOOST_CHECK(slave->step(step_size));
    BOOST_CHECK(slave->read_real(vr, ref));

    BOOST_CHECK_EQUAL(298.15, ref);

    BOOST_CHECK(slave->terminate());
}


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

#ifndef BOOST_GRAPH_RELAX_HPP
#define BOOST_GRAPH_RELAX_HPP

#include <functional>
#include <boost/limits.hpp> // for numeric limits
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map.hpp>

namespace boost {

    // The following version of the plus functor prevents
    // problems due to overflow at positive infinity.

    template <class T>
    struct closed_plus
    {
      // std::abs just isn't portable :(
      template <class X>
      inline X my_abs(const X& x) const { return x < 0 ? -x : x; }

      T operator()(const T& a, const T& b) const {
        using namespace std;
        T inf = numeric_limits<T>::max();
        if (b > 0 && my_abs(inf - a) < b)
          return inf;
        return a + b;
      }
    };

    template <class Graph, class WeightMap,
            class PredecessorMap, class DistanceMap,
            class BinaryFunction, class BinaryPredicate>
    bool relax(typename graph_traits<Graph>::edge_descriptor e,
               const Graph& g, WeightMap w,
               PredecessorMap p, DistanceMap d,
               BinaryFunction combine, BinaryPredicate compare)
    {
      typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
      Vertex u = source(e, g), v = target(e, g);
      typedef typename property_traits<DistanceMap>::value_type D;
      typedef typename property_traits<WeightMap>::value_type W;
      D d_u = get(d, u), d_v = get(d, v);
      W w_e = get(w, e);

      if ( compare(combine(d_u, w_e), d_v) ) {
        put(d, v, combine(d_u, w_e));
        put(p, v, u);
        return true;
      } else
        return false;
    }

    template <class Graph, class WeightMap,
      class PredecessorMap, class DistanceMap>
    bool relax(typename graph_traits<Graph>::edge_descriptor e,
               const Graph& g, WeightMap w, PredecessorMap p, DistanceMap d)
    {
      typedef typename property_traits<DistanceMap>::value_type D;
      typedef closed_plus<D> Combine;
      typedef std::less<D> Compare;
      return relax(e, g, w, p, d, Combine(), Compare());
    }

} // namespace boost

#endif /* BOOST_GRAPH_RELAX_HPP */


/*
 * Copyright (c) 2015 University of Basel
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * Neither the name of the project's author nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>

#include <itkCompositeTransform.h>
#include <itkImage.h>
#include <itkMesh.h>
#include <itkPointsLocator.h>
#include <itkStandardImageRepresenter.h>
#include <itkStandardMeshRepresenter.h>
#include <itkStatisticalModel.h>
#include <itkVersorRigid3DTransform.h>

#include "utils/statismo-fitting-utils.h"

const unsigned Dimensionality3D = 3;
const unsigned Dimensionality2D = 2;

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

struct programOptions {
    bool bDisplayHelp;

    string strModelType;
    string strInputModelFileName;
    string strOutputModelFileName;

    string strInputMeshFileName;

    string strInputFixedLandmarksFileName;
    string strInputMovingLandmarksFileName;
    double dVariance;

    unsigned uNumberOfDimensions;
};

po::options_description initializeProgramOptions(programOptions& poParameters);
bool isOptionsConflictPresent(programOptions& opt);
void buildPosteriorShapeModel(programOptions& opt);
template<unsigned Dimensionality>
void buildPosteriorDeformationModel(programOptions& opt);

int main(int argc, char** argv) {
    po::positional_options_description optPositional;
    optPositional.add("output-file", 1);

    programOptions poParameters;
    po::options_description optAllOptions = initializeProgramOptions(poParameters);

    po::variables_map vm;
    try {
        po::parsed_options parsedOptions = po::command_line_parser(argc, argv).options(optAllOptions).positional(optPositional).run();
        po::store(parsedOptions, vm);
        po::notify(vm);
    } catch (po::error& e) {
        cerr << "An exception occurred while parsing the Command line:"<<endl;
        cerr << e.what() << endl << endl;
        cout << optAllOptions << endl;
        return EXIT_FAILURE;
    }

    if (poParameters.bDisplayHelp == true) {
        cout << optAllOptions << endl;
        return EXIT_SUCCESS;
    }
    if (isOptionsConflictPresent(poParameters) == true)	{
        cerr << "A conflict in the options exists or insufficient options were set." << endl;
        cout << optAllOptions << endl;
        return EXIT_FAILURE;
    }

    try {
        if (poParameters.strModelType == "shape") {
            buildPosteriorShapeModel(poParameters);
        } else {
            if (poParameters.uNumberOfDimensions == Dimensionality2D) {
                buildPosteriorDeformationModel<Dimensionality2D>(poParameters);
            } else {
                buildPosteriorDeformationModel<Dimensionality3D>(poParameters);
            }
        }
    } catch (ifstream::failure & e) {
        cerr << "Could not read a file:" << endl;
        cerr << e.what() << endl;
        return EXIT_FAILURE;
    } catch (itk::ExceptionObject & e) {
        cerr << "Could not build the posterior model:" << endl;
        cerr << e.what() << endl;
        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}

bool isOptionsConflictPresent(programOptions& opt) {
    //if one set of the landmarks-file is provided, then both have to be provided (-> use XOR)
    if (((opt.strInputFixedLandmarksFileName != "") ^ (opt.strInputMovingLandmarksFileName != "")) == true) {
        return true;
    }

    //either a mesh is used as reference or landmarks are used. It's not possible to use both at the same time or use none. (-> again XOR)
    if (((opt.strInputFixedLandmarksFileName != "") ^ (opt.strInputMeshFileName != "")) == false) {
        return true;
    }

    if (opt.strInputModelFileName == "" || opt.strOutputModelFileName == "") {
        return true;
    }

    boost::algorithm::to_lower(opt.strModelType);
    if (opt.strModelType != "shape" && opt.strModelType != "deformation") {
        return true;
    }

    //A mesh in correspondence can only be used if the model type is shape
    if (opt.strModelType == "deformation" && opt.strInputMeshFileName != "") {
        return true;
    }

    if (opt.strModelType == "deformation" && opt.uNumberOfDimensions != 2 && opt.uNumberOfDimensions != 3) {
        return true;
    }

    if (opt.strModelType == "shape" && opt.uNumberOfDimensions != 3) {
        return true;
    }

    return false;
}

void buildPosteriorShapeModel(programOptions& opt) {
    typedef itk::Mesh<float, Dimensionality3D> DataType;
    typedef itk::StatisticalModel<DataType> StatisticalModelType;
    typedef itk::StandardMeshRepresenter<float, Dimensionality3D> RepresenterType;
    RepresenterType::Pointer pRepresenter = RepresenterType::New();
    StatisticalModelType::Pointer pModel = StatisticalModelType::New();
    pModel->Load(pRepresenter.GetPointer(), opt.strInputModelFileName.c_str());

    StatisticalModelType::Pointer pConstrainedModel;
    if (opt.strInputMeshFileName == "") {
        typedef itk::PointsLocator<DataType::PointsContainer > PointsLocatorType;
        pConstrainedModel = buildPosteriorShapeModel<DataType, StatisticalModelType, PointsLocatorType>(pModel, opt.strInputFixedLandmarksFileName, opt.strInputMovingLandmarksFileName, opt.dVariance);
    } else {
        typedef itk::MeshFileReader<DataType> MeshReaderType;
        MeshReaderType::Pointer pMeshReader = MeshReaderType::New();
        pMeshReader->SetFileName(opt.strInputMeshFileName.c_str());
        pMeshReader->Update();
        DataType::Pointer pMeshInCorrespondence = pMeshReader->GetOutput();
        pConstrainedModel = buildPosteriorShapeModel<DataType, StatisticalModelType>(pModel, pMeshInCorrespondence, opt.dVariance);
    }

    pConstrainedModel->Save(opt.strOutputModelFileName.c_str());
}

template<unsigned Dimensionality>
void buildPosteriorDeformationModel(programOptions& opt) {
    typedef itk::Vector<float, Dimensionality> VectorPixelType;
    typedef itk::Image<VectorPixelType, Dimensionality> DataType;
    typedef itk::StatisticalModel<DataType> StatisticalModelType;
    typedef itk::StandardImageRepresenter<VectorPixelType, Dimensionality> RepresenterType;
    typename RepresenterType::Pointer pRepresenter = RepresenterType::New();
    typename StatisticalModelType::Pointer pModel = StatisticalModelType::New();
    pModel->Load(pRepresenter.GetPointer(), opt.strInputModelFileName.c_str());

    typename StatisticalModelType::Pointer pConstrainedModel;
    pConstrainedModel = buildPosteriorDeformationModel<DataType, StatisticalModelType>(pModel, opt.strInputFixedLandmarksFileName, opt.strInputMovingLandmarksFileName, opt.dVariance);

    pConstrainedModel->Save(opt.strOutputModelFileName.c_str());
}

po::options_description initializeProgramOptions(programOptions& poParameters) {
    po::options_description optMandatory("Mandatory options");
    optMandatory.add_options()
    ("type,t", po::value<string>(&poParameters.strModelType)->default_value("SHAPE"), "Specifies the type of the model: SHAPE and DEFORMATION are the two available types.")
    ("dimensionality,d", po::value<unsigned>(&poParameters.uNumberOfDimensions)->default_value(3), "Dimensionality of the input model.")
    ("input-file,i", po::value<string>(&poParameters.strInputModelFileName), "The path to the model file from which the posterior model will be built.")
    ("output-file,o", po::value<string>(&poParameters.strOutputModelFileName), "The path where the posterior model will be saved.")
    ;

    po::options_description optLandmarks("Landmarks");
    optLandmarks.add_options()
    ("landmarks-fixed,f", po::value<string>(&poParameters.strInputFixedLandmarksFileName), "Name of the file where the fixed Landmarks are saved.")
    ("landmarks-moving,m", po::value<string>(&poParameters.strInputMovingLandmarksFileName), "Name of the file where the moving Landmarks are saved.")
    ("landmarks-variance,v", po::value<double>(&poParameters.dVariance)->default_value(1), "The variance that will be used to build the posterior model.")
    ;

    po::options_description optMesh("Mesh in correspondence (either set this or the Landmarks)");
    optMesh.add_options()
    ("corresponding-mesh,c", po::value<string>(&poParameters.strInputMeshFileName), "Path to the Mesh in correspondence. This is only available if the type is SHAPE.")
    ;

    po::options_description optAdditional("Optional options");
    optAdditional.add_options()
    ("help,h", po::bool_switch(&poParameters.bDisplayHelp), "Display this help message")
    ;

    po::options_description optAllOptions;
    optAllOptions.add(optMandatory).add(optMesh).add(optLandmarks).add(optAdditional);
    return optAllOptions;
}


#include "lib/info/printf_dbg.h"
#include "lib/info/printf_dbg_eigen.h"

#include <boost/hana/equal.hpp>

#include <string_view>

using namespace printf_dbg::detail;
using namespace printf_dbg;
using namespace std::literals;
using namespace hana::literals;

static_assert([] {
  static_assert(Formattable<bool>);
  static_assert(Formattable<char *>);
  static_assert(Formattable<const char *>);
  static_assert(Formattable<int *>);
  static_assert(Formattable<const int *>);
  static_assert(Formattable<std::array<int, 0>>);
  static_assert(Formattable<std::array<int, 3>>);
  static_assert(fmt_t<float> == float_fmt);
  static_assert(fmt_t<double> == float_fmt);
  static_assert(fmt_t<int> == s("%i"));
  static_assert(fmt_t<short int> == s("%hi"));
  static_assert(fmt_t<unsigned> == s("%u"));
  static_assert(fmt_t<long unsigned> == s("%lu"));
  static_assert(fmt_t<bool> == s("%s"));
  static_assert(fmt_t<std::array<int, 0>> == s("{}"));
  static_assert(fmt_t<std::array<int, 1>> == s("{%i}"));
  static_assert(fmt_t<std::array<int, 3>> == s("{%i, %i, %i}"));
  static_assert(fmt_vals(8) == hana::make_tuple(8));
  static_assert(fmt_vals(std::array{1, 5, 9}) == hana::make_tuple(1, 5, 9));
  static_assert(fmt_vals(true)[0_c] == "true"sv);
  static_assert(fmt_vals(false)[0_c] == "false"sv);
  static_assert(fmt_t<Eigen::Array3f> == s("{") + float_fmt + s("}\n") +
                                             s("{") + float_fmt + s("}\n") +
                                             s("{") + float_fmt + s("}"));
  static_assert(fmt_t<Eigen::Vector3f> == s("{") + float_fmt + s("}\n") +
                                              s("{") + float_fmt + s("}\n") +
                                              s("{") + float_fmt + s("}"));
  static_assert(fmt_t<Eigen::RowVector3f> ==
                s("{") + s(", ").join_n<3>(float_fmt) + s("}"));

  return true;
}());


/*
@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)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/tuple.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto contains = infix(elem);

    BOOST_HANA_CONSTEXPR_CHECK(
        flip(all_of)(make<Tuple>(1, '2', 3.3) ^ contains,
            make<Tuple>(1, '2', 3.3)
        )
    );

    BOOST_HANA_CONSTEXPR_CHECK(
        flip(all_of)(contains ^ '2',
            make<Tuple>(
                make<Tuple>(1, '2'),
                make<Tuple>('2'),
                make<Tuple>('2', 3.3)
            )
        )
    );

    BOOST_HANA_CONSTEXPR_CHECK(make<Tuple>(1, '2', 3.3) ^contains^ 3.3);
    //! [main]
}


#pragma once

#include <vector>
#include <chrono>
#include <mutex>
#include <functional>
#include <initializer_list>
#include <thread>
#include <map>
#include <list>

#include <boost/accumulators/accumulators.hpp>
#include <boost/any.hpp>

class Stats
{
public:
    class TimeValueDatum
    {
    public:
        TimeValueDatum(const boost::any& value) : time(std::chrono::steady_clock::now()), value(value) {}
		const std::chrono::steady_clock::time_point time;
        const boost::any value;
    };

    class StatVal
    {
    public:
        template <typename Feature>
        static StatVal makeStatVal(std::function<bool (const TimeValueDatum&)>   filter,
                                   std::function<double (const TimeValueDatum&)> accessor,
                                   const Feature&                                  accumulator,
                                   const std::string&                              name)
        {
            auto filterAccExtractorMaker = [filter, accessor]()
            {
                auto acc = new boost::accumulators::accumulator_set<double, boost::accumulators::features<Feature> >();

                auto filterAcc = [acc, filter, accessor](const Stats::TimeValueDatum& datum)
                {
                    if (filter(datum))
                    {
                        (*acc)(accessor(datum.value));
                    }
                };

                auto extractor = [acc]()
                {
                    boost::accumulators::extractor<Feature> ex;
                    return ex(*acc);
                };

				return FilterAccExtractor(filterAcc, extractor);
            };

            return StatVal(filterAccExtractorMaker, name);
        }

    private:
        friend class Stats;

        typedef std::pair<std::function<void (const Stats::TimeValueDatum&)>, std::function<double ()> > FilterAccExtractor;
        typedef std::function<FilterAccExtractor ()> FilterAccExtractorMaker;

        StatVal(FilterAccExtractorMaker filterAccExtractorMaker,
                const std::string& name)
                :
                filterAccExtractorMaker(filterAccExtractorMaker),
                name(name) {}

        FilterAccExtractorMaker filterAccExtractorMaker;
        std::string name;
    };

    Stats();

    // events
    void store(const boost::any& datum);

    // utility

    typedef std::function<void(std::map<std::string, double>&)> PostProcessor;
    typedef std::function<std::list<Stats::StatVal>(std::chrono::microseconds,
		                                              std::chrono::steady_clock::time_point)> StatValsMaker;
    typedef std::function<PostProcessor(std::chrono::microseconds,
		                                  std::chrono::steady_clock::time_point)> PostProcessorMaker;

	std::string summary(std::chrono::microseconds window,
		                StatValsMaker               statValsMaker,
		                PostProcessorMaker          postProcessorMaker,
		                const std::string&          format);
    void autoSummary(std::chrono::microseconds frequency,
					 StatValsMaker               statValsMaker,
					 PostProcessorMaker          postProcessorMaker,
                     const std::string&          format);
	void stopAutoSummary();

private:
    std::list<TimeValueDatum>* activeStorage;
    std::list<TimeValueDatum>* processingStorage;

    std::list<TimeValueDatum> storage1;
    std::list<TimeValueDatum> storage2;

    std::map<std::string, double> query(std::initializer_list<StatVal>                         statVals,
                                        std::function<void (std::map<std::string, double>&)> postCalculator);
	std::map<std::string, double> query(std::list<StatVal>                                    statVals,
                                        std::function<void(std::map<std::string, double>&)> postCalculator);

    std::string formatDuration(std::chrono::microseconds duration);

    std::mutex mutex;
    std::thread autoSummaryThread;
	bool stopAutoSummary_;
    void autoSummaryLoop(std::chrono::microseconds frequency,
						 StatValsMaker               statValsMaker,
						 PostProcessorMaker          postProcessorMaker,
						 const std::string&          format);
};


#include "ModeTransformationPlugin.h"

#include <iostream>

#include <Eigen/Dense>
#include <parameters/RichParameterWidget.h>

#include "TransformationWidget.h"
#include "TransformationTool.h"

using Eigen::Vector4d;

ModeTransformationPlugin::ModeTransformationPlugin()
{
    transformationWidget = nullptr;
    controlDockWidget = nullptr;
}

/**
 * @brief FilterTransformationPlugin::create
 * 插件入口函数
 */
void ModeTransformationPlugin::create()
{
    if(!transformationWidget && !controlDockWidget) {
        // 初次初始化插件
        controlDockWidget = new ModePluginDockWidget(tr("Transformation"), mainWindow());
        transformationWidget = new TransformationWidget(controlDockWidget);
        controlDockWidget->setWidget(transformationWidget);
        controlDockWidget->setFeatures(QDockWidget::NoDockWidgetFeatures);
        mainWindow()->addDockWidget(Qt::RightDockWidgetArea, controlDockWidget);
    }

    // 显示控制界面
    controlDockWidget->setVisible(true);
    // 建立事件处理函数链接
    initConnections();
    selectionChanged(safeCast(document()->selectedModel()));
}

/**
 * @brief ModeTransformationPlugin::destroy
 * 停止使用插件
 */
void ModeTransformationPlugin::destroy()
{
    if(controlDockWidget) {
        controlDockWidget->setVisible(false);
    }

    // 断开事件注册
    releaseConnections();
}

/**
 * @brief FilterTransformationPlugin::isApplicable
 * 是否可以应用该插件
 * @return
 */
bool ModeTransformationPlugin::isApplicable()
{
    return isA(this->document()->selectedModel());
}

/**
 * @brief ModeTransformationPlugin::suspend
 * 触发暂定操作
 */
void ModeTransformationPlugin::suspend(){releaseConnections();}

/**
 * @brief ModeTransformationPlugin::resume
 * 触发继续操作
 */
void ModeTransformationPlugin::resume(){
    initConnections();
    selectionChanged(safeCast(document()->selectedModel()));
}

/**
 * @brief ModeTransformationPlugin::initConnections
 */
void ModeTransformationPlugin::initConnections()
{
    // document selectionChanged 事件
    connect(document(), SIGNAL(selectionChanged(Model*)),
            this, SLOT(selectionChanged(Model*)));

    // 应用位移操作
    connect(transformationWidget, SIGNAL(transformationChagned(Vector3d, Vector3d, Vector3d, Vector3d)),
            this, SLOT(applyTransform(Vector3d, Vector3d, Vector3d, Vector3d)),
            Qt::UniqueConnection);

    // 更新坐标原点到模型的中心
    connect(transformationWidget->pushButtonSetModelCenterAsOrigin, &QPushButton::clicked,
            [&](){
        // 减少不必要的调用
        Vector3d localCenter3d = mesh()->bbox().center();
        if(equal(localCenter3d, Vector3d::Zero())){
            std::cout << __FUNCTION__ << __LINE__ << std::endl;
                return;
        }

        // 获得 世界坐标 和 局部坐标 之间的转换矩阵
        Matrix4d transMat = mesh()->getTransformationMatrix();

        // Center 的两种坐标
        Vector4d localCenter4d;
        localCenter4d << localCenter3d, 1;
        Vector4d worldCenter4d = transMat * localCenter4d;

        // Position 的两种坐标
        Vector4d worldPosition4d;
        worldPosition4d << mesh()->getPosition(), 1;

        Vector4d localOrigin4d(0, 0, 0, 1);

        // 模型顶点的坐标变换
        setModelOrigin(mesh(), localCenter4d.segment(0, 3));
        mesh()->setPosition(worldCenter4d.segment(0, 3));
        transformationWidget->updateUI(mesh()->getPosition(), mesh()->getRotation(), mesh()->getScale(), mesh()->bbox());
        transformationWidget->pushButtonSetModelCenterAsOrigin->setEnabled(false);
        transformationWidget->pushButtonSetAxisOriginAsOrigin->setEnabled(true);
    });

    // 更新坐标原点到坐标系原点
    connect(transformationWidget->pushButtonSetAxisOriginAsOrigin, &QPushButton::clicked,
            [&](){
        // 获得 世界坐标 和 局部坐标 之间的转换矩阵
        Matrix4d transMat = mesh()->getTransformationMatrix();
        Matrix4d invTransMat = transMat.inverse();

        // old model local origin
        Vector4d localOldOrigin4d(0, 0, 0, 1);

        // Axis Origin
        Vector4d worldOrigin4d(0.0, 0.0, 0.0, 1.0);
        Vector4d localOrigin4d = invTransMat * worldOrigin4d;

        // 修改模型顶点数据
        setModelOrigin(mesh(), (localOrigin4d - localOldOrigin4d).segment(0, 3));
        mesh()->setPosition((worldOrigin4d).segment(0, 3));
        transformationWidget->updateUI(mesh()->getPosition(), mesh()->getRotation(), mesh()->getScale(), mesh()->bbox());
        transformationWidget->pushButtonSetModelCenterAsOrigin->setEnabled(true);
        transformationWidget->pushButtonSetAxisOriginAsOrigin->setEnabled(false);
    });

    connectTrigger();
}

void ModeTransformationPlugin::connectTrigger()
{
    /// Position
    connect(transformationWidget->doubleSpinBoxPosX, SIGNAL(valueChanged(double)),
            this, SLOT(apply()), Qt::UniqueConnection);
    connect(transformationWidget->doubleSpinBoxPosY, SIGNAL(valueChanged(double)),
            this, SLOT(apply()), Qt::UniqueConnection);
    connect(transformationWidget->doubleSpinBoxPosZ, SIGNAL(valueChanged(double)),
            this, SLOT(apply()), Qt::UniqueConnection);

    /// Rotation
    connect(transformationWidget->doubleSpinBoxRotX, SIGNAL(valueChanged(double)),
            this, SLOT(apply()), Qt::UniqueConnection);
    connect(transformationWidget->doubleSpinBoxRotY, SIGNAL(valueChanged(double)),
            this, SLOT(apply()), Qt::UniqueConnection);
    connect(transformationWidget->doubleSpinBoxRotZ, SIGNAL(valueChanged(double)),
            this, SLOT(apply()), Qt::UniqueConnection);

    /// Scale
    connect(transformationWidget->doubleSpinBoxBboxX, SIGNAL(valueChanged(double)),
            this, SLOT(apply()), Qt::UniqueConnection);
    connect(transformationWidget->doubleSpinBoxBboxY, SIGNAL(valueChanged(double)),
            this, SLOT(apply()), Qt::UniqueConnection);
    connect(transformationWidget->doubleSpinBoxBboxZ, SIGNAL(valueChanged(double)),
            this, SLOT(apply()), Qt::UniqueConnection);
}

void ModeTransformationPlugin::disconnectTrigger()
{
    /// Position
    disconnect(transformationWidget->doubleSpinBoxPosX, SIGNAL(valueChanged(double)),
            this, SLOT(apply()));
    disconnect(transformationWidget->doubleSpinBoxPosY, SIGNAL(valueChanged(double)),
            this, SLOT(apply()));
    disconnect(transformationWidget->doubleSpinBoxPosZ, SIGNAL(valueChanged(double)),
            this, SLOT(apply()));

    /// Rotation
    disconnect(transformationWidget->doubleSpinBoxRotX, SIGNAL(valueChanged(double)),
            this, SLOT(apply()));
    disconnect(transformationWidget->doubleSpinBoxRotY, SIGNAL(valueChanged(double)),
            this, SLOT(apply()));
    disconnect(transformationWidget->doubleSpinBoxRotZ, SIGNAL(valueChanged(double)),
            this, SLOT(apply()));

    /// Scale
    disconnect(transformationWidget->doubleSpinBoxBboxX, SIGNAL(valueChanged(double)),
            this, SLOT(apply()));
    disconnect(transformationWidget->doubleSpinBoxBboxY, SIGNAL(valueChanged(double)),
            this, SLOT(apply()));
    disconnect(transformationWidget->doubleSpinBoxBboxZ, SIGNAL(valueChanged(double)),
            this, SLOT(apply()));
}

/**
 * @brief ModeTransformationPlugin::releaseConnections
 */
void ModeTransformationPlugin::releaseConnections()
{
    disconnect(document(), SIGNAL(selectionChanged(Model*)),
            this, SLOT(selectionChanged(Model*)));
    disconnect(transformationWidget, SIGNAL(transformationChagned(Vector3d, Vector3d, Vector3d, Vector3d)),
            this, SLOT(applyTransform(Vector3d, Vector3d, Vector3d, Vector3d)));
}

/**
 * @brief ModeTransformationPlugin::selectionChanged
 * @param model
 */
void ModeTransformationPlugin::selectionChanged(Model *model)
{
    disconnectTrigger();

    transformationWidget->update(model);

    connectTrigger();
}

void ModeTransformationPlugin::apply()
{
    Vector3d position;
    Vector3d rotation;
    Vector3d size;

    position.x() = transformationWidget->doubleSpinBoxPosX->value();
    position.y() = transformationWidget->doubleSpinBoxPosY->value();
    position.z() = transformationWidget->doubleSpinBoxPosZ->value();

    rotation.x() = transformationWidget->ang2rad(transformationWidget->doubleSpinBoxRotX->value());
    rotation.y() = transformationWidget->ang2rad(transformationWidget->doubleSpinBoxRotY->value());
    rotation.z() = transformationWidget->ang2rad(transformationWidget->doubleSpinBoxRotZ->value());

    size.x() = transformationWidget->doubleSpinBoxBboxX->value();
    size.y() = transformationWidget->doubleSpinBoxBboxY->value();
    size.z() = transformationWidget->doubleSpinBoxBboxZ->value();

    applyTransform(position, rotation, size);
}

void ModeTransformationPlugin::applyTransform(
        Eigen::Vector3d position,
        Eigen::Vector3d rotation,
        Eigen::Vector3d size)
{
    SurfaceMeshModel* model = mesh();
    if(model == nullptr){
        std::cout << __FILE__ << __LINE__ << __PRETTY_FUNCTION__
                  << "model == nullptr" << std::endl;
        return;
    }

    // 计算 Scale
    Vector3d scale;
    Vector3d bboxSize = model->bbox().max()-model->bbox().min();
    scale.x() = size.x()/bboxSize.x();
    scale.y() = size.y()/bboxSize.y();
    scale.z() = size.z()/bboxSize.z();

    // 更新模型的数据
    model->setPosition(position);
    model->setRotation(rotation);
    model->setScale(scale);

    // 手动触发更新
    drawArea()->updateGL();
    selectionChanged(model);
}


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
// Tests built to exercise sequenceSlice() in a simplification of the bert
// attention loop.

#define BOOST_TEST_MODULE BertSlicing
#include <boost/program_options.hpp>
#include <iostream>
#include <math.h>
#include <poplar/Engine.hpp>
#include <poplar/VariableMappingMethod.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poplin/MatMul.hpp>
#include <poplin/codelets.hpp>
#include <popops/DynamicSlice.hpp>
#include <popops/ElementWise.hpp>
#include <popops/Fill.hpp>
#include <popops/SequenceSlice.hpp>
#include <popops/Zero.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <poputil/Util.hpp>
#include <poputil/VarStructure.hpp>

using namespace poplar;
using namespace poplar::program;

// Check slicing in a similar manner to the bert attention layer.
// Some elements are missed, and the tile mapings are not representative of
// what frameworks will achieve.
BOOST_AUTO_TEST_CASE(AttentionSlice) {
  bool verbose = false;
  if (isSimulator(TEST_TARGET)) {
    std::cout << "AttentionSlice is too slow to check on simulators\n";
    return;
  }
  auto device = createTestDeviceFullSize(TEST_TARGET);
  Target target = device.getTarget();

  std::cout << "Creating graph\n";
  Graph graph(device.getTarget());
  Sequence prog;
  DebugContext di{"SimpleSliceTest"};
  popops::addCodelets(graph);
  poplin::addCodelets(graph);
  constexpr unsigned maxTokensPerBatch = 4 * 512;
  constexpr unsigned maxSequencesPerBatch = 8;
  constexpr unsigned maxTokensPerSequence = 512;
  constexpr unsigned nSequencesPerInnerLoop = 2;
  constexpr unsigned maxTokensPerInnerLoop =
      maxTokensPerSequence * nSequencesPerInnerLoop;
  constexpr bool large = true;
  // BERT large causes problems with a large variable not fitting on the
  // cpu/ipuModel targets
  const unsigned nFeatures = large && isHw(TEST_TARGET) ? 1024 : 64;
  constexpr unsigned nHidden = large ? 64 : 8;
  constexpr unsigned nHeads = large ? 16 : 8;

  Tensor tOffsets = graph.addVariable(UNSIGNED_INT, {maxSequencesPerBatch},
                                      {di, "seqOffsets"});
  Tensor tLengths = graph.addVariable(UNSIGNED_INT, {maxSequencesPerBatch},
                                      {di, "seqLengths"});
  graph.setTileMapping(tOffsets, 0);
  graph.setTileMapping(tLengths, 0);

  // Incoming symbols.
  // TBD: map these more realistically.
  Tensor tBatchIn = graph.addVariable(HALF, {maxTokensPerBatch, nFeatures},
                                      {di, "batchTokensIn"});
  poputil::mapTensorLinearly(graph, tBatchIn);

  Tensor qkvWeights = poplin::createMatMulInputRHS(
      graph, HALF, HALF, {maxTokensPerBatch, nFeatures},
      {nFeatures, 3 * nHeads * nHidden}, {di, "QKVWeights"});

  // Project. qkv={maxTokensPerBatch, nHeads * 3 * nHidden}
  Tensor qkv = poplin::matMul(graph, tBatchIn, qkvWeights, prog, HALF,
                              {di, "input projection qkv=batchIn*qkvWeights"});
  auto qkvGroupings = poputil::detectDimGroupings(graph, qkv);
  std::cerr << "qkv shape " << qkv.shapeToString() << ", expr " << qkv << "\n";
  for (const auto &grouping : qkvGroupings)
    std::cerr << "qkv grouping " << grouping.first << " : " << grouping.second
              << "\n";

  // Ignore masking.
  qkv = qkv.reshape({maxTokensPerBatch, nHeads, 3, nHidden});
  Tensor q = qkv.slice({0, 0, 0, 0}, {maxTokensPerBatch, nHeads, 1, nHidden});
  Tensor k = qkv.slice({0, 0, 1, 0}, {maxTokensPerBatch, nHeads, 2, nHidden});
  Tensor v = qkv.slice({0, 0, 2, 0}, {maxTokensPerBatch, nHeads, 3, nHidden});

  // Apply attention on nSequencesPerInnerLoop at a time.
  // Loop counter.
  auto seqIdx = graph.addVariable(UNSIGNED_INT, {1}, {di, "seqIdx"});
  auto one = graph.addConstant(UNSIGNED_INT, {1}, 1, {di, "one"});
  graph.setTileMapping(one, 0);
  graph.setTileMapping(seqIdx, 0);
  popops::zero(graph, seqIdx, prog, {di, "initSeqIdx"});
  Sequence loop;
  DebugContext loopDi{di, "Attention Inner Loop"};
  // Calculate slicing indices and lengths.
  // Note this is taking a single segment per seq, it needs to be 2 to support
  // masking.
  std::vector<unsigned> innerOffsets;
  for (unsigned i{0}; i != nSequencesPerInnerLoop; ++i)
    innerOffsets.emplace_back(i * maxTokensPerInnerLoop);

  if (verbose)
    std::cerr << "tOffsets shape " << tOffsets.shapeToString() << "\n";

  Tensor tBatchOffsets =
      popops::dynamicSlice(
          graph,
          tOffsets.reshape({maxSequencesPerBatch / nSequencesPerInnerLoop,
                            nSequencesPerInnerLoop}),
          seqIdx, {0}, {1}, loop, {loopDi, "sliceOffsets"})
          .flatten();
  Tensor tInnerLengths =
      popops::dynamicSlice(
          graph,
          tLengths.reshape({maxSequencesPerBatch / nSequencesPerInnerLoop,
                            nSequencesPerInnerLoop}),
          seqIdx, {0}, {1}, loop, {loopDi, "sliceLengths"})
          .flatten();

  // Slice inputs.
  Tensor matMulQInput = poplin::createMatMulGroupedInputLHS(
      graph, HALF, HALF,
      {nHeads * nSequencesPerInnerLoop, maxTokensPerSequence, nHidden},
      {nHeads * nSequencesPerInnerLoop, nHidden, maxTokensPerSequence},
      {loopDi, "matMulQInput"});
  Tensor matMulKTInput = poplin::createMatMulGroupedInputRHS(
      graph, HALF, HALF,
      {nHeads * nSequencesPerInnerLoop, maxTokensPerSequence, nHidden},
      {nHeads * nSequencesPerInnerLoop, nHidden, maxTokensPerSequence},
      {loopDi, "matMulKInput"});
  Tensor matMulVInput = poplin::createMatMulGroupedInputLHS(
      graph, HALF, HALF,
      {nHeads * nSequencesPerInnerLoop, nHidden, maxTokensPerSequence},
      {nHeads * nSequencesPerInnerLoop, maxTokensPerSequence,
       maxTokensPerSequence},
      {loopDi, "matMulVInput"});
  Tensor tInnerOffsets =
      graph.addConstant(UNSIGNED_INT, {nSequencesPerInnerLoop},
                        innerOffsets.data(), {loopDi, "inner offsets"});
  graph.setTileMapping(tInnerOffsets, 0);
  // Rearrange the matmul inputs to make the nSequnecesPerInnerLoop and
  // tokensPerInnerLoop dimensions the outermost, then flatten them together.
  auto matMulQInputRearranged = matMulQInput
                                    .reshape({nSequencesPerInnerLoop, nHeads,
                                              maxTokensPerSequence, nHidden})
                                    .dimShuffle({0, 2, 1, 3})
                                    .flatten(0, 2);
  auto matMulKTInputRearranged = matMulKTInput
                                     .reshape({nSequencesPerInnerLoop, nHeads,
                                               nHidden, maxTokensPerSequence})
                                     .dimShuffle({0, 3, 1, 2})
                                     .flatten(0, 2);
  auto matMulVInputRearranged = matMulVInput
                                    .reshape({nSequencesPerInnerLoop, nHeads,
                                              nHidden, maxTokensPerSequence})
                                    .dimShuffle({0, 3, 1, 2})
                                    .flatten(0, 2);
  if (verbose) {
    std::cerr << "matMulQInput shape " << matMulQInput.shapeToString() << "\n";
    std::cerr << "q shape " << q.shapeToString() << "\n";
    std::cerr << "tInnerLengths shape " << tInnerLengths.shapeToString()
              << "\n";
    std::cerr << "tBatchOffsets shape " << tBatchOffsets.shapeToString()
              << "\n";
    std::cerr << "tInnerOffsets shape " << tInnerOffsets.shapeToString()
              << "\n";
    std::cerr << "k shape " << k.shapeToString() << "\n";
  }
  popops::sequenceSlice(graph, q, matMulQInputRearranged, tInnerLengths,
                        tBatchOffsets, tInnerOffsets, true, loop,
                        {loopDi, "getQ"});

  popops::sequenceSlice(graph, k, matMulKTInputRearranged, tInnerLengths,
                        tBatchOffsets, tInnerOffsets, true, loop,
                        {loopDi, "getK"});
  popops::sequenceSlice(graph, v, matMulVInputRearranged, tInnerLengths,
                        tBatchOffsets, tInnerOffsets, true, loop,
                        {loopDi, "getV"});

  // Grouped matmul to calculate q*kt {nHeads*nSequencesPerInnerLoop,
  // maxTokensPerSequence, maxTokensPerSequence}.
  Tensor qKt = poplin::matMulGrouped(graph, matMulQInput, matMulKTInput, loop,
                                     HALF, {loopDi, "qKt=q*Kt"});
  // Ignore masking step.
  // Ignore softmax and scaling steps.
  // Grouped matmul to calculate z=(q*kt)*v
  //   {nSequencesPerInnerLoop*nHeads, nHidden, maxTokensPerSequence}
  Tensor z = poplin::matMulGrouped(graph, matMulVInput, qKt, loop, HALF,
                                   {loopDi, "z=v*qKt"});
  z = z.reshape(
      {nSequencesPerInnerLoop, nHeads, nHidden, maxTokensPerSequence});
  //{nSequencesPerInnerLoop * maxTokensPerSequence, nHeads, nHidden}
  z = z.dimShuffle({0, 3, 1, 2}).flatten(0, 2);

  // Add z into zBatch
  Tensor zBatch = poplin::createMatMulInputLHS(
      graph, HALF, HALF, {maxTokensPerBatch, nHeads * nHidden},
      {nHeads * nHidden, nFeatures}, {di, "zBatch"});

  popops::fill(graph, zBatch, prog, 0u, {di, "zero output batch"});
  if (verbose) {
    std::cerr << "z shape " << z.shapeToString() << "\n";
    std::cerr << "zBatch shape " << zBatch.shapeToString() << "\n";
  }
  popops::sequenceSlice(graph, z, zBatch, tInnerLengths, tInnerOffsets,
                        tBatchOffsets, false, loop, {loopDi, "updateOutput"});
  popops::addInPlace(graph, seqIdx, one, loop, {loopDi, "+1"});
  prog.add(Repeat(nSequencesPerInnerLoop, loop, {loopDi, "loop"}));

  // Final projection matmul on the batch.
  Tensor wProj = poplin::createMatMulInputRHS(
      graph, HALF, HALF, {maxTokensPerBatch, nHeads * nHidden},
      {nHeads * nHidden, nFeatures}, {di, "WProjection"});
  if (verbose) {
    std::cerr << "zbatch" << zBatch.shapeToString() << "\n";
    std::cerr << "wProj" << wProj.shapeToString() << "\n";
  }
  Tensor tBatchOut =
      poplin::matMul(graph, zBatch, wProj, prog, HALF, {di, "output=z*wProj"});
  if (verbose) {
    std::cerr << "tBatchOut" << tBatchOut.shapeToString() << "\n";
  }

  graph.createHostWrite("offset", tOffsets);
  graph.createHostWrite("length", tLengths);
  graph.createHostWrite("tokensToIpu", tBatchIn);
  graph.createHostRead("tokensFromIpu", tBatchOut);

  std::cerr << "Create engine\n";
  Engine engine(graph, prog);
  device.bind([&](const Device &d) {
    std::cerr << "Load engine\n";
    engine.load(d);
    std::cerr << "Load params\n";
    unsigned hOffset[maxSequencesPerBatch] = {10, 0, 0, 0};
    unsigned hLength[maxSequencesPerBatch] = {5, 0, 10, 0};
    std::vector<float> hTokens(maxTokensPerBatch * nFeatures);
    engine.writeTensor("offset", hOffset, &hOffset[maxSequencesPerBatch]);
    engine.writeTensor("length", hLength, &hLength[maxSequencesPerBatch]);
    engine.writeTensor("tokensToIpu", hTokens.data(),
                       hTokens.data() + hTokens.size());
    engine.run();
    engine.readTensor("tokensFromIpu", hTokens.data(),
                      hTokens.data() + hTokens.size());
  });
}


// Copyright (c) 2015-2018 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "read_side_bias.hpp"

#include <algorithm>
#include <iterator>
#include <random>
#include <functional>
#include <cmath>
#include <cassert>

#include <boost/variant.hpp>

#include "io/variant/vcf_record.hpp"
#include "basics/aligned_read.hpp"
#include "core/types/allele.hpp"
#include "utils/maths.hpp"
#include "utils/genotype_reader.hpp"
#include "../facets/samples.hpp"
#include "../facets/read_assignments.hpp"

namespace octopus { namespace csr {

const std::string ReadSideBias::name_ = "RSB";

std::unique_ptr<Measure> ReadSideBias::do_clone() const
{
    return std::make_unique<ReadSideBias>(*this);
}

namespace {

struct PositionCounts
{
    unsigned head, tail;
};

bool overlaps_rhs(const Allele& allele, const AlignedRead& read)
{
    using D = GenomicRegion::Distance;
    return overlaps(allele, expand_lhs(mapped_region(read), -static_cast<D>(region_size(read) / 2)));
}

void update_counts(const Allele& allele, const AlignedRead& read, PositionCounts& counts)
{
    if (overlaps(allele, read)) {
        if (region_size(allele) >= region_size(read) / 2) {
            ++counts.head;
            ++counts.tail;
        } else if (overlaps_rhs(allele, read)) {
            if (is_forward_strand(read)) {
                ++counts.tail;
            } else {
                ++counts.head;
            }
        } else {
            if (is_forward_strand(read)) {
                ++counts.head;
            } else {
                ++counts.tail;
            }
        }
    }
}

auto compute_allele_positions(const Allele& allele, const ReadRefSupportSet& support)
{
    PositionCounts forward_counts {}, reverse_counts {};
    for (const auto& read : support) {
        if (is_forward_strand(read)) {
            update_counts(allele, read, forward_counts);
        } else {
            update_counts(allele, read, reverse_counts);
        }
    }
    return std::make_pair(forward_counts, reverse_counts);
}

double calculate_position_bias(const PositionCounts forward_counts, const PositionCounts reverse_counts,
                               const double tolerance = 0.5)
{
    assert(tolerance > 0.0 && tolerance < 1.0);
    const auto num_lhs = forward_counts.head + reverse_counts.tail;
    const auto num_rhs = forward_counts.tail + reverse_counts.head;
    const auto prob_lhs_biased = maths::beta_sf(static_cast<double>(num_lhs + 1), static_cast<double>(num_rhs + 1), 0.5 + tolerance / 2);
    const auto prob_rhs_biased = maths::beta_cdf(static_cast<double>(num_lhs + 1), static_cast<double>(num_rhs + 1), 0.5 - tolerance / 2);
    return prob_lhs_biased + prob_rhs_biased;
}

double calculate_position_bias(const Allele& allele, const ReadRefSupportSet& support)
{
    PositionCounts forward_counts, reverse_counts;
    std::tie(forward_counts, reverse_counts) = compute_allele_positions(allele, support);
    return calculate_position_bias(forward_counts, reverse_counts);
}

double calculate_position_bias(const AlleleSupportMap& support)
{
    double result {0};
    for (const auto& p : support) {
        auto bias = calculate_position_bias(p.first, p.second);
        result = std::max(result, bias);
    }
    return result;
}

} // namespace

Measure::ResultType ReadSideBias::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto& samples = get_value<Samples>(facets.at("Samples"));
    const auto& assignments = get_value<ReadAssignments>(facets.at("ReadAssignments"));
    std::vector<double> result {};
    result.reserve(samples.size());
    for (const auto& sample : samples) {
        std::vector<Allele> alleles; bool has_ref;
        std::tie(alleles, has_ref) = get_called_alleles(call, sample, true);
        if (!alleles.empty()) {
            const auto allele_support = compute_allele_support(alleles, assignments, sample);
            auto position_bias = calculate_position_bias(allele_support);
            result.push_back(position_bias);
        } else {
            result.push_back(0);
        }
    }
    return result;
}

Measure::ResultCardinality ReadSideBias::do_cardinality() const noexcept
{
    return ResultCardinality::num_samples;
}

const std::string& ReadSideBias::do_name() const
{
    return name_;
}

std::string ReadSideBias::do_describe() const
{
    return "Bias of variant side (head or tail half) in supporting reads";
}

std::vector<std::string> ReadSideBias::do_requirements() const
{
    return {"Samples", "ReadAssignments"};
}

} // namespace csr
} // namespace octopus


#ifndef BOOST_GEOMETRY_PROJECTIONS_TPEQD_HPP
#define BOOST_GEOMETRY_PROJECTIONS_TPEQD_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// 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 <boost/geometry/util/math.hpp>
#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/aasincos.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct tpeqd {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace tpeqd
    {
            template <typename T>
            struct par_tpeqd
            {
                T cp1, sp1, cp2, sp2, ccs, cs, sc, r2z0, z02, dlam2;
                T hz0, thz0, rhshz0, ca, sa, lp, lamc;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_tpeqd_spheroid : public base_t_fi<base_tpeqd_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_tpeqd<CalculationType> m_proj_parm;

                inline base_tpeqd_spheroid(const Parameters& par)
                    : base_t_fi<base_tpeqd_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par) {}

                // FORWARD(s_forward)  sphere
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    CalculationType t, z1, z2, dl1, dl2, sp, cp;

                    sp = sin(lp_lat);
                    cp = cos(lp_lat);
                    z1 = aacos(this->m_proj_parm.sp1 * sp + this->m_proj_parm.cp1 * cp * cos(dl1 = lp_lon + this->m_proj_parm.dlam2));
                    z2 = aacos(this->m_proj_parm.sp2 * sp + this->m_proj_parm.cp2 * cp * cos(dl2 = lp_lon - this->m_proj_parm.dlam2));
                    z1 *= z1;
                    z2 *= z2;
                    xy_x = this->m_proj_parm.r2z0 * (t = z1 - z2);
                    t = this->m_proj_parm.z02 - t;
                    xy_y = this->m_proj_parm.r2z0 * asqrt(4. * this->m_proj_parm.z02 * z2 - t * t);
                    if ((this->m_proj_parm.ccs * sp - cp * (this->m_proj_parm.cs * sin(dl1) - this->m_proj_parm.sc * sin(dl2))) < 0.)
                        xy_y = -xy_y;
                }

                // INVERSE(s_inverse)  sphere
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    CalculationType cz1, cz2, s, d, cp, sp;

                    cz1 = cos(boost::math::hypot(xy_y, xy_x + this->m_proj_parm.hz0));
                    cz2 = cos(boost::math::hypot(xy_y, xy_x - this->m_proj_parm.hz0));
                    s = cz1 + cz2;
                    d = cz1 - cz2;
                    lp_lon = - atan2(d, (s * this->m_proj_parm.thz0));
                    lp_lat = aacos(boost::math::hypot(this->m_proj_parm.thz0 * s, d) * this->m_proj_parm.rhshz0);
                    if ( xy_y < 0. )
                        lp_lat = - lp_lat;
                    /* lam--phi now in system relative to P1--P2 base equator */
                    sp = sin(lp_lat);
                    cp = cos(lp_lat);
                    lp_lat = aasin(this->m_proj_parm.sa * sp + this->m_proj_parm.ca * cp * (s = cos(lp_lon -= this->m_proj_parm.lp)));
                    lp_lon = atan2(cp * sin(lp_lon), this->m_proj_parm.sa * cp * s - this->m_proj_parm.ca * sp) + this->m_proj_parm.lamc;
                }

                static inline std::string get_name()
                {
                    return "tpeqd_spheroid";
                }

            };

            // Two Point Equidistant
            template <typename Parameters, typename T>
            inline void setup_tpeqd(Parameters& par, par_tpeqd<T>& proj_parm)
            {
                T lam_1, lam_2, phi_1, phi_2, A12, pp;

                /* get control point locations */
                phi_1 = pj_param(par.params, "rlat_1").f;
                lam_1 = pj_param(par.params, "rlon_1").f;
                phi_2 = pj_param(par.params, "rlat_2").f;
                lam_2 = pj_param(par.params, "rlon_2").f;
                if (phi_1 == phi_2 && lam_1 == lam_2)
                    BOOST_THROW_EXCEPTION( projection_exception(-25) );
                par.lam0 = adjlon(0.5 * (lam_1 + lam_2));
                proj_parm.dlam2 = adjlon(lam_2 - lam_1);
                proj_parm.cp1 = cos(phi_1);
                proj_parm.cp2 = cos(phi_2);
                proj_parm.sp1 = sin(phi_1);
                proj_parm.sp2 = sin(phi_2);
                proj_parm.cs = proj_parm.cp1 * proj_parm.sp2;
                proj_parm.sc = proj_parm.sp1 * proj_parm.cp2;
                proj_parm.ccs = proj_parm.cp1 * proj_parm.cp2 * sin(proj_parm.dlam2);
                proj_parm.z02 = aacos(proj_parm.sp1 * proj_parm.sp2 + proj_parm.cp1 * proj_parm.cp2 * cos(proj_parm.dlam2));
                proj_parm.hz0 = .5 * proj_parm.z02;
                A12 = atan2(proj_parm.cp2 * sin(proj_parm.dlam2),
                    proj_parm.cp1 * proj_parm.sp2 - proj_parm.sp1 * proj_parm.cp2 * cos(proj_parm.dlam2));
                proj_parm.ca = cos(pp = aasin(proj_parm.cp1 * sin(A12)));
                proj_parm.sa = sin(pp);
                proj_parm.lp = adjlon(atan2(proj_parm.cp1 * cos(A12), proj_parm.sp1) - proj_parm.hz0);
                proj_parm.dlam2 *= .5;
                proj_parm.lamc = geometry::math::half_pi<T>() - atan2(sin(A12) * proj_parm.sp1, cos(A12)) - proj_parm.dlam2;
                proj_parm.thz0 = tan(proj_parm.hz0);
                proj_parm.rhshz0 = .5 / sin(proj_parm.hz0);
                proj_parm.r2z0 = 0.5 / proj_parm.z02;
                proj_parm.z02 *= proj_parm.z02;
                par.es = 0.;
            }

    }} // namespace detail::tpeqd
    #endif // doxygen

    /*!
        \brief Two Point Equidistant projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Projection parameters
         - lat_1: Latitude of first standard parallel (degrees)
         - lon_1 (degrees)
         - lat_2: Latitude of second standard parallel (degrees)
         - lon_2 (degrees)
        \par Example
        \image html ex_tpeqd.gif
    */
    template <typename CalculationType, typename Parameters>
    struct tpeqd_spheroid : public detail::tpeqd::base_tpeqd_spheroid<CalculationType, Parameters>
    {
        inline tpeqd_spheroid(const Parameters& par) : detail::tpeqd::base_tpeqd_spheroid<CalculationType, Parameters>(par)
        {
            detail::tpeqd::setup_tpeqd(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::tpeqd, tpeqd_spheroid, tpeqd_spheroid)

        // Factory entry(s)
        template <typename CalculationType, typename Parameters>
        class tpeqd_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<tpeqd_spheroid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        inline void tpeqd_init(detail::base_factory<CalculationType, Parameters>& factory)
        {
            factory.add_to_factory("tpeqd", new tpeqd_entry<CalculationType, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_TPEQD_HPP


//
// Created by nvidia on 6/25/18.
//

//
// Created by nvidia on 6/22/18.
//

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/ximgproc.hpp>
#include <algorithm> // for copy
#include <iterator> // for ostream_iterator
#include <boost/graph/graph_utility.hpp>
#include "../lib/SpVec.hpp"
#include "../lib/superpixel_seg.hpp"
#include "../lib/distance.hpp"
#include "../lib/FloodFillSsdSuperpixelAlgorithm.hpp"
#include "../lib/RTreesClassifier.hpp"
#include "../lib/util.hpp"

using namespace cv;
using namespace std;
using namespace cv::ximgproc;
using namespace cove;

int main(int argc, char** argv) {
#ifdef DEBUG
    namedWindow("params", 1);
#endif
    ALGORITHM_PARAM(blurKernel, 3, 10)
    ALGORITHM_PARAM(iterations, 10, 20)
    ALGORITHM_PARAM(blurDev, 0, 100)
    ALGORITHM_PARAM(regionSize, 7, 50)
    ALGORITHM_PARAM(ratio, 70, 200)
    ALGORITHM_PARAM(cannyLow, 70, 150)
    ALGORITHM_PARAM(ssdEpsilonSlider, 10, 250)

    int frameno = 0;
    Mat frame;

    RTreesClassifier classifier;
//    Ptr<ml::RTrees> model;
//    model = ml::StatModel::load<ml::RTrees>("RTREES");
    if( classifier.loadModelFromPath("RTREES") ) {
        cout << "The classifier " << "RTREES" << " is loaded.\n";
    } else {
        cout << "Could not read the classifier " << "RTREES" << endl;
        return 1;
    }

    FloodFillSsdSuperpixelAlgorithm algorithm(80, 60, regionSize, static_cast<float>(ssdEpsilonSlider) / 10, blurKernel, blurDev, iterations,
                                              static_cast<float>(ratio) / 1000, cannyLow, cannyLow*2.5);

    if (argc < 2) {
        cout << "specify in arg[1] a camera id" << endl;
        return 1;
    }

    VideoCapture cap(stoi(argv[1], nullptr, 10));
    for (;;) {
        clock_t start=CLOCK();

#ifdef DEBUG
        if (blurKernel % 2 == 0) {
            blurKernel++;
        }
#endif

        cap >> frame;
        if (frame.empty()) {
            cout << "frame empty" << endl;
            break;
        }

        Mat floorMask;
        Mat sizedFrame;
        Mat coloredSizedFrame;

        auto super = algorithm.getSuperPixels(frame, floorMask, sizedFrame);
        cvtColor(sizedFrame, coloredSizedFrame, COLOR_BGR2Lab);
        super->iterate(iterations);
        super->enforceLabelConnectivity(15);
        Mat spxLabels;
        super->getLabels(spxLabels);
        classifier.getFloorMask(spxLabels, coloredSizedFrame, super->getNumberOfSuperpixels(), floorMask);

        dilate(floorMask, floorMask, Mat(), Point(-1, -1), 2);
        erode(floorMask, floorMask, Mat(), Point(-1, -1), 2);

        util::overlayMask(sizedFrame, floorMask, Scalar(0,0,255), sizedFrame);
        SHOW_ALWAYS("mask", sizedFrame);

        // =========== time fps and exit ===========
        double dur = CLOCK()-start;
        printf("avg time per frame %f ms. fps %f. frameno = %d\n",avgdur(dur),avgfps(),frameno++ );

        int key = waitKey(10);
        if (key == 27) {
            break;
        }
    }
    return 0;
}


// Copyright (c) 2000, 2001, 2002, 2003 by David Scherer and others.
// Copyright (c) 2003, 2004 by Jonathan Brandmeyer and others.
// See the file license.txt for complete license terms.
// See the file authors.txt for a complete list of contributors.

#include "ring.hpp"
#include "util/displaylist.hpp"
#include "util/errors.hpp"
#include "util/gl_enable.hpp"

#include <utility>
#include <boost/scoped_array.hpp>
using boost::scoped_array;

namespace cvisual {

bool
ring::degenerate()
{
	return radius == 0.0;
}

ring::ring()
	: thickness(0.0), model_rings(-1)
{
}

ring::~ring()
{
}

void
ring::set_thickness( double t)
{
	thickness = t;
}

double
ring::get_thickness()
{
	return thickness;
}

void
ring::gl_pick_render(view& scene)
{
	gl_render(scene);
}

void
ring::gl_render(view& scene)
{
	if (degenerate())
		return;
	// Level of detail estimation.  See sphere::gl_render().

	// The number of subdivisions around the hoop's radial direction.
	double band_coverage = (thickness ? scene.pixel_coverage( pos, thickness)
			: scene.pixel_coverage(pos, radius*0.1));
	if (band_coverage<0) band_coverage = 1000;
	int bands = static_cast<int>( sqrt(band_coverage * 4.0) );
	bands = clamp( 4, bands, 40);
	// The number of subdivions around the hoop's tangential direction.
	double ring_coverage = scene.pixel_coverage( pos, radius);
	if (ring_coverage<0) ring_coverage = 1000;
	int rings = static_cast<int>( sqrt(ring_coverage * 4.0) );
	rings = clamp( 4, rings, 80);

	if (model_rings != rings || model_bands != bands || model_radius != radius || model_thickness != thickness) {
		model_rings = rings; model_bands = bands; model_radius = radius; model_thickness = thickness;
		create_model( rings, bands, model );
	}

	clear_gl_error();
	{
		gl_enable_client vertex_array( GL_VERTEX_ARRAY);
		gl_enable_client normal_array( GL_NORMAL_ARRAY);

		gl_matrix_stackguard guard;
		model_world_transform( scene.gcf, vector(radius,radius,radius) ).gl_mult();

		color.gl_set(opacity);

		glVertexPointer( 3, GL_FLOAT, 0, &model.vertex_pos[0] );
		glNormalPointer( GL_FLOAT, 0, &model.vertex_normal[0] );
		glDrawElements( GL_TRIANGLES, model.indices.size(), GL_UNSIGNED_SHORT, &model.indices[0] );
	}

	check_gl_error();
	return;
}

void
ring::grow_extent( extent& world)
{
	if (degenerate())
		return;
	// TODO: Not perfectly accurate (a couple more circles would help)
	vector a = axis.norm();
	double t = thickness ? thickness : radius * .1;
	world.add_circle( pos, a, radius + t );
	world.add_circle( pos + a*t, a, radius );
	world.add_circle( pos - a*t, a, radius );
	world.add_body();
}

void
ring::create_model( int rings, int bands, class model& m )
{
	// In Visual 3, rendered thickness was (incorrectly) double what was documented.
	// The documentation said that thickness was the diameter of a cross section of
	// a solid part of the ring, but in fact ring.thickness was the radius of the
	// cross section. Presumably we have to maintain the incorrect Visual 3 behavior
	// and change the documentation.
	double scaled_radius = 1.0;
	double scaled_thickness = 0.2;
	if (thickness != 0.0) scaled_thickness = 2*thickness / radius;

	// First generate a circle of radius thickness in the xy plane
	if (bands > 80) throw std::logic_error("ring::create_model: More bands than expected.");
	vector circle[80];
	circle[0] = vector(0,scaled_thickness*0.5,0);
	tmatrix rotator = rotation( 2.0 * M_PI / bands, vector( 0,0,1 ), vector( 0,0,0 ) );
	for (int i = 1; i < bands; i ++)
		circle[i] = rotator * circle[i-1];

	m.vertex_pos.resize( rings * bands );
	m.vertex_normal.resize( rings * bands );
	fvertex* vertexes = &m.vertex_pos[0];
	fvertex* normals = &m.vertex_normal[0];

	// ... and then sweep it in a circle around the x axis
	vector radial = vector(0,1,0);
	int i=0;
	rotator = rotation( 2.0 * M_PI / rings, vector( 1,0,0 ), vector( 0,0,0 ) );
	for(int r=0; r<rings; r++) {
		for(int b=0; b<bands; b++, i++) {
			normals[i].x = circle[b].x;
			normals[i].y = radial.y * circle[b].y;
			normals[i].z = radial.z * circle[b].y;
			vertexes[i].x = normals[i].x;
			vertexes[i].y = normals[i].y + radial.y;
			vertexes[i].z = normals[i].z + radial.z;
		}
		radial = rotator * radial;
	}

	// Now generate triangle indices... could do this with triangle strips but I'm looking
	// ahead to next renderer design, where it would be nice to always use indexed tris
	m.indices.resize( rings*bands*6 );
	unsigned short *ind = &m.indices[0];
	i = 0;
	for(int r=0; r<rings; r++) {
		for(int b=0; b<bands; b++,i++,ind+=6) {
			ind[0] = i; ind[1] = i+bands; ind[2] = i+1;
			ind[3] = i+bands; ind[4] = i+bands+1; ind[5] = i+1;
		}
		ind[2-6] -= bands;
		ind[4-6] -= bands;
		ind[5-6] -= bands;
	}
	ind -= 6*bands;
	for(int b=0; b<bands; b++,ind+=6) {
		ind[1] -= rings*bands;
		ind[3] -= rings*bands;
		ind[4] -= rings*bands;
	}
}

void
ring::get_material_matrix(const view&, tmatrix& out) {
	out.translate( vector(.5,.5,.5) );
	out.scale( vector(radius,radius,radius) * (.5 / (radius+thickness)) );
}

PRIMITIVE_TYPEINFO_IMPL(ring)

} // !namespace cvisual


///////////////////////////////////////////////////////////////////////////////
/// \file statistics.hpp
/// Includes all of the Statistical Accumulators Library
//
//  Copyright 2005 Eric Niebler. 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_ACCUMULATORS_STATISTICS_HPP_EAN_01_17_2006
#define BOOST_ACCUMULATORS_STATISTICS_HPP_EAN_01_17_2006

#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/covariance.hpp>
#include <boost/accumulators/statistics/density.hpp>
#include <boost/accumulators/statistics/error_of.hpp>
#include <boost/accumulators/statistics/error_of_mean.hpp>
#include <boost/accumulators/statistics/extended_p_square.hpp>
#include <boost/accumulators/statistics/extended_p_square_quantile.hpp>
#include <boost/accumulators/statistics/kurtosis.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/moment.hpp>
#include <boost/accumulators/statistics/peaks_over_threshold.hpp>
#include <boost/accumulators/statistics/pot_tail_mean.hpp>
#include <boost/accumulators/statistics/pot_quantile.hpp>
#include <boost/accumulators/statistics/p_square_cumulative_distribution.hpp>
#include <boost/accumulators/statistics/p_square_quantile.hpp>
#include <boost/accumulators/statistics/skewness.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/tail.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>
#include <boost/accumulators/statistics/tail_mean.hpp>
#include <boost/accumulators/statistics/tail_variate.hpp>
#include <boost/accumulators/statistics/tail_variate_means.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/weighted_covariance.hpp>
#include <boost/accumulators/statistics/weighted_density.hpp>
#include <boost/accumulators/statistics/weighted_kurtosis.hpp>
#include <boost/accumulators/statistics/weighted_extended_p_square.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>
#include <boost/accumulators/statistics/weighted_median.hpp>
#include <boost/accumulators/statistics/weighted_moment.hpp>
#include <boost/accumulators/statistics/weighted_peaks_over_threshold.hpp>
#include <boost/accumulators/statistics/weighted_p_square_cumulative_distribution.hpp>
#include <boost/accumulators/statistics/weighted_p_square_quantile.hpp>
#include <boost/accumulators/statistics/weighted_skewness.hpp>
#include <boost/accumulators/statistics/weighted_sum.hpp>
#include <boost/accumulators/statistics/weighted_tail_quantile.hpp>
#include <boost/accumulators/statistics/weighted_tail_mean.hpp>
#include <boost/accumulators/statistics/weighted_tail_variate_means.hpp>
#include <boost/accumulators/statistics/weighted_variance.hpp>
#include <boost/accumulators/statistics/with_error.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>

#endif


#pragma once

#if defined(__clang__)
#define STRINGIFY(x) #x
#define DECLARE_MATRIX_SUM_REDUCTION(Scalar,Rows,Cols) _Pragma(STRINGIFY(omp declare reduction (+: Eigen::Matrix<Scalar,Rows,Cols>: omp_out = omp_out + omp_in) initializer(omp_priv = Eigen::Matrix<Scalar,Rows,Cols>::Zero(omp_orig.rows(), omp_orig.cols()))))
#define MATRIX_SUM_REDUCTION(Scalar,Rows,Cols,var) reduction (+: var)
#elif defined(_MSC_VER)
// Never lose hope... :)
#define DECLARE_MATRIX_SUM_REDUCTION(Scalar,Rows,Cols) __pragma(omp declare reduction (+: Eigen::Matrix<Scalar,Rows,Cols>: omp_out = omp_out + omp_in) initializer(omp_priv = Eigen::Matrix<Scalar,Rows,Cols>::Zero(omp_orig.rows(), omp_orig.cols())))
#define MATRIX_SUM_REDUCTION(Scalar,Rows,Cols,var) reduction (+: var)
#else
#include <Eigen/Dense>

namespace cilantro {
    namespace internal {
        template <typename ScalarT, ptrdiff_t NRows, ptrdiff_t NCols>
        struct MatrixReductions {
#pragma omp declare reduction (+: Eigen::Matrix<ScalarT,NRows,NCols>: omp_out = omp_out + omp_in) initializer(omp_priv = Eigen::Matrix<ScalarT,NRows,NCols>::Zero(omp_orig.rows(), omp_orig.cols()))
        };
    }
}

#define DECLARE_MATRIX_SUM_REDUCTION(Scalar,Rows,Cols)
#define MATRIX_SUM_REDUCTION(Scalar,Rows,Cols,var) reduction (cilantro::internal::MatrixReductions<Scalar,Rows,Cols>::operator+: var)
#endif


//Author:Camilo Perez
//Date:Oct 22 2015

#include <ros/ros.h>
#include "std_msgs/String.h"
#include "normal_surface_calc2/targetPoints.h"
#include "rgb_visualization/pathData.h"

#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/octree/octree.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/integral_image_normal.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>

#include <Eigen/Geometry>
#include <Eigen/Dense>
#include "math_helper.h"
#include "string_convertor.h"

#define MAX_ITERATION    100
#define RESOLUTION       0.02   //Octree Resolution
using namespace std;
using namespace cv;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));

pcl::PointXYZ hitPoint;
pcl::PointXYZ initialPoint;
pcl::PointXYZ finalPoint;

pcl::PointXYZ targetPoint;
pcl::PointXYZ normalPoint;
pcl::PointXYZ trackingPoint;
pcl::PointXYZ robotEndEffector;
pcl::PointXYZ robotEndEffector_point;
pcl::PointXYZ kinect_robot_x_axis;
pcl::PointXYZ kinect_robot_y_axis;
pcl::PointXYZ kinect_robot_z_axis;
pcl::PointXYZ kinect_robot_zero;
std::vector<pcl::PointXYZ> xyz_path_coordinate;
std::vector<pcl::PointXYZ> xyz_path_normal;
std::vector<pcl::PointXYZ> xyz_path_normal_point;

//pcl::PointXYZ robot; //CP

Eigen::Quaternionf robot_orientation;
Eigen::Vector3f output3_vector, z_input_vector;
Eigen::Matrix4f transformationMatrix, inverse_transformationMatrix;
Eigen::Vector4f robot_position, kinect_robot_position,kinect_robot_normal_point, robot_normal_point, robot_normal_vector; //robot_orientation,z_vector
Eigen::Vector4f kinect_robot_frame_reference_zero,kinect_robot_frame_reference_x,kinect_robot_frame_reference_y,kinect_robot_frame_reference_z,robot_frame_reference_x,robot_frame_reference_y,robot_frame_reference_z, robot_frame_reference_zero;
int target = 0, saving_flag = 0, path_size=0;
float u = 100, v = 100, z = 0, uTarget = 100, vTarget = 100;   //depth of u and v coordinate

float linePoint[4][3] = {0.0, 0.0, 0.0};
std::vector<int> u_path;
std::vector<int> v_path;
bool new_path=false, new_path_robot=false;
std::vector<Eigen::Vector4f> path_robot_v;
std::vector<Eigen::Vector4f> path_robot_r;

std::vector<Eigen::Vector4f> normals_robot_v;
std::vector<Eigen::Vector4f> normals_robot_r;

Eigen::Vector3f normals_robot_r_normalized;
vector< vector <Point> > vPtSignature;

vector< vector <path_point> > getBackStrokes(vector < vector < Point> > _inputPoints, normal_surface_calc2::targetPoints msg)
{
  vector< vector <path_point> > rtVects;
    int strokesNum=_inputPoints.size();
    int counter=0;
    for(int i=0;i<strokesNum;i++)
    {
       vector<path_point> thisStroke;
       for(int j=0;j<_inputPoints[i].size();j++)
       {
           Point3d _p(msg.path_robot[counter].x, msg.path_robot[counter].y, msg.path_robot[counter].z);
           Point3d _n(msg.normals_robot[i].x, msg.normals_robot[i].y, msg.normals_robot[i].z);
           path_point thisP(_p,_n);
           thisStroke.push_back(thisP);
           counter++;
       }
       rtVects.push_back(thisStroke);
     }
     return rtVects;
}

void signature_data_callback(const std_msgs::String::ConstPtr& msg)
{
    if(msg->data=="reset")
    {
       vPtSignature.clear();
       return ;
    }
    vector<string> strokeStrs=string_convertor::split(msg->data, ';');//get different strokes.
    size_t strokesNum=strokeStrs.size();
    if(strokesNum>0)
    {
         vPtSignature.clear();
        vector<vector<Point> >().swap(vPtSignature);
        for(int i=0;i<strokesNum;i++)
        {
          string thisStroke=strokeStrs[i];
          vector<double> points=string_convertor::fromString2Array(thisStroke);
          size_t pointNum=points.size()/2;
          vector<Point> strokePoints;
          for(int j=0;j<pointNum;j++)
              strokePoints.push_back(Point(points[2*j],points[2*j+1]));
          vPtSignature.push_back(strokePoints);
        }
        //published=false;
        u =0;
        v = 0;
        uTarget = 0;
        vTarget = 0;
        if(saving_flag==0) {
          //READ new normal data
          saving_flag = 1;
          u_path.clear();
          v_path.clear();
          u_path=math_helper::getU_Path(vPtSignature);
          v_path=math_helper::getV_Path(vPtSignature);
          path_size=u_path.size();
          new_path=true;
        }
    }
}

Eigen::Matrix4f read_transform_matrix() {
    std::ifstream transform_file;
    std::string kinova_workspace = getenv("CALIBRATION_WORKSPACE");
    std::string transform_file_path = kinova_workspace + "/kinect_wam_transform.txt";
    transform_file.open(transform_file_path.c_str(), std::ios_base::in | std::ios_base::binary);

    if(!transform_file) {
        std::cerr << "Can't open transform file" << std::endl;
        std::exit(-1);
    }

    std::string line;
    Eigen::Matrix4f transform_matrix;
    int i = 0;
    while(getline(transform_file, line) && i < 4) {
        std::istringstream in(line);
        float c1, c2, c3, c4;
        in >> c1 >> c2 >> c3 >> c4;

        transform_matrix(i, 0) = c1;
        transform_matrix(i, 1) = c2;
        transform_matrix(i, 2) = c3;
        transform_matrix(i, 3) = c4;
        ++i;
    }

    //std::cout << transform_matrix <<std::endl;
    return transform_matrix;
    //    return transform_matrix.inverse();
}

void wamPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& poseMessage) {
  robot_orientation.x() =-poseMessage->pose.orientation.x;
  robot_orientation.y() =-poseMessage->pose.orientation.y;
  robot_orientation.z() =-poseMessage->pose.orientation.z;
  robot_orientation.w() =poseMessage->pose.orientation.w;
  robot_position.x()=poseMessage->pose.position.x;
    robot_position.y()=poseMessage->pose.position.y;
    robot_position.z()=poseMessage->pose.position.z;
robot_position.w()=1.0;

output3_vector=robot_orientation._transformVector(z_input_vector);
 robot_normal_vector.x()=0.2*output3_vector.x();
 robot_normal_vector.y()=0.2*output3_vector.y();
 robot_normal_vector.z()=0.2*output3_vector.z();
 robot_normal_vector.w()=1.0;
robot_normal_point=robot_position+robot_normal_vector;
//This is to keep homogeneous coordinates.
robot_normal_point.w()=1.0;
}

void chatterCallback(const rgb_visualization::pathData::ConstPtr& msg2) {
  u = msg2->x;
  v = msg2->y;
  uTarget = msg2->targetx;
  vTarget = msg2->targety;
  if(msg2->savingFlag == 1 && saving_flag==0) {
    //READ new normal data
    saving_flag = 1;
    path_size=msg2->u_path.size();

    u_path.resize(msg2->u_path.size());
    v_path.resize(msg2->v_path.size());

    for (int i = 0; i < path_size; i++) {
      u_path[i]=msg2->u_path[i];
      v_path[i]=msg2->v_path[i];
      //    std::cout << "(u,v)=" << u_path[i]<<" , "<<v_path[i]<< std::endl;
    }
    new_path=true;
  }
  else if(msg2->savingFlag==0)
    {
      saving_flag=0;
    }

}

void callback(const PointCloud::ConstPtr& msg) {
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (RESOLUTION);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);  //pointcloud filter
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);

    /////////////////////////////////
    // Create the filtering object
    /////////////////////////////////
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(msg);
    pass.setFilterFieldName("x");
    pass.setFilterLimits(-1.5, 0.9);
    pass.setKeepOrganized( true );
    pass.filter(*cloud_filtered);

    //std::cerr << "Cloud after filtering: " << std::endl;

    pcl::IntegralImageNormalEstimation<pcl::PointXYZ,pcl::Normal>ne;
    ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT);//COVARIANCE_MATRIX
    ne.setMaxDepthChangeFactor(0.02f);
    ne.setNormalSmoothingSize(10.0f);
    ne.setInputCloud(cloud_filtered);
    ne.compute(*normals);
    //int colIndex=u;
    //int rowIndex=v;
    // pcl::Normal normal;
    //ne.computePointNormal((int)u,(int)v,normal);
    //std::cerr << "Cloud after filtering: " << std::endl;

    //CP:
    if(new_path)
      {
	xyz_path_coordinate.resize(path_size);
	xyz_path_normal.resize(path_size);
	xyz_path_normal_point.resize(path_size);

	//resize robot vectors
	path_robot_v.resize(path_size);
	path_robot_r.resize(path_size);
	normals_robot_v.resize(path_size);
	normals_robot_r.resize(path_size);

	for(int i = 0; i <path_size; i++) {
	  xyz_path_coordinate[i]=cloud_filtered->points[v_path[i] * cloud_filtered->width + u_path[i]];

  //	  xyz_path_normal[i]=normals->points[v_path[i]* cloud_filtered->width + u_path[i]];
	  xyz_path_normal[i].x=normals->points[v_path[i]* cloud_filtered->width + u_path[i]].normal_x;
	  xyz_path_normal[i].y=normals->points[v_path[i]* cloud_filtered->width + u_path[i]].normal_y;
	  xyz_path_normal[i].z=normals->points[v_path[i]* cloud_filtered->width + u_path[i]].normal_z;

	  //	   xyz_path_normal_point[i]=xyz_path_coordinate+0.2*xyz_path_normal;
	  xyz_path_normal_point[i].x=xyz_path_coordinate[i].x+0.2*xyz_path_normal[i].x;
	  xyz_path_normal_point[i].y=xyz_path_coordinate[i].y+0.2*xyz_path_normal[i].y;
	  xyz_path_normal_point[i].z=xyz_path_coordinate[i].z+0.2*xyz_path_normal[i].z;

	}

	//CP--

	//std::cerr << "Fill eigen vector: " << std::endl;
	//Fill the eigen vector
	for (int h = 0; h < path_size; h++) {
	  path_robot_v[h].x()=xyz_path_coordinate[h].x;
	  path_robot_v[h].y()=xyz_path_coordinate[h].y;
	  path_robot_v[h].z()=xyz_path_coordinate[h].z;
	  path_robot_v[h].w()=1.0;
	  //Normals to homogeneous representation
	  normals_robot_v[h].x()=xyz_path_normal[h].x;
	  normals_robot_v[h].y()=xyz_path_normal[h].y;
	  normals_robot_v[h].z()=xyz_path_normal[h].z;
	  normals_robot_v[h].w()=1.0;

	}

	//std::cerr << "matrix multipl: " << std::endl;

	//transform kinect path points into robot path points
	for (int k = 0; k < path_size; k++) {
	  path_robot_r[k]=transformationMatrix*path_robot_v[k];
	  //std::cout << "This is the coordinate in the  robot frame of reference" << path_robot_r[k] << std::endl;

	  //CP Working
	  normals_robot_r[k]=transformationMatrix*normals_robot_v[k];
	  //std::cout << "this is the normal in the robot frame of reference" << normals_robot_r[k] << std::endl;

	}
	new_path_robot=true;
	//std::cerr << "TERMINO matrixmultp: " << std::endl;

	new_path=false;
      }

//

    trackingPoint = cloud_filtered->points[(int)v * cloud_filtered->width + (int)u];
    targetPoint = cloud_filtered->points[(int)vTarget * cloud_filtered->width + (int)uTarget];

    float x_normal=normals->points[(int)vTarget * cloud_filtered->width + (int)uTarget].normal_x;

    float y_normal=normals->points[(int)vTarget * cloud_filtered->width + (int)uTarget].normal_y;

    float z_normal=normals->points[(int)vTarget * cloud_filtered->width + (int)uTarget].normal_z;

    //std::cerr << "This is the x component of the normal: "<<x_normal << std::endl;
    normalPoint=targetPoint;

    normalPoint.x=targetPoint.x+x_normal*0.2;
    normalPoint.y=targetPoint.y+y_normal*0.2;
    normalPoint.z=targetPoint.z+z_normal*0.2;

    //std::cout << "Here is the robot Position" << robot_position << std::endl;
    kinect_robot_position=inverse_transformationMatrix*robot_position;
    kinect_robot_normal_point=inverse_transformationMatrix*robot_normal_point; //CP

    //adding robot frame inside the kinect frame

    //end adding robot frame inside the kinect frame

    //std::cout << "Here is the kinect robot Position" << kinect_robot_position << std::endl;

    robotEndEffector.x=kinect_robot_position.x();
    robotEndEffector.y=kinect_robot_position.y();
    robotEndEffector.z=kinect_robot_position.z();

    robotEndEffector_point.x=kinect_robot_normal_point.x();
    robotEndEffector_point.y=kinect_robot_normal_point.y();
    robotEndEffector_point.z=kinect_robot_normal_point.z();

    std::basic_string<char> name = "arrow";
    std::basic_string<char> name2 = "arrow2";

    std::basic_string<char> name3 = "robot_frame_reference_x";
    std::basic_string<char> name4 = "robot_frame_reference_y";
    std::basic_string<char> name5 = "robot_frame_reference_z";

    //viewer->addPointCloud<pcl::PointXYZ>(cloud_filtered, "sample cloud2");
    viewer->addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud_filtered,normals,100,0.02,"sample cloud2",0);

    for (int j = 0; j < xyz_path_coordinate.size(); j++) {
      viewer->addSphere( xyz_path_coordinate[j], 0.015, 0.0, 1.0, 0.0, "point"+boost::lexical_cast<std::string>(j));
      viewer->addArrow<pcl::PointXYZ>( xyz_path_normal_point[j],xyz_path_coordinate[j],0.0,1.0,0.0,false,"normal"+boost::lexical_cast<std::string>(j));
    }

    viewer->addSphere(robotEndEffector, 0.025, 1.0, 0.0, 0.0, "robotEndEffector");
    viewer->addSphere(targetPoint, 0.025, 1.0, 0.0, 0.0, "targetPoint");
    viewer->addArrow<pcl::PointXYZ>(normalPoint,targetPoint,1.0,0.0,0.0,false,name);
    viewer->addArrow<pcl::PointXYZ>(robotEndEffector_point,robotEndEffector,1.0,0.0,0.0,false,name2);
  viewer->addArrow<pcl::PointXYZ>(kinect_robot_x_axis,kinect_robot_zero,1.0,0.0,0.0,false,name3);
  viewer->addArrow<pcl::PointXYZ>(kinect_robot_y_axis,kinect_robot_zero,1.0,0.0,0.0,false,name4);
  viewer->addArrow<pcl::PointXYZ>(kinect_robot_z_axis,kinect_robot_zero,1.0,0.0,0.0,false,name5);

    viewer->spinOnce(100);
    viewer->removePointCloud("sample cloud2");
    viewer->removeShape("targetPoint");
    for (int j = 0; j < xyz_path_coordinate.size(); j++) {
   viewer->removeShape("point"+boost::lexical_cast<std::string>(j));
   viewer->removeShape("normal"+boost::lexical_cast<std::string>(j));
    }

    viewer->removeShape("robotEndEffector");
    viewer->removeShape("arrow2");
    viewer->removeShape("arrow");
    viewer->removeShape("robot_frame_reference_x");
    viewer->removeShape("robot_frame_reference_y");
    viewer->removeShape("robot_frame_reference_z");

}

int main(int argc, char** argv) {
    ros::init(argc, argv, "getPointCloud");
    ros::NodeHandle nh;

    ros::Subscriber sub2 = nh.subscribe("path_data", 100, chatterCallback);
    ros::Subscriber sub3 = nh.subscribe("/zeus/wam/pose", 1, wamPoseCallback);
    ros::Publisher chatter_pub3 = nh.advertise<normal_surface_calc2::targetPoints>("targetPoints", 100);
    ros::Subscriber sub4 = nh.subscribe("/chris/strokes", 1000, signature_data_callback);
    //ros::spinOnce();
    ros::Publisher pubTask2 = nh.advertise<std_msgs::String>("/chris/final_drawing_task", 1, true);//task will be only published once

    viewer->setBackgroundColor(0, 0, 0);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();

    ros::Subscriber sub = nh.subscribe<PointCloud>("camera/depth_registered/points", 1, callback); //camera/depth_registered/points

    ros::Rate loop_rate(10);
    ROS_INFO("Main loop Normal_surface_calc");

    transformationMatrix = read_transform_matrix();
    //std::cout << "Here is the transformation matrix" << transformationMatrix << std::endl;
    inverse_transformationMatrix=transformationMatrix.inverse();
    //std::cout << "Here is the inverse transformation matrix" << inverse_transformationMatrix << std::endl;
    /*z_vector.x()=0;
    z_vector.y()=0;
    z_vector.z()=1;
    z_vector.w()=0;
    */

    z_input_vector.x()=0;
    z_input_vector.y()=0;
    z_input_vector.z()=1;

    //robot_frame_reference
    robot_frame_reference_x.x()=0.2;
    robot_frame_reference_x.y()=0.0;
    robot_frame_reference_x.z()=0.0;
    robot_frame_reference_x.w()=1.0;

    robot_frame_reference_y.x()=0.0;
    robot_frame_reference_y.y()=0.2;
    robot_frame_reference_y.z()=0.0;
    robot_frame_reference_y.w()=1.0;

    robot_frame_reference_z.x()=0.0;
    robot_frame_reference_z.y()=0.0;
    robot_frame_reference_z.z()=0.2;
    robot_frame_reference_z.w()=1.0;

    robot_frame_reference_zero.x()=0.0;
    robot_frame_reference_zero.y()=0.0;
    robot_frame_reference_zero.z()=0.0;
    robot_frame_reference_zero.w()=1.0;

    kinect_robot_frame_reference_x=inverse_transformationMatrix*robot_frame_reference_x;
    kinect_robot_frame_reference_y=inverse_transformationMatrix*robot_frame_reference_y;
    kinect_robot_frame_reference_z=inverse_transformationMatrix*robot_frame_reference_z;
    kinect_robot_frame_reference_zero=inverse_transformationMatrix*robot_frame_reference_zero;

    kinect_robot_x_axis.x=kinect_robot_frame_reference_x.x();
    kinect_robot_x_axis.y=kinect_robot_frame_reference_x.y();
    kinect_robot_x_axis.z=kinect_robot_frame_reference_x.z();

    kinect_robot_y_axis.x=kinect_robot_frame_reference_y.x();
    kinect_robot_y_axis.y=kinect_robot_frame_reference_y.y();
    kinect_robot_y_axis.z=kinect_robot_frame_reference_y.z();

    kinect_robot_z_axis.x=kinect_robot_frame_reference_z.x();
    kinect_robot_z_axis.y=kinect_robot_frame_reference_z.y();
    kinect_robot_z_axis.z=kinect_robot_frame_reference_z.z();

    kinect_robot_zero.x=kinect_robot_frame_reference_zero.x();
    kinect_robot_zero.y=kinect_robot_frame_reference_zero.y();
    kinect_robot_zero.z=kinect_robot_frame_reference_zero.z();
    /*
    //CP:ERASE
  robot_orientation.x() =0.0;// poseMessage->pose.orientation.x;
  robot_orientation.y() =0.0; //poseMessage->pose.orientation.y;
  robot_orientation.z() =0.0; //poseMessage->pose.orientation.z;
  robot_orientation.w() =1.0; //poseMessage->pose.orientation.w;
  robot_position.x()=0.3;//poseMessage->pose.position.x;
  robot_position.y()=0.0;//poseMessage->pose.position.y;
  robot_position.z()=0.4;//poseMessage->pose.position.z;
robot_position.w()=1.0;

output3_vector=robot_orientation._transformVector(z_input_vector);
// output3_vector=robot_orientation.toRotationMatrix()*z_input_vector;

std::cout << "Here is the ZZZ vector after multiply by rotation" << output3_vector << std::endl;
 robot_normal_vector.x()=0.2*output3_vector.x();
 robot_normal_vector.y()=0.2*output3_vector.y();
 robot_normal_vector.z()=0.2*output3_vector.z();
 robot_normal_vector.w()=1.0;
//robot_normal_vector=robot_orientation.conjugate()*z_vector*robot_orientation;
robot_normal_point=robot_position+robot_normal_vector;
 robot_normal_point.w()=1.0;

std::cout << "Esto debe ser 0.6 en Z" << robot_normal_point << std::endl;

//END ERASE
*/

    while(ros::ok()) {
        normal_surface_calc2::targetPoints msg_targetPoints;

	/*ROS_INFO("u:%f \n", u);
	ROS_INFO("u:%f \n", v);
        ROS_INFO("uTarget:%f \n", uTarget);
        ROS_INFO("vTarget:%f \n", vTarget);
        ROS_INFO("Tracking Point(x, y, z):(%f, %f, %f) \n", trackingPoint.x, trackingPoint.y, trackingPoint.z);
        ROS_INFO("Target Point(x, y, z):(%f, %f, %f) \n", targetPoint.x, targetPoint.y, targetPoint.z);
	*/
	//	std::cout << "Murio antes del resize" <<std::endl;

	msg_targetPoints.path_robot.resize(path_size);
	msg_targetPoints.normals_robot.resize(path_size);
	//std::cout << "Murio despues del resize" <<std::endl;

	for (int i = 0; i < path_size; i++) {
	  //CP--
	  //std::cout << "Murio antes de asignar" <<std::endl;
	  if(new_path_robot)
	    {
	    //std::cout << "Este es la coordenada en robot" << path_robot_r[i] << std::endl;
	      msg_targetPoints.path_robot[i].x=path_robot_r[i].x();
	      msg_targetPoints.path_robot[i].y=path_robot_r[i].y();
	      msg_targetPoints.path_robot[i].z=path_robot_r[i].z();

	      /* msg_targetPoints.path_robot[i].x=1.0;
		 msg_targetPoints.path_robot[i].y=1.0;
		 msg_targetPoints.path_robot[i].z=1.0;*/

	      // std::cout << "Este es el vector antes de normalizar" << normals_robot_r[i] << std::endl;
	      //normals_robot_r_normalized = normals_robot_r[i].normalized();
	      normals_robot_r_normalized.x()=normals_robot_r[i].x();
	      normals_robot_r_normalized.y()=normals_robot_r[i].y();
	      normals_robot_r_normalized.z()=normals_robot_r[i].z();
	      normals_robot_r_normalized=normals_robot_r_normalized.normalized();
	      //std::cout << "Este es el vector despues de normalizar" << normals_robot_r_normalized << std::endl;
	      //msg_targetPoints.normals_robot[i].x=normals_robot_r[i].x();
	      //msg_targetPoints.normals_robot[i].y=normals_robot_r[i].y();
	      //msg_targetPoints.normals_robot[i].z=normals_robot_r[i].z();

	      //Uncomment this if you want dynamic  normals

	      msg_targetPoints.normals_robot[i].x=normals_robot_r_normalized.x();
	      msg_targetPoints.normals_robot[i].y=normals_robot_r_normalized.y();
	      msg_targetPoints.normals_robot[i].z=normals_robot_r_normalized.z();

	      //Uncomment this if you want fix normals in z direction
	      /* msg_targetPoints.normals_robot[i].x=0;
	      msg_targetPoints.normals_robot[i].y=0;
	      msg_targetPoints.normals_robot[i].z=1;
	      */

	    }

	}

        if(pcl::isFinite(targetPoint) || pcl::isFinite(trackingPoint)) {
            if(pcl::isFinite(targetPoint)) {
                msg_targetPoints.targetPoint[0] = targetPoint.x;
                msg_targetPoints.targetPoint[1] = targetPoint.y;
                msg_targetPoints.targetPoint[2] = targetPoint.z;
            }
            if(pcl::isFinite(trackingPoint)) {
                msg_targetPoints.trackingPoint[0] = trackingPoint.x;
                msg_targetPoints.trackingPoint[1] = trackingPoint.y;
                msg_targetPoints.trackingPoint[2] = trackingPoint.z;
            }
            chatter_pub3.publish(msg_targetPoints);
            vector< vector <path_point> > destPoints=getBackStrokes(vPtSignature,msg_targetPoints);
            std_msgs::String msg;
            msg.data = string_convertor::constructPubStr(destPoints);
            pubTask2.publish(msg);
            //published=true;
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
}


#include <iostream>
// #include <iterator>
// #include <algorithm>
#include<fstream>
//#include <boost/graph/graphml.hpp>
//#include <boost/graph/graphviz.hpp>
#include <boost/config.hpp>
#include <boost/version.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/static_assert.hpp>
#include <boost/graph/property_maps/null_property_map.hpp>
#include <boost/graph/random.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
//#include <boost/graph/dijkstra_shortest_paths.hpp>
//#include <boost/graph/betweenness_centrality.hpp>

using namespace boost;

#include "search/Graph.hpp"
#include "search/LexDFS.hpp"
#include "search/CommunityDetection.hpp"
#include "search/Parameters.hpp"
#include "search/CommonIntervalDetection.hpp"
#include "search/EdgeWeightDetection.hpp"

int main(int argc, char* argv[])
{
  Parameters::analyse_args(argc, argv);

  /*
    Read file
   */
  std::ifstream infile;
  infile.open(Parameters::getInputGraphFile(), std::ifstream::in);
  Graph g;
  //Reads edgelist file
  read_edgelist(infile, g);
  infile.close();

  infile.open(Parameters::getDendFile(), std::ifstream::in);
  std::string line;
  std::vector<std::pair<long, long> > merges;
  while(getline(infile, line)) {
    long i, j;
    sscanf(line.c_str(), "%li %li", &i, &j);
    merges.push_back(std::make_pair(i, j));
  }

  g[boost::graph_bundle].m = num_edges(g);
  g[boost::graph_bundle].n = num_vertices(g);
  long max_id = g[boost::graph_bundle].n;
  //std::cout << "n : " << g[boost::graph_bundle].n << " m : " << g[boost::graph_bundle].m << std::endl;

  //Keeps tracks of which community id is which
  std::map<long, Community*> id_to_community;
  std::vector<Community*> map_vertex_community(g[boost::graph_bundle].n);
  std::pair<vertex_iter, vertex_iter> vp;
  for (vp = vertices(g); vp.first != vp.second; ++vp.first) {
    Community* c = new Community(*vp.first, g);
    init_experiments_create(*c);
    map_vertex_community[*vp.first] = c;
    id_to_community[*vp.first] = c;
  }
  init_experiments();

  for(long i = 0; i < merges.size(); i++) {
    Community *c1, *c2;
    long id1 = merges[i].first, id2 = merges[i].second;
    c1 = id_to_community[id1];
    c2 = id_to_community[id2];
    if(c1->size < c2->size) {
      Community *temp = c1;
      c1 = c2;
      c2 = temp;
    }
    pre_merged_community_experiments(*c1, *c2, map_vertex_community);
    c1->merge(*c2, map_vertex_community);
    merged_community_experiments(*c1, map_vertex_community);
    id_to_community[max_id] = c1;
    max_id++;
  }
}