#include <boost/serialization/export.hpp>

#include "core/Common.h"
#include "math/Random.h"
#include "Approximator.h"
#include "tiles2.h"

namespace OpenNero
{
    /// @param info information about the agent for which this approximator is to be used
    TableApproximator::TableApproximator(const AgentInitInfo& info, const int actions, const int states) :
        Approximator(info)
        , table()
        , action_bins(actions)
        , state_bins(states)
    {
    }

    /// copy constructor
    TableApproximator::TableApproximator(const TableApproximator& a) :
        Approximator(a)
        , table(a.table)
        , action_bins(a.action_bins)
        , state_bins(a.state_bins)
    {
    }

    /// destructor
    TableApproximator::~TableApproximator()
    {
    }

    /// predict the value associated with a particular feature vector
    /// @param observation observation to update
    /// @param action action to update
    /// @return currently approximated (exact) value
    double TableApproximator::predict(const FeatureVector& observation, const FeatureVector& action)
    {
        const FeatureVector& s = quantize_state(observation);
        const FeatureVector& a = quantize_action(action);
        StateActionDoubleMap::iterator found = table.find(StateActionPair(s, a));
        if (found == table.end())
        {
            return 0;
        }
        else
        {
            return found->second;
        }

    }

    /// @param observation observation to update
    /// @param action action to update
    /// @param target new value for this state/action pair
    void TableApproximator::update(const FeatureVector& observation, const FeatureVector& action, double target)
    {
        const FeatureVector& s = quantize_state(observation);
        const FeatureVector& a = quantize_action(action);
        table[StateActionPair(s, a)] = target;
    }

    /// given a feature vector from a continuous space, quantize each component
    /// based on the range for the component and the number of discrete (linear)
    /// bins we want for each dimension of our discretized space.
    ///
    /// This is similar in spirit to the action space enumeration in initialize
    /// above. I'll continue the example from there, i.e., that we want to map a
    /// dimension that spans [-1, 1] into 5 equal bins, with centers at the
    /// values {-1 (bin #0), -.5, 0, .5, 1 (bin #4)}.
    ///
    /// Suppose we get a continuous value of 0.3 for this dimension. This should
    /// probably fit into bin #3 (by a "closest center" metric -- similarly, 0.2
    /// should fit into bin #2). We first map [-1, 1] onto {0, 1, 2, 3, 4}, and
    /// then we map the integral bin number back into [-1, 1] by adding the
    /// lower bound (-1) to the appropriate multiple of
    /// "inc" == span / (bins - 1).
    ///
    /// The tricky bit here is that to map each bin's interval (e.g., for bin
    /// #3, the interval is [.25, .75)) we basically end up subtracting inc / 2
    /// from everything, which effectively converts the bin centers into the
    /// barriers that divide them.
    ///
    /// So, an input value of 0.3 would be normalized to (0.3 - (-1 - 0.5 / 2))
    /// / (1 - -1) == 1.55 / 2 = 0.775, which maps into bin int(0.775 * 4) == 3,
    /// which maps back into the action space as -1 + 3 * 0.5 == 0.5. Similarly,
    /// an input value of 0.2 would be normalized to 0.725, which maps into bin
    /// int(0.725 * 4) == 2.
    FeatureVector quantize(const FeatureVector& continuous,
                           const FeatureVectorInfo& info,
                           const int bins)
    {
        if (bins == 0)
            return continuous;
        FeatureVector discrete(continuous);
        for (size_t i = 0; i < continuous.size(); ++i)
        {
            if (info.isDiscrete(i))
            {
                discrete[i] = int(continuous[i]);
            }
            else
            {
                double lo = info.getMin(i), hi = info.getMax(i), span = hi - lo;
                double inc = span / (bins - 1);
                double interp = (continuous[i] - (lo - inc / 2)) / span;
                discrete[i] = lo + inc * int((bins - 1) * interp);
            }
        }
        return discrete;
    }

    /// quantize a continuous action vector
    FeatureVector TableApproximator::quantize_action(const FeatureVector& action) const
    {
        return quantize(action, mInfo.actions, action_bins);
    }

    /// quantize a continuous state vector
    FeatureVector TableApproximator::quantize_state(const FeatureVector& state) const
    {
        return quantize(state, mInfo.sensors, state_bins);
    }

    /// Create a new tiles approximator based on the agent description provided
    /// @param info agent interface description
    TilesApproximator::TilesApproximator(const AgentInitInfo& info, const int num_tiles, const int num_weights)
        : Approximator(info)
        , mAlpha(0.1f)
        , ints_index()
        , floats_index()
        , ints()
        , floats()
        , tiles()
        , weights()
    {
        LOG_F_DEBUG("ai", "TilesApproximator( "  << info << " )");
        size_t num_sensors = info.sensors.size();
        size_t num_actions = info.actions.size();
        for (size_t i = 0; i < info.sensors.size(); ++i)
        {
            if (info.sensors.isDiscrete(i))
            {
                ints_index.push_back(i);
            }
            else
            {
                floats_index.push_back(i);
            }
        }
        for (size_t i = num_sensors; i < num_sensors + num_actions; ++i)
        {
            if (info.actions.isDiscrete(i - num_sensors))
            {
                ints_index.push_back(i);
            }
            else
            {
                floats_index.push_back(i);
            }
        }
        Assert(floats_index.size() + ints_index.size() == num_sensors + num_actions);
        ints.resize(ints_index.size());
        floats.resize(floats_index.size());
        tiles.resize(num_tiles);
        weights.resize(num_weights);
        for (size_t i = 0; i < weights.size(); ++i)
        {
            weights[i] = RANDOM.normalF(0,1);
        }
    }

    TilesApproximator::TilesApproximator(const TilesApproximator& a)
        : Approximator(a.mInfo)
        , mAlpha(a.mAlpha)
        , ints_index(a.ints_index)
        , floats_index(a.floats_index)
        , ints(a.ints)
        , floats(a.floats)
        , tiles(a.tiles)
        , weights(a.weights)
    {
    }

    TilesApproximator::~TilesApproximator()
    {
    }

    /// convert feature vector into tiles
    void TilesApproximator::to_tiles(const FeatureVector& observation, const FeatureVector& action)
    {
        size_t num_sensors = mInfo.sensors.size();
        size_t num_actions = mInfo.actions.size();
        Assert(num_sensors == observation.size());
        Assert(num_actions == action.size());
        // shuffle feature vector into the arrays
        for (size_t i = 0; i < ints_index.size(); ++i)
        {
            if (ints_index[i] < num_sensors)
            {
                ints[i] = (int)observation[ints_index[i]];
            }
            else
            {
                ints[i] = (int)action[ints_index[i] - num_sensors];
            }
        }
        for (size_t i = 0; i < floats_index.size(); ++i)
        {
            if (floats_index[i] < num_sensors)
            {
                floats[i] = (float)observation[floats_index[i]];
            }
            else
            {
                floats[i] = (float)action[floats_index[i] - num_sensors];
            }
        }
        // convert the input values to tiles
        GetTiles(tiles, weights.size(), floats, ints);
    }

    /// @param observation sensor vector
    /// @param action action vector
    double TilesApproximator::predict(const FeatureVector& observation, const FeatureVector& action)
    {
        to_tiles(observation, action);
        double result=0.0;
        for (size_t i = 0; i < tiles.size(); ++i)
        {
            result += weights[tiles[i]];
        }
        return result;
    }

    /// Adapt the tile weights for the tiles that are triggered by the given example
    /// @param observation sensor vector
    /// @param action action vector
    /// @param target output target
    void TilesApproximator::update(const FeatureVector& observation, const FeatureVector& action, double target)
    {
        double x = predict(observation, action);
        // then, adapt weights towards the prediction
        for (size_t i = 0; i < tiles.size(); ++i)
        {
            weights[tiles[i]] += (float)(mAlpha / tiles.size() * (target - x));
        }
    }
}

BOOST_CLASS_EXPORT(OpenNero::Approximator)
BOOST_CLASS_EXPORT(OpenNero::TableApproximator)
BOOST_CLASS_EXPORT(OpenNero::TilesApproximator)


#include <iostream>

#include <boost/hana.hpp>

int main(void)
{
  using namespace boost;
  auto map = hana::make_map(
    hana::make_pair(hana::type_c<char>,   "char"),
    hana::make_pair(hana::type_c<int>,    "int"),
    hana::make_pair(hana::type_c<long>,   "long"),
    hana::make_pair(hana::type_c<float>,  "float"),
    hana::make_pair(hana::type_c<double>, "double")
  );
}


