// Copyright 2015-2019 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <benchmark/benchmark.h>
#include <boost/histogram/axis/regular.hpp>
#include <boost/histogram/storage_adaptor.hpp>
#include <memory>
#include "../test/throw_exception.hpp"
#include "../test/utility_histogram.hpp"
#include "generator.hpp"

#include <cassert>
struct assert_check {
  assert_check() {
    assert(false); // don't run with asserts enabled
  }
} _;

using SStore = std::vector<double>;

// make benchmark compatible with older versions of the library
#if __has_include(<boost/histogram/unlimited_storage.hpp>)
#include <boost/histogram/unlimited_storage.hpp>
using DStore = boost::histogram::unlimited_storage<>;
#else
#include <boost/histogram/adaptive_storage.hpp>
using DStore = boost::histogram::adaptive_storage<>;
#endif

using namespace boost::histogram;
namespace op = boost::histogram::axis::option;
using reg = axis::regular<>;
using reg_closed =
    axis::regular<double, boost::use_default, boost::use_default, op::none_t>;

class reg_closed_unsafe {
public:
  reg_closed_unsafe(axis::index_type n, double start, double stop)
      : min_{start}, delta_{stop - start}, size_{n} {}

  axis::index_type index(double x) const noexcept {
    // Runs in hot loop, please measure impact of changes
    auto z = (x - min_) / delta_;
    // assume that z < 0 and z > 1 never happens, promised by inclusive()
    if (z == 1) return size() - 1;
    return static_cast<axis::index_type>(z * size());
  }

  axis::index_type size() const noexcept { return size_; }

  static constexpr bool inclusive() { return true; }

private:
  double min_;
  double delta_;
  axis::index_type size_;
};

template <class Distribution, class Tag, class Storage = SStore>
static void fill_1d(benchmark::State& state) {
  auto h = make_s(Tag(), Storage(), reg(100, 0, 1));
  auto gen = generator<Distribution>();
  for (auto _ : state) benchmark::DoNotOptimize(h(gen()));
  state.SetItemsProcessed(state.iterations());
}

template <class Distribution, class Tag, class Storage = SStore>
static void fill_1d_closed(benchmark::State& state) {
  auto h = make_s(Tag(), Storage(), reg_closed(100, 0, 1));
  auto gen = generator<Distribution>();
  for (auto _ : state) benchmark::DoNotOptimize(h(gen()));
  state.SetItemsProcessed(state.iterations());
}

template <class Distribution, class Tag, class Storage = SStore>
static void fill_1d_closed_unsafe(benchmark::State& state) {
  auto h = make_s(Tag(), Storage(), reg_closed_unsafe(100, 0, 1));
  auto gen = generator<Distribution>();
  for (auto _ : state) benchmark::DoNotOptimize(h(gen()));
  state.SetItemsProcessed(state.iterations());
}

template <class Distribution, class Tag, class Storage = SStore>
static void fill_n_1d(benchmark::State& state) {
  auto h = make_s(Tag(), Storage(), reg(100, 0, 1));
  auto gen = generator<Distribution>();
  for (auto _ : state) h.fill(gen);
  state.SetItemsProcessed(state.iterations() * gen.size());
}

template <class Distribution, class Tag, class Storage = SStore>
static void fill_n_1d_closed(benchmark::State& state) {
  auto h = make_s(Tag(), Storage(), reg_closed(100, 0, 1));
  auto gen = generator<Distribution>();
  for (auto _ : state) h.fill(gen);
  state.SetItemsProcessed(state.iterations() * gen.size());
}

template <class Distribution, class Tag, class Storage = SStore>
static void fill_n_1d_closed_unsafe(benchmark::State& state) {
  auto h = make_s(Tag(), Storage(), reg_closed_unsafe(100, 0, 1));
  auto gen = generator<Distribution>();
  for (auto _ : state) h.fill(gen);
  state.SetItemsProcessed(state.iterations() * gen.size());
}

