#include <ripple/basics/base_uint.h>
#include <ripple/basics/Blob.h>
#include <ripple/basics/hardened_hash.h>
#include <ripple/beast/unit_test.h>
#include <boost/algorithm/string.hpp>
#include <complex>

#include <type_traits>

namespace ripple {
namespace test {

template <std::size_t Bits>
struct nonhash
{
    static constexpr std::size_t WIDTH = Bits / 8;
    std::array<std::uint8_t, WIDTH> data_;
    static beast::endian const endian = beast::endian::big;

    nonhash() = default;

    void
    operator() (void const* key, std::size_t len) noexcept
    {
        assert(len == WIDTH);
        memcpy(data_.data(), key, len);
    }

    explicit
    operator std::size_t() noexcept { return WIDTH; }
};

struct base_uint_test : beast::unit_test::suite
{
    using test96 = base_uint<96>;
    static_assert(std::is_copy_constructible<test96>::value, "");
    static_assert(std::is_copy_assignable<test96>::value, "");

    void run() override
    {
        static_assert(!std::is_constructible<test96, std::complex<double>>::value, "");
        static_assert(!std::is_assignable<test96&, std::complex<double>>::value, "");
        std::unordered_set<test96, hardened_hash<>> uset;

        Blob raw { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12 };
        BEAST_EXPECT(test96::bytes == raw.size());

        test96 u { raw };
        uset.insert(u);
        BEAST_EXPECT(raw.size() == u.size());
        BEAST_EXPECT(to_string(u) == "0102030405060708090A0B0C");
        BEAST_EXPECT(*u.data() == 1);
        BEAST_EXPECT(u.signum() == 1);
        BEAST_EXPECT(!!u);
        BEAST_EXPECT(!u.isZero());
        BEAST_EXPECT(u.isNonZero());
        unsigned char t = 0;
        for (auto& d : u)
        {
            BEAST_EXPECT(d == ++t);
        }

        nonhash<96> h;
        hash_append(h, u);
        test96 w {std::vector<std::uint8_t>(h.data_.begin(), h.data_.end())};
        BEAST_EXPECT(w == u);

        test96 v { ~u };
        uset.insert(v);
        BEAST_EXPECT(to_string(v) == "FEFDFCFBFAF9F8F7F6F5F4F3");
        BEAST_EXPECT(*v.data() == 0xfe);
        BEAST_EXPECT(v.signum() == 1);
        BEAST_EXPECT(!!v);
        BEAST_EXPECT(!v.isZero());
        BEAST_EXPECT(v.isNonZero());
        t = 0xff;
        for (auto& d : v)
        {
            BEAST_EXPECT(d == --t);
        }

        BEAST_EXPECT(compare(u, v) < 0);
        BEAST_EXPECT(compare(v, u) > 0);

        v = u;
        BEAST_EXPECT(v == u);

        test96 z { beast::zero };
        uset.insert(z);
        BEAST_EXPECT(to_string(z) == "000000000000000000000000");
        BEAST_EXPECT(*z.data() == 0);
        BEAST_EXPECT(*z.begin() == 0);
        BEAST_EXPECT(*std::prev(z.end(), 1) == 0);
        BEAST_EXPECT(z.signum() == 0);
        BEAST_EXPECT(!z);
        BEAST_EXPECT(z.isZero());
        BEAST_EXPECT(!z.isNonZero());
        for (auto& d : z)
        {
            BEAST_EXPECT(d == 0);
        }

        test96 n { z };
        n++;
        BEAST_EXPECT(n == test96(1));
        n--;
        BEAST_EXPECT(n == beast::zero);
        BEAST_EXPECT(n == z);
        n--;
        BEAST_EXPECT(to_string(n) == "FFFFFFFFFFFFFFFFFFFFFFFF");
        n = beast::zero;
        BEAST_EXPECT(n == z);

        test96 zp1 { z };
        zp1++;
        test96 zm1 { z };
        zm1--;
        test96 x { zm1 ^ zp1 };
        uset.insert(x);
        BEAST_EXPECTS(to_string(x) == "FFFFFFFFFFFFFFFFFFFFFFFE", to_string(x));

        BEAST_EXPECT(uset.size() == 4);

        test96 fromHex;
        BEAST_EXPECT(fromHex.SetHexExact(to_string(u)));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;

        BEAST_EXPECT(! fromHex.SetHexExact("A" + to_string(u)));
        fromHex = z;

        BEAST_EXPECT(! fromHex.SetHexExact(to_string(u) + "A"));
        fromHex = z;

        BEAST_EXPECT(fromHex.SetHex(to_string(u)));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;

        BEAST_EXPECT(fromHex.SetHex("  0x" + to_string(u)));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;

        BEAST_EXPECT(fromHex.SetHex("FEFEFE" + to_string(u)));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;

        BEAST_EXPECT(! fromHex.SetHex(
            boost::algorithm::replace_all_copy(to_string(u), "0", "Z")));
        fromHex = z;

        BEAST_EXPECT(fromHex.SetHex(to_string(u), true));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;

        BEAST_EXPECT(! fromHex.SetHex("  0x" + to_string(u), true));
        fromHex = z;

        BEAST_EXPECT(fromHex.SetHex("DEAD" + to_string(u), true ));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;

        BEAST_EXPECT(fromHex.SetHex("DEAD" + to_string(u), false ));
        BEAST_EXPECT(fromHex == u);
        fromHex = z;
    }
};

BEAST_DEFINE_TESTSUITE(base_uint, ripple_basics, ripple);

}
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = ::boost::dispatch;
  namespace bs = ::boost::simd;