/*ckwg +29
 * Copyright 2012 by Kitware, Inc.
 * 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 name of Kitware, Inc. nor the names of any 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 AUTHORS 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 <iostream>
#include <ctime>
#include <vul/vul_arg.h>

#include <vil/vil_image_view.h>

#include <vil/algo/vil_gauss_filter.h>

#include <viscl/core/manager.h>
#include <viscl/tasks/gaussian_smooth.h>
#include <viscl/vxl/transfer.h>

#include <boost/chrono.hpp>

int main(int argc, char *argv[])
{
  vul_arg<unsigned> arg_iw("-iw", "Image width", 4096);
  vul_arg<unsigned> arg_ih("-ih", "Image height", 4096);
  vul_arg<unsigned> arg_nj("-nj", "# of iterations", 10);
  vul_arg<bool>     arg_use_cpu("-cpu", "run CPU timing", false);
  vul_arg<bool>     arg_use_gpu("-gpu", "run GPU timing", false);

  vul_arg_parse(argc, argv);

  unsigned iw = arg_iw();
  unsigned ih = arg_ih();
  const unsigned iter = arg_nj();

  if (!arg_use_cpu() && !arg_use_gpu())
  {
    std::cerr << "must specify either -cpu or -gpu flag, or both" << std::endl;
    return -1;
  }

  viscl::manager::inst()->report_opencl_specs();

  vil_image_view<vxl_byte> output, img = vil_image_view<vxl_byte>(iw,ih);
  std::srand(std::time(NULL));
  for(unsigned j=0; j<img.nj(); ++j)
  {
    for(unsigned i=0; i<img.ni(); ++i)
    {
      img(i,j) = std::rand();
    }
  }

  int radii[3] = {2, 3, 4};

  viscl::gaussian_smooth_t smoother = NEW_VISCL_TASK(viscl::gaussian_smooth);
  viscl::image img_cl = viscl::upload_image(img);

  for (int r = 0; r < 3; r++)
  {
    boost::chrono::system_clock::time_point start;
    boost::chrono::duration<double> sec;
    if (arg_use_gpu())
    {
      start = boost::chrono::system_clock::now();
      for (int i = 0; i < iter; i++)
        viscl::image result = smoother->smooth( img_cl, 2.0, radii[r]);
      sec = boost::chrono::system_clock::now() - start;
      std::cout << "viscl took " << sec.count() / iter
               << " seconds to smooth a "
               << img.ni() << "x" << img.nj() << " image "
               << iter << "x w/ kernel size=" << 2*radii[r]+1 << ".\n";
    }
    if (arg_use_cpu())
    {
      start = boost::chrono::system_clock::now();
      for (int i = 0; i < iter; i++)
        vil_gauss_filter_2d<vxl_byte, vxl_byte>(img, output, 2.0, radii[r]);
      sec = boost::chrono::system_clock::now() - start;
      std::cout << "VXL took "   << sec.count() / iter
               << " seconds to smooth a "
               << img.ni() << "x" << img.nj() << " img "
               << iter << "x w/ kernel size=" << 2*radii[r]+1 << ".\n";
    }
  }

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/bitwise/include/functions/bitwise_notor.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/bitincrement.hpp>
#include <boost/simd/include/constants/valmin.hpp>

NT2_TEST_CASE_TPL(bitwise_notor_real, BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::bitwise_notor;
  using boost::simd::tag::bitwise_notor_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<bitwise_notor_(vT,vT)>::type r_t;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(bitwise_notor(boost::simd::Inf<vT>(), boost::simd::Inf<vT>()), boost::simd::Nan<r_t>());
  NT2_TEST_EQUAL(bitwise_notor(boost::simd::Minf<vT>(), boost::simd::Minf<vT>()), boost::simd::Nan<r_t>());
  NT2_TEST_EQUAL(bitwise_notor(boost::simd::Nan<vT>(), boost::simd::Nan<vT>()), boost::simd::Nan<r_t>());
#endif
  NT2_TEST_EQUAL(bitwise_notor(boost::simd::Zero<vT>(), boost::simd::Zero<vT>()), boost::simd::Nan<r_t>());
}

NT2_TEST_CASE_TPL(bitwise_notor_ui, BOOST_SIMD_SIMD_UNSIGNED_TYPES)
{
  using boost::simd::bitwise_notor;
  using boost::simd::tag::bitwise_notor_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<bitwise_notor_(vT,vT)>::type r_t;

  // specific values tests
  NT2_TEST_EQUAL(bitwise_notor(boost::simd::Zero<vT>(), boost::simd::Zero<vT>()), boost::simd::Valmax<r_t>());
}

NT2_TEST_CASE_TPL(bitwise_notor_si, BOOST_SIMD_SIMD_INTEGRAL_SIGNED_TYPES)
{
  using boost::simd::bitwise_notor;
  using boost::simd::tag::bitwise_notor_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<bitwise_notor_(vT,vT)>::type r_t;

  // specific values tests
  NT2_TEST_EQUAL(bitwise_notor(boost::simd::Zero<vT>(), boost::simd::Zero<vT>()), boost::simd::Mone<r_t>());
}

NT2_TEST_CASE_TPL(bitwise_notor_mix, BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::bitwise_notor;
  using boost::simd::tag::bitwise_notor_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T, ext_t>                  vT;
  typedef typename  boost::dispatch::meta::as_integer<vT>::type sivT;
  typedef typename  boost::dispatch::meta::as_integer<vT, unsigned>::type uivT;

  // return type conformity test
  NT2_TEST_TYPE_IS(typename boost::dispatch::meta::call<bitwise_notor_(vT,uivT)>::type, vT);
  NT2_TEST_TYPE_IS(typename boost::dispatch::meta::call<bitwise_notor_(vT,sivT)>::type, vT);
  NT2_TEST_TYPE_IS(typename boost::dispatch::meta::call<bitwise_notor_(uivT, vT)>::type, uivT);
  NT2_TEST_TYPE_IS(typename boost::dispatch::meta::call<bitwise_notor_(sivT, vT)>::type, sivT);

  // specific values tests
  NT2_TEST_EQUAL(bitwise_notor(boost::simd::Nan<vT>(),boost::simd::One<uivT>()), boost::simd::Bitincrement<vT>());
  NT2_TEST_EQUAL(bitwise_notor(boost::simd::Nan<vT>(), boost::simd::One<sivT>()), boost::simd::Bitincrement<vT>());
  NT2_TEST_EQUAL(bitwise_notor(boost::simd::Valmin<sivT>(),boost::simd::Zero<vT>()), boost::simd::Valmax<sivT>());
  NT2_TEST_EQUAL(bitwise_notor(boost::simd::Zero<uivT>(), boost::simd::Zero<vT>()), boost::simd::Valmax<uivT>());
}


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

#include <cnpy.h>

#include <boost/program_options.hpp>
#include <ckp19/dataset.hpp>
#include <ckp19/plaintext_clustering.hpp>
#include <filesystem>
#include <fstream>
#include <nlohmann/json.hpp>
#include <string>

#include "utils.hpp"

namespace fs = std::filesystem;
namespace bpo = boost::program_options;
using json = nlohmann::json;

void benchmarkClustering(const bpo::variables_map &opts) {
  long seed = opts["seed"].as<long>();
  long repeat = opts["repeat"].as<long>();
  std::string dataset = opts["dataset"].as<std::string>();

  srand(seed);

  fs::path output_dir;
  output_dir = fs::path(opts["output"].as<std::string>());

  long num_dusts = opts["dusts"].as<long>();
  long iterations = opts["iterations"].as<long>();
  long mode_kdegree = opts["mode-kdegree"].as<long>();
  long label_kdegree = opts["label-kdegree"].as<long>();
  long mode_inv_steps = opts["mode-invsteps"].as<long>();
  long label_inv_steps = opts["label-invsteps"].as<long>();
  long minidx_t = opts["min-idx-t"].as<long>();

  Dataset ds = Dataset::loadNpy(dataset);
  ds.rescaleDatasetInPlace(0.5);

  json info;
  info["details"] = {{"seed", seed},
                     {"repeat", repeat},
                     {"dataset", dataset},
                     {"dusts", num_dusts},
                     {"iterations", iterations},
                     {"mode-kdegree", mode_kdegree},
                     {"label-kdegree", label_kdegree},
                     {"mode-invsteps", mode_inv_steps},
                     {"label-invsteps", label_inv_steps},
                     {"min-idx-t", minidx_t}};

  std::cout << "--- Information ---\n";
  for (json::iterator it = info["details"].begin(); it != info["details"].end();
       ++it) {
    std::cout << it.key() << " : " << it.value() << "\n";
  }
  std::cout << std::endl;

  info["benchmarks"] = json::array();

  fs::create_directories(output_dir);
  auto info_file = output_dir / "info.json";

  for (long i = 0; i < repeat; ++i) {
    std::cout << "--- Repetition " << i + 1 << " ---\n";

    PlainMeanShift pmeanshift(seed + i);

    TimePoint start;
    std::vector<std::vector<double>> labels;
    pmeanshift.cluster(labels, ds.getPoints(), num_dusts, iterations,
                       mode_kdegree, label_kdegree, mode_inv_steps,
                       label_inv_steps, minidx_t);
    TimePoint end;

    auto time = end - start;
    std::cout << "Time: " << time << " ms" << std::endl;

    json iter;
    iter["time"] = time;
    info["benchmarks"].push_back(iter);
    saveJson(info, info_file);

    std::vector<double> flat_labels(labels.size() * num_dusts);
    for (size_t i = 0; i < labels.size(); ++i) {
      for (size_t j = 0; j < num_dusts; ++j) {
        flat_labels[i * num_dusts + j] = labels[i][j];
      }
    }

    auto data_file =
        (output_dir / ("rep_" + std::to_string(i) + ".npy")).string();
    cnpy::npy_save(data_file, flat_labels.data(),
                   {labels.size(), static_cast<unsigned long>(num_dusts)}, "w");

    std::cout << std::endl;
  }
}

// clang-format off
bpo::options_description generic_program_options() {
  bpo::options_description desc("Followig options are supported by config file too");
  desc.add_options()
    ("output,o", bpo::value<std::string>()->required(), "Directory to save data.")
    ("seed", bpo::value<long>()->default_value(2602), "Seed used for RNG.")
    ("repeat,r", bpo::value<long>()->default_value(1), "Number of times to run clustering algorithm (with different seed).")
    ("dataset", bpo::value<std::string>()->required(), "Path to dataset in npy format.")
    ("dusts", bpo::value<long>()->required(), "Number of dusts.")
    ("iterations", bpo::value<long>()->required(), "Number of iterations for mode seeking.")
    ("mode-kdegree", bpo::value<long>()->required(), "Log of kernel degree in mode seeking (\\Gamma_1).")
    ("label-kdegree", bpo::value<long>()->required(), "Log of kernel degree in point labeling (\\Gamma_2).")
    ("mode-invsteps", bpo::value<long>()->required(), "Number of iterations for inverse in mode seeking (\\zeta_1).")
    ("label-invsteps", bpo::value<long>()->required(), "Number of iterations for inverse in point labeling (\\zeta_2).")
    ("min-idx-t", bpo::value<long>()->required(), "Log of degree for minimum index algorithm (t).");

  return desc;
}
// clang-format off

int main(int argc, char *argv[]) {
  auto generic(generic_program_options());

  bpo::options_description cmdline("Run plaintext variation of CKP19 mean shift clustering.");
  cmdline.add(generic);
  cmdline.add_options()(
      "config,c", bpo::value<std::string>(),
      "Configuration file for easy specification of cmd line arguments.")(
      "help,h", "Produce help message.");

  bpo::variables_map opts;
  bpo::store(bpo::command_line_parser(argc, argv).options(cmdline).run(), opts);

  if (opts.count("help") != 0) {
    std::cout << cmdline << std::endl;
    return 0;
  }

  if (opts.count("config") != 0) {
    std::string cpath(opts["config"].as<std::string>());
    std::ifstream fin(cpath.c_str());

    if (fin.fail()) {
      std::cerr << "Could not open configuration file at " << cpath << "\n";
      return 1;
    }

    bpo::store(bpo::parse_config_file(fin, generic), opts);
  }

  try {
    bpo::notify(opts);

    // Check if output file already exists
    if (fs::exists(opts["output"].as<std::string>())) {
      throw std::runtime_error("Output directory aready exists.");
    }
  } catch (const std::exception &ex) {
    std::cerr << ex.what() << std::endl;
    return 1;
  }

  try {
    benchmarkClustering(opts);
  } catch (const std::exception &ex) {
    std::cerr << ex.what() << "\nFatal error" << std::endl;
    return 1;
  }

  return 0;
}


// Copyright 2013 Krzysztof Czainski
// 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)

/// \file numeric.cpp

/// \brief Unit test for Numeric extension

#include <boost/gil/image.hpp>
#include <boost/gil/typedefs.hpp>

#include <boost/gil/extension/numeric/resample.hpp>
#include <boost/gil/extension/numeric/sampler.hpp>

#include <boost/test/unit_test.hpp>

using namespace boost;
using namespace gil;

template < class F, class I >
struct TestMapFn
{
    typedef point2<F> point_t;
    typedef point_t result_type;
    result_type operator()( point2<I> const& src ) const
    {
        F x = static_cast<F>( src.x ) - 0.5;
        F y = static_cast<F>( src.y ) - 0.5;

        return point_t( x, y );
    }
};

namespace boost { namespace gil {

// NOTE: I suggest this could be the default behavior:

template <typename T> struct mapping_traits;

template < class F, class I >
struct mapping_traits< TestMapFn<F,I> > {
    typedef typename TestMapFn<F,I>::result_type result_type;
};

template < class F, class I >
inline
point2<F> transform( TestMapFn<F,I> const& mf, point2<I> const& src )
{
    return mf(src);
}

}} // boost::gil

BOOST_AUTO_TEST_SUITE(Numeric_Tests)

BOOST_AUTO_TEST_CASE( pixel_numeric_operations_plus )
{
    rgb8_pixel_t a( 10, 20, 30 );
    bgr8_pixel_t b( 30, 20, 10 );

    pixel_plus_t< rgb8_pixel_t
                , bgr8_pixel_t
                , rgb8_pixel_t
                > op;
    rgb8_pixel_t c = op( a, b );

    BOOST_ASSERT( get_color( c, red_t()   ) == 20 );
    BOOST_ASSERT( get_color( c, green_t() ) == 40 );
    BOOST_ASSERT( get_color( c, blue_t()  ) == 60 );

    pixel_plus_t< rgb8_pixel_t
                , bgr8_pixel_t
                , bgr8_pixel_t
                > op2;
    bgr8_pixel_t d = op2( a, b );

    BOOST_ASSERT( get_color( d, red_t()   ) == 20 );
    BOOST_ASSERT( get_color( d, green_t() ) == 40 );
    BOOST_ASSERT( get_color( d, blue_t()  ) == 60 );
}

BOOST_AUTO_TEST_CASE( pixel_numeric_operations_multiply )
{
    rgb32f_pixel_t a( 1.f, 2.f, 3.f );
    bgr32f_pixel_t b( 2.f, 2.f, 2.f );

    pixel_multiply_t< rgb32f_pixel_t
                    , bgr32f_pixel_t
                    , rgb32f_pixel_t
                    > op;
    rgb32f_pixel_t c = op( a, b );

    float epsilon = 1e-6f;
    BOOST_CHECK_CLOSE( static_cast<float>( get_color( c,   red_t() )), 2.f, epsilon );
    BOOST_CHECK_CLOSE( static_cast<float>( get_color( c, green_t() )), 4.f, epsilon );
    BOOST_CHECK_CLOSE( static_cast<float>( get_color( c,  blue_t() )), 6.f, epsilon );
}

BOOST_AUTO_TEST_CASE( pixel_numeric_operations_divide )
{
    // integer
    {
        rgb8_pixel_t a( 10, 20, 30 );
        bgr8_pixel_t b(  2,  2,  2 );

        pixel_divide_t< rgb8_pixel_t
                      , bgr8_pixel_t
                      , rgb8_pixel_t
                      > op;
        rgb32f_pixel_t c = op( a, b );

        BOOST_ASSERT( get_color( c,   red_t() ) ==  5 );
        BOOST_ASSERT( get_color( c, green_t() ) == 10 );
        BOOST_ASSERT( get_color( c,  blue_t() ) == 15 );
    }

    // float
    {
        rgb32f_pixel_t a( 1.f, 2.f, 3.f );
        bgr32f_pixel_t b( 2.f, 2.f, 2.f );

        pixel_divide_t< rgb32f_pixel_t
                      , bgr32f_pixel_t
                      , rgb32f_pixel_t
                      > op;
        rgb32f_pixel_t c = op( a, b );

        float epsilon = 1e-6f;
        BOOST_CHECK_CLOSE( static_cast< float >( get_color( c,   red_t() )), 0.5f, epsilon );
        BOOST_CHECK_CLOSE( static_cast< float >( get_color( c, green_t() )),  1.f, epsilon );
        BOOST_CHECK_CLOSE( static_cast< float >( get_color( c,  blue_t() )), 1.5f, epsilon );
    }
}

BOOST_AUTO_TEST_CASE(bilinear_sampler_test)
{
    // R G B
    // G W R
    // B R G
    rgb8_image_t img(3,3);
    rgb8_view_t v = view(img);
    v(0,0) = v(1,2) = v(2,1) = rgb8_pixel_t(128,0,0);
    v(0,1) = v(1,0) = v(2,2) = rgb8_pixel_t(0,128,0);
    v(0,2) = v(2,0) = rgb8_pixel_t(0,0,128);
    v(1,1) = rgb8_pixel_t(128,128,128);

    rgb8_image_t dimg(4,4);
    rgb8c_view_t dv = const_view(dimg);

    TestMapFn<double,rgb8_image_t::coord_t> mf;

    resample_pixels( const_view(img), view(dimg), mf, bilinear_sampler() );

    assert( rgb8_pixel_t(128,0,0) == dv(0,0) );
    assert( rgb8_pixel_t(64,64,0) == dv(0,1) );
    assert( rgb8_pixel_t(0,64,64) == dv(0,2) );
    assert( rgb8_pixel_t(0,0,128) == dv(0,3) );

    assert( rgb8_pixel_t(64,64,0) == dv(1,0) );
    assert( rgb8_pixel_t(64,96,32) == dv(1,1) );
    assert( rgb8_pixel_t(64,64,64) == dv(1,2) );
    assert( rgb8_pixel_t(64,0,64) == dv(1,3) );

    assert( rgb8_pixel_t(0,64,64) == dv(2,0) );
    assert( rgb8_pixel_t(64,64,64) == dv(2,1) );
    assert( rgb8_pixel_t(96,64,32) == dv(2,2) );
    assert( rgb8_pixel_t(64,64,0) == dv(2,3) );

    assert( rgb8_pixel_t(0,0,128) == dv(3,0) );
    assert( rgb8_pixel_t(64,0,64) == dv(3,1) );
    assert( rgb8_pixel_t(64,64,0) == dv(3,2) );
    assert( rgb8_pixel_t(0,128,0) == dv(3,3) );
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <fstream>
#include <iomanip>
#include <cmath>
#include <deque>
#include <vector>

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

#include "maybe_omp.h"
#ifdef EIGEN_USE_MKL_ALL
#include <mkl.h>
#endif

#include "util.h"

extern double drand48();

using namespace Eigen;
using namespace std;
using namespace boost::random;

namespace nplm
{

void splitBySpace(const std::string &line, std::vector<std::string> &items)
{
    string copy(line);
    boost::trim_if(copy, boost::is_any_of(" \t"));
    if (copy == "")
    {
	items.clear();
	return;
    }
    boost::split(items, copy, boost::is_any_of(" \t"), boost::token_compress_on);
}

void readWeightsFile(ifstream &TRAININ, vector<float> &weights) {
  string line;
  while (getline(TRAININ, line) && line != "")
  {
    vector<string> items;
    splitBySpace(line, items);
    if (items.size() != 1)
    {
        cerr << "Error: weights file should have only one weight per line" << endl;
        exit(-1);
    }
    weights.push_back(boost::lexical_cast<float>(items[0]));
  }
}

void readWordsFile(ifstream &TRAININ, vector<string> &word_list)
{
  string line;
  while (getline(TRAININ, line) && line != "")
  {
    vector<string> words;
    splitBySpace(line, words);
    if (words.size() != 1)
    {
        cerr << "Error: vocabulary file must have only one word per line" << endl;
        exit(-1);
    }
    word_list.push_back(words[0]);
  }
}

void readWordsFile(const string &file, vector<string> &word_list)
{
  cerr << "Reading word list from: " << file<< endl;

  ifstream TRAININ;
  TRAININ.open(file.c_str());
  if (! TRAININ)
  {
    cerr << "Error: can't read word list from file " << file<< endl;
    exit(-1);
  }

  readWordsFile(TRAININ, word_list);
  TRAININ.close();
}

void writeWordsFile(const vector<string> &words, ofstream &file)
{
    for (int i=0; i<words.size(); i++)
    {
	file << words[i] << endl;
    }
}

void writeWordsFile(const vector<string> &words, const string &filename)
{
    ofstream OUT;
    OUT.open(filename.c_str());
    if (! OUT)
    {
      cerr << "Error: can't write to file " << filename << endl;
      exit(-1);
    }
    writeWordsFile(words, OUT);
    OUT.close();
}

// Read a data file of unknown size into a flat vector<int>.
// If this takes too much memory, we should create a vector of minibatches.
void readDataFile(const string &filename, int &ngram_size, vector<int> &data, int minibatch_size)
{
  cerr << "Reading minibatches from file " << filename << ": ";

  ifstream DATAIN(filename.c_str());
  if (!DATAIN)
  {
    cerr << "Error: can't read data from file " << filename<< endl;
    exit(-1);
  }

  vector<int> data_vector;

  string line;
  long long int n_lines = 0;
  while (getline(DATAIN, line))
  {
    vector<string> ngram;
    splitBySpace(line, ngram);

    if (ngram_size == 0)
        ngram_size = ngram.size();

    if (ngram.size() != ngram_size)
    {
        cerr << "Error: expected " << ngram_size << " fields in instance, found " << ngram.size() << endl;
	exit(-1);
    }

    for (int i=0;i<ngram_size;i++)
        data.push_back(boost::lexical_cast<int>(ngram[i]));

    n_lines++;
    if (minibatch_size && n_lines % (minibatch_size * 10000) == 0)
      cerr << n_lines/minibatch_size << "...";
  }
  cerr << "done." << endl;
  DATAIN.close();
}

double logadd(double x, double y)
{
    if (x > y)
        return x + log1p(std::exp(y-x));
    else
        return y + log1p(std::exp(x-y));
}

#ifdef USE_CHRONO
void Timer::start(int i)
{
    m_start[i] = clock_type::now();
}

void Timer::stop(int i)
{
    m_total[i] += clock_type::now() - m_start[i];
}

void Timer::reset(int i) { m_total[i] = duration_type(); }

double Timer::get(int i) const
{
    return boost::chrono::duration<double>(m_total[i]).count();
}

Timer timer(20);
#endif

int setup_threads(int n_threads)
{
    #ifdef _OPENMP
    if (n_threads)
        omp_set_num_threads(n_threads);
    n_threads = omp_get_max_threads();
    if (n_threads > 1)
        cerr << "Using " << n_threads << " threads" << endl;

    Eigen::initParallel();
    Eigen::setNbThreads(n_threads);

    #ifdef MKL_SINGLE
    // Set the threading layer to match the compiler.
    // This lets MKL automatically go single-threaded in parallel regions.
    #ifdef __INTEL_COMPILER
    mkl_set_threading_layer(MKL_THREADING_INTEL);
    #elif defined __GNUC__
    mkl_set_threading_layer(MKL_THREADING_GNU);
    #endif
    mkl_set_num_threads(n_threads);
    #endif
    #endif

    return n_threads;
}

} // namespace nplm


/******************************************************************************
Copyright (c) 2017, Farbod Farshidian. 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 copyright holder 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 <gtest/gtest.h>
#include <boost/filesystem.hpp>
#include <functional>
#include <iostream>

#include "LinearSystemDynamicsAD.h"
#include "ocs2_core/dynamics/LinearSystemDynamics.h"
#include "ocs2_core/test/testTools.h"

using namespace ocs2;

class testCppADCG_dynamicsFixture : public ::testing::Test {
 public:
  const size_t stateDim_ = 4;
  const size_t inputDim_ = 2;

  testCppADCG_dynamicsFixture() { create(); };

  void create() {
    // Define dynamics parameters
    matrix_t A = matrix_t::Random(stateDim_, stateDim_);
    matrix_t B = matrix_t::Random(stateDim_, inputDim_);
    matrix_t G = matrix_t::Random(stateDim_, stateDim_);

    linearSystem_.reset(new LinearSystemDynamics(A, B, G));

    boost::filesystem::path filePath(__FILE__);
    std::string libraryFolder = filePath.parent_path().generic_string() + "/testCppADCG_generated";
    adLinearSystem_.reset(new LinearSystemDynamicsAD(A, B, G));

    adLinearSystem_->initialize(stateDim_, inputDim_, "testCppADCG_dynamics", libraryFolder, true, true);
  }

  std::unique_ptr<LinearSystemDynamics> linearSystem_;
  std::unique_ptr<LinearSystemDynamicsAD> adLinearSystem_;
};

/******************************************************************************/
/******************************************************************************/
/***************************************************************************** */
void checkSystemDynamics(const size_t numTests, SystemDynamicsBase* const linearSystem1, SystemDynamicsBase* const linearSystem2,
                         bool& success, size_t stateDim_, size_t inputDim_) {
  vector_t x;
  vector_t u;
  const scalar_t t = 0;
  const scalar_t precision = 1e-9;

  success = true;
  for (size_t it = 0; it < numTests && success; it++) {
    x.setRandom(stateDim_);
    u.setRandom(inputDim_);

    VectorFunctionLinearApproximation f1 = linearSystem1->linearApproximation(t, x, u);
    VectorFunctionLinearApproximation f2 = linearSystem2->linearApproximation(t, x, u);

    if (!isApprox(f1, f2, precision)) {
      std::cout << "systemDynamic1:\n" << f1 << std::endl;
      std::cout << "systemDynamic2:\n" << f2 << std::endl;
      success = false;
    }

    VectorFunctionLinearApproximation g1 = linearSystem1->jumpMapLinearApproximation(t, x);
    VectorFunctionLinearApproximation g2 = linearSystem2->jumpMapLinearApproximation(t, x);
    if (!isApprox(g1, g2, precision)) {
      std::cout << "jumpMap1:\n" << g1.dfdx << std::endl;
      std::cout << "jumpMap2:\n" << g2.dfdx << std::endl;
      success = false;
    }
  }
}