template <class Distribution, class Tag, class Storage = SStore>
static void fill_2d(benchmark::State& state) {
  auto h = make_s(Tag(), Storage(), reg(100, 0, 1), reg(100, 0, 1));
  auto gen = generator<Distribution>();
  for (auto _ : state) benchmark::DoNotOptimize(h(gen(), gen()));
  state.SetItemsProcessed(state.iterations() * 2);
}

template <class Distribution, class Tag, class Storage = SStore>
static void fill_n_2d(benchmark::State& state) {
  auto h = make_s(Tag(), Storage(), reg(100, 0, 1), reg(100, 0, 1));
  auto gen = generator<Distribution>();
  auto v = {gen, gen};
  for (auto _ : state) h.fill(v);
  state.SetItemsProcessed(state.iterations() * 2 * gen.size());
}

template <class Distribution, class Tag, class Storage = SStore>
static void fill_3d(benchmark::State& state) {
  auto h = make_s(Tag(), Storage(), reg(100, 0, 1), reg(100, 0, 1), reg(100, 0, 1));
  auto gen = generator<Distribution>();
  for (auto _ : state) benchmark::DoNotOptimize(h(gen(), gen(), gen()));
  state.SetItemsProcessed(state.iterations() * 3);
}

template <class Distribution, class Tag, class Storage = SStore>
static void fill_n_3d(benchmark::State& state) {
  auto h = make_s(Tag(), Storage(), reg(100, 0, 1), reg(100, 0, 1), reg(100, 0, 1));
  auto gen = generator<Distribution>();
  auto v = {gen, gen, gen};
  for (auto _ : state) h.fill(v);
  state.SetItemsProcessed(state.iterations() * 3 * gen.size());
}

template <class Distribution, class Tag, class Storage = SStore>
static void fill_6d(benchmark::State& state) {
  auto h = make_s(Tag(), Storage(), reg(10, 0, 1), reg(10, 0, 1), reg(10, 0, 1),
                  reg(10, 0, 1), reg(10, 0, 1), reg(10, 0, 1));
  auto gen = generator<Distribution>();
  for (auto _ : state)
    benchmark::DoNotOptimize(h(gen(), gen(), gen(), gen(), gen(), gen()));
  state.SetItemsProcessed(state.iterations() * 6);
}

template <class Distribution, class Tag, class Storage = SStore>
static void fill_n_6d(benchmark::State& state) {
  auto h = make_s(Tag(), Storage(), reg(10, 0, 1), reg(10, 0, 1), reg(10, 0, 1),
                  reg(10, 0, 1), reg(10, 0, 1), reg(10, 0, 1));
  auto gen = generator<Distribution>();
  auto v = {gen, gen, gen, gen, gen, gen};
  for (auto _ : state) h.fill(v);
  state.SetItemsProcessed(state.iterations() * 6 * gen.size());
}

BENCHMARK_TEMPLATE(fill_1d, uniform, static_tag);
// BENCHMARK_TEMPLATE(fill_1d, uniform, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_1d, normal, static_tag);
// BENCHMARK_TEMPLATE(fill_1d, normal, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_1d, uniform, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_1d, uniform, dynamic_tag, DStore);
BENCHMARK_TEMPLATE(fill_1d, normal, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_1d, normal, dynamic_tag, DStore);
BENCHMARK_TEMPLATE(fill_1d_closed, uniform, static_tag);
BENCHMARK_TEMPLATE(fill_1d_closed_unsafe, uniform, static_tag);

BENCHMARK_TEMPLATE(fill_n_1d, uniform, static_tag);
// BENCHMARK_TEMPLATE(fill_n_1d, uniform, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_n_1d, normal, static_tag);
// BENCHMARK_TEMPLATE(fill_n_1d, normal, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_n_1d, uniform, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_n_1d, uniform, dynamic_tag, DStore);
BENCHMARK_TEMPLATE(fill_n_1d, normal, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_n_1d, normal, dynamic_tag, DStore);
BENCHMARK_TEMPLATE(fill_n_1d_closed, uniform, static_tag);
BENCHMARK_TEMPLATE(fill_n_1d_closed_unsafe, uniform, static_tag);