  //------------------------------------------------------------------------------------------------
  // splat from a scalar into a pack of 8 bits integers
  BOOST_DISPATCH_OVERLOAD ( splat_
                          , (typename Target, typename Value)
                          , bs::avx2_
                          , bd::scalar_< bd::unspecified_<Value> >
                          , bd::target_<bs::pack_<bd::ints8_<Target>,bs::avx_>>
                          )
  {
    using target = typename Target::type;

    BOOST_FORCEINLINE target operator()(Value const& v, Target const&) const BOOST_NOEXCEPT
    {
      return _mm256_set1_epi8(v);
    }
  };

  //------------------------------------------------------------------------------------------------
  // splat from a scalar into a pack of 16 bits integers
  BOOST_DISPATCH_OVERLOAD ( splat_
                          , (typename Target, typename Value)
                          , bs::avx2_
                          , bd::scalar_< bd::unspecified_<Value> >
                          , bd::target_<bs::pack_<bd::ints16_<Target>,bs::avx_>>
                          )
  {
    using target = typename Target::type;

    BOOST_FORCEINLINE target operator()(Value const& v, Target const&) const BOOST_NOEXCEPT
    {
      return _mm256_set1_epi16(v);
    }
  };

  //------------------------------------------------------------------------------------------------
  // splat from a scalar into a pack of 32 bits integers
  BOOST_DISPATCH_OVERLOAD ( splat_
                          , (typename Target, typename Value)
                          , bs::avx2_
                          , bd::scalar_< bd::unspecified_<Value> >
                          , bd::target_<bs::pack_<bd::ints32_<Target>,bs::avx_>>
                          )
  {
    using target = typename Target::type;

    BOOST_FORCEINLINE target operator()(Value const& v, Target const&) const BOOST_NOEXCEPT
    {
      return _mm256_set1_epi32(v);
    }
  };

  //------------------------------------------------------------------------------------------------
  // splat from a scalar into a pack of 64 bits integers
  BOOST_DISPATCH_OVERLOAD ( splat_
                          , (typename Target, typename Value)
                          , bs::avx2_
                          , bd::scalar_< bd::unspecified_<Value> >
                          , bd::target_<bs::pack_<bd::ints64_<Target>,bs::avx_>>
                          )
  {
    using target = typename Target::type;

    BOOST_FORCEINLINE target operator()(Value const& v, Target const&) const BOOST_NOEXCEPT
    {
      return _mm256_set1_epi64x(v);
    }
  };
} } }