/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
TEST_F(testCppADCG_dynamicsFixture, system_dynamics_test) {
  bool success;
  checkSystemDynamics(100, linearSystem_.get(), adLinearSystem_.get(), success, stateDim_, inputDim_);

  ASSERT_TRUE(success);
}

/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
TEST_F(testCppADCG_dynamicsFixture, clone_test) {
  std::unique_ptr<SystemDynamicsBase> adLinearSystemPtr(adLinearSystem_->clone());
  bool success;
  checkSystemDynamics(100, linearSystem_.get(), adLinearSystemPtr.get(), success, stateDim_, inputDim_);

  ASSERT_TRUE(success);
}

/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
TEST_F(testCppADCG_dynamicsFixture, multithread_test) {
  std::unique_ptr<SystemDynamicsBase> linearSystemPtr(linearSystem_->clone());
  std::unique_ptr<SystemDynamicsBase> adLinearSystemPtr(adLinearSystem_->clone());

  bool success = false;
  std::thread thread1(checkSystemDynamics, 10000, linearSystem_.get(), adLinearSystem_.get(), std::ref(success), stateDim_, inputDim_);

  bool successClone = false;
  std::thread thread2(checkSystemDynamics, 10000, linearSystemPtr.get(), adLinearSystemPtr.get(), std::ref(successClone), stateDim_,
                      inputDim_);

  if (thread1.joinable()) {
    thread1.join();
  }
  if (thread2.joinable()) {
    thread2.join();
  }

  ASSERT_TRUE(success && successClone);
}


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include <mitkBaseData.h>
#include <mitkImageCast.h>
#include <mitkImageToItk.h>
#include <metaCommand.h>
#include "mitkCommandLineParser.h"
#include <usAny.h>
#include <itkImageFileWriter.h>
#include <mitkIOUtil.h>
#include <boost/lexical_cast.hpp>
#include <itkEvaluateDirectionImagesFilter.h>
#include <itkTractsToVectorImageFilter.h>
#include <mitkCoreObjectFactory.h>