BENCHMARK_TEMPLATE(fill_2d, uniform, static_tag);
// BENCHMARK_TEMPLATE(fill_2d, uniform, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_2d, normal, static_tag);
// BENCHMARK_TEMPLATE(fill_2d, normal, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_2d, uniform, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_2d, uniform, dynamic_tag, DStore);
BENCHMARK_TEMPLATE(fill_2d, normal, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_2d, normal, dynamic_tag, DStore);

BENCHMARK_TEMPLATE(fill_n_2d, uniform, static_tag);
// BENCHMARK_TEMPLATE(fill_n_2d, uniform, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_n_2d, normal, static_tag);
// BENCHMARK_TEMPLATE(fill_n_2d, normal, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_n_2d, uniform, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_n_2d, uniform, dynamic_tag, DStore);
BENCHMARK_TEMPLATE(fill_n_2d, normal, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_n_2d, normal, dynamic_tag, DStore);

BENCHMARK_TEMPLATE(fill_3d, uniform, static_tag);
// BENCHMARK_TEMPLATE(fill_3d, uniform, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_3d, normal, static_tag);
// BENCHMARK_TEMPLATE(fill_3d, normal, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_3d, uniform, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_3d, uniform, dynamic_tag, DStore);
BENCHMARK_TEMPLATE(fill_3d, normal, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_3d, normal, dynamic_tag, DStore);

BENCHMARK_TEMPLATE(fill_n_3d, uniform, static_tag);
// BENCHMARK_TEMPLATE(fill_n_3d, uniform, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_n_3d, normal, static_tag);
// BENCHMARK_TEMPLATE(fill_n_3d, normal, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_n_3d, uniform, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_n_3d, uniform, dynamic_tag, DStore);
BENCHMARK_TEMPLATE(fill_n_3d, normal, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_n_3d, normal, dynamic_tag, DStore);

BENCHMARK_TEMPLATE(fill_6d, uniform, static_tag);
// BENCHMARK_TEMPLATE(fill_6d, uniform, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_6d, normal, static_tag);
// BENCHMARK_TEMPLATE(fill_6d, normal, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_6d, uniform, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_6d, uniform, dynamic_tag, DStore);
BENCHMARK_TEMPLATE(fill_6d, normal, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_6d, normal, dynamic_tag, DStore);

BENCHMARK_TEMPLATE(fill_n_6d, uniform, static_tag);
// BENCHMARK_TEMPLATE(fill_n_6d, uniform, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_n_6d, normal, static_tag);
// BENCHMARK_TEMPLATE(fill_n_6d, normal, static_tag, DStore);
BENCHMARK_TEMPLATE(fill_n_6d, uniform, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_n_6d, uniform, dynamic_tag, DStore);
BENCHMARK_TEMPLATE(fill_n_6d, normal, dynamic_tag);
// BENCHMARK_TEMPLATE(fill_n_6d, normal, dynamic_tag, DStore);


#include <iostream>
#include <string>

#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/preprocessor/seq/for_each.hpp>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/value_type/complex.hpp>
#include <amgcl/value_type/static_matrix.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/block_matrix.hpp>

#include <amgcl/solver/runtime.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/preconditioner/runtime.hpp>
#include <amgcl/make_solver.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/io/mm.hpp>
#include <amgcl/io/binary.hpp>

#include <amgcl/profiler.hpp>

#include "sample_problem.hpp"

#ifndef AMGCL_BLOCK_SIZES
#  define AMGCL_BLOCK_SIZES (2)(3)(4)
#endif

namespace amgcl { profiler<> prof; }
using amgcl::prof;
using amgcl::precondition;