#endif


/*
 * File:   main.cpp
 * Author: malone
 *
 * Created on August 6, 2012, 9:05 PM
 */

#include <string>
#include <iostream>
#include <ostream>
#include <vector>
#include <cstdlib>

#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include <boost/asio.hpp>
#include <boost/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include "urlearning/base/node.h"
#include "urlearning/base/typedefs.h"
#include "urlearning/score_cache/score_cache.h"

#include "urlearning/priority_queue/priority_queue.h"
#include "urlearning/fileio/hugin_structure_writer.h"

#include "urlearning/score_cache/best_score_calculator.h"
#include "urlearning/score_cache/best_score_creator.h"

#include "urlearning/heuristic/heuristic.h"
#include "urlearning/heuristic/heuristic_creator.h"

//#define RUN_GOBNILP

namespace po = boost::program_options;

/**
 * A timer to keep track of how long the algorithm has been running.
 */
boost::asio::io_service io;

/**
 * A variable to check if the user-specified time limit has expired.
 */
bool outOfTime;

/**
 * The path to the score cache file.
 */
std::string scoreFile;

/**
 * The data structure to use to calculate best parent set scores.
 * "tree", "list", "bitwise"
 */
std::string bestScoreCalculator;

/**
 * The type of heuristic to use.
 */
std::string heuristicType;

/**
 * The argument for creating the pattern database.
 */
std::string heuristicArgument;

/**
 * The file to write the learned network.
 */
std::string netFile;

/**
 * The ancestor-only variables for the search.
 * See UAI '14.
 */
std::string ancestorsArgument;       // csv list
varset ancestors;

/**
 * The variables for this search.
 */
std::string sccArgument;      //csv list
varset scc;

/**
 * The number of nodes expanded during the search.
 */
int nodesExpanded;

/**
 * The maximum running time for the algorithm.
 */
int runningTime;

/**
 * Handler when out of time.
 */
void timeout(const boost::system::error_code& /*e*/) {
    printf("Out of time\n");
    outOfTime = true;
}

std::vector<varset> reconstructSolution(Node *goal, std::vector<bestscorecalculators::BestScoreCalculator*> &spgs, NodeMap &closedList) {
    std::vector<varset> optimalParents;
    for (int i = 0; i < spgs.size(); i++) {
        optimalParents.push_back(VARSET(spgs.size()));
    }

    VARSET_COPY(goal->getSubnetwork(), remainingVariables);
    Node *current = goal;
    float score = 0;
    int count = cardinality(scc);
    for (int i = 0; i < count; i++) {
        int leaf = current->getLeaf();
        score += spgs[leaf]->getScore(remainingVariables);
        varset parents = spgs[leaf]->getParents();
        optimalParents[leaf] = parents;

        VARSET_CLEAR(remainingVariables, leaf);

        current = closedList[remainingVariables];
    }

    return optimalParents;
}