#define _USE_MATH_DEFINES
#include <math.h>

/*!
\brief Extract principal fiber directions from a tractogram
*/
int main(int argc, char* argv[])
{
  mitkCommandLineParser parser;

  parser.setTitle("Fiber Direction Extraction");
  parser.setCategory("Fiber Tracking and Processing Methods");
  parser.setDescription("Extract principal fiber directions from a tractogram");
  parser.setContributor("MIC");

  parser.setArgumentPrefix("--", "-");
  parser.addArgument("input", "i", mitkCommandLineParser::InputFile, "Input:", "input tractogram (.fib/.trk)", us::Any(), false);
  parser.addArgument("out", "o", mitkCommandLineParser::OutputDirectory, "Output:", "output root", us::Any(), false);
  parser.addArgument("mask", "m", mitkCommandLineParser::InputFile, "Mask:", "mask image");
  parser.addArgument("athresh", "a", mitkCommandLineParser::Float, "Angular threshold:", "angular threshold in degrees. closer fiber directions are regarded as one direction and clustered together.", 25, true);
  parser.addArgument("peakthresh", "t", mitkCommandLineParser::Float, "Peak size threshold:", "peak size threshold relative to largest peak in voxel", 0.2, true);
  parser.addArgument("verbose", "v", mitkCommandLineParser::Bool, "Verbose:", "output optional and intermediate calculation results");
  parser.addArgument("numdirs", "d", mitkCommandLineParser::Int, "Max. num. directions:", "maximum number of fibers per voxel", 3, true);
  parser.addArgument("normalization", "n", mitkCommandLineParser::Int, "Normalization method:", "1=global maximum, 2=single vector, 3=voxel-wise maximum", 1);
  parser.addArgument("file_ending", "f", mitkCommandLineParser::String, "Image type:", ".nrrd, .nii, .nii.gz");

  std::map<std::string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
  if (parsedArgs.size()==0)
    return EXIT_FAILURE;

  std::string fibFile = us::any_cast<std::string>(parsedArgs["input"]);

  std::string maskImage("");
  if (parsedArgs.count("mask"))
    maskImage = us::any_cast<std::string>(parsedArgs["mask"]);

  float peakThreshold = 0.2;
  if (parsedArgs.count("peakthresh"))
    peakThreshold = us::any_cast<float>(parsedArgs["peakthresh"]);

  float angularThreshold = 25;
  if (parsedArgs.count("athresh"))
    angularThreshold = us::any_cast<float>(parsedArgs["athresh"]);

  std::string outRoot = us::any_cast<std::string>(parsedArgs["out"]);

  bool verbose = false;
  if (parsedArgs.count("verbose"))
    verbose = us::any_cast<bool>(parsedArgs["verbose"]);

  int maxNumDirs = 3;
  if (parsedArgs.count("numdirs"))
    maxNumDirs = us::any_cast<int>(parsedArgs["numdirs"]);

  int normalization = 1;
  if (parsedArgs.count("normalization"))
    normalization = us::any_cast<int>(parsedArgs["normalization"]);

  std::string file_ending = ".nrrd";
  if (parsedArgs.count("file_ending"))
    file_ending = us::any_cast<std::string>(parsedArgs["file_ending"]);

  try
  {
    typedef itk::Image<unsigned char, 3>                                    ItkUcharImgType;

    // load fiber bundle
    mitk::FiberBundle::Pointer inputTractogram = dynamic_cast<mitk::FiberBundle*>(mitk::IOUtil::Load(fibFile)[0].GetPointer());

    // load/create mask image
    ItkUcharImgType::Pointer itkMaskImage = nullptr;
    if (maskImage.compare("")!=0)
    {
      std::cout << "Using mask image";
      itkMaskImage = ItkUcharImgType::New();
      mitk::Image::Pointer mitkMaskImage = dynamic_cast<mitk::Image*>(mitk::IOUtil::Load(maskImage)[0].GetPointer());
      mitk::CastToItkImage(mitkMaskImage, itkMaskImage);
    }

    // extract directions from fiber bundle
    itk::TractsToVectorImageFilter<float>::Pointer fOdfFilter = itk::TractsToVectorImageFilter<float>::New();
    fOdfFilter->SetFiberBundle(inputTractogram);
    fOdfFilter->SetMaskImage(itkMaskImage);
    fOdfFilter->SetAngularThreshold(cos(angularThreshold*M_PI/180));
    switch (normalization)
    {
    case 1:
      fOdfFilter->SetNormalizationMethod(itk::TractsToVectorImageFilter<float>::NormalizationMethods::GLOBAL_MAX);
      break;
    case 2:
      fOdfFilter->SetNormalizationMethod(itk::TractsToVectorImageFilter<float>::NormalizationMethods::SINGLE_VEC_NORM);
      break;
    case 3:
      fOdfFilter->SetNormalizationMethod(itk::TractsToVectorImageFilter<float>::NormalizationMethods::MAX_VEC_NORM);
      break;
    }
    fOdfFilter->SetUseWorkingCopy(false);
    fOdfFilter->SetSizeThreshold(peakThreshold);
    fOdfFilter->SetMaxNumDirections(maxNumDirs);
    fOdfFilter->Update();

    {
      itk::TractsToVectorImageFilter<float>::ItkDirectionImageType::Pointer itkImg = fOdfFilter->GetDirectionImage();
      typedef itk::ImageFileWriter< itk::TractsToVectorImageFilter<float>::ItkDirectionImageType > WriterType;
      WriterType::Pointer writer = WriterType::New();

      std::string outfilename = outRoot;
      outfilename.append("_DIRECTIONS");
      outfilename.append(file_ending);

      writer->SetFileName(outfilename.c_str());
      writer->SetInput(itkImg);
      writer->Update();
    }

    if (verbose)
    {
      // write num direction image
      ItkUcharImgType::Pointer numDirImage = fOdfFilter->GetNumDirectionsImage();
      typedef itk::ImageFileWriter< ItkUcharImgType > WriterType;
      WriterType::Pointer writer = WriterType::New();

      std::string outfilename = outRoot;
      outfilename.append("_NUM_DIRECTIONS");
      outfilename.append(file_ending);

      writer->SetFileName(outfilename.c_str());
      writer->SetInput(numDirImage);
      writer->Update();
    }
  }
  catch (itk::ExceptionObject e)
  {
    std::cout << e;
    return EXIT_FAILURE;
  }
  catch (std::exception e)
  {
    std::cout << e.what();
    return EXIT_FAILURE;
  }
  catch (...)
  {
    std::cout << "ERROR!?!";
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//
#pragma once

#include <armnn/Descriptors.hpp>
#include <armnn/INetwork.hpp>
#include <armnn/IRuntime.hpp>

#include <Profiling.hpp>
#include <QuantizeHelper.hpp>
#include <ResolveType.hpp>

#include <boost/test/unit_test.hpp>

#include <vector>

namespace
{

using namespace armnn;

template<typename T>
bool ConstantUsageTest(const std::vector<BackendId>& computeDevice,
                       const TensorInfo& commonTensorInfo,
                       const std::vector<T>& inputData,
                       const std::vector<T>& constantData,
                       const std::vector<T>& expectedOutputData)
{
    // Create runtime in which test will run
    IRuntime::CreationOptions options;
    IRuntimePtr runtime(IRuntime::Create(options));

    // Builds up the structure of the network.
    INetworkPtr net(INetwork::Create());

    IConnectableLayer* input = net->AddInputLayer(0);
    IConnectableLayer* constant = net->AddConstantLayer(ConstTensor(commonTensorInfo, constantData));
    IConnectableLayer* add = net->AddAdditionLayer();
    IConnectableLayer* output = net->AddOutputLayer(0);

    input->GetOutputSlot(0).Connect(add->GetInputSlot(0));
    constant->GetOutputSlot(0).Connect(add->GetInputSlot(1));
    add->GetOutputSlot(0).Connect(output->GetInputSlot(0));

    // Sets the tensors in the network.
    input->GetOutputSlot(0).SetTensorInfo(commonTensorInfo);
    constant->GetOutputSlot(0).SetTensorInfo(commonTensorInfo);
    add->GetOutputSlot(0).SetTensorInfo(commonTensorInfo);

    // optimize the network
    IOptimizedNetworkPtr optNet = Optimize(*net, computeDevice, runtime->GetDeviceSpec());

    // Loads it into the runtime.
    NetworkId netId;
    runtime->LoadNetwork(netId, std::move(optNet));

    // Creates structures for input & output.
    std::vector<T> outputData(inputData.size());

    InputTensors inputTensors
    {
        {0, ConstTensor(runtime->GetInputTensorInfo(netId, 0), inputData.data())}
    };
    OutputTensors outputTensors
    {
        {0, Tensor(runtime->GetOutputTensorInfo(netId, 0), outputData.data())}
    };

    // Does the inference.
    runtime->EnqueueWorkload(netId, inputTensors, outputTensors);

    // Checks the results.
    return outputData == expectedOutputData;
}

inline bool ConstantUsageFloat32Test(const std::vector<BackendId>& backends)
{
    const TensorInfo commonTensorInfo({ 2, 3 }, DataType::Float32);

    return ConstantUsageTest(backends,
        commonTensorInfo,
        std::vector<float>{ 1.f, 2.f, 3.f, 4.f, 5.f, 6.f }, // Input.
        std::vector<float>{ 6.f, 5.f, 4.f, 3.f, 2.f, 1.f }, // Const input.
        std::vector<float>{ 7.f, 7.f, 7.f, 7.f, 7.f, 7.f }  // Expected output.
    );
}

inline bool ConstantUsageUint8Test(const std::vector<BackendId>& backends)
{
    TensorInfo commonTensorInfo({ 2, 3 }, DataType::QAsymmU8);

    const float scale = 0.023529f;
    const int8_t offset = -43;

    commonTensorInfo.SetQuantizationScale(scale);
    commonTensorInfo.SetQuantizationOffset(offset);

    return ConstantUsageTest(backends,
        commonTensorInfo,
        armnnUtils::QuantizedVector<uint8_t>({ 1.f, 2.f, 3.f, 4.f, 5.f, 6.f }, scale, offset), // Input.
        armnnUtils::QuantizedVector<uint8_t>({ 6.f, 5.f, 4.f, 3.f, 2.f, 1.f }, scale, offset), // Const input.
        armnnUtils::QuantizedVector<uint8_t>({ 7.f, 7.f, 7.f, 7.f, 7.f, 7.f }, scale, offset)  // Expected output.
    );
}

// Utility template for comparing tensor elements
template<DataType ArmnnType, typename T = ResolveType<ArmnnType>>
bool Compare(T a, T b, float tolerance = 0.000001f)
{
    if (ArmnnType == DataType::Boolean)
    {
        // NOTE: Boolean is represented as uint8_t (with zero equals
        // false and everything else equals true), therefore values
        // need to be casted to bool before comparing them
        return static_cast<bool>(a) == static_cast<bool>(b);
    }

    // NOTE: All other types can be cast to float and compared with
    // a certain level of tolerance
    return std::fabs(static_cast<float>(a) - static_cast<float>(b)) <= tolerance;
}

// Utility function to find the number of instances of a substring within a string.
int SubStringCounter(std::string& string, std::string&& substring)
{
    std::size_t found = 0;
    int count = 0;
    // Look for the substring starting from where we last found the substring
    while((found = string.find(substring, found)) != std::string::npos)
    {
        count++;
        // Offset by substring length to avoid finding the same substring twice
        found += substring.length();
    }
    return count;
}

template<DataType ArmnnIType, DataType ArmnnOType,
         typename TInput = ResolveType<ArmnnIType>, typename TOutput = ResolveType<ArmnnOType>>
void EndToEndLayerTestImpl(INetworkPtr network,
                           const std::map<int, std::vector<TInput>>& inputTensorData,
                           const std::map<int, std::vector<TOutput>>& expectedOutputData,
                           std::vector<BackendId> backends,
                           float tolerance = 0.000001f)
{
    // Create runtime in which test will run
    IRuntime::CreationOptions options;
    IRuntimePtr runtime(IRuntime::Create(options));

    // optimize the network
    IOptimizedNetworkPtr optNet = Optimize(*network, backends, runtime->GetDeviceSpec());

    // Loads it into the runtime.
    NetworkId netId;
    runtime->LoadNetwork(netId, std::move(optNet));

    InputTensors inputTensors;
    inputTensors.reserve(inputTensorData.size());
    for (auto&& it : inputTensorData)
    {
        inputTensors.push_back({it.first,
                                ConstTensor(runtime->GetInputTensorInfo(netId, it.first), it.second.data())});
    }
    OutputTensors outputTensors;
    outputTensors.reserve(expectedOutputData.size());
    std::map<int, std::vector<TOutput>> outputStorage;
    for (auto&& it : expectedOutputData)
    {
        std::vector<TOutput> out(it.second.size());
        outputStorage.emplace(it.first, out);
        outputTensors.push_back({it.first,
                                 Tensor(runtime->GetOutputTensorInfo(netId, it.first),
                                               outputStorage.at(it.first).data())});
    }

    // Does the inference.
    runtime->EnqueueWorkload(netId, inputTensors, outputTensors);

    // Checks the results.
    for (auto&& it : expectedOutputData)
    {
        std::vector<TOutput> out = outputStorage.at(it.first);
        for (unsigned int i = 0; i < out.size(); ++i)
        {
            BOOST_CHECK_MESSAGE(Compare<ArmnnOType>(it.second[i], out[i], tolerance) == true,
                    "Actual output: " << out[i] << ". Expected output:" << it.second[i]);

        }
    }
}

inline void ImportNonAlignedInputPointerTest(std::vector<BackendId> backends)
{
    using namespace armnn;

    // Create runtime in which test will run
    IRuntime::CreationOptions options;
    IRuntimePtr runtime(armnn::IRuntime::Create(options));

    // build up the structure of the network
    INetworkPtr net(INetwork::Create());

    IConnectableLayer* input = net->AddInputLayer(0);

    ActivationDescriptor descriptor;
    descriptor.m_Function = ActivationFunction::Square;
    IConnectableLayer* pooling = net->AddActivationLayer(descriptor);

    IConnectableLayer* output = net->AddOutputLayer(0);

    input->GetOutputSlot(0).Connect(pooling->GetInputSlot(0));
    pooling->GetOutputSlot(0).Connect(output->GetInputSlot(0));

    input->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 1, 4 }, DataType::Float32));
    pooling->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 1, 4 }, DataType::Float32));

    // Optimize the network
    IOptimizedNetworkPtr optNet = Optimize(*net, backends, runtime->GetDeviceSpec());
    BOOST_CHECK(optNet);

    // Loads it into the runtime.
    NetworkId netId;
    std::string ignoredErrorMessage;
    // Enable Importing
    INetworkProperties networkProperties(true, false);
    runtime->LoadNetwork(netId, std::move(optNet), ignoredErrorMessage, networkProperties);

    // Creates structures for input & output
    std::vector<float> inputData
    {
        1.0f, 2.0f, 3.0f, 4.0f
    };

    // Misaligned input
    float* misalignedInputData = reinterpret_cast<float*>(reinterpret_cast<char*>(inputData.data()) + 1);

    std::vector<float> outputData(4);

    // Aligned output
    float* alignedOutputData = outputData.data();

    InputTensors inputTensors
    {
        {0,armnn::ConstTensor(runtime->GetInputTensorInfo(netId, 0), misalignedInputData)},
    };
    OutputTensors outputTensors
    {
        {0,armnn::Tensor(runtime->GetOutputTensorInfo(netId, 0), alignedOutputData)}
    };

    runtime->GetProfiler(netId)->EnableProfiling(true);

    // Do the inference and expect it to fail with a ImportMemoryException
    BOOST_CHECK_THROW(runtime->EnqueueWorkload(netId, inputTensors, outputTensors), MemoryImportException);
}