//---------------------------------------------------------------------------
template <class Precond, class Matrix>
std::tuple<size_t, double> solve(
        const Matrix &A,
        const boost::property_tree::ptree &prm,
        std::vector< std::complex<double> > const &f,
        std::vector< std::complex<double> >       &x
        )
{
    typedef typename Precond::backend_type Backend;

    typedef typename amgcl::math::rhs_of<typename Backend::value_type>::type rhs_type;
    size_t n = amgcl::backend::rows(A);

    rhs_type const * fptr = reinterpret_cast<rhs_type const *>(&f[0]);
    rhs_type       * xptr = reinterpret_cast<rhs_type       *>(&x[0]);
    amgcl::iterator_range<rhs_type const *> frng(fptr, fptr + n);
    amgcl::iterator_range<rhs_type       *> xrng(xptr, xptr + n);

    typedef amgcl::make_solver<
        Precond,
        amgcl::runtime::solver::wrapper<Backend>
        > Solver;

    prof.tic("setup");
    Solver solve(A, prm);
    prof.toc("setup");

    std::cout << solve << std::endl;

    {
        auto t = prof.scoped_tic("solve");
        return solve(frng, xrng);
    }
}

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    namespace po = boost::program_options;
    namespace io = amgcl::io;

    using amgcl::prof;
    using std::vector;
    using std::string;

    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "Show this help.")
        ("prm-file,P",
         po::value<string>(),
         "Parameter file in json format. "
        )
        (
         "prm,p",
         po::value< vector<string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        ("matrix,A",
         po::value<string>(),
         "System matrix in the MatrixMarket format. "
         "When not specified, solves a Poisson problem in 3D unit cube. "
        )
        (
         "rhs,f",
         po::value<string>(),
         "The RHS vector in the MatrixMarket format. "
         "When omitted, a vector of ones is used by default. "
         "Should only be provided together with a system matrix. "
        )
        (
         "null,N",
         po::value<string>(),
         "The near null-space vectors in the MatrixMarket format. "
         "Should be a dense matrix of size N*M, where N is the number of "
         "unknowns, and M is the number of null-space vectors. "
         "Should only be provided together with a system matrix. "
        )
        (
         "binary,B",
         po::bool_switch()->default_value(false),
         "When specified, treat input files as binary instead of as MatrixMarket. "
         "It is assumed the files were converted to binary format with mm2bin utility. "
        )
        (
         "block-size,b",
         po::value<int>()->default_value(1),
         "The block size of the system matrix. "
         "When specified, the system matrix is assumed to have block-wise structure. "
         "This usually is the case for problems in elasticity, structural mechanics, "
         "for coupled systems of PDE (such as Navier-Stokes equations), etc. "
        )
        (
         "size,n",
         po::value<int>()->default_value(32),
         "The size of the Poisson problem to solve when no system matrix is given. "
         "Specified as number of grid nodes along each dimension of a unit cube. "
         "The resulting system will have n*n*n unknowns. "
        )
        (
         "single-level,1",
         po::bool_switch()->default_value(false),
         "When specified, the AMG hierarchy is not constructed. "
         "Instead, the problem is solved using a single-level smoother as preconditioner. "
        )
        (
         "initial,x",
         po::value<double>()->default_value(0),
         "Value to use as initial approximation. "
        )
        (
         "output,o",
         po::value<string>(),
         "Output file. Will be saved in the MatrixMarket format. "
         "When omitted, the solution is not saved. "
        )
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return 0;
    }

    boost::property_tree::ptree prm;
    if (vm.count("prm-file")) {
        read_json(vm["prm-file"].as<string>(), prm);
    }

    if (vm.count("prm")) {
        for(const string &v : vm["prm"].as<vector<string> >()) {
            amgcl::put(prm, v);
        }
    }

    size_t rows;
    vector<ptrdiff_t> ptr, col;
    vector< std::complex<double> > val, rhs, null, x;

    if (vm.count("matrix")) {
        auto t = prof.scoped_tic("reading");

        string Afile = vm["matrix"].as<string>();
        bool   binary = vm["binary"].as<bool>();

        if (binary) {
            io::read_crs(Afile, rows, ptr, col, val);
        } else {
            size_t cols;
            std::tie(rows, cols) = io::mm_reader(Afile)(ptr, col, val);
            precondition(rows == cols, "Non-square system matrix");
        }

        if (vm.count("rhs")) {
            string bfile = vm["rhs"].as<string>();

            size_t n, m;
            if (binary) {
                io::read_dense(bfile, n, m, rhs);
            } else {
                std::tie(n, m) = io::mm_reader(bfile)(rhs);
            }

            precondition(n == rows && m == 1, "The RHS vector has wrong size");
        } else {
            rhs.resize(rows, 1.0);
        }

        if (vm.count("null")) {
            string nfile = vm["null"].as<string>();

            size_t m, nv;

            if (binary) {
                io::read_dense(nfile, m, nv, null);
            } else {
                std::tie(m, nv) = io::mm_reader(nfile)(null);
            }

            precondition(m == rows, "Near null-space vectors have wrong size");

            prm.put("precond.coarsening.nullspace.cols", nv);
            prm.put("precond.coarsening.nullspace.rows", rows);
            prm.put("precond.coarsening.nullspace.B",    &null[0]);
        }
    } else {
        auto t = prof.scoped_tic("assembling");
        rows = sample_problem(vm["size"].as<int>(), val, col, ptr, rhs);
    }

    x.resize(rows, vm["initial"].as<double>());

    size_t iters;
    double error;

    if (vm["single-level"].as<bool>())
        prm.put("precond.class", "relaxation");

    int block_size = vm["block-size"].as<int>();