void astar() {
    printf("URLearning, A*\n");
    printf("Dataset: '%s'\n", scoreFile.c_str());
    printf("Net file: '%s'\n", netFile.c_str());
    printf("Best score calculator: '%s'\n", bestScoreCalculator.c_str());
    printf("Heuristic type: '%s'\n", heuristicType.c_str());
    printf("Heuristic argument: '%s'\n", heuristicArgument.c_str());
    printf("Ancestors: '%s'\n", ancestorsArgument.c_str());
    printf("SCC: '%s'\n", sccArgument.c_str());

    boost::timer::auto_cpu_timer act;

    printf("Reading score cache.\n");
    scoring::ScoreCache cache;
    cache.read(scoreFile);
    printf("Done reading score cache.\n");
    int variableCount = cache.getVariableCount();
    printf("Variable count is %d\n",variableCount);
    // parse the ancestor and scc variables
    VARSET_NEW(ancestors,variableCount);
    VARSET_NEW(scc,variableCount);

    // if no scc was specified, assume we just want to learn everything
    if (sccArgument == "") {
        VARSET_SET_ALL(scc, variableCount);
    } else {
        setFromCsv(scc, sccArgument);
        setFromCsv(ancestors, ancestorsArgument);
    }

    act.start();
    printf("Creating BestScore calculators.\n");
    std::vector<bestscorecalculators::BestScoreCalculator*> spgs = bestscorecalculators::create(bestScoreCalculator, cache);
    act.stop();
    act.report();

    act.start();
    printf("Creating heuristic.\n");
    heuristics::Heuristic *heuristic = heuristics::createWithAncestors(heuristicType, heuristicArgument, spgs, ancestors, scc);

#ifdef DEBUG
    heuristic->print();
#endif

    act.stop();
    act.report();

    NodeMap generatedNodes;
    init_map(generatedNodes);

    PriorityQueue openList;

    byte leaf(0);
    Node *root = new Node(0.0f, 0.0f, ancestors, leaf);
    openList.push(root);

#ifdef DEBUG
    printf("The start is '%s'\n", varsetToString(ancestors).c_str());

    bool c;
    float lb = heuristic->h(ancestors, c);

    printf("The lb is %.2f\n", lb);
#endif

    Node *goal = NULL;
    VARSET_NEW(allVariables, variableCount);
    VARSET_SET_VALUE(allVariables, ancestors);
    allVariables = VARSET_OR(allVariables, scc);

#ifdef DEBUG
    printf("The goal is '%s'\n", varsetToString(allVariables).c_str());
#endif

    float upperBound = std::numeric_limits<float>::max();
    bool complete = false;

    act.start();
    printf("Beginning search\n");
    nodesExpanded = 0;
    while (openList.size() > 0 && !outOfTime) {
        //printf("top of while\n");
        Node *u = openList.pop();
        //printf("Expanding: '%s', g: '%.2f'\n", varsetToString(u->getSubnetwork()).c_str(), u->getG());
        nodesExpanded++;

        //        if (nodesExpanded % 100 == 0) {
        //            std::cout << ".";
        //            std::cout.flush();
        //        }

        varset variables = u->getSubnetwork();

        // check if it is the goal
        if (variables == allVariables) {
            goal = u;
            break;
        }

        // TODO: this is mostly broken
        if (u->getF() > upperBound) {
            break;
        }

        // note that it is in the closed list
        u->setPqPos(-2);

        // expand
        for (byte leaf = 0; leaf < variableCount; leaf++) {
            // make sure this variable was not already present
            if (VARSET_GET(variables, leaf)) continue;

            // also make sure this is one of the variables we care about
            if (!VARSET_GET(scc, leaf)) continue;

            // get the new variable set
            VARSET_COPY(variables, newVariables);
            VARSET_SET(newVariables, leaf);

            Node *succ = generatedNodes[newVariables];

            // check if this is the first time we have generated this node
            if (succ == NULL) {
                // get the cost along this path

                //printf("About to check for using leaf '%d', newVariables: '%s'\n", leaf, varsetToString(newVariables).c_str());

                float g = u->getG() + spgs[leaf]->getScore(newVariables);
                //printf("I have g\n");
                // calculate the heuristic estimate
                complete = false;
                float h = heuristic->h(newVariables, complete);
                //printf("I have h: %.2f\n", h);

#ifdef RUN_GOBNILP
                std::string filename = "exclude_" + TO_STRING(newVariables) + ".pss";
                time_t t;
                double seconds;

                t = time(NULL);
                int count = cache.writeExclude(filename, newVariables);
                seconds = difftime(time(NULL), t);

                // check whether to execute or not
                if (count < 10000 && variableCount > 25) {

                    // now, call gobnilp on the subproblem
                    FILE *fpipe;
                    char buffer[1048576];
                    char cmd[4096];
                    int i;

                    t = time(NULL);
                    snprintf(cmd, sizeof (cmd), "gobnilp %s", filename.c_str());

                    if (0 == (fpipe = (FILE*) popen(cmd, "r"))) {
                        perror("popen() failed.");
                        exit(1);
                    }

                    while ((i = fread(buffer, sizeof (char), sizeof buffer, fpipe))) {

                        /* We found some solutions, so split them up and add the cutting planes. */

                        // make sure to clean up the buffer so it behaves as a string
                        buffer[i] = 0;

                        //printf("buffer: '%s'\n", buffer);

                        char *pch;
                        pch = strtok(buffer, "\n");

                        bool optimal = false;

                        while (pch != NULL) {

                            // check if this is a "Dual Bound         : -4.62524687000000e+02" line

                            // I believe this is the only line in the output which starts with a 'D'
                            // this is really brittle, though
                            if (pch[0] == 'D') {
                                std::vector<std::string> tokens;
                                boost::split(tokens, pch, boost::is_any_of(" "), boost::token_compress_on);
                                float score = -1 * atof(tokens[3].c_str());
                                printf("Node: %s, g: %f, (%s) h(gobnilp): %f, f(gobnilp): %f, h(pd): %f, f(pd): %f\n",
                                        varsetToString(newVariables).c_str(), g, (optimal ? "optimal" : "bound"), score, (score + g), h, (g + h));
                                h = score;
                            }

                            // also, check if this line contains the word "optimal"
                            // if so, then this is actually the globally optimal solution
                            if (strstr(pch, "optimal") != NULL) {
                                optimal = true;
                            }

                            pch = strtok(NULL, "\n");
                        }
                    }
                    pclose(fpipe);

                    remove(filename.c_str());

                    seconds = difftime(time(NULL), t);
                    printf("Time to solve and parse ip: %f(s)\n", seconds);
                }

#endif

#ifdef CHECK_COMPLETE
                if (complete) {
                    float score = g + h;
                    if (score < upperBound) {
                        upperBound = score;
                        printf("new upperBound: %f, nodes expanded: %d, open list size: %d\n", upperBound, nodesExpanded, openList.size());
                    }
                    continue;
                }
#endif

                // update all the values
                succ = new Node(g, h, newVariables, leaf);

                //printf("I have created succ\n");

                // add it to the open list
                openList.push(succ);

                //printf("pushed succ\n");

                // and to the list of generated nodes
                generatedNodes[newVariables] = succ;
                //printf("added succ to generatedNodes\n");
                continue;
            }

            // assume the heuristic is consistent
            // so check if it was in the closed list
            if (succ->getPqPos() == -2) {
                continue;
            }
            // so we have generated a node in the open list
            // see if the new path is better
            float g = u->getG() + spgs[leaf]->getScore(variables);
            if (g < succ->getG()) {
                // the update the information
                succ->setLeaf(leaf);
                succ->setG(g);

                // and the priority queue
                openList.update(succ);
            }
        }
    }
    act.stop();
    act.report();

    printf("Nodes expanded: %d, open list size: %d\n", nodesExpanded, openList.size());

    heuristic->printStatistics();

    if (goal != NULL) {
        //printf("Found solution: %f\n", goal->getF());
        printf("Found solution: %f\n", goal->getG());

        if (netFile.length() > 0) {

            datastructures::BayesianNetwork *network = cache.getNetwork();
            network->fixCardinalities();
            std::vector<varset> optimalParents = reconstructSolution(goal, spgs, generatedNodes);
            network->setParents(optimalParents);
            network->setUniformProbabilities();

            fileio::HuginStructureWriter writer;
            writer.write(network, netFile);
        }
    } else {
        Node *u = openList.pop();
        printf("No solution found.\n");
        printf("Lower bound: %f\n", u->getF());
    }

    for (auto spg = spgs.begin(); spg != spgs.end(); spg++) {
        delete *spg;
    }

    for (auto pair = generatedNodes.begin(); pair != generatedNodes.end(); pair++) {
        delete (pair->second);
    }

    delete heuristic;

    io.stop();
}