inline void ExportNonAlignedOutputPointerTest(std::vector<BackendId> backends)
{
    using namespace armnn;

    // Create runtime in which test will run
    IRuntime::CreationOptions options;
    IRuntimePtr runtime(armnn::IRuntime::Create(options));

    // build up the structure of the network
    INetworkPtr net(INetwork::Create());

    IConnectableLayer* input = net->AddInputLayer(0);

    ActivationDescriptor descriptor;
    descriptor.m_Function = ActivationFunction::Square;
    IConnectableLayer* pooling = net->AddActivationLayer(descriptor);

    IConnectableLayer* output = net->AddOutputLayer(0);

    input->GetOutputSlot(0).Connect(pooling->GetInputSlot(0));
    pooling->GetOutputSlot(0).Connect(output->GetInputSlot(0));

    input->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 1, 4 }, DataType::Float32));
    pooling->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 1, 4 }, DataType::Float32));

    // Optimize the network
    IOptimizedNetworkPtr optNet = Optimize(*net, backends, runtime->GetDeviceSpec());
    BOOST_CHECK(optNet);

    // Loads it into the runtime.
    NetworkId netId;
    std::string ignoredErrorMessage;
    // Enable Importing and Exporting
    INetworkProperties networkProperties(true, true);
    runtime->LoadNetwork(netId, std::move(optNet), ignoredErrorMessage, networkProperties);

    // Creates structures for input & output
    std::vector<float> inputData
    {
        1.0f, 2.0f, 3.0f, 4.0f, 5.0f
    };

    // Aligned input
    float* alignedInputData = inputData.data();

    std::vector<float> outputData(5);

    // Misaligned output
    float* misalignedOutputData = reinterpret_cast<float*>(reinterpret_cast<char*>(outputData.data()) + 1);

    InputTensors inputTensors
    {
        {0,armnn::ConstTensor(runtime->GetInputTensorInfo(netId, 0), alignedInputData)},
    };
    OutputTensors outputTensors
    {
        {0,armnn::Tensor(runtime->GetOutputTensorInfo(netId, 0), misalignedOutputData)}
    };

    // Do the inference and expect it to fail with a ExportMemoryException
    if (backends[0] == Compute::CpuAcc)
    {
        // For CpuAcc the NeonTensorHandle will throw its own exception on misaligned memory
        BOOST_CHECK_THROW(runtime->EnqueueWorkload(netId, inputTensors, outputTensors), MemoryImportException);
    }
    else
    {
        BOOST_CHECK_THROW(runtime->EnqueueWorkload(netId, inputTensors, outputTensors), MemoryExportException);
    }
}

inline void ImportAlignedPointerTest(std::vector<BackendId> backends)
{
    using namespace armnn;

    // Create runtime in which test will run
    IRuntime::CreationOptions options;
    IRuntimePtr runtime(armnn::IRuntime::Create(options));

    // build up the structure of the network
    INetworkPtr net(INetwork::Create());

    IConnectableLayer* input = net->AddInputLayer(0);

    ActivationDescriptor descriptor;
    descriptor.m_Function = ActivationFunction::Square;
    IConnectableLayer* pooling = net->AddActivationLayer(descriptor);

    IConnectableLayer* output = net->AddOutputLayer(0);

    input->GetOutputSlot(0).Connect(pooling->GetInputSlot(0));
    pooling->GetOutputSlot(0).Connect(output->GetInputSlot(0));

    input->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 1, 4 }, DataType::Float32));
    pooling->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 1, 4 }, DataType::Float32));

    // Optimize the network
    IOptimizedNetworkPtr optNet = Optimize(*net, backends, runtime->GetDeviceSpec());
    BOOST_CHECK(optNet);

    // Loads it into the runtime.
    NetworkId netId;
    std::string ignoredErrorMessage;
    // Enable Importing
    INetworkProperties networkProperties(true, true);
    runtime->LoadNetwork(netId, std::move(optNet), ignoredErrorMessage, networkProperties);

    // Creates structures for input & output
    std::vector<float> inputData
    {
        1.0f, 2.0f, 3.0f, 4.0f
    };

    std::vector<float> outputData(4);

    std::vector<float> expectedOutput
    {
        1.0f, 4.0f, 9.0f, 16.0f
    };

    InputTensors inputTensors
    {
        {0,armnn::ConstTensor(runtime->GetInputTensorInfo(netId, 0), inputData.data())},
    };
    OutputTensors outputTensors
    {
        {0,armnn::Tensor(runtime->GetOutputTensorInfo(netId, 0), outputData.data())}
    };

    runtime->GetProfiler(netId)->EnableProfiling(true);

    // Do the inference
    runtime->EnqueueWorkload(netId, inputTensors, outputTensors);

    // Retrieve the Profiler.Print() output to get the workload execution
    ProfilerManager& profilerManager = armnn::ProfilerManager::GetInstance();
    std::stringstream ss;
    profilerManager.GetProfiler()->Print(ss);;
    std::string dump = ss.str();

    // Contains ActivationWorkload
    std::size_t found = dump.find("ActivationWorkload");
    BOOST_TEST(found != std::string::npos);

    // Contains SyncMemGeneric
    found = dump.find("SyncMemGeneric");
    BOOST_TEST(found != std::string::npos);

    // Does not contain CopyMemGeneric
    found = dump.find("CopyMemGeneric");
    BOOST_TEST(found == std::string::npos);

    // Check output is as expected
    BOOST_TEST(outputData == expectedOutput);
}

inline void ImportOnlyWorkload(std::vector<BackendId> backends)
{
    using namespace armnn;

    IRuntime::CreationOptions options;
    IRuntimePtr runtime(IRuntime::Create(options));

    // Builds up the structure of the network.
    INetworkPtr net(INetwork::Create());

    IConnectableLayer* input = net->AddInputLayer(0);

    ActivationDescriptor descriptor;
    descriptor.m_Function = ActivationFunction::Square;
    IConnectableLayer* pooling = net->AddActivationLayer(descriptor);

    IConnectableLayer* output = net->AddOutputLayer(0);

    input->GetOutputSlot(0).Connect(pooling->GetInputSlot(0));
    pooling->GetOutputSlot(0).Connect(output->GetInputSlot(0));

    input->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 1, 4 }, DataType::Float32));
    pooling->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 1, 4 }, DataType::Float32));

    // optimize the network
    IOptimizedNetworkPtr optNet = Optimize(*net, backends, runtime->GetDeviceSpec());

    BOOST_TEST_CHECKPOINT("Load Network");
    // Load it into the runtime. It should pass.
    NetworkId netId;
    std::string ignoredErrorMessage;
    INetworkProperties networkProperties(true, false);
    BOOST_TEST(runtime->LoadNetwork(netId, std::move(optNet),ignoredErrorMessage, networkProperties)
               == Status::Success);

    BOOST_TEST_CHECKPOINT("Generate Data");
    // Creates structures for input & output
    std::vector<float> inputData
    {
        1.0f, 2.0f, 3.0f, 4.0f
    };

    std::vector<float> outputData(4);

    std::vector<float> expectedOutput
    {
         1.0f, 4.0f, 9.0f, 16.0f
    };

    BOOST_TEST_CHECKPOINT("Create Network");
    InputTensors inputTensors
    {
        {0,armnn::ConstTensor(runtime->GetInputTensorInfo(netId, 0), inputData.data())},
    };
    OutputTensors outputTensors
    {
        {0,armnn::Tensor(runtime->GetOutputTensorInfo(netId, 0), outputData.data())}
    };

    BOOST_TEST_CHECKPOINT("Get Profiler");

    runtime->GetProfiler(netId)->EnableProfiling(true);

    BOOST_TEST_CHECKPOINT("Run Inference");
    // Do the inference
    runtime->EnqueueWorkload(netId, inputTensors, outputTensors);

    BOOST_TEST_CHECKPOINT("Print Profiler");
    // Retrieve the Profiler.Print() output to get the workload execution
    ProfilerManager& profilerManager = armnn::ProfilerManager::GetInstance();
    std::stringstream ss;
    profilerManager.GetProfiler()->Print(ss);;
    std::string dump = ss.str();

    // Check there are no SyncMemGeneric workloads as we didn't export
    BOOST_TEST_CHECKPOINT("Find SyncMemGeneric");
    int count = SubStringCounter(dump, "SyncMemGeneric");
    BOOST_TEST(count == 0);

    // Should only be 1 CopyMemGeneric for the output as we imported
    BOOST_TEST_CHECKPOINT("Find CopyMemGeneric");
    count = SubStringCounter(dump, "CopyMemGeneric");
    BOOST_TEST(count == 1);

    // Check the output is correct
    BOOST_CHECK_EQUAL_COLLECTIONS(outputData.begin(), outputData.end(), expectedOutput.begin(), expectedOutput.end());
}