#define CALL_BLOCK_SOLVER(z, data, B)                                                    \
        case B:                                                                          \
            {                                                                            \
                typedef amgcl::static_matrix<std::complex<double>,B,B> value_type;       \
                typedef amgcl::backend::builtin<value_type> Backend;                     \
                std::tie(iters, error) = solve<amgcl::runtime::preconditioner<Backend>>( \
                        amgcl::adapter::block_matrix<value_type>(                        \
                            std::tie(rows, ptr, col, val)),                              \
                        prm, rhs, x);                                                    \
            }                                                                            \
            break;

    switch (block_size) {
        case 1:
            {
                typedef amgcl::backend::builtin<std::complex<double>> Backend;
                std::tie(iters, error) = solve<amgcl::runtime::preconditioner<Backend>>(
                        std::tie(rows, ptr, col, val), prm, rhs, x);
            }
            break;
        BOOST_PP_SEQ_FOR_EACH(CALL_BLOCK_SOLVER, ~, AMGCL_BLOCK_SIZES)
    }

#undef CALL_BLOCK_SOLVER

    if (vm.count("output")) {
        auto t = prof.scoped_tic("write");
        amgcl::io::mm_write(vm["output"].as<string>(), &x[0], x.size());
    }

    std::cout << "Iterations: " << iters << std::endl
              << "Error:      " << error << std::endl
              << prof << std::endl;
}


#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/common.h>
#include <thread>
#include <chrono>

#include <Eigen/Dense>

#include "vtkPlaneSource.h"

vtkSmartPointer<vtkPolyData> createPlane(const pcl::ModelCoefficients &coefficients, double x, double y, double z, double scale);