int main(int argc, char** argv) {
    boost::timer::auto_cpu_timer act;
    srand(time(NULL));

    std::string description = std::string("Learn an optimal Bayesian network using A*.  Example usage: ") + argv[0] + " iris.pss";
    po::options_description desc(description);

    desc.add_options()
            ("scoreFile", po::value<std::string > (&scoreFile)->required(), "The file containing the local scores in pss format. First positional argument.")
            ("bestScore,b", po::value<std::string > (&bestScoreCalculator)->default_value("list"), bestscorecalculators::bestScoreCalculatorString.c_str())
            ("heuristic,e", po::value<std::string > (&heuristicType)->default_value("static"), heuristics::heuristicTypeString.c_str())
            ("argument,a", po::value<std::string > (&heuristicArgument)->default_value("2"), heuristics::heuristicArgumentString.c_str())
            ("pc_{i-1},p", po::value<std::string > (&ancestorsArgument)->default_value(""), "Variables which can only be used as ancestors. They will not be added in the search. CSV-list of variable indices. See UAI '14.")
            ("scc_i,s", po::value<std::string > (&sccArgument)->default_value(""), "Variables which will be added in the search. CSV-list of variable indices. Leave blank to add all variables. See UAI '14.")
            ("runningTime,r", po::value<int> (&runningTime)->default_value(0), "The maximum running time for the algorithm.  0 means no running time.")
            ("netFile,n", po::value<std::string > (&netFile)->default_value(""), "The file to which the learned network is written.  Leave blank to not create the file.")
            ("help,h", "Show this help message.")
            ;

    po::positional_options_description positionalOptions;
    positionalOptions.add("scoreFile", 1);

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

    if (vm.count("help") || argc == 1) {
        std::cout << desc;
        return 0;
    }

    po::notify(vm);
    outOfTime = false;

    boost::to_lower(bestScoreCalculator);

    boost::asio::deadline_timer t(io);
    if (runningTime > 0) {
        printf("Maximum running time: %d\n", runningTime);
        t.expires_from_now(boost::posix_time::seconds(runningTime));
        t.async_wait(timeout);
        boost::thread workerThread(astar);
        io.run();
        workerThread.join();
    } else {
        astar();
    }

    return 0;
}


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