inline void ExportOnlyWorkload(std::vector<BackendId> backends)
{
    using namespace armnn;

    IRuntime::CreationOptions options;
    IRuntimePtr runtime(IRuntime::Create(options));

    // Builds up the structure of the network.
    INetworkPtr net(INetwork::Create());

    IConnectableLayer* input = net->AddInputLayer(0);

    ActivationDescriptor descriptor;
    descriptor.m_Function = ActivationFunction::Square;
    IConnectableLayer* pooling = net->AddActivationLayer(descriptor);

    IConnectableLayer* output = net->AddOutputLayer(0);

    input->GetOutputSlot(0).Connect(pooling->GetInputSlot(0));
    pooling->GetOutputSlot(0).Connect(output->GetInputSlot(0));

    input->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 1, 4 }, DataType::Float32));
    pooling->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 1, 4 }, DataType::Float32));

    // optimize the network
    IOptimizedNetworkPtr optNet = Optimize(*net, backends, runtime->GetDeviceSpec());

    BOOST_TEST_CHECKPOINT("Load Network");
    // Load it into the runtime. It should pass.
    NetworkId netId;
    std::string ignoredErrorMessage;
    INetworkProperties networkProperties(false, true);
    BOOST_TEST(runtime->LoadNetwork(netId, std::move(optNet),ignoredErrorMessage, networkProperties)
               == Status::Success);

    BOOST_TEST_CHECKPOINT("Generate Data");
    // Creates structures for input & output
    std::vector<float> inputData
    {
        1.0f, 2.0f, 3.0f, 4.0f
    };

    std::vector<float> outputData(4);

    std::vector<float> expectedOutput
    {
         1.0f, 4.0f, 9.0f, 16.0f
    };

    BOOST_TEST_CHECKPOINT("Create Network");
    InputTensors inputTensors
    {
        {0,armnn::ConstTensor(runtime->GetInputTensorInfo(netId, 0), inputData.data())},
    };
    OutputTensors outputTensors
    {
        {0,armnn::Tensor(runtime->GetOutputTensorInfo(netId, 0), outputData.data())}
    };

    BOOST_TEST_CHECKPOINT("Get Profiler");

    runtime->GetProfiler(netId)->EnableProfiling(true);

    BOOST_TEST_CHECKPOINT("Run Inference");
    // Do the inference
    runtime->EnqueueWorkload(netId, inputTensors, outputTensors);

    BOOST_TEST_CHECKPOINT("Print Profiler");
    // Retrieve the Profiler.Print() output to get the workload execution
    ProfilerManager& profilerManager = armnn::ProfilerManager::GetInstance();
    std::stringstream ss;
    profilerManager.GetProfiler()->Print(ss);;
    std::string dump = ss.str();

    // Check there is a SyncMemGeneric workload as we exported
    BOOST_TEST_CHECKPOINT("Find SyncMemGeneric");
    int count = SubStringCounter(dump, "SyncMemGeneric");
    BOOST_TEST(count == 1);

    // Should be 1 CopyMemGeneric for the output as we did not import
    BOOST_TEST_CHECKPOINT("Find CopyMemGeneric");
    count = SubStringCounter(dump, "CopyMemGeneric");
    BOOST_TEST(count == 1);

    // Check the output is correct
    BOOST_CHECK_EQUAL_COLLECTIONS(outputData.begin(), outputData.end(), expectedOutput.begin(), expectedOutput.end());
}

inline void ImportAndExportWorkload(std::vector<BackendId> backends)
{
    using namespace armnn;

    IRuntime::CreationOptions options;
    IRuntimePtr runtime(IRuntime::Create(options));

    // Builds up the structure of the network.
    INetworkPtr net(INetwork::Create());

    IConnectableLayer* input = net->AddInputLayer(0);

    ActivationDescriptor descriptor;
    descriptor.m_Function = ActivationFunction::Square;
    IConnectableLayer* pooling = net->AddActivationLayer(descriptor);

    IConnectableLayer* output = net->AddOutputLayer(0);

    input->GetOutputSlot(0).Connect(pooling->GetInputSlot(0));
    pooling->GetOutputSlot(0).Connect(output->GetInputSlot(0));

    input->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 1, 4 }, DataType::Float32));
    pooling->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 1, 4 }, DataType::Float32));

    IOptimizedNetworkPtr optNet = Optimize(*net, backends, runtime->GetDeviceSpec());

    BOOST_TEST_CHECKPOINT("Load Network");
    // Load it into the runtime. It should pass.
    NetworkId netId;
    std::string ignoredErrorMessage;
    INetworkProperties networkProperties(true, true);
    BOOST_TEST(runtime->LoadNetwork(netId, std::move(optNet),ignoredErrorMessage, networkProperties)
               == Status::Success);

    BOOST_TEST_CHECKPOINT("Generate Data");
    // Creates structures for input & output
    std::vector<float> inputData
    {
        1.0f, 2.0f, 3.0f, 4.0f
    };

    std::vector<float> outputData(4);

    std::vector<float> expectedOutput
    {
         1.0f, 4.0f, 9.0f, 16.0f
    };

    BOOST_TEST_CHECKPOINT("Create Network");
    InputTensors inputTensors
    {
        {0,armnn::ConstTensor(runtime->GetInputTensorInfo(netId, 0), inputData.data())},
    };
    OutputTensors outputTensors
    {
        {0,armnn::Tensor(runtime->GetOutputTensorInfo(netId, 0), outputData.data())}
    };

    BOOST_TEST_CHECKPOINT("Get Profiler");

    runtime->GetProfiler(netId)->EnableProfiling(true);

    BOOST_TEST_CHECKPOINT("Run Inference");
    // Do the inference
    runtime->EnqueueWorkload(netId, inputTensors, outputTensors);

    BOOST_TEST_CHECKPOINT("Print Profiler");
    // Retrieve the Profiler.Print() output to get the workload execution
    ProfilerManager& profilerManager = armnn::ProfilerManager::GetInstance();
    std::stringstream ss;
    profilerManager.GetProfiler()->Print(ss);;
    std::string dump = ss.str();

    // Check there is a SyncMemGeneric workload as we exported
    BOOST_TEST_CHECKPOINT("Find SyncMemGeneric");
    int count = SubStringCounter(dump, "SyncMemGeneric");
    BOOST_TEST(count == 1);

    // Shouldn't be any CopyMemGeneric workloads
    BOOST_TEST_CHECKPOINT("Find CopyMemGeneric");
    count = SubStringCounter(dump, "CopyMemGeneric");
    BOOST_TEST(count == 0);

    // Check the output is correct
    BOOST_CHECK_EQUAL_COLLECTIONS(outputData.begin(), outputData.end(), expectedOutput.begin(), expectedOutput.end());
}

inline void ExportOutputWithSeveralOutputSlotConnectionsTest(std::vector<BackendId> backends)
{
    using namespace armnn;

    // Create runtime in which test will run
    IRuntime::CreationOptions options;
    IRuntimePtr runtime(armnn::IRuntime::Create(options));

    // build up the structure of the network
    INetworkPtr net(INetwork::Create());

    IConnectableLayer* input = net->AddInputLayer(0);

    ActivationDescriptor descriptor;
    descriptor.m_Function = ActivationFunction::Square;
    IConnectableLayer* activation = net->AddActivationLayer(descriptor);

    IConnectableLayer* output0 = net->AddOutputLayer(0);
    IConnectableLayer* output1 = net->AddOutputLayer(1);

    input->GetOutputSlot(0).Connect(activation->GetInputSlot(0));
    activation->GetOutputSlot(0).Connect(output0->GetInputSlot(0));
    activation->GetOutputSlot(0).Connect(output1->GetInputSlot(0));

    input->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 4, 1 }, DataType::Float32));
    activation->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 1, 1, 4, 1 }, DataType::Float32));

    // Optimize the network
    IOptimizedNetworkPtr optNet = Optimize(*net, backends, runtime->GetDeviceSpec());

    // Loads it into the runtime.
    NetworkId netId;
    std::string ignoredErrorMessage;
    // Enable Importing
    INetworkProperties networkProperties(true, true);
    runtime->LoadNetwork(netId, std::move(optNet), ignoredErrorMessage, networkProperties);

    // Creates structures for input & output
    std::vector<float> inputData
    {
        1.0f, 2.0f, 3.0f, 4.0f
    };

    std::vector<float> outputData0(4);
    std::vector<float> outputData1(4);

    std::vector<float> expectedOutput
    {
         1.0f, 4.0f, 9.0f, 16.0f
    };

    InputTensors inputTensors
    {
        {0,armnn::ConstTensor(runtime->GetInputTensorInfo(netId, 0), inputData.data())},
    };
    OutputTensors outputTensors
    {
        {0,armnn::Tensor(runtime->GetOutputTensorInfo(netId, 0), outputData0.data())},
        {1,armnn::Tensor(runtime->GetOutputTensorInfo(netId, 1), outputData1.data())}
    };

    // The result of the inference is not important, just the fact that there
    // should not be CopyMemGeneric workloads.
    runtime->GetProfiler(netId)->EnableProfiling(true);

    // Do the inference
    runtime->EnqueueWorkload(netId, inputTensors, outputTensors);

    // Retrieve the Profiler.Print() output to get the workload execution
    ProfilerManager& profilerManager = armnn::ProfilerManager::GetInstance();
    std::stringstream ss;
    profilerManager.GetProfiler()->Print(ss);
    std::string dump = ss.str();

    std::size_t found = std::string::npos;

    if (backends[0] == Compute::CpuRef)
    {
        found = dump.find("RefActivationWorkload");
    }
    else if (backends[0] == Compute::CpuAcc)
    {
        found = dump.find("NeonActivationWorkload");
    }
    else if (backends[0] == Compute::GpuAcc)
    {
        found = dump.find("ClActivationWorkload");
    }

    BOOST_TEST(found != std::string::npos);
    // No contains SyncMemGeneric
    found = dump.find("SyncMemGeneric");
    BOOST_TEST(found == std::string::npos);
    // Contains CopyMemGeneric
    found = dump.find("CopyMemGeneric");
    BOOST_TEST(found != std::string::npos);

    // Check that the outputs are correct
    BOOST_CHECK_EQUAL_COLLECTIONS(outputData0.begin(), outputData0.end(),
                                  expectedOutput.begin(), expectedOutput.end());
    BOOST_CHECK_EQUAL_COLLECTIONS(outputData1.begin(), outputData1.end(),
                                  expectedOutput.begin(), expectedOutput.end());
}