int main(int argc, char** argv){
	//cloud = ReadTxt("pontos.txt");
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inliers (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

	if(argc < 2){
		std:cerr << "insira o arquivo para carregar" << std::endl;
		return -1;
	}

	std::string filename (argv[1]);
	// std::cout << filename << std::endl;

	if ( pcl::io::loadPCDFile(filename,*cloud) == -1){
		PCL_ERROR("Nao deu pra abrir arquivo \n");
		return -1;
	}

	std::cout << "Size = " << cloud->size() << std::endl;
	pcl::PointXYZ min,max;
	pcl::getMinMax3D(*cloud,min,max);
	std::cout << "max x = " << max.x << std::endl;
	std::cout << "min x = " << min.x << std::endl;
	//Filtro "PassThrough"

	pcl::PointIndices filtered_indices;
	std::vector<int>indices;

	pcl::PointIndices innn;

	pcl::PassThrough<pcl::PointXYZ> pfilter;
	pfilter.setInputCloud(cloud);
	pfilter.setFilterFieldName("x");
	pfilter.setFilterLimits(max.x-2,max.x+2);
	pfilter.filter(*cloud_filtered);

	pcl::ModelCoefficients coefficients;
	pcl::PointIndices inliers;
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	seg.setOptimizeCoefficients(true);

	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setDistanceThreshold(1);

	// seg.setInputCloud(cloud);
	seg.setInputCloud(cloud_filtered); // vem do passthrough
	seg.segment(inliers,coefficients);

  if (inliers.indices.size () == 0)
  {
    PCL_ERROR ("Could not estimate a planar model for the given dataset.");
    return (-1);
  }

  std::cerr << "Model coefficients: " << coefficients.values[0] << " "
                                      << coefficients.values[1] << " "
                                      << coefficients.values[2] << " "
                                      << coefficients.values[3] << std::endl;

  std::cerr << "Model inliers: " << inliers.indices.size () << std::endl;
  for (std::size_t i = 0; i < inliers.indices.size (); ++i){
	  cloud_inliers->push_back(cloud->points[inliers.indices[i]]);

    // std::cerr << inliers.indices[i] << "    " << cloud->points[inliers.indices[i]].x << " "
    //                                            << cloud->points[inliers.indices[i]].y << " "
    //                                            << cloud->points[inliers.indices[i]].z << std::endl;

  }

double x0 = cloud_inliers->points[0].x;
double y0 = cloud_inliers->points[0].y;
double z0 = cloud_inliers->points[0].z;

pcl::ModelCoefficients coeff_test;
coeff_test.values.resize(4);
coeff_test.values[0] = 1;
coeff_test.values[1] = 1;
coeff_test.values[2] = 1;
coeff_test.values[3] = 0;

pcl::PointXYZ p1;
p1.x = cloud_inliers->points[0].x + coefficients.values[0];
p1.y = cloud_inliers->points[0].y + coefficients.values[1];
p1.z = cloud_inliers->points[0].z + coefficients.values[2];

// vtkSmartPointer<vtkPolyData> plane = createPlane(coefficients,x0,y0,z0,2); // 1 = cumprimento do eixo

pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor(0,0,0);
 viewer->addPointCloud<pcl::PointXYZ> (cloud, "input cloud");
 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud_inliers, 0, 255, 0); //pinta de verde
 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color_(cloud_filtered, 255, 0, 0); //pinta de vermelho
viewer->addPointCloud<pcl::PointXYZ> (cloud_inliers,single_color, "final cloud");
//viewer->addPointCloud<pcl::PointXYZ> (cloud_filtered,single_color_, "passthrough");
viewer->addPlane(coefficients,"plane");
//viewer->addPlane(coeff_test,"plane2");
//viewer->addSphere (cloud->points[0], 0.2, 0.5, 0.5, 0.0, "sphere");
//viewer->addArrow(p1,cloud_inliers->points[0],1,0,0,"arr");
//viewer->addModelFromPolyData(plane,"myplane");

 viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "input cloud");
 viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "final cloud");
 viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "passthrough");

 viewer->addCoordinateSystem (1.0);
 viewer->initCameraParameters();
 viewer->setCameraPosition(0,0,10,0,0,0,1);

 pcl::io::savePCDFile("filtered.pcd",*cloud_filtered,true);
 pcl::io::savePCDFile("inliers.pcd",*cloud_inliers,true);

	while(!viewer->wasStopped()){
	viewer->spinOnce (100);
	std::this_thread::sleep_for(std::chrono::milliseconds(100));

	}
	return 0;
}