//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 <boost/qvm/scalar_traits.hpp>
#include <boost/qvm/vec.hpp>
#include <boost/qvm/mat.hpp>
#include <boost/qvm/quat.hpp>

template <bool>
struct tester;

template <>
struct
tester<true>
    {
    };

using namespace boost::qvm;
tester<is_scalar<char>::value> t1;
tester<is_scalar<signed char>::value> t2;
tester<is_scalar<unsigned char>::value> t3;
tester<is_scalar<signed short>::value> t4;
tester<is_scalar<unsigned short>::value> t5;
tester<is_scalar<signed int>::value> t6;
tester<is_scalar<unsigned int>::value> t7;
tester<is_scalar<signed long>::value> t8;
tester<is_scalar<unsigned long>::value> t9;
tester<is_scalar<float>::value> t10;
tester<is_scalar<double>::value> t11;
tester<is_scalar<long double>::value> t13;
tester<!is_scalar<vec<float,4> >::value> t14;
tester<!is_scalar<mat<float,4,4> >::value> t15;
tester<!is_scalar<quat<float> >::value> t16;


#include <vector>

#include <boost/shared_ptr.hpp>

#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
//#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d/calib3d.hpp>

// NOTE Also removes NaNs
template <typename PointT>
void load_cloud(std::string pcd_path, boost::shared_ptr<pcl::PointCloud<PointT> > cloud)
{
    if (pcl::io::loadPCDFile(pcd_path, *cloud) < 0)
    {
        std::cout << "Error loading input point cloud " << pcd_path << std::endl;
        throw;
    }
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
}