inline void StridedSliceInvalidSliceEndToEndTest(std::vector<BackendId> backends)
{
    using namespace armnn;

    // Create runtime in which test will run
    IRuntime::CreationOptions options;
    IRuntimePtr runtime(armnn::IRuntime::Create(options));

    // build up the structure of the network
    INetworkPtr net(INetwork::Create());

    IConnectableLayer* input = net->AddInputLayer(0);

    // Configure a strided slice with a stride the same size as the input but with a ShrinkAxisMask on the first
    // dim of the output to make it too small to hold the specified slice.
    StridedSliceDescriptor descriptor;
    descriptor.m_Begin          = {0, 0};
    descriptor.m_End            = {2, 3};
    descriptor.m_Stride         = {1, 1};
    descriptor.m_BeginMask      = 0;
    descriptor.m_EndMask        = 0;
    descriptor.m_ShrinkAxisMask = 1;
    IConnectableLayer* stridedSlice = net->AddStridedSliceLayer(descriptor);

    IConnectableLayer* output0 = net->AddOutputLayer(0);

    input->GetOutputSlot(0).Connect(stridedSlice->GetInputSlot(0));
    stridedSlice->GetOutputSlot(0).Connect(output0->GetInputSlot(0));

    input->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 2, 3 }, DataType::Float32));
    stridedSlice->GetOutputSlot(0).SetTensorInfo(TensorInfo({ 3 }, DataType::Float32));

    // Attempt to optimize the network and check that the correct exception is thrown
    BOOST_CHECK_THROW(Optimize(*net, backends, runtime->GetDeviceSpec()), armnn::LayerValidationException);
}

} // anonymous namespace


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file Signature.cpp
 * @brief signatures for conditional densities
 * @author Frank Dellaert
 * @date Feb 27, 2011
 */

#include <sstream>

#include "Signature.h"

#include <boost/spirit/include/qi.hpp> // for parsing
#include <boost/spirit/include/phoenix.hpp> // for qi::_val

namespace gtsam {

  using namespace std;

  namespace qi = boost::spirit::qi;
  namespace ph = boost::phoenix;

  // parser for strings of form "99/1 80/20" etc...
  namespace parser {
    typedef string::const_iterator It;
    using boost::phoenix::val;
    using boost::phoenix::ref;
    using boost::phoenix::push_back;

    // Special rows, true and false
    Signature::Row createF() {
      Signature::Row r(2);
      r[0] = 1;
      r[1] = 0;
      return r;
    }
    Signature::Row createT() {
      Signature::Row r(2);
      r[0] = 0;
      r[1] = 1;
      return r;
    }
    Signature::Row T = createT(), F = createF();

    // Special tables (inefficient, but do we care for user input?)
    Signature::Table logic(bool ff, bool ft, bool tf, bool tt) {
      Signature::Table t(4);
      t[0] = ff ? T : F;
      t[1] = ft ? T : F;
      t[2] = tf ? T : F;
      t[3] = tt ? T : F;
      return t;
    }

    struct Grammar {
      qi::rule<It, qi::space_type, Signature::Table()> table, or_, and_, rows;
      qi::rule<It, Signature::Row()> true_, false_, row;
      Grammar() {
        table = or_ | and_ | rows;
        or_ = qi::lit("OR")[qi::_val = logic(false, true, true, true)];
        and_ = qi::lit("AND")[qi::_val = logic(false, false, false, true)];
        rows = +(row | true_ | false_); // only loads first of the rows under boost 1.42
        row = qi::double_ >> +("/" >> qi::double_);
        true_ = qi::lit("T")[qi::_val = T];
        false_ = qi::lit("F")[qi::_val = F];
      }
    } grammar;

    // Create simpler parsing function to avoid the issue of only parsing a single row
    bool parse_table(const string& spec, Signature::Table& table) {
      // check for OR, AND on whole phrase
      It f = spec.begin(), l = spec.end();
      if (qi::parse(f, l,
            qi::lit("OR")[ph::ref(table) = logic(false, true, true, true)]) ||
          qi::parse(f, l,
            qi::lit("AND")[ph::ref(table) = logic(false, false, false, true)]))
        return true;

      // tokenize into separate rows
      istringstream iss(spec);
      string token;
      while (iss >> token) {
        Signature::Row values;
        It tf = token.begin(), tl = token.end();
        bool r = qi::parse(tf, tl,
            qi::double_[push_back(ph::ref(values), qi::_1)] >> +("/" >> qi::double_[push_back(ph::ref(values), qi::_1)]) |
            qi::lit("T")[ph::ref(values) = T] |
            qi::lit("F")[ph::ref(values) = F] );
        if (!r)
          return false;
        table.push_back(values);
      }

      return true;
    }
  } // \namespace parser

  ostream& operator <<(ostream &os, const Signature::Row &row) {
    os << row[0];
    for (size_t i = 1; i < row.size(); i++)
      os << " " << row[i];
    return os;
  }

  ostream& operator <<(ostream &os, const Signature::Table &table) {
    for (size_t i = 0; i < table.size(); i++)
      os << table[i] << endl;
    return os;
  }

  Signature::Signature(const DiscreteKey& key) :
    key_(key) {
  }

  DiscreteKeys Signature::discreteKeysParentsFirst() const {
    DiscreteKeys keys;
    for(const DiscreteKey& key: parents_)
      keys.push_back(key);
    keys.push_back(key_);
    return keys;
  }

  vector<Key> Signature::indices() const {
    vector<Key> js;
    js.push_back(key_.first);
    for(const DiscreteKey& key: parents_)
      js.push_back(key.first);
    return js;
  }

  vector<double> Signature::cpt() const {
    vector<double> cpt;
    if (table_) {
      for(const Row& row: *table_)
              for(const double& x: row)
                      cpt.push_back(x);
    }
    return cpt;
  }

  Signature& Signature::operator,(const DiscreteKey& parent) {
    parents_.push_back(parent);
    return *this;
  }

  static void normalize(Signature::Row& row) {
    double sum = 0;
    for (size_t i = 0; i < row.size(); i++)
      sum += row[i];
    for (size_t i = 0; i < row.size(); i++)
      row[i] /= sum;
  }

  Signature& Signature::operator=(const string& spec) {
    spec_.reset(spec);
    Table table;
    // NOTE: using simpler parse function to ensure boost back compatibility
//    parser::It f = spec.begin(), l = spec.end();
    bool success = //
//        qi::phrase_parse(f, l, parser::grammar.table, qi::space, table); // using full grammar
          parser::parse_table(spec, table);
    if (success) {
      for(Row& row: table)
        normalize(row);
      table_.reset(table);
    }
    return *this;
  }

  Signature& Signature::operator=(const Table& t) {
    Table table = t;
    for(Row& row: table)
      normalize(row);
    table_.reset(table);
    return *this;
  }

  ostream& operator <<(ostream &os, const Signature &s) {
    os << s.key_.first;
    if (s.parents_.empty()) {
      os << " % ";
    } else {
      os << " | " << s.parents_[0].first;
      for (size_t i = 1; i < s.parents_.size(); i++)
        os << " && " << s.parents_[i].first;
      os << " = ";
    }
    os << (s.spec_ ? *s.spec_ : "no spec") << endl;
    if (s.table_)
      os << (*s.table_);
    else
      os << "spec could not be parsed" << endl;
    return os;
  }

  Signature operator|(const DiscreteKey& key, const DiscreteKey& parent) {
    Signature s(key);
    return s, parent;
  }

  Signature operator%(const DiscreteKey& key, const string& parent) {
    Signature s(key);
    return s = parent;
  }

  Signature operator%(const DiscreteKey& key, const Signature::Table& parent) {
    Signature s(key);
    return s = parent;
  }

} // namespace gtsam


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"
#include "graph.hh"
#include "graph_properties.hh"

#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct get_planar_embedding
{
    template <class EdgeMap>
    class edge_inserter
    {
    public:
        edge_inserter(EdgeMap edge_map): _edge_map(edge_map) {}

        edge_inserter& operator++() { return *this; }
        edge_inserter& operator++(int) { return *this; }
        edge_inserter& operator*() { return *this; }

        template <class Key>
        edge_inserter& operator=(const Key& e)
        {
            _edge_map[e] = 1;
            return *this;
        }

    private:
        EdgeMap _edge_map;
    };

    template <class Graph, class VertexIndex, class EdgeIndex, class EmbedMap,
              class KurMap>
    void operator()(Graph& g, VertexIndex vertex_index, EdgeIndex edge_index,
                    EmbedMap embed_map, KurMap kur_map, bool& is_planar) const
    {
        edge_inserter<KurMap> kur_insert(kur_map);
        unchecked_vector_property_map
            <vector<typename graph_traits<Graph>::edge_descriptor>, VertexIndex>
            embedding(vertex_index, num_vertices(g));
        is_planar = boyer_myrvold_planarity_test
            (boyer_myrvold_params::graph = g,
             boyer_myrvold_params::edge_index_map = edge_index,
             boyer_myrvold_params::embedding = embedding,
             boyer_myrvold_params::kuratowski_subgraph = kur_insert);

        parallel_vertex_loop
            (g,
             [&](auto v)
             {
                 embed_map[v].clear();
                 for (auto& e : embedding[v])
                     embed_map[v].push_back(edge_index[e]);
             });
    }

    template <class Graph, class VertexIndex, class EdgeIndex, class KurMap>
    void operator()(Graph& g, VertexIndex, EdgeIndex edge_index,
                    dummy_property_map, KurMap kur_map,
                    bool& is_planar) const
    {
        edge_inserter<KurMap> kur_insert(kur_map);
        is_planar = boyer_myrvold_planarity_test
            (boyer_myrvold_params::graph = g,
             boyer_myrvold_params::edge_index_map = edge_index,
             boyer_myrvold_params::kuratowski_subgraph = kur_insert);
    }
};

bool is_planar(GraphInterface& gi, boost::any embed_map, boost::any kur_map)
{
    bool is_planar;

    if (embed_map.empty())
        embed_map = dummy_property_map();
    if (kur_map.empty())
        kur_map = dummy_property_map();

    typedef mpl::push_back<writable_edge_scalar_properties,
                           dummy_property_map>::type edge_map_types;
    typedef mpl::push_back<vertex_scalar_vector_properties,
                           dummy_property_map>::type vertex_map_types;

    run_action<graph_tool::detail::never_directed>()
        (gi, std::bind(get_planar_embedding(), std::placeholders::_1, gi.get_vertex_index(),
                       gi.get_edge_index(), std::placeholders::_2, std::placeholders::_3,
                       std::ref(is_planar)),
         vertex_map_types(), edge_map_types())
        (embed_map, kur_map);
    return is_planar;
}


#include <iostream>
#include <boost/scoped_ptr.hpp>
#include <vtkPolyData.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkVersion.h>
#include "StatisticalModel.h"
#include "vtkStandardMeshRepresenter.h"
#include "computeMeanCLP.h"

using namespace statismo;

void saveSample(const vtkPolyData* pd, const std::string& resdir, const std::string& basename)
{
    std::string filename = resdir +std::string("/") + basename;

    vtkPolyDataWriter* w = vtkPolyDataWriter::New();
#if (VTK_MAJOR_VERSION == 5 )
    w->SetInput(const_cast<vtkPolyData*>(pd));
#else
    w->SetInputData(const_cast<vtkPolyData*>(pd));
#endif
    w->SetFileName(filename.c_str());
    w->Update();
}

// illustrates how to load a shape model and the basic sampling functinality
int main(int argc, char** argv)
{
    PARSE_ARGS;

    if(argc < 7)
    {
        std::cout << "Usage " << argv[0] << " [--groupnumber <int>] [--shapemodel <std::string>] [--resultdir <std::string>] " << std::endl;
        return 1;
    }

    typedef vtkStandardMeshRepresenter RepresenterType;
    typedef StatisticalModel<vtkPolyData> StatisticalModelType;

    RepresenterType* representer = RepresenterType::Create();
    boost::scoped_ptr<StatisticalModelType> model(StatisticalModelType::Load(representer, shapemodel));

    // Get the model mean
    vtkPolyData* mean = model->DrawMean();
    std::string vtkFile = "meanGroup" + std::to_string(groupnumber) + ".vtk";
    saveSample(mean, resultdir, vtkFile);
    mean->Delete();

    std::cout << "Successfully saved mean as " << "meanGroup" << groupnumber << ".vtk" << std::endl;

    return 0;
}


//---- tests/matrix_tests.cc ------------------------------- -*- C++ -*- ----//
//
//                                 Snap
//
//                      Copyright (c) 2016 Rob Clucas
//                    Distributed under the MIT License
//                (See accompanying file LICENSE or copy at
//                   https://opensource.org/licenses/MIT)
//
// ========================================================================= //
//
/// \file  matrix_tests.cc
/// \brief Test file to test the snap Matrix class.
//
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE SnapMatrixTests

#include <boost/test/unit_test.hpp>
#include "snap/matrix/matrix.hpp"