vtkSmartPointer<vtkPolyData> createPlane(const pcl::ModelCoefficients &coefficients, double x, double y, double z, double scale = 1)
{
	vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New();

	double norm_sqr = 1.0 / (coefficients.values[0] * coefficients.values[0] +
		coefficients.values[1] * coefficients.values[1] +
		coefficients.values[2] * coefficients.values[2]);

	plane->SetNormal(coefficients.values[0], coefficients.values[1], coefficients.values[2]);
	double t = x * coefficients.values[0] + y * coefficients.values[1] + z * coefficients.values[2] + coefficients.values[3];
	x -= coefficients.values[0] * t * norm_sqr;
	y -= coefficients.values[1] * t * norm_sqr;
	z -= coefficients.values[2] * t * norm_sqr;

	//gerar dois vetores ortogonais à normal e entre si

	Eigen::Vector3d p1,p2;
	Eigen::Vector3d n;
	n.x() = x + coefficients.values[0];
	n.y() = y + coefficients.values[1];
	n.z() = z + coefficients.values[2];

	p1.x() = x+1.0; //pega um x arbritrário
	p1.y() = y;
	p1.z() = (coefficients.values[3] - coefficients.values[0]*p1.x() - coefficients.values[1]*p1.y())/coefficients.values[2];

	p2 = n.cross(p1);

	p1.normalize();
	p2.normalize();

	p1 = p1*scale;
	p2 = p2*scale;

	double point1[3];
	double point2[3];

	point1[0] = p1.x();
	point1[1] = p1.y();
	point1[2] = p1.z();

	point2[0] = p2.x();
	point2[1] = p2.y();
	point2[2] = p2.z();

	std::cout << "p1 = " << p1 << std::endl;
	std::cout << "p2 = " << p2 << std::endl;

	plane->SetOrigin(x, y, z);
	plane->SetPoint1(point1);
	plane->SetPoint2(point2);

	plane->Update();

	return (plane->GetOutput());
}


/*
 * RigidBody.cpp
 *
 *  Created on: July 18, 2019
 *      Author: Quincy Jones
 *
 * Copyright (c) <2019> <Quincy Jones - quincy@implementedrobotics.com/>
 * 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.
 */

// Primary Include
#include <Systems/RigidBody.hpp>

// C System Includes

// C++ System Includes
#include <iostream>
#include <string>

// Third Party Includes
#include <Eigen/Dense>

// Project Include Files
#include <OptimalControl/ControlsLibrary.hpp>

RigidBlock1D::RigidBlock1D() : LinearDynamicalSystem(2, 1, 1e-1),
                               mass_(1)
{
    length_ = 1;
    width_ = 1;
    height_ = 1;
}
RigidBlock1D::RigidBlock1D(const double &mass,
                           const Eigen::Vector3d &box_shape,
                           const double &T_s /* = 1e-1*/) : LinearDynamicalSystem(2, 1, T_s),
                                                            mass_(mass)
{
    //std::cout << "Num States: " << num_states << std::endl;
    length_ = box_shape[0];
    width_ = box_shape[1];
    height_ = box_shape[2];

    //std::cout << "TS: " << T_s << "AND: " << T_s_ << std::endl;
    // Setup initial states
    SetState(Eigen::VectorXd::Zero(num_states_));

    // Setup System Matrices
    A_ << 0, 1,
        0, 0;

    // Setup Input Matrix
    B_ << 0,
        1.0 / mass_;

    // Cache Discrete Time Variant
//    ControlsLibrary::ContinuousToDiscrete(A_, B_, T_s_, A_d_, B_d_);
}

void RigidBlock1D::Step(const Eigen::VectorXd &u)
{
    x_ = A_d_ * x_ + B_d_ * u;
}

void RigidBlock1D::Step(double u)
{
    x_ = A_d_ * x_ + B_d_ * u;
}

void RigidBlock1D::Update()
{
}