template <typename PointT>
void load_clouds(std::vector<std::string> pcd_paths, boost::shared_ptr<pcl::PointCloud<PointT> > cloud)
{
    BOOST_FOREACH(std::string pcd_path, pcd_paths)
    {
        pcl::PointCloud<PointXYZ>::Ptr new_cloud(new pcl::PointCloud<PointT>());
        load_cloud(pcd_path, new_cloud);
        *cloud += *new_cloud;
    }
}

template<typename PointT>
void align_clouds_viz(const boost::shared_ptr<pcl::PointCloud<PointT> > src_cloud, const boost::shared_ptr<pcl::PointCloud<PointT> > tgt_cloud, boost::shared_ptr<pcl::PointCloud<PointT> > aligned_cloud, const pcl::Correspondences& correspondences, bool viz_normals)
{
    pcl::visualization::PCLVisualizer viz("align clouds viz");
    //viz.addCoordinateSystem(3.0);

    // Add point clouds and normals

    pcl::visualization::PointCloudColorHandlerCustom<PointT> green(tgt_cloud, 0, 255, 0);
    viz.addPointCloud(tgt_cloud, green, "tgt_cloud");

    pcl::visualization::PointCloudColorHandlerCustom<PointT> blue(tgt_cloud, 0, 0, 255);
    viz.addPointCloud(src_cloud, blue, "src_cloud");

    if (viz_normals)
    {
        viz.addPointCloudNormals<PointT>(tgt_cloud, 1, 0.5f, "tgt_cloud_normals", 0);
        viz.addPointCloudNormals<PointT>(src_cloud, 1, 0.5f, "src_cloud_normals", 0);
    }

    // Add correspondences

    if (correspondences.size() > 0)
        viz.addCorrespondences<PointT>(src_cloud, tgt_cloud, correspondences, "correspondences", 0);

    // Add the final aligned cloud

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZINormal> red(aligned_cloud, 255, 0, 0);
    viz.addPointCloud(aligned_cloud, red, "aligned_cloud");

    viz.spin();
}

template <typename PointT>
void project_cloud(boost::shared_ptr<pcl::PointCloud<PointT> > cloud, cv::Mat& translation_vector, cv::Mat& rotation_vector, cv::Mat& intrinsics, cv::Mat& distortions, std::vector<cv::Point2f>& imagePoints) {
  int length = cloud->points.size();
  std::cout << "projecting " << length << " points" << std::endl;
  cv::Mat objectPoints(length, 3, CV_32FC1);

  // Copy over PCL cloud points into cv::Mat
  for(int i = 0; i < length; i++) {
      // FIXME Assuming points have to be in front of camera plane
    objectPoints.at<float>(i, 0) = cloud->points[i].x;
    objectPoints.at<float>(i, 1) = cloud->points[i].y;
    objectPoints.at<float>(i, 2) = cloud->points[i].z;
  }

  //std::cout << rotation_vector << std::endl;
  //std::cout << translation_vector << std::endl;
  //std::cout << intrinsics << std::endl;
  //std::cout << distortions << std::endl;
  cv::projectPoints(objectPoints, rotation_vector, translation_vector, intrinsics, distortions, imagePoints);
}