using namespace snap;

BOOST_AUTO_TEST_SUITE(SnapMatrixSuite)

BOOST_AUTO_TEST_CASE(canDefaultConstructMatGrey8) {
  Matrix<mat::FM_GREY_8> mat;

  BOOST_CHECK(mat.rows() == 0);
  BOOST_CHECK(mat.cols() == 0);
  BOOST_CHECK(mat.size() == 0);
}

BOOST_AUTO_TEST_SUITE_END()


/*=============================================================================

  NifTK: A software platform for medical image computing.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "niftkQDSCommon.h"
#include <boost/gil/gil_all.hpp>
#include <boost/static_assert.hpp>
#include <cassert>
#include <stdexcept>

#ifdef _OPENMP
#include <omp.h>
#endif

namespace niftk
{

//-----------------------------------------------------------------------------
bool RefPoint::operator<(const RefPoint& rhs) const
{
  if (x < rhs.x)
    return true;
  if (x > rhs.x)
    return false;
  if (y < rhs.y)
    return true;
  if (y > rhs.y)
    return false;
  return false;
}

//-----------------------------------------------------------------------------
void BuildTextureDescriptor(const boost::gil::gray8c_view_t src, const boost::gil::gray8_view_t dst)
{
  if (src.dimensions() != dst.dimensions())
  {
    throw std::runtime_error("Input image dimensions does not match output image");
  }

  // filled only to put a defined value along the image edge which is not processed by the loop below.
  // this shouldnt be necessary though because the propagation stays away from the border anyway.
  boost::gil::fill_pixels(dst, boost::gil::gray8_pixel_t(0));

  // no point running more than 2 threads here, loop is very simple to start with.
  // with more threads we'll just end up with cache thrash, etc.
#ifdef _OPENMP
  #pragma omp parallel for num_threads(2)
#endif
  for (int y = 1; y < src.height() - 1; ++y)
  {
    for (int x = 1; x < src.width() - 1; ++x)
    {
      const boost::gil::gray8c_pixel_t& pixel = src(x, y);

      //difference
      int a = std::abs(pixel - src(x - 1, y));
      int b = std::abs(pixel - src(x + 1, y));
      int c = std::abs(pixel - src(x, y - 1));
      int d = std::abs(pixel - src(x, y + 1));

      // dan had this as max(), but min() would make more sense?
      // pyramidal qds totally breaks with min().
      int val = std::max(a, std::max(b, std::max(c, d)));
      //int val = std::min(a, std::min(b, std::min(c, d)));

      // clamp?
      assert(val >= 0);
      assert(val <= 255);

      dst(x, y) = val;
    }
  }
}

//-----------------------------------------------------------------------------
// NOTE: returns values in the range of [-1...+1]
float Zncc_C1(int p0x, int p0y, int p1x, int p1y, int w,
              boost::gil::gray8c_view_t img0,
              boost::gil::gray8c_view_t img1,
              boost::gil::gray32sc_view_t integral0,
              boost::gil::gray32sc_view_t integral1,
              boost::gil::gray64fc_view_t square0,
              boost::gil::gray64fc_view_t square1)
{
  // random variables used by code below
  // this is a relic from dan's code
  const unsigned char*  data0 = &img0(0, 0)[0];
  const unsigned char*  data1 = &img1(0, 0)[0];
  const int*            sum0  = &integral0(0, 0)[0];
  const int*            sum1  = &integral1(0, 0)[0];
  const double*         ssum0 = (double*) &square0(0, 0)[0];
  const double*         ssum1 = (double*) &square1(0, 0)[0];

  int   Step  = &img0(0, 1)[0] - &img0(0, 0)[0];
  int   Steps = &integral0(0, 1)[0] - &integral0(0, 0)[0];

  int x,
      y,
      otl,
      otr,
      obl,
      obr;

  double  m0,
          m1,
          s0,
          s1;

  const float wa = (float) ((2 * w + 1) * (2 * w + 1));

  int boy0 = (p0y - w) * Step + (p0x - w);
  int boy1 = (p1y - w) * Step + (p1x - w);

  int oy0=boy0,
      oy1=boy1;

  // offsets for corners top-left, top-right, bottom-left, bottom-right
  int   w1 = w + 1;

  // offsets for left image
  otl = (p0y -  w) * Steps + (p0x - w);
  otr = (p0y -  w) * Steps + (p0x + w1);
  obl = (p0y + w1) * Steps + (p0x - w);
  obr = (p0y + w1) * Steps + (p0x + w1);

  // sum and squared sum for left window
  m0 = ((sum0[obr] +  sum0[otl]) - ( sum0[obl] +  sum0[otr]));
  s0 = (ssum0[obr] + ssum0[otl]) - (ssum0[obl] + ssum0[otr]);

  // offsets for right image
  otl = (p1y -  w) * Steps + (p1x - w);
  otr = (p1y -  w) * Steps + (p1x + w1);
  obl = (p1y + w1) * Steps + (p1x - w);
  obr = (p1y + w1) * Steps + (p1x + w1);

  // sum and squares sum for right window
  m1 = ((sum1[obr] +  sum1[otl]) - ( sum1[obl] + sum1[otr]));
  s1 = (ssum1[obr] + ssum1[otl]) - (ssum1[obl] + ssum1[otr]);

  // window means
  m0 /= wa;
  m1 /= wa;

  // standard deviations
  s0 = std::sqrt(s0 - wa * m0 * m0);
  s1 = std::sqrt(s1 - wa * m1 * m1);

  // we need to check for these specifically otherwise the final eq blows up
  if ((s0 <= 0) || (s1 <= 0))
  {
    // if stddev is zero then we have flat-line input
    // if both are flat then we have perfect correlation
    if ((s0 <= 0) && (s1 <= 0))
    {
      return 1;
    }
    // otherwise, don't know actually...
    return 0;
  }

  float zncc = 0;
  for (y = -w; y <= w; ++y, oy1 += Step, oy0 += Step)
  {
    int ox0 = 0;
    int ox1 = 0;
    const unsigned char* line0 = &data0[oy0];
    const unsigned char* line1 = &data1[oy1];
    for (x = -w; x <= w; ++x)
    {
      zncc += (float) line0[ox0++] * (float) line1[ox1++];
    }
  }

  // the final result
  zncc = (zncc - wa * m0 * m1) / (s0 * s1);
  return zncc;
}

//-----------------------------------------------------------------------------
QDSInterface::~QDSInterface()
{
}

} // namespace


#include <fstream>
#include <iostream>
#include <vector>

#include "boost/filesystem.hpp"
#include <boost/algorithm/string.hpp>
#include <liblas/detail/reader/reader.hpp>

#include "LASPointReader.h"

namespace fs = boost::filesystem;

using std::ifstream;
using std::cout;
using std::endl;
using std::vector;
using boost::iequals;
using std::ios;
using liblas::VariableRecord;

AABB LIBLASReader::getAABB(){
    AABB aabb;

    const liblas::Header &header = reader.GetHeader();

    Point minp = transform(header.GetMinX(), header.GetMinY(), header.GetMinZ());
    Vector3<double> min = Vector3<double>(minp.x, minp.y, minp.z);
    Point maxp = transform(header.GetMaxX(), header.GetMaxY(), header.GetMaxZ());
    Vector3<double> max = Vector3<double>(maxp.x, maxp.y, maxp.z);
    aabb.update(min);
    aabb.update(max);

    return aabb;
}

LASPointReader::LASPointReader(string path){
	this->path = path;

	if(fs::is_directory(path)){
		// if directory is specified, find all las and laz files inside directory

		for(fs::directory_iterator it(path); it != fs::directory_iterator(); it++){
			fs::path filepath = it->path();
			if(fs::is_regular_file(filepath)){
				if(iequals(fs::extension(filepath), ".las") || iequals(fs::extension(filepath), ".laz")){
					files.push_back(filepath.string());
				}
			}
		}
	}else{
		files.push_back(path);
	}

	// read bounding box
	for(int i = 0; i < files.size(); i++){
		string file = files[i];

		LIBLASReader aabbReader(file);
		AABB lAABB = aabbReader.getAABB();

		aabb.update(lAABB.min);
		aabb.update(lAABB.max);

		aabbReader.close();
	}

	// open first file
	currentFile = files.begin();
	reader = new LIBLASReader(*currentFile);
//    cout << "let's go..." << endl;
}

LASPointReader::~LASPointReader(){
	close();
}

void LASPointReader::close(){
	if(reader != NULL){
		reader->close();
		delete reader;
		reader = NULL;
	}
}

long LASPointReader::numPoints(){
	return reader->reader.GetHeader().GetPointRecordsCount();
}

bool LASPointReader::readNextPoint(){
	bool hasPoints = reader->reader.ReadNextPoint();

	if(!hasPoints){
		// try to open next file, if available
		reader->close();
		delete reader;
		reader = NULL;

		currentFile++;

		if(currentFile != files.end()){
			reader = new LIBLASReader(*currentFile);
			hasPoints = reader->reader.ReadNextPoint();
		}
	}

	return hasPoints;
}

Point LASPointReader::getPoint(){
	Point const p = reader->GetPoint();
    return p;
}

AABB LASPointReader::getAABB(){
	return aabb;
}

Vector3<double> LASPointReader::getScale(){

	Vector3<double> scale;
	scale.x = reader->reader.GetHeader().GetScaleX();
	scale.y = reader->reader.GetHeader().GetScaleY();
	scale.z = reader->reader.GetHeader().GetScaleZ();

	return scale;
}


#pragma once
#include <boost/function.hpp>
#include <Eigen/Dense>
#include <boost/shared_ptr.hpp>
/*
 * Numerical derivatives
 */

namespace sco {
using boost::function;
using Eigen::VectorXd;
using Eigen::MatrixXd;
class ScalarOfVector;
class VectorOfVector;
class MatrixOfVector;
typedef boost::shared_ptr<ScalarOfVector> ScalarOfVectorPtr;
typedef boost::shared_ptr<VectorOfVector> VectorOfVectorPtr;
typedef boost::shared_ptr<MatrixOfVector> MatrixOfVectorPtr;

class ScalarOfVector {
public:
  virtual double operator()(const VectorXd& x) const = 0;
  double call(const VectorXd& x) const {return operator()(x);}
  virtual ~ScalarOfVector() {}

  typedef function<double(VectorXd)> boost_func;
  static ScalarOfVectorPtr construct(const boost_func&);
  //  typedef VectorXd (*c_func)(const VectorXd&);
  //  static ScalarOfVectorPtr construct(const c_func&);

};
class VectorOfVector {
public:
  virtual VectorXd operator()(const VectorXd& x) const = 0;
  VectorXd call(const VectorXd& x) const {return operator()(x);}
  virtual ~VectorOfVector() {}

  typedef function<VectorXd(VectorXd)> boost_func;
  static VectorOfVectorPtr construct(const boost_func&);
  //  typedef VectorXd (*c_func)(const VectorXd&);
  //  static VectorOfVectorPtr construct(const c_func&);

};
class MatrixOfVector {
public:
  virtual MatrixXd operator()(const VectorXd& x) const = 0;
  MatrixXd call(const VectorXd& x) const {return operator()(x);}
  virtual ~MatrixOfVector() {}

  typedef function<MatrixXd(VectorXd)> boost_func;
  static MatrixOfVectorPtr construct(const boost_func&);
  //  typedef VectorMatrixXd (*c_func)(const VectorXd&);
  //  static MatrixOfVectorPtr construct(const c_func&);
};

VectorXd calcForwardNumGrad(const ScalarOfVector& f, const VectorXd& x, double epsilon);
MatrixXd calcForwardNumJac(const VectorOfVector& f, const VectorXd& x, double epsilon);
void calcGradAndDiagHess(const ScalarOfVector& f, const VectorXd& x, double epsilon,
    double& y, VectorXd& grad, VectorXd& hess);
void calcGradHess(ScalarOfVectorPtr f, const VectorXd& x, double epsilon,
    double& y, VectorXd& grad, MatrixXd& hess);
VectorOfVectorPtr forwardNumGrad(ScalarOfVectorPtr f, double epsilon);
MatrixOfVectorPtr forwardNumJac(VectorOfVectorPtr f, double epsilon);

}