template <typename PointT>
void project_cloud_eigen(boost::shared_ptr<pcl::PointCloud<PointT> > cloud, const Eigen::Vector3f& translation_vector, const Eigen::Matrix3f& rotation_matrix, const Eigen::Matrix3f& intrinsics, const Eigen::VectorXf distortions, std::vector<cv::Point2f>& imagePoints)
//void project_cloud_eigen(boost::shared_ptr<pcl::PointCloud<PointT> > cloud, const Eigen::Vector3f& translation_vector, const Eigen::Matrix3f& rotation_matrix, const Eigen::Matrix3f& intrinsics, const Eigen::VectorXf distortions, Eigen::MatrixX2f& imagePoints)
{
    cv::Mat tvec, rmat, rvec, rotmat, K, D;
    //std::vector<cv::Point2f> cv_pts;

    cv::eigen2cv(translation_vector, tvec);
    cv::eigen2cv(rotation_matrix, rmat);
    cv::Rodrigues(rmat, rvec);
    cv::eigen2cv(intrinsics, K);
    cv::eigen2cv(distortions, D);

    project_cloud(cloud, tvec, rvec, K, D, imagePoints);

    //imagePoints.resize(2, cv_pts.size());
    //for (int k = 0; k < cv_pts.size(); k++)
    //{
        //imagePoints(0, k) = cv_pts[k].x;
        //imagePoints(1, k) = cv_pts[k].y;
    //}
}

template <typename PointT>
void filter_cloud(boost::shared_ptr<pcl::PointCloud<PointT> > cloud, boost::shared_ptr<pcl::PointCloud<PointT> > filtered_cloud, std::vector<int>& filtered_indices, const std::string& filter_dim, float range_min, float range_max)
{
    filtered_indices.clear();

    pcl::PassThrough<PointT> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName(filter_dim);
    pass.setFilterLimits(range_min, range_max);
    pass.filter(filtered_indices);
    pass.filter(*filtered_cloud);
}

template<typename PointT>
void filter_lidar_cloud(boost::shared_ptr<pcl::PointCloud<PointT> > cloud, boost::shared_ptr<pcl::PointCloud<PointT> > filtered_cloud, std::vector<int>& filtered_indices)
{
    filter_cloud<PointT>(cloud, filtered_cloud, filtered_indices, "z", 0, std::numeric_limits<float>::max());
}

template <typename PointT>
void filter_field(boost::shared_ptr<pcl::PointCloud<PointT> > cloud, std::string field, float lower, float upper) {
  pcl::PassThrough<PointT> pass;
  pass.setFilterFieldName(field.c_str());
  pass.setFilterLimits(lower, upper);
  pass.setInputCloud(cloud);

  pass.filter(*cloud);
}

template <typename PointT>
void extract_clusters_euclidean(boost::shared_ptr<pcl::PointCloud<PointT> > cloud, std::vector<pcl::PointIndices>& cluster_indices, float cluster_tol, int min_cluster_size, int max_cluster_size)
{
    boost::shared_ptr<pcl::search::KdTree<PointT> > tree (new pcl::search::KdTree<PointT>);
    tree->setEpsilon(0.0);
    tree->setInputCloud (cloud);

    pcl::EuclideanClusterExtraction<PointT> ec;
    ec.setClusterTolerance (cluster_tol);
    ec.setMinClusterSize (min_cluster_size);
    ec.setMaxClusterSize (max_cluster_size);
    ec.setSearchMethod (tree);
    ec.setInputCloud (cloud);
    ec.extract (cluster_indices);
}

template <typename PointT>
void extract_clusters_euclidean(boost::shared_ptr<pcl::PointCloud<PointT> > cloud, std::vector<boost::shared_ptr<pcl::PointCloud<PointT> > >& clusters, float cluster_tol, int min_cluster_size, int max_cluster_size)
{
    std::vector<pcl::PointIndices> cluster_indices;
    extract_clusters_euclidean(cloud, cluster_indices, cluster_tol, min_cluster_size, max_cluster_size);

    pcl::ExtractIndices<PointT> extract;
    extract.setInputCloud(cloud);
    clusters.clear();
    for (size_t i = 0; i < cluster_indices.size(); i++) {
        boost::shared_ptr<pcl::PointCloud<PointT> > cluster(new pcl::PointCloud<PointT>());
        boost::shared_ptr<pcl::PointIndices> ptr = boost::make_shared<pcl::PointIndices> ();
        *ptr = cluster_indices[i];
        extract.setIndices(ptr);
        extract.setNegative(false);
        extract.filter(*cluster);
        clusters.push_back(cluster);
    }
}