// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE MergeVarUpdatesSGD1Transformation0

#include <../../random_util.hpp>
#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/l1.hpp>
#include <popart/session.hpp>
#include <popart/sgd.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/testdevice.hpp>

#include <algorithm>
#include <map>
#include <memory>
#include <tuple>
#include <vector>

using namespace popart;

namespace {
TensorId
bnconv(Builder *b, TensorId act, ConstVoidData cwdata, ConstVoidData bwdata) {
  auto aiOnnx      = b->aiOnnxOpset9();
  auto convWeights = b->addInitializedInputTensor(cwdata);
  auto scale       = b->addInitializedInputTensor(bwdata);
  auto bias        = b->addInitializedInputTensor(bwdata);
  auto mean        = b->addInitializedInputTensor(bwdata);
  auto var         = b->addInitializedInputTensor(bwdata);
  auto act0 =
      aiOnnx.conv({act, convWeights}, {1, 1}, 1, {}, {1, 1, 1, 1}, {1, 1});
  auto act1 = aiOnnx.batchnormalization({act0, scale, bias, mean, var}, 5);
  // return the output of batch-norm
  return act1[0];
}
} // namespace

BOOST_AUTO_TEST_CASE(Transformation_MergeMultiSGD1) {

  auto test = [](MergeVarUpdateType mvu, std::string dtype, bool sgd1) {
    // we will generate random input data
    int seed = 1013;
    DefaultRandomEngine eng(seed);
    UniformRealDistribution<float> fdis(-4.f, +4.f);

    // construct onnx model
    auto builder = Builder::create();
    auto aiOnnx  = builder->aiOnnxOpset9();

    int nInChans  = 3;
    int batchsize = 1;
    TensorInfo in0info{dtype, std::vector<int64_t>{batchsize, nInChans, 6, 6}};
    auto in0 = builder->addInputTensor(in0info);
    std::vector<float> in0data(in0info.nelms());
    for (auto &val : in0data) {
      val = fdis(eng);
    }

    // The model will be
    // reduce(bn(conv(bn(conv(bn(conv(bn(conv(....(bn(conv(input))...)))), nConv
    // convolutions chained together, with the number of channels increasing by
    // 1 at each subsequent conv.
    constexpr int nConv = 8;
    std::array<std::vector<float>, nConv> convWeights;
    std::array<std::vector<float>, nConv> bnWeights;
    std::array<ConstVoidData, nConv> cvds;
    std::array<ConstVoidData, nConv> bnds;
    std::array<TensorId, nConv + 1> actIds;
    actIds[0] = in0;

    int64_t copyElms = 0;
    int64_t sgdElms  = 0;
    for (int i = 0; i < nConv; ++i) {
      int nOutChans = nInChans + 1;
      TensorInfo conv_weight_info{
          dtype, std::vector<int64_t>{nOutChans, nInChans, 1, 1}};
      TensorInfo bn_weight_info(dtype, std::vector<int64_t>{nOutChans});

      copyElms += 2 * nOutChans;
      sgdElms += 2 * nOutChans;
      sgdElms += nOutChans * nInChans;

      convWeights[i] = std::vector<float>(nOutChans * nInChans, 0);
      for (auto &x : convWeights[i]) {
        x = fdis(eng);
      }
      bnWeights[i] = std::vector<float>(nOutChans, 0);
      for (auto &x : bnWeights[i]) {
        x = fdis(eng);
      }

      cvds[i] = {convWeights[i].data(), conv_weight_info};
      bnds[i] = {bnWeights[i].data(), bn_weight_info};

      actIds[i + 1] = bnconv(builder.get(), actIds[i], cvds[i], bnds[i]);
      nInChans      = nOutChans;
    }

    auto reduced =
        aiOnnx.reducesum({actIds[nConv]}, std::vector<int64_t>{{1, 2, 3}});
    float lossLambda = 0.26;
    auto l1 = builder->aiGraphcoreOpset1().l1loss({reduced}, lossLambda);
    std::string proto = builder->getModelProto();
    auto modelProto   = io::getModelFromString(proto);

    // create the IR
    auto art      = AnchorReturnType("All");
    auto dataFlow = DataFlow(1, {{reduced, art}});

    auto device = popart::createTestDevice(TEST_TARGET);

    auto opts = SessionOptions();

    // accurate scheduling is quite slow for this one. Imposing this limit on
    // how many swaps to perform in the scheduling algorithm reduces test time
    // from 280 seconds to 50 seconds.
    opts.swapLimitScheduler = 20;
    opts.firstDotOp         = 0;
    opts.finalDotOp         = 200;
    opts.dotChecks.insert("Final");
    opts.logDir                     = ".";
    opts.mergeVarUpdate             = mvu;
    opts.mergeVarUpdateMemThreshold = mvu == MergeVarUpdateType::AutoLoose
                                          ? 10000
                                          : 24; // 24 bytes = 7 floats
    opts.looseThresholdAtPeak = 10000;

    float learningRate = 0.1;

    // Use a relatively complex SGD optimizer.
    auto optimizer = SGD({{"defaultDampening", {0.05f, true}},
                          {"defaultLearningRate", {1.0f, false}},
                          {"defaultWeightDecay", {0.02f, false}},
                          {"defaultVelocityScaling", {0.25f, true}},
                          {"lossScaling", {0.2f, true}},
                          {"defaultMomentum", {0.9f, false}}},
                         {},
                         sgd1 ? SGDAccumulatorAndMomentum::Combined
                              : SGDAccumulatorAndMomentum::Separate);

    Ir ir;
    ir.prepare({modelProto,
                InputShapeInfo(),
                dataFlow,
                l1,
                &optimizer,
                *device,
                opts,
                Patterns()});

    // Check the ir
    //
    // For case MergeVarUpdateType::All, all the ConstSgdVarUpdates have the
    // same learning rate, weight decay, so should all be merged into a single
    // group.
    // For all cases we chould also have:
    // 1 Accumulate and 1 SGD1AcclUpdate for each SGD1VarUpdate.
    // 0 SGD1Combo ops.

    // Lambda to check the number of ops are as expected.
    auto checkOps = [&ir](const popart::AiGraphcoreOpIdV1 &opType,
                          int expected) {
      auto count = ir.opsOfType(opType).size();
      std::cout << opType.type << " Elements : " << count
                << " Expected: " << expected << std::endl;
      BOOST_CHECK(count == expected);
    };

    // For both SGD1 and 2 in the above model, we happen to expect the same
    // ops after decomposition.
    // Note SGD2VarUpdate and SGD2AcclUpdate ops have same opids as their sgd1
    // counterparts.

    const auto comboOpId = sgd1 ? Onnx::CustomOperators::SGD1Combo
                                : Onnx::CustomOperators::SGD2Combo;

    if (mvu == MergeVarUpdateType::All) {

      // 10 reshapes per layer are:
      // 3) conv filters, bn bias, bn scale
      // 3) grads of each of the above
      // 2) running mean & variance
      // 2) updates for each of the above

      checkOps(Onnx::CustomOperators::ReshapeInplace, nConv * 10);

      // 4 ConcatInplace
      checkOps(Onnx::CustomOperators::ConcatInplace, 4);

      checkOps(Onnx::CustomOperators::SGD1VarUpdate, 1);

      checkOps(Onnx::CustomOperators::Accumulate, 1);
      checkOps(Onnx::CustomOperators::SGD1AcclUpdate, 1);
      checkOps(comboOpId, 0);

      checkOps(Onnx::CustomOperators::CopyVarUpdate, 1);

    } else if (mvu == MergeVarUpdateType::None) {

      checkOps(Onnx::CustomOperators::SGD1VarUpdate, 3 * nConv);
      checkOps(Onnx::CustomOperators::CopyVarUpdate, 2 * nConv);

      checkOps(Onnx::CustomOperators::Accumulate, 3 * nConv);
      checkOps(Onnx::CustomOperators::SGD1AcclUpdate, 3 * nConv);
      checkOps(comboOpId, 0);

      checkOps(Onnx::CustomOperators::ReshapeInplace, 0);
      checkOps(Onnx::CustomOperators::ConcatInplace, 0);
    } else if (mvu == MergeVarUpdateType::AutoTight) {

      // Halve the memory threshold for FLOAT16 (multiply the divisor by 2)
      auto thr = dtype == "FLOAT" ? opts.mergeVarUpdateMemThreshold
                                  : opts.mergeVarUpdateMemThreshold * 2;
      std::cout << "Data type: " << dtype << " memory threshold : " << thr
                << std::endl;
      auto expectednsgd = (sgdElms * 4) / thr + ((sgdElms * 4) % thr != 0);
      checkOps(Onnx::CustomOperators::SGD1VarUpdate, expectednsgd);
      checkOps(Onnx::CustomOperators::Accumulate, expectednsgd);
      checkOps(Onnx::CustomOperators::SGD1AcclUpdate, expectednsgd);

      checkOps(comboOpId, 0);

      auto expectedncopy = (copyElms * 4) / thr + ((copyElms * 4) % thr != 0);
      checkOps(Onnx::CustomOperators::CopyVarUpdate, expectedncopy);

    } else if (mvu == MergeVarUpdateType::AutoLoose) {
      // because both thresholds are greater than the total memory, there should
      // be just 1 SGDVarUpdate and 1 CopyVarUpdate
      checkOps(Onnx::CustomOperators::SGD1VarUpdate, 1);

      checkOps(Onnx::CustomOperators::Accumulate, 1);
      checkOps(Onnx::CustomOperators::SGD1AcclUpdate, 1);
      checkOps(comboOpId, 0);

      checkOps(Onnx::CustomOperators::CopyVarUpdate, 1);
    }
  };

  for (const auto sgd1 : {true, false}) {
    for (const auto mvu : {MergeVarUpdateType::All,
                           MergeVarUpdateType::None,
                           MergeVarUpdateType::AutoTight,
                           MergeVarUpdateType::AutoLoose}) {
      for (const auto dtype : {"FLOAT", "FLOAT16"}) {
        test(mvu, dtype, sgd1);
      }
    }
  }
}


/**	\file gridmap.h
 * \brief Implementation an occupancymap updated from laser range scans for mobile robot.
 * The included is an Astar pathfinder, such that optimal obstacle-free paths can be
 * generated from the robots location to any goal location. OpenCV and Eigen are required.
 *
 * \author Hjalte Bested Møller
 * \date 4. April 2018
 *
*/

#ifndef GRIDMAP_H
#define GRIDMAP_H

#include <iostream>
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#define ASTAR_USE_OPENCV
#include "astar.hpp"

using namespace std;
using namespace Eigen;
using namespace cv;

/*
#ifndef USE_CV_DISTANCE_TRANSFORM
%define USE_CV_DISTANCE_TRANSFORM
#endif
*/

/** Class representing a laser scanner:
* It contains data structures for maintaining the laser scan data and the scanners pose in the inertial frame,
* functions for doing various coordinate systems transformations, and also implements
* a simulated laser scanner that facilitates development
*/
class LaserScanner {
public:
    /** Construct LaserScanner object */
    LaserScanner() {
        // Laser Parameters
        this->maxDistance = 4.0f;
        this->resolDeg = 0.36f;
        this->fov = 180.0f;
        this->init();
    }

    /**  Max Distance (Laser Measurement Range) in meters - If points are detected further from the scanner than this value, the point is ignored */
    float maxDistance;
    /** Minimum Distance (Laser Measurement Range) in meters - If points are detected closer to the scanner than this value, the point is ignored */
    float minDistance;
    /** Laser Scanner Resolution in Deg (Only Used By Simulator) */
    float resolDeg;
    /** Laser Scanner Resolution in Rad (Only Used By Simulator) */
    float resolRad;
    /** Laser Scanner Field-of-view in Deg (Only Used By Simulator) */
    float fov;

    /** Number of scanning lines (deduced from scan width and resolution in case of ) */
    int nScanLines;

    /** Matrix for holding the laser scan data format is scanData(2,nScanLines), where phi(i) = scanData(0,i), r(i) = scanData(1,i) */
    MatrixXd scanData;

    /** Matrix for holding the laser scan data format is scanCart(2,nScanLines), where x^s(i) = scanData(0,i), y^s(i) = scanData(1,i) */
    MatrixXd scanCart;

    /** Matrix for holding the laser scan data format is scanWorld(2,nScanLines), where x^i(i) = scanData(0,i), y^i(i) = scanData(1,i) */
    MatrixXd scanWorld;

    /** Initialise LaserScanner: Calculate the resolution in radian and determine the number of scanning lines nScanLines */
    void init(){
        this->resolRad = resolDeg*M_PI/180.0;
        this->nScanLines = round(fov/resolDeg+1);
    }

    /** Scan Pose in world coordinates (x,y,theta) */
    Vector3f pose;

    /** Set Scan Pose in world coordinates (x,y,theta) */
    void setPose(Vector3f const& newpose){
        pose = newpose;
    }
    /** Set Scan Pose in world coordinates (x,y,theta) */
    void setPose(float x, float y, float th){
        pose << x,y,th;
    }

    /** Return pose */
    Vector3f getPose(){
        return pose;
    }

    /** Convert polar coordinates to cartesian coordinates */
    MatrixXd polarToCart(MatrixXd const& scanPolar){
        const int nScanLines = scanPolar.cols();
        scanCart.resize(2,nScanLines);
        for(int i=0; i<nScanLines; i++){
            double const& phi = scanPolar(0,i);
            double const& rho = scanPolar(1,i);
            double x = rho * cos(phi);
            double y = rho * sin(phi);
            scanCart(0,i) = x;
            scanCart(1,i) = y;
        }
        return scanCart;
    }

    /** Convert cartesian coordinates in the scanner frame to cartesian coordinates in the world frame */
    MatrixXd cartToWorld(MatrixXd const& scanCart, Vector3f const& pose){
        const int& nScanLines = scanCart.cols();
        scanWorld.resize(2,nScanLines);
        double const& x = pose(0);
        double const& y = pose(1);
        double const& th = pose(2);
        double costh = cos(th);
        double sinth = sin(th);

        // Transform all the coordinates and store them in scanWorld
        for(int i=0; i<nScanLines; i++){
            double const& xs = scanCart(0,i);
            double const& ys = scanCart(1,i);
            double xw = xs*costh - ys*sinth + x;
            double yw = ys*costh + xs*sinth + y;
            scanWorld(0,i) = xw;
            scanWorld(1,i) = yw;
        }
        return scanWorld;
    }

    /** Convert polar coordinates in the scanner frame to cartesian coordinates in the world frame directly */
    inline MatrixXd polarToWorld(MatrixXd const& scanPolar, Vector3f const& pose){
        return cartToWorld(polarToCart(scanPolar),pose);
    }

    /** Overloaded convinience method: Convert cartesian coordinates in the scanner frame to cartesian coordinates in the world frame */
    inline MatrixXd cartToWorld(MatrixXd const& scanCart){
        return cartToWorld(scanCart, this->pose);
    }

    /** Overloaded convinience method: Convert polar coordinates in the scanner frame to cartesian coordinates in the world frame directly */
    inline MatrixXd polarToWorld(MatrixXd const& scanPolar){
        return polarToWorld(scanPolar,pose);
    }

    /** Return scan in polar (default) coordinates */
    inline MatrixXd getScanPolar(){
        return scanData;
    }

    /** Return scan in Laser local cartesian coordinates */
    inline MatrixXd getScanCart(){
        return polarToCart(scanData);
    }

    /** Return scan in world cartesian coordinates, based on the internal pose */
    inline MatrixXd getScanCartWorld(){
        return polarToWorld(scanData, pose);
    }

    /** Resize MatrixXd scanData such that it has two rows and nScanLines columns */
    void resize(int nScanLines){
        this->nScanLines = nScanLines;
        scanData.resize(2,nScanLines);
    }

    /** When working with a real laserscanner this is the way to fill the in the scanData */
    void setScanPoint(int i, double phi, double r){
        scanData(0,i) = phi;
        scanData(1,i) = r;
    }

    /** Simulate a laser scan - The lines are defined as a matrix where each column represents a line and the rows are [x1, y1, x2, y2] */
    MatrixXd simScan(Vector3f const& pose, MatrixXd const& lines){
        int nLines = lines.cols();
        double x = pose(0);
        double y = pose(1);
        double th = pose(2);
        double costh = cos(th);
        double sinth = sin(th);
        // cout << "th=" << th << endl;

        /* ----------------------------------------
         * ---- Pre-allocate the memory needs -----
         * --------------------------------------*/

        // Global system coordinates
        VectorXd xStart(nLines);
        VectorXd yStart(nLines);
        VectorXd xEnd(nLines);
        VectorXd yEnd(nLines);

        // Laser scanner local system coordinates
        VectorXd xStartTrans(nLines);
        VectorXd yStartTrans(nLines);
        VectorXd xEndTrans(nLines);
        VectorXd yEndTrans(nLines);

        // Parameters for the line equations of the form: a*x+ b*y=c
        VectorXd a(nLines);
        VectorXd b(nLines);
        VectorXd c(nLines);

        // Rezise the scanData matrix, if the size is correct already, Eigen
        // ensures that this is a no-operation
        scanData.resize(2,nScanLines);

        /* ------------------------------------------------------------------------
         * --- Conversion of the lines from global frame to scanner local frame ---
         * --------------------------------------------------------------------- */

        // For each line:
        // cout << "nLines" << nLines << endl;

        for(int i=0; i<nLines; i++){
            // Start and end values for the lines ending points.
            xStart(i) = lines(0,i);
            yStart(i) = lines(1,i);
            xEnd(i)   = lines(2,i);
            yEnd(i)   = lines(3,i);
            // cout << "Line = (" << xStart(i) << "," << yStart(i) << "," << xEnd(i) << "," << yEnd(i) << ")" << endl;

            // Transformation of the lines to the laser scanner's coordinate system.
            // Lines are converted to the new coordinate system.
            xStartTrans(i) = (xStart(i)-x)*costh + (yStart(i)-y)*sinth;
            yStartTrans(i) = (yStart(i)-y)*costh - (xStart(i)-x)*sinth;
            xEndTrans(i) = (xEnd(i)-x)*costh   + (yEnd(i)-y)*sinth;
            yEndTrans(i) = (yEnd(i)-y)*costh   - (xEnd(i)-x)*sinth;
            // cout << "LineTrans = (" << xStartTrans(i) << "," << yStartTrans(i) << "," << xEndTrans(i) << "," << yEndTrans(i) << ")" << endl;

            // Starting and ending points are swapped if the x value of the
            // starting point is bigger than the x value of the ending point.
            if( xStartTrans(i) > xEndTrans(i)){
                double temp = xStartTrans(i);
                xStartTrans(i) = xEndTrans(i);
                xEndTrans(i) = temp;
                temp = yStartTrans(i);
                yStartTrans(i) = yEndTrans(i);
                yEndTrans(i) = temp;
            }

            if( xStartTrans(i) == xEndTrans(i)){
                xEndTrans(i) += 1e-6;
            }
            // cout << "LineTrans = (" << xStartTrans(i) << "," << yStartTrans(i) << "," << xEndTrans(i) << "," << yEndTrans(i) << ")" << endl;

            // The line equations are calculated
            // a*x+ b*y=c - IMPORTANT: Variables related to the line equations must
            // be double precition to avoid numerical issues!
            a(i) = yStartTrans(i)-yEndTrans(i);
            b(i) = xEndTrans(i)-xStartTrans(i);
            double a2 = a(i)*a(i);
            double b2 = b(i)*b(i);
            double l = sqrt(a2+b2);
            a(i) = a(i)/l;
            b(i) = b(i)/l;
            c(i) = a(i)*xStartTrans(i)+b(i)*yStartTrans(i);
            // cout << "Line " << i << ": " << a(i) << "x + " << b(i) << "y = " << c(i)<< endl;
        }

        /* -------------------------------------------------------------------
         * Conversion of what the laser scanner sees to polar coordinates.
         * -------------------------------------------------------------------*/

        // For each laser scanner angle:
        for(int i=0; i<nScanLines; i++){
            // Laser scanner maximum measured distance maxDistance(in meters)
            // Closest distance from the laser scanner.
            double min_dist = maxDistance;

            // Current laser scanner angle
            double phi = i*resolRad-fov/2.0*M_PI/180.0;
            // Find distance from the lines to laser scanner in the current angle
            double cosphi = cos(phi);
            double sinphi = sin(phi);
            // cout << "cos(phi)=" << cosphi << ", sin(phi)=" << sinphi << endl;
            for(int j=0; j<nLines; j++){
                double temp = a(j)*cosphi + b(j)*sinphi;
                if(abs(temp) > 1e-8){
                    double t = c(j)/temp;
                    if(t>0 && t<min_dist){
                        if(abs(xStartTrans(j)-xEndTrans(j)) > 1e-8){
                            if(t*cosphi < xEndTrans(j) && t*cosphi > xStartTrans(j)) min_dist=t;
                        else
                            if(yEndTrans(j) > yStartTrans(j)){
                                if(t*sinphi < yEndTrans(j) && t*sinphi > yStartTrans(j)) min_dist=t;
                            } else {
                                if(t*sinphi > yEndTrans(j) && t*sinphi < yStartTrans(j)) min_dist=t;
                            }

                        }
                    }
                }
            }

            // The polar coordinates returned
            scanData(0,i) = phi;
            scanData(1,i) = min_dist;
        }
        return scanData;
    }

    /** Simulate a laser scan - The lines are defined as a matrix where each column represents a line and the rows are [x1, y1, x2, y2] */
    inline MatrixXd simScan(MatrixXd const& lines){
        return simScan(pose,lines);
    }
};

/** Implementation an occupancy grid-map updated from laser range scans including path finder for mobile robot.
 * The included is an Astar pathfinder, such that optimal obstacle-free paths can be
 * generated from the robots location to any goal location.
 * OpenCV and Eigen are required.
 */
class GridMap {
public:
    /** The raw map data: -1:Unknown, 0: Free, 1:Obstacle */
    Mat mapData;
    /** The dialated map data: -1:Unknown, 0: Free, 1:Obstacle */
    Mat dialatedMap;
    /** OpenCV Structuring Element used for map dilation.  */
    Mat strel;

    #ifdef USE_CV_DISTANCE_TRANSFORM
    /** The inverse of the dialated map, used for distance transform */
    Mat invDialatedMap;
    /** The distance transformed map as a 32bit float */
    Mat distanceMap_32F;
    /** The distance transformed map as char */
    Mat distanceMap;
    #endif

    /** If wrapMap = true, the map can wrap around the edges such that the map can be used locally */
    bool wrapMap = true;

    /** The size of the map cells in square meters */
    float cellSize = 1;
    /** The with of the map in pixels */
    unsigned long width = 0;
    /** The height of the map in pixels */
    unsigned long height = 0;
    /** The size of the map in pixels, i.e. width*height */
    unsigned long size = 0;

    /** The position of x=0 in the map, specified in meters */
    float xoffset = 0;
    /** The position of y=0 in the map, specified in meters */
    float yoffset = 0;

    /** Flag that determines how cells are set to occupied in the map */
    int fillMode = -1;
    /** Flag that determines how cells are set to free in the map */
    int clearMode = 0;

    /** A* search algorithm */
    Astar astar;
    /** The current cell position of the LaserScanner */
    Point scanPosCell;

    /** Construct an uninitialized GridMap object */
    GridMap() { }
    /** Construct an initialized GridMap object */
    GridMap(float widthInMeters, float heightInMeters, float cellSize) {
        this->resize(widthInMeters, heightInMeters, cellSize);
    }

    /** Resize the and all the related data structures */
    void resize(float widthInMeters, float heightInMeters, float cellSize){
        this->cellSize = cellSize;
        this->height = ceil(heightInMeters/cellSize);
        this->width  = ceil(widthInMeters/cellSize);
        this->size = width * height;
        this->setOffset(widthInMeters/2.0,heightInMeters/2.0);
        mapData.create(height,width,CV_8SC1);
        dialatedMap.create(height,width,CV_8SC1);
        #ifdef USE_CV_DISTANCE_TRANSFORM
        invDialatedMap.create(height,width,CV_8SC1);
        distanceMap_32F.create(height,width,CV_32F);
        distanceMap.create(height,width,CV_8SC1);
        #endif
        astar.resize(height, width);
        astar.wrapMap = wrapMap;
        this->clear();
    }

    /** Clear the map */
    void clear(){
        mapData.setTo(-1);
        dialatedMap.setTo(0);
        #ifdef USE_CV_DISTANCE_TRANSFORM
        distanceMap.setTo(0);
        #endif
        astar.clearAll();
    }

    /** Return the width of the map in meters, i.e. width*cellSize */
    float widthInMeters() {
        return width*cellSize;
    }
    /** Return the height of the map in meters, i.e. height*cellSize */
    float heightInMeters() {
        return height*cellSize;
    }

    /** Set the position of (x=0,y=0) in the map, specified in meters */
    void setOffset(float xoffset, float yoffset){
        this->xoffset = xoffset;
        this->yoffset = yoffset;
    }

    /** If wrapMap is enabled the map is wrapped around the edges. This allows the map to be used in a more local sence. */
    void setWrapMap(bool wrapMap){
        this->wrapMap = wrapMap;
        astar.wrapMap = wrapMap;
    }

    /** Make Structuring Element for Map dilation - Dilates as circle by default */
    void makeStrel(float width, int dialation_type = MORPH_ELLIPSE){
        int dilationSize = round(0.5*(width-cellSize)/cellSize);
        cout << "dilationSize=" << dilationSize << endl;
        strel = getStructuringElement(
            dialation_type,
            Size( 2*dilationSize+1, 2*dilationSize+1 ),
            Point( dilationSize, dilationSize ) );
    }

    /** An implementation of Bresenham's line algorithm, see http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm */
    vector<Point> bresenhamPoints(int x0, int y0, int x1, int y1){
        Point point;
        vector<Point> points;
        int ystep = -1;

        bool steep=abs(y1-y0)>abs(x1-x0);

        if(steep){ // slope more than 1
            int tempx0=x0; x0 = y0; y0 = tempx0; // Swapping
            int tempx1=x1; x1 = y1; y1 = tempx1; // Swapping
        }
        if(x0>x1){ // line goes to the left
            int tempx0=x0; x0=x1; x1=tempx0; // Swapping
            int tempy0=y0; y0=y1; y1=tempy0; // Swapping
        }

        int deltaX = x1-x0;
        int deltaY = abs(y1-y0);
        int N = deltaX+1;
        float error=-N/2;

        int x=x0;
        int y=y0;

        if(y0<y1) ystep=1;

        for(int i=0; i<N; i++){
            if(steep){
                point.x = y;
                point.y = x;
            } else {
                point.x = x;
                point.y = y;
            }
            x++;
            error=error+deltaY;
            if(error >= 0){
                y=y+ystep;
                error=error-deltaX;
            }
            points.push_back(point);
        }
        return points;
    }

    /** Round towards zero */
    int fix(float val){
        if(val > 0) return floor(val);
        if(val < 0) return ceil(val);
        return val;
    }

    /** Convert a point from world coordinates to map cell coordinates */
    inline Point worldToCell(float x, float y){
        int& rows = mapData.rows;
        x = x+xoffset;
        y = y+yoffset;
        Point cell;
        cell.x = x/cellSize;
        cell.y = rows - floor(y/cellSize + 1);
        return cell;
    }

    /** Convert a point from world coordinates to map cell coordinates */
    inline Point worldToCell(Point2f world){
        return worldToCell(world.x,world.y);
    }

    /** Convert a point from map cell coordinates to world coordinates */
    inline Point2f cellToWorld(int x, int y){
        Point2f world;
        // Translate from map coordinates to world coordinates (center of cell)
        world.x =  (x+0.5) * cellSize - xoffset;
        world.y =  (mapData.rows-y-0.5) * cellSize - yoffset;
        return world;
    }

    /** Convert a point from map cell coordinates to world coordinates */
    inline Point2f cellToWorld(Point cell){
        return cellToWorld(cell.x,cell.y);
    }

    /** wrap a point around the edges of the map. */
    inline Point wrap(Point cell){
        // Wrapping the map
        while(cell.x < 0)               cell.x += mapData.cols;
        while(cell.x >= mapData.cols)   cell.x -= mapData.cols;
        while(cell.y < 0)               cell.y += mapData.rows;
        while(cell.y >= mapData.rows)   cell.y -= mapData.rows;
        return cell;
    }

    /** wrap an x-coordinate around the edges of the map. If wrapMap is false this is a no-operation. */
    inline int wx(int x){
        if(wrapMap){ // Wrapping the map
            while(x < 0)                x += mapData.cols;
            while(x >= mapData.cols)    x -= mapData.cols;
        }
        return x;
    }
    /** wrap an y-coordinate around the edges of the map. If wrapMap is false this is a no-operation. */
    inline int wy(int y){
        if(wrapMap){ // Wrapping the map
            while(y < 0)                y += mapData.rows;
            while(y >= mapData.rows)    y -= mapData.rows;
        }
        return y;
    }

    /** Reset a MapNode in the Astar instance and set its (x,y) and type this is needed for wrapMap to work */
    inline void resetMapNode(int x, int y, char value){
        MapNode*node = astar.mapAt(x,y);
        if(node != NULL){
            node->x = x;
            node->y = y;
            node->clear(value);
        }
    }

    /** Set a cell to a new value (nodetype) */
    void setCell(int const& x, int const& y, char const& value, int const& mode){
        int rows = mapData.rows;
        int cols = mapData.cols;
        int xm = wx(x);
        int ym = wy(y);

        if((0 <= xm && xm < cols) && (0 <= ym && ym < rows)){
            resetMapNode(x,y,value);
            mapData.at<char>(ym,xm) = value;

            int xm1=wx(xm-1); int xp1=wx(xm+1); int ym1=wy(ym-1); int yp1=wy(ym+1);
            if(value > 0 && mode == -1){
                if(0 <= xm1   && mapData.at<char>(ym,xm1)<0){  mapData.at<char>(ym,xm1) = value; resetMapNode(x-1,y,value);}
                if(xp1 < cols && mapData.at<char>(ym,xp1)<0){  mapData.at<char>(ym,xp1) = value; resetMapNode(x+1,y,value);}
                if(0 <= ym1   && mapData.at<char>(ym1,xm)<0){  mapData.at<char>(ym1,xm) = value; resetMapNode(x,y-1,value);}
                if(yp1 < rows && mapData.at<char>(yp1,xm)<0){  mapData.at<char>(yp1,xm) = value; resetMapNode(x,y+1,value);}
                if(0 <= xm1   && 0 <= ym1   && mapData.at<char>(ym1,xm1)<0){ mapData.at<char>(ym1,xm1)=value; resetMapNode(x-1,y-1,value);}
                if(0 <= xm1   && yp1 < rows && mapData.at<char>(yp1,xm1)<0){ mapData.at<char>(yp1,xm1)=value; resetMapNode(x-1,y+1,value);}
                if(xp1 < cols && 0 <= ym-1  && mapData.at<char>(ym1,xp1)<0){ mapData.at<char>(ym1,xp1)=value; resetMapNode(x+1,y-1,value);}
                if(xp1 < cols && yp1 < rows && mapData.at<char>(yp1,xp1)<0){ mapData.at<char>(yp1,xp1)=value; resetMapNode(x+1,y+1,value);}
            }

            if(mode > 0){
                xm1=wx(xm-1); xp1=wx(xm+1); ym1=wy(ym-1); yp1=wy(ym+1);
                if(0 <= xm1){    mapData.at<char>(ym,xm1) = value; resetMapNode(x-1,y,value);}
                if(xp1 < cols){  mapData.at<char>(ym,xp1) = value; resetMapNode(x+1,y,value);}
                if(0 <= ym1){    mapData.at<char>(ym1,xm) = value; resetMapNode(x,y-1,value);}
                if(yp1 < rows){  mapData.at<char>(yp1,xm) = value; resetMapNode(x,y+1,value);}
            }
            if(mode > 1){
                if(0 <= xm1 && 0 <= ym1){    mapData.at<char>(ym1,xm1)=value;  resetMapNode(x-1,y-1,value);}
                if(0 <= xm1 && yp1 < rows){  mapData.at<char>(yp1,xm1)=value;  resetMapNode(x-1,y+1,value);}
                if(xp1 < cols && 0 <= ym1){  mapData.at<char>(ym1,xp1)=value;  resetMapNode(x+1,y-1,value);}
                if(xp1 < cols && yp1 < rows){mapData.at<char>(yp1,xp1)=value;  resetMapNode(x+1,y+1,value);}
            }
            // cout << "SetCell (" << x << "," << y << ") = " << int(value) << ",mode=" << mode << endl;
        }
    }

    /** Set cells along a line from (x1,y1) to (x2,y2) to a new value (nodetype) */
    void setLine(int x1, int y1, int x2, int y2, int value, int mode=0){
        vector<Point> pointsToSet = bresenhamPoints(x1, y1, x2, y2);
        for(uint i=0; i<pointsToSet.size(); i++){
            setCell(pointsToSet[i].x, pointsToSet[i].y, value, mode);
        }
    }

    /** Set a square of cells with center point (x,y) to a new value (nodetype) */
    void setSquare(int x, int y, int size, int value, int mode=0){
        for(int iy=y-size; iy<=y+size; iy++){
            for(int ix=x-size; ix<=x+size; ix++){
                setCell(ix,iy,value, mode);
                setCell(ix,iy,value, mode);
            }
        }
    }

    /** Return the value of the obstacle map at coordinate (x,y) */
    inline char mapAt(int x, int y){
        // Wrapping the map
        if(wrapMap){
            while(x < 0)                x += mapData.cols;
            while(x >= mapData.cols)    x -= mapData.cols;
            while(y < 0)                y += mapData.rows;
            while(y >= mapData.rows)    y -= mapData.rows;
            return mapData.at<char>(y,x);
        }

        if((0 <= x && x < mapData.cols) && (0 <= y && y < mapData.rows)){
            return mapData.at<char>(y,x);
        }

        return 0;
    }

    /** Return the value of the dialated map at coordinate (x,y) */
    inline uchar dialatedMapAt(int x, int y){
        int& rows = mapData.rows;
        int& cols = mapData.cols;

        // Wrapping the map
        if(wrapMap){
            while(x < 0)       x += cols;
            while(x >= cols)   x -= cols;
            while(y < 0)       y += rows;
            while(y >= rows)   y -= rows;
            return dialatedMap.at<uchar>(y,x);
        }

        if((0 <= x && x < cols) && (0 <= y && y < rows)){
            return dialatedMap.at<uchar>(y,x);
        }

        return 0xFF;
    }

    /** Return the value of the distance map at coordinate (x,y) */
    #ifdef USE_CV_DISTANCE_TRANSFORM
    inline uchar distanceMapAt(int x, int y){
        int& rows = mapData.rows;
        int& cols = mapData.cols;

        // Wrapping the map
        if(wrapMap){
            while(x < 0)       x += cols;
            while(x >= cols)   x -= cols;
            while(y < 0)       y += rows;
            while(y >= rows)   y -= rows;
            return distanceMap.at<uchar>(y,x);
        }

        if((0 <= x && x < cols) && (0 <= y && y < rows)){
            return distanceMap.at<uchar>(y,x);
        }

        return 0;
    }
    #endif

    /** Update the map. This should be called whenever new laser scan data is available. */
    void updateMap(LaserScanner *scan, float maxLSDist){
        // Cell Laser Scanner Position
        scanPosCell = worldToCell(scan->pose(0),scan->pose(1));
        // cout << "scanPosCell = (" << scanPosCell.x << "," << scanPosCell.y << ")" << endl;

        // Convert polar scan coordinates to laser cartesian coordinates
        const MatrixXd& scanPolar = scan->getScanPolar();
        // Convert laser cartesian coordinates to world cartesian coordinates
        const MatrixXd& scanWorld = scan->getScanCartWorld();

        vector<Point> pointsToFill;

        // For each scanline i
        for(int i=0; i<scan->nScanLines; i++){
            // Convert scan from world cartesian coordinates to cell position
            double const& rho = scanPolar(1,i);
            if(rho < 0.020) continue;
            Point cell = worldToCell(scanWorld(0,i), scanWorld(1,i));

            // Clear cells using bresenham's algorithm
            // cout << "ScanPosCell: (" << scanPosCell.x << "," << scanPosCell.y << "), cell: (" << cell.x << "," << cell.y << ")" << endl;
            vector<Point> pointsToClear = bresenhamPoints(scanPosCell.x, scanPosCell.y, cell.x, cell.y);
            // cout << "Number of Points To Clear = " << pointsToClear.size() << endl;

            for(uint ii=0; ii<pointsToClear.size(); ii++){
                setCell(pointsToClear[ii].x, pointsToClear[ii].y, 0, clearMode);
            }

            if(0.025 < rho && rho <= maxLSDist) pointsToFill.push_back(cell);
        }

        for(uint ii=0; ii<pointsToFill.size(); ii++){
            setCell(pointsToFill[ii].x, pointsToFill[ii].y, 1, fillMode);
        }

        // cout << "scanWorldCell:\n" << scanWorldCell.transpose() << endl;
    }

    /** Compute various transformations, i.e. dialatedMap and distanceMap */
    void transform(){
        dilate( mapData > 0, dialatedMap, strel);

        #ifdef USE_CV_DISTANCE_TRANSFORM
        bitwise_not(dialatedMap, invDialatedMap);
        distanceTransform(invDialatedMap, distanceMap_32F, CV_DIST_C, 5, CV_32F);
        distanceMap_32F.convertTo(distanceMap,dialatedMap.type());
        #endif
   }

    /** Determine next waypoint defined as the furthest point seen by the laserscanner which is
    not occupied in the dialated obstacle map. The search has a preference for scanlines with small angles, which is achieved by
    iterating from the middle and out and only replacing if a longer distance is found. Thus, small angles are only used in case of ties.*/
    Point determineNextWaypointCell(LaserScanner *scan, float maxLSDist=1e7){
        float biggestDistanceInCells = 0;
        Point mostDistantCell;
        int nScanLinesHalf = scan->nScanLines/2;

        // Convert polar scan coordinates to laser cartesian coordinates
        MatrixXd scanPolar = scan->getScanPolar();

        // Limit the range of the scan
        if(maxLSDist < 1e6){
            for(int i=0; i<scanPolar.cols(); i++){
                double& rho = scanPolar(1,i);
                if(rho > maxLSDist) rho = maxLSDist;
            }
        }
        // Convert laser cartesian coordinates to world cartesian coordinates
        MatrixXd scanWorld = scan->polarToWorld(scanPolar);

        // Convert scan from world cartesian coordinates to cell position
        for(int i=0; i<=nScanLinesHalf; i++){
            int k;
            for(int j=0; j<2; j++){
                if(j==0)   k = nScanLinesHalf-i;
                else       k = nScanLinesHalf+i;
                if(k >= 0 && k < scan->nScanLines){
                    double& rho = scanPolar(1,k);
                    double& xs  = scanWorld(0,k);
                    double& ys  = scanWorld(1,k);
                    // float& phi = scanPolar(0,k);

                    if(rho < biggestDistanceInCells*cellSize) continue;
                    Point cell = worldToCell(xs, ys);
                    // int obstdist = distanceMapAt(cell.x,cell.y);

                    // Clear search for furthest available point using Bresenham's algorithm
                    Point closestOccupiedPoint;
                    vector<Point> points = bresenhamPoints(scanPosCell.x, scanPosCell.y, cell.x, cell.y);

                    // cout << "Diff:" <<scanPosCell-cell << endl;
                    for(uint ii=0; ii<points.size(); ii++){
                        Point point = points[ii];
                        float dx = point.x - scanPosCell.x;
                        float dy = point.y - scanPosCell.y;
                        float celldist = sqrt(dx*dx+dy*dy);
                        if(celldist > biggestDistanceInCells && dialatedMapAt(point.x,point.y) == 0){
                            biggestDistanceInCells = celldist;
                            mostDistantCell = point;
                        }
                    }
                }
            }
        }

        return mostDistantCell;
    }

    /** Determine next waypoint defined as the furthest point seen by the laserscanner,
    not occupied in the dialated map, with preference for scanlines with small angles, achieved by
    iterating from the middle and out and only replacing if a longer distance is found. */
    inline Point2f determineNextWaypoint(LaserScanner *scan, float maxLSDist=1e8){
        return cellToWorld(determineNextWaypointCell(scan));
    }

    /** Determine next waypoint defined as the furthest point seen by the laserscanner which is
    not occupied in the dialated obstacle map. The search has a preference for scanlines with small angles, which is achieved by
    iterating from the middle and out and only replacing if a longer distance is found. Thus, small angles are only used in case of ties.*/
    Point determineNextWaypointCellB(LaserScanner *scan, float maxLSDist=1e4){
        float biggestDistanceInCells = 0;
        Point mostDistantCell;
        int nScanLinesHalf = scan->nScanLines/2;

        // Convert polar scan coordinates to laser cartesian coordinates
        MatrixXd scanPolar = scan->getScanPolar();

        // Limit the range of the scan
        if(maxLSDist < 1e3){
            for(int i=0; i<scanPolar.cols(); i++){
                double& rho = scanPolar(1,i);
                if(rho > maxLSDist) rho = maxLSDist;
            }
        }
        // Convert laser cartesian coordinates to world cartesian coordinates
        MatrixXd scanWorld = scan->polarToWorld(scanPolar);

        // Convert scan from world cartesian coordinates to cell position
        for(int i=0; i<=nScanLinesHalf; i++){
            int k;
            for(int j=0; j<2; j++){
                if(j==0)   k = nScanLinesHalf-i;
                else       k = nScanLinesHalf+i;
                if(k >= 0 && k < scan->nScanLines){
                    double& rho = scanPolar(1,k);
                    double& xs  = scanWorld(0,k);
                    double& ys  = scanWorld(1,k);
                    // float& phi = scanPolar(0,k);

                    if(rho <= biggestDistanceInCells*cellSize) continue;
                    Point cell = worldToCell(xs, ys);
                    // int obstdist = distanceMapAt(cell.x,cell.y);

                    // Clear search for furthest available point using Bresenham's algorithm
                    Point closestOccupiedPoint;
                    vector<Point> points = bresenhamPoints(scanPosCell.x, scanPosCell.y, cell.x, cell.y);
                    if(scanPosCell != points[0]) reverse(points.begin(), points.end());

                    #if (0)
                    cout << "Scan:(" << scanPosCell.x << "," << scanPosCell.y << ")";
                    for(int p=0; p<points.size(); p++){
                    	cout << "->(" << points[p].x << "," << points[p].y << ")";
                    }
                    cout << endl;
					#endif

                    // cout << "Diff:" <<scanPosCell-cell << endl;
                    uint ii=1;
                    while(ii<points.size()){
                        Point point = points[ii];
                        if(dialatedMapAt(point.x,point.y) != 0) break;
                        float dx = point.x - scanPosCell.x;
                        float dy = point.y - scanPosCell.y;
                        float celldist = sqrt(dx*dx+dy*dy);
                        if(celldist > biggestDistanceInCells){
                            biggestDistanceInCells = celldist;
                            mostDistantCell = point;
                        }
                        ii++;
                    }
                }
            }
        }

        return mostDistantCell;
    }

    /** Determine next waypoint defined as the furthest point seen by the laserscanner,
    not occupied in the dialated map, with preference for scanlines with small angles, achieved by
    iterating from the middle and out and only replacing if a longer distance is found. */
    inline Point2f determineNextWaypointB(LaserScanner *scan, float maxLSDist=1e8){
        return cellToWorld(determineNextWaypointCellB(scan));
    }

    /** This function will find the minimal cost path from the starting cell to the target cell.
    The function uses the A* search algorithm with tie-breaker and an additional cost related to
    the obstacle distance implemented as an obstacle repulsive potential. */
    vector<MapNode *> findpath(int xStart, int yStart, int xTarget, int yTarget, unsigned long maxIter = 10000){
        // vector<MapNode *> path;
        astar.clear();
        for (uint y = 0; y < mapData.rows; y++) {
            for (uint x = 0; x < mapData.cols; x++) {
                MapNode *node = astar.mapAt(x,y);
                if(dialatedMapAt(x,y) > 0) node->type = NODE_TYPE_OBSTACLE;
                else if(mapAt(x,y) == -1)  node->type = NODE_TYPE_UNKNOWN;
                else node->type = NODE_TYPE_ZERO;

                #ifdef USE_CV_DISTANCE_TRANSFORM
                    node->obstdist = distanceMapAt(x,y);
                #else
                    node->obstdist = -1; // -1 means that the distance is still unknown, and thus it will be calculated by astar during node expansion.
                #endif
            }
        }
        return astar.findpath(xStart, yStart, xTarget, yTarget, maxIter);
    }

    /** Wrap a value into the [-pi,pi) range */
    inline float wrapToPi(float angle){
        float TWO_PI = 2*M_PI;
        while(angle > M_PI)     angle -= TWO_PI;
        while(angle <= -M_PI)   angle += TWO_PI;
        return angle;
    }

    /** Simplify the path by removing points along straight line segments, keeping only the corner points. */
    inline vector<MapNode *> simplifyPath(vector<MapNode *> path){
        const bool DEBUG = false;

        vector<MapNode *> newpath;
        path = astar.simplifyPath(path);
        //return path;

        newpath.push_back(path.front());
        for(int i=1; i<(path.size()-1); i++){
            MapNode * P0 = path[i-1];
            MapNode * P1 = path[i];
            MapNode * P2 = path[i+1];
            int dx01 = P1->x - P0->x;
            int dy01 = P1->y - P0->y;
            int dx12 = P2->x - P1->x;
            int dy12 = P2->y - P1->y;
            if(abs(dx12) == 1 && abs(dy12) == 1)  continue;
            if(dx01 == dx12 && dy01 == dy12) continue;
            newpath.push_back(P1);
        }
        newpath.push_back(path.back());

        if(DEBUG){
            cout << "newpath2:--";
            for(int i=0; i<newpath.size(); i++){
                cout << "->(" << newpath[i]->x << "," << newpath[i]->y << ")";
            }
            cout << endl;
        }

        return newpath;
    }

    /** Convert a path consisting of a vector of MapNode pointers to a vector of world coordinate waypoints */
    vector<Point2f> pathToWorld(vector<MapNode *> const& path){
        vector<Point2f> waypoints;
        for(int i=0; i<path.size(); i++){
            waypoints.push_back(cellToWorld(path[i]->x,path[i]->y));
        }

        /*
        cout << "wayPoints:";
        for(int i=0; i<waypoints.size(); i++){
            cout << "->(" << waypoints[i].x << "," << waypoints[i].y << ")";
        }
        cout << endl;
        */
        return waypoints;
    }

    inline vector<Point2f> findpathAsWaypoints(int const&  xStart, int const& yStart, int const& xTarget, int const& yTarget){
        return pathToWorld(simplifyPath(findpath(xStart, yStart, xTarget, yTarget)));
    }

};

#endif


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

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 <mitkCoreObjectFactory.h>
#include "mitkImage.h"
#include "itkAnalyticalDiffusionQballReconstructionImageFilter.h"
#include <boost/lexical_cast.hpp>
#include "mitkCommandLineParser.h"
#include <mitkIOUtil.h>
#include <itksys/SystemTools.hxx>
#include <mitkDiffusionPropertyHelper.h>
#include <mitkITKImageImport.h>
#include <mitkImageCast.h>
#include <mitkProperties.h>
#include <mitkIOUtil.h>
#include <mitkPreferenceListReaderOptionsFunctor.h>

/**
 * Perform Q-ball reconstruction using a spherical harmonics basis
 */
int main(int argc, char* argv[])
{
  mitkCommandLineParser parser;
  parser.setArgumentPrefix("--", "-");
  parser.addArgument("input", "i", mitkCommandLineParser::InputFile, "Input file", "input raw dwi (.dwi or .nii/.nii.gz)", us::Any(), false);
  parser.addArgument("outFile", "o", mitkCommandLineParser::OutputFile, "Output file", "output file", us::Any(), false);
  parser.addArgument("shOrder", "sh", mitkCommandLineParser::Int, "Spherical harmonics order", "spherical harmonics order", 4, true);
  parser.addArgument("b0Threshold", "t", mitkCommandLineParser::Int, "b0 threshold", "baseline image intensity threshold", 0, true);
  parser.addArgument("lambda", "r", mitkCommandLineParser::Float, "Lambda", "ragularization factor lambda", 0.006, true);
  parser.addArgument("csa", "csa", mitkCommandLineParser::Bool, "Constant solid angle consideration", "use constant solid angle consideration");
  parser.addArgument("outputCoeffs", "shc", mitkCommandLineParser::Bool, "Output coefficients", "output file containing the SH coefficients");
  parser.addArgument("mrtrix", "mb", mitkCommandLineParser::Bool, "MRtrix", "use MRtrix compatible spherical harmonics definition");

  parser.setCategory("Signal Modelling");
  parser.setTitle("Qball Reconstruction");
  parser.setDescription("");
  parser.setContributor("MIC");

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

  std::string inFileName = us::any_cast<std::string>(parsedArgs["input"]);
  std::string outfilename = us::any_cast<std::string>(parsedArgs["outFile"]);
  outfilename = itksys::SystemTools::GetFilenamePath(outfilename)+"/"+itksys::SystemTools::GetFilenameWithoutExtension(outfilename);

  int threshold = 0;
  if (parsedArgs.count("b0Threshold"))
    threshold = us::any_cast<int>(parsedArgs["b0Threshold"]);

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

  float lambda = 0.006;
  if (parsedArgs.count("lambda"))
    lambda = us::any_cast<float>(parsedArgs["lambda"]);

  int normalization = 0;
  if (parsedArgs.count("csa") && us::any_cast<bool>(parsedArgs["csa"]))
    normalization = 6;

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

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

  try
  {
    mitk::PreferenceListReaderOptionsFunctor functor = mitk::PreferenceListReaderOptionsFunctor({"Diffusion Weighted Images"}, {});
    std::vector< mitk::BaseData::Pointer > infile = mitk::IOUtil::Load(inFileName, &functor);

    mitk::Image::Pointer dwi = dynamic_cast<mitk::Image*>(infile.at(0).GetPointer());
    mitk::DiffusionPropertyHelper propertyHelper(dwi);
    propertyHelper.AverageRedundantGradients(0.001);
    propertyHelper.InitializeImage();

    mitk::OdfImage::Pointer image = mitk::OdfImage::New();
    mitk::Image::Pointer coeffsImage = mitk::Image::New();

    std::cout << "SH order: " << shOrder;
    std::cout << "lambda: " << lambda;
    std::cout << "B0 threshold: " << threshold;
    switch ( shOrder )
    {
    case 4:
    {
      typedef itk::AnalyticalDiffusionQballReconstructionImageFilter<short,short,float,4,ODF_SAMPLING_SIZE> FilterType;
      mitk::DiffusionPropertyHelper::ImageType::Pointer itkVectorImagePointer = mitk::DiffusionPropertyHelper::ImageType::New();
      mitk::CastToItkImage(dwi, itkVectorImagePointer);

      FilterType::Pointer filter = FilterType::New();
      filter->SetBValue(mitk::DiffusionPropertyHelper::GetReferenceBValue(dwi));
      filter->SetGradientImage( mitk::DiffusionPropertyHelper::GetGradientContainer(dwi), itkVectorImagePointer );
      filter->SetThreshold( threshold );
      filter->SetLambda(lambda);
      filter->SetUseMrtrixBasis(mrTrix);
      if (normalization==0)
        filter->SetNormalizationMethod(FilterType::QBAR_STANDARD);
      else
        filter->SetNormalizationMethod(FilterType::QBAR_SOLID_ANGLE);
      filter->Update();
      image->InitializeByItk( filter->GetOutput() );
      image->SetVolume( filter->GetOutput()->GetBufferPointer() );
      coeffsImage->InitializeByItk( filter->GetCoefficientImage().GetPointer() );
      coeffsImage->SetVolume( filter->GetCoefficientImage()->GetBufferPointer() );
      break;
    }
    case 6:
    {
      typedef itk::AnalyticalDiffusionQballReconstructionImageFilter<short,short,float,6,ODF_SAMPLING_SIZE> FilterType;
      mitk::DiffusionPropertyHelper::ImageType::Pointer itkVectorImagePointer = mitk::DiffusionPropertyHelper::ImageType::New();
      mitk::CastToItkImage(dwi, itkVectorImagePointer);

      FilterType::Pointer filter = FilterType::New();
      filter->SetBValue(mitk::DiffusionPropertyHelper::GetReferenceBValue(dwi));
      filter->SetGradientImage( mitk::DiffusionPropertyHelper::GetGradientContainer(dwi), itkVectorImagePointer );
      filter->SetThreshold( threshold );
      filter->SetLambda(lambda);
      filter->SetUseMrtrixBasis(mrTrix);
      if (normalization==0)
        filter->SetNormalizationMethod(FilterType::QBAR_STANDARD);
      else
        filter->SetNormalizationMethod(FilterType::QBAR_SOLID_ANGLE);
      filter->Update();
      image->InitializeByItk( filter->GetOutput() );
      image->SetVolume( filter->GetOutput()->GetBufferPointer() );
      coeffsImage->InitializeByItk( filter->GetCoefficientImage().GetPointer() );
      coeffsImage->SetVolume( filter->GetCoefficientImage()->GetBufferPointer() );
      break;
    }
    case 8:
    {
      typedef itk::AnalyticalDiffusionQballReconstructionImageFilter<short,short,float,8,ODF_SAMPLING_SIZE> FilterType;
      mitk::DiffusionPropertyHelper::ImageType::Pointer itkVectorImagePointer = mitk::DiffusionPropertyHelper::ImageType::New();
      mitk::CastToItkImage(dwi, itkVectorImagePointer);

      FilterType::Pointer filter = FilterType::New();
      filter->SetBValue(mitk::DiffusionPropertyHelper::GetReferenceBValue(dwi));
      filter->SetGradientImage( mitk::DiffusionPropertyHelper::GetGradientContainer(dwi), itkVectorImagePointer );
      filter->SetThreshold( threshold );
      filter->SetLambda(lambda);
      filter->SetUseMrtrixBasis(mrTrix);
      if (normalization==0)
        filter->SetNormalizationMethod(FilterType::QBAR_STANDARD);
      else
        filter->SetNormalizationMethod(FilterType::QBAR_SOLID_ANGLE);
      filter->Update();
      image->InitializeByItk( filter->GetOutput() );
      image->SetVolume( filter->GetOutput()->GetBufferPointer() );
      coeffsImage->InitializeByItk( filter->GetCoefficientImage().GetPointer() );
      coeffsImage->SetVolume( filter->GetCoefficientImage()->GetBufferPointer() );
      break;
    }
    case 10:
    {
      typedef itk::AnalyticalDiffusionQballReconstructionImageFilter<short,short,float,10,ODF_SAMPLING_SIZE> FilterType;
      mitk::DiffusionPropertyHelper::ImageType::Pointer itkVectorImagePointer = mitk::DiffusionPropertyHelper::ImageType::New();
      mitk::CastToItkImage(dwi, itkVectorImagePointer);

      FilterType::Pointer filter = FilterType::New();
      filter->SetBValue(mitk::DiffusionPropertyHelper::GetReferenceBValue(dwi));
      filter->SetGradientImage( mitk::DiffusionPropertyHelper::GetGradientContainer(dwi), itkVectorImagePointer );
      filter->SetThreshold( threshold );
      filter->SetLambda(lambda);
      filter->SetUseMrtrixBasis(mrTrix);
      if (normalization==0)
        filter->SetNormalizationMethod(FilterType::QBAR_STANDARD);
      else
        filter->SetNormalizationMethod(FilterType::QBAR_SOLID_ANGLE);
      filter->Update();
      image->InitializeByItk( filter->GetOutput() );
      image->SetVolume( filter->GetOutput()->GetBufferPointer() );
      coeffsImage->InitializeByItk( filter->GetCoefficientImage().GetPointer() );
      coeffsImage->SetVolume( filter->GetCoefficientImage()->GetBufferPointer() );
      break;
    }
    case 12:
    {
      typedef itk::AnalyticalDiffusionQballReconstructionImageFilter<short,short,float,12,ODF_SAMPLING_SIZE> FilterType;
      mitk::DiffusionPropertyHelper::ImageType::Pointer itkVectorImagePointer = mitk::DiffusionPropertyHelper::ImageType::New();
      mitk::CastToItkImage(dwi, itkVectorImagePointer);

      FilterType::Pointer filter = FilterType::New();
      filter->SetBValue(mitk::DiffusionPropertyHelper::GetReferenceBValue(dwi));
      filter->SetGradientImage( mitk::DiffusionPropertyHelper::GetGradientContainer(dwi), itkVectorImagePointer );
      filter->SetThreshold( threshold );
      filter->SetLambda(lambda);
      if (normalization==0)
        filter->SetNormalizationMethod(FilterType::QBAR_STANDARD);
      else
        filter->SetNormalizationMethod(FilterType::QBAR_SOLID_ANGLE);
      filter->Update();
      image->InitializeByItk( filter->GetOutput() );
      image->SetVolume( filter->GetOutput()->GetBufferPointer() );
      coeffsImage->InitializeByItk( filter->GetCoefficientImage().GetPointer() );
      coeffsImage->SetVolume( filter->GetCoefficientImage()->GetBufferPointer() );
      break;
    }
    default:
    {
      std::cout << "Supplied SH order not supported. Using default order of 4.";
      typedef itk::AnalyticalDiffusionQballReconstructionImageFilter<short,short,float,4,ODF_SAMPLING_SIZE> FilterType;
      mitk::DiffusionPropertyHelper::ImageType::Pointer itkVectorImagePointer = mitk::DiffusionPropertyHelper::ImageType::New();
      mitk::CastToItkImage(dwi, itkVectorImagePointer);

      FilterType::Pointer filter = FilterType::New();
      filter->SetBValue(mitk::DiffusionPropertyHelper::GetReferenceBValue(dwi));
      filter->SetGradientImage( mitk::DiffusionPropertyHelper::GetGradientContainer(dwi), itkVectorImagePointer );
      filter->SetThreshold( threshold );
      filter->SetLambda(lambda);
      filter->SetUseMrtrixBasis(mrTrix);
      if (normalization==0)
        filter->SetNormalizationMethod(FilterType::QBAR_STANDARD);
      else
        filter->SetNormalizationMethod(FilterType::QBAR_SOLID_ANGLE);
      filter->Update();
      image->InitializeByItk( filter->GetOutput() );
      image->SetVolume( filter->GetOutput()->GetBufferPointer() );
      coeffsImage->InitializeByItk( filter->GetCoefficientImage().GetPointer() );
      coeffsImage->SetVolume( filter->GetCoefficientImage()->GetBufferPointer() );
    }
    }

    std::string coeffout = outfilename;
    coeffout += "_shcoeffs.nrrd";

    outfilename += ".odf";
    mitk::IOUtil::Save(image, outfilename);

    if (outCoeffs)
      mitk::IOUtil::Save(coeffsImage, coeffout);
  }
  catch ( itk::ExceptionObject &err)
  {
    std::cout << "Exception: " << err;
  }
  catch ( std::exception err)
  {
    std::cout << "Exception: " << err.what();
  }
  catch ( ... )
  {
    std::cout << "Exception!";
  }
  return EXIT_SUCCESS;
}


//------------------------------------------------------------------------------
/*
    This file is part of rippled: https://github.com/ripple/rippled
    Copyright (c) 2012, 2013 Ripple Labs Inc.

    Permission to use, copy, modify, and/or distribute this software for any
    purpose  with  or without fee is hereby granted, provided that the above
    copyright notice and this permission notice appear in all copies.

    THE  SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
    WITH  REGARD  TO  THIS  SOFTWARE  INCLUDING  ALL  IMPLIED  WARRANTIES  OF
    MERCHANTABILITY  AND  FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
    ANY  SPECIAL ,  DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
    WHATSOEVER  RESULTING  FROM  LOSS  OF USE, DATA OR PROFITS, WHETHER IN AN
    ACTION  OF  CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
//==============================================================================

#include <BeastConfig.h>
#include <boost/algorithm/string.hpp>
#include <ripple/app/tx/impl/RingDeposit.h>
#include <ripple/app/main/Application.h>
#include <ripple/app/misc/NetworkOPs.h>
#include <ripple/basics/Log.h>
#include <ripple/protocol/digest.h>
#include <ripple/protocol/Indexes.h>
#include <ripple/protocol/TxFlags.h>
#include <ripple/crypto/AltBn128.h>

namespace ripple {

static
uint256
createRingHash(const STArray pks, const uint64_t value, const uint32_t ringIndex){
    Serializer s;
    for(auto const& pk : pks){
        STVector256 keyPair = pk.getFieldV256(sfPublicKeyPair);
        s.add256(keyPair[0]);
        s.add256(keyPair[1]);
    }
    s.add32(ringIndex);
    s.add64(value);
    return altbn128::sha256_s(s.getHex());
}

TER
RingDeposit::preflight (PreflightContext const& ctx)
{
    // check stx config
    if(!ctx.app.config ().exists (SECTION_SECRET_TX)){
        return tefINTERNAL;
    }

    // field check
    auto const amount = ctx.tx.getFieldAmount(sfAmount);
    std::string strLegalValues = get<std::string> (ctx.app.config ()[SECTION_SECRET_TX], "legal_values");
    std::vector<std::string> vecLegalValues;
    boost::split (vecLegalValues, strLegalValues, boost::is_any_of (","));
    if(std::find(vecLegalValues.begin(), vecLegalValues.end(), to_string(amount.mantissa())) == vecLegalValues.end()){
        JLOG(ctx.j.info) << "amount value:" << amount << " is illegal";
        return temBAD_AMOUNT;
    }

    const STVector256  stealthPk = ctx.tx.getFieldV256(sfPublicKeyPair);

    if(!isVBC(amount)){
        return temBAD_AMOUNT;
    }

    if(stealthPk.size() != 2){
        return telBAD_PUBLIC_KEY;
    }

    // check x/y on curve?
    openssl::bn_ctx bc;
    uint256 pkx = stealthPk[0];
    uint256 pky = stealthPk[1];
    if(!altbn128::onCurve(openssl::bignum(pkx), openssl::bignum(pky), bc)){
        JLOG(ctx.j.info) << "Stealth public key is not on curve.";
        return telBAD_PUBLIC_KEY;
    }

    return tesSUCCESS;
}

TER
RingDeposit::preclaim(PreclaimContext const& ctx){
    auto const id = ctx.tx[sfAccount];

    // check balance
    STAmount const amount (ctx.tx.getFieldAmount(sfAmount));
    auto const sle = ctx.view.read (keylet::account (id));
    if(!sle){
        return terNO_ACCOUNT;
    }
    STAmount balanceVBC = sle->getFieldAmount (sfBalanceVBC);
    if (balanceVBC < amount)
    {
        return tecUNFUNDED_TRANSFER;
    }

    // check request ringIndex
    uint32_t const ringIndexReq = ctx.tx.getFieldU32(sfRingIndex);
    if(ringIndexReq <= 0){
        JLOG(ctx.j.info) << "Ring:" << ringIndexReq << " is not found";
        return tecNO_ENTRY;
    }

    auto ringInfo = ctx.view.read(keylet::ring(amount.mantissa(), amount.issue(), 0));
    if(!ringInfo){  // first one
        if(ringIndexReq != 1){
            JLOG(ctx.j.info) << "First ringIndex:" << ringIndexReq << " starts from 1.";
            return tecNO_ENTRY;
        }
    }else{
        auto const lastRingIndex = ringInfo->getFieldU32(sfRingIndex);
        if(lastRingIndex != ringIndexReq){
            JLOG(ctx.j.info) << "RingIndex:" << ringIndexReq << " is obsolete";
            return tefRING_CLOSED;
        }

        // check dup publickey
        auto const ringSle = ctx.view.read(keylet::ring(amount.mantissa(), amount.issue(), lastRingIndex));
        if(ringSle && ringSle->isFieldPresent(sfPublicKeys)){
            STArray const ringPks = ringSle->getFieldArray(sfPublicKeys);
            STVector256 const newPk = ctx.tx.getFieldV256(sfPublicKeyPair);
            for(auto const pks : ringPks){
                STVector256 pkPair = pks.getFieldV256(sfPublicKeyPair);
                uint256 pkx = pkPair[0];
                if(pkx == newPk[0] || pkx == newPk[1]){
                    JLOG(ctx.j.info) << "Dup public key:" << pkx;
                    return telBAD_PUBLIC_KEY;
                }
            }
        }
    }

    return tesSUCCESS;

}

TER
RingDeposit::doApply()
{
    auto& app = ctx_.app;
    const uint32_t curLgrSeq = view().info().seq;

    auto const aCore = parseBase58<AccountID>(get<std::string> (app.config ()[SECTION_SECRET_TX], "public_key"));
    if(!aCore){
        return tefINTERNAL;
    }
    auto const coreSle = view().read(keylet::account(*aCore));
    if(!coreSle){
        JLOG(j_.error) << "Secret transaction publickey account not found.";
        return tefINTERNAL;
    }

    STAmount const amount (ctx_.tx.getFieldAmount(sfAmount));
    uint32_t partNum = get<uint32_t> (app.config ()[SECTION_SECRET_TX], "participants_number");

    STVector256 const newPk = ctx_.tx.getFieldV256(sfPublicKeyPair);

    auto ringInfo = view().peek(keylet::ring(amount.mantissa(), amount.issue(), 0));
    uint32_t lastRingIndex = 0;
    if (ringInfo){
        // lastest index
        lastRingIndex = ringInfo->getFieldU32(sfRingIndex);
    } else {
        // create first ring
        ringInfo = std::make_shared<SLE>(keylet::ring(amount.mantissa(), amount.issue(), 0));
        lastRingIndex += 1;
        ringInfo->setFieldU32(sfRingIndex, lastRingIndex);
        view().insert(ringInfo);
        JLOG(j_.debug) << "Create index sle,quantity:" << amount.mantissa() << ",issue:" << amount.issue() << ",ringIndex:" << lastRingIndex;
    }

    // make sure index in params eq ringInfo
    uint32_t const ringIndexReq = ctx_.tx.getFieldU32(sfRingIndex);
    if (lastRingIndex != ringIndexReq){
        return tefRING_CLOSED;
    }

    auto ringSle = view().peek(keylet::ring(amount.mantissa(), amount.issue(), lastRingIndex));
    if (ringSle){
        int fastInv = get<int> (app.config ()[SECTION_SECRET_TX], "fast_interval");
        int maxParts = get<int> (app.config ()[SECTION_SECRET_TX], "max_participants");
        const auto startSeq = ringSle->getFieldU32(sfLedgerSequence);
        bool bFastDelay = fastInv > 0 && curLgrSeq - startSeq < fastInv; // true: close delayed

        const auto depositedNum = ringSle->getFieldU32(sfRingDeposited);
        auto ringHash = ringSle->getFieldH256(sfRingHash);
        const auto ringPartNum = ringSle->getFieldU32(sfParticipantsNum);
        if (ringHash == 0 && (depositedNum <= ringPartNum || bFastDelay)){ // ring not closed
            // push account hash if not redundant
            auto acctHash = sha512Half(std::string("D"), account_, amount.mantissa(), lastRingIndex);
            auto accounts = ringSle->getFieldV256(sfAccounts);
            if (std::find(accounts.begin(), accounts.end(), acctHash)!=accounts.end())
                return tefRING_REDUNDANT;
            accounts.push_back(acctHash);
            ringSle->setFieldV256(sfAccounts, accounts);
            // update deposited
            ringSle->setFieldU32(sfRingDeposited, depositedNum + 1);
            // push new pk
            STArray ringPks = ringSle->getFieldArray(sfPublicKeys);
            ringPks.push_back(STObject(sfRingHolder));
            STObject& obj = ringPks.back();
            obj.setFieldV256(sfPublicKeyPair, newPk);
            ringSle->setFieldArray(sfPublicKeys, ringPks);

            if((depositedNum + 1 >= ringPartNum && !bFastDelay) || depositedNum + 1 >= maxParts){
                // become full, close this ring
                ringSle->setFieldH256(sfRingHash, createRingHash(ringPks, amount.mantissa(), lastRingIndex));
                lastRingIndex += 1;
                ringInfo->setFieldU32(sfRingIndex, lastRingIndex);
                view().update(ringInfo);
            }
            if(depositedNum + 1 > ringPartNum){
                ringSle->setFieldU32(sfParticipantsNum, depositedNum+1);
            }
            view().update(ringSle);
            JLOG(j_.info) << "udpate ring:" << lastRingIndex << ",parts:" << ringPartNum
                << ",deposited:" << depositedNum+1
                << (bFastDelay ? ",close delayed" : ",not delayed");

            return transferXRP(view(), account_, *aCore, amount, j_);
        }else{
            // already full, close
            if (ringHash == 0){
                // set ring hash if needed
                uint256 ringHash = createRingHash(ringSle->getFieldArray(sfPublicKeys), amount.mantissa(), lastRingIndex);
                ringSle->setFieldH256(sfRingHash, ringHash);
                JLOG(j_.info) << "close ringhash:" << ringHash << ",nextRingIndex:" << lastRingIndex;
                view().update(ringSle);
            }
            lastRingIndex += 1;
            ringInfo->setFieldU32(sfRingIndex, lastRingIndex);
            view().update(ringInfo);
        }
    }

    // create new ring
    auto newRingSle = std::make_shared<SLE>(keylet::ring(amount.mantissa(), amount.issue(), lastRingIndex));
    newRingSle->setFieldU32(sfLedgerSequence, curLgrSeq);

    STVector256 accounts(sfAccounts);
    accounts.push_back(sha512Half(std::string("D"), account_, amount.mantissa(), lastRingIndex));
    newRingSle->setFieldV256(sfAccounts, accounts);

    STArray ringPks (sfPublicKeys);
    ringPks.push_back(STObject(sfRingHolder));
    STObject& obj = ringPks.back();
    obj.setFieldV256(sfPublicKeyPair, newPk);
    newRingSle->setFieldArray(sfPublicKeys, ringPks);

    newRingSle->setFieldAmount(sfAmount, amount);
    newRingSle->setFieldU32(sfRingDeposited, 1);
    newRingSle->setFieldU32(sfRingWithdrawed, 0);
    newRingSle->setFieldU32(sfRingIndex, lastRingIndex);
    newRingSle->setFieldU32(sfParticipantsNum, partNum);
    view().insert(newRingSle);

    JLOG(j_.info) << "Generate new ring:" << lastRingIndex << ",pk:" << newPk[0] << ",amount:" << amount << ",participantsNumber:" << partNum;
    return transferXRP(view(), account_, *aCore, amount, j_);
}

}


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

#include "../ParserHelper.hpp"

#include <armnn/Tensor.hpp>
#include <armnn/Types.hpp>

#include <boost/test/unit_test.hpp>

using namespace armnn;
using namespace armnnUtils;

BOOST_AUTO_TEST_SUITE(ParserHelperSuite)

BOOST_AUTO_TEST_CASE(CalculateReducedOutputTensoInfoTest)
{
    bool keepDims = false;

    unsigned int inputShape[] = { 2, 3, 4 };
    TensorInfo inputTensorInfo(3, &inputShape[0], DataType::Float32);

    // Reducing all dimensions results in one single output value (one dimension)
    std::set<unsigned int> axisData1 = { 0, 1, 2 };
    TensorInfo outputTensorInfo1;

    CalculateReducedOutputTensoInfo(inputTensorInfo, axisData1, keepDims, outputTensorInfo1);

    BOOST_ASSERT(outputTensorInfo1.GetNumDimensions() == 1);
    BOOST_ASSERT(outputTensorInfo1.GetShape()[0] == 1);

    // Reducing dimension 0 results in a 3x4 size tensor (one dimension)
    std::set<unsigned int> axisData2 = { 0 };
    TensorInfo outputTensorInfo2;

    CalculateReducedOutputTensoInfo(inputTensorInfo, axisData2, keepDims, outputTensorInfo2);

    BOOST_ASSERT(outputTensorInfo2.GetNumDimensions() == 1);
    BOOST_ASSERT(outputTensorInfo2.GetShape()[0] == 12);

    // Reducing dimensions 0,1 results in a 4 size tensor (one dimension)
    std::set<unsigned int> axisData3 = { 0, 1 };
    TensorInfo outputTensorInfo3;

    CalculateReducedOutputTensoInfo(inputTensorInfo, axisData3, keepDims, outputTensorInfo3);

    BOOST_ASSERT(outputTensorInfo3.GetNumDimensions() == 1);
    BOOST_ASSERT(outputTensorInfo3.GetShape()[0] == 4);

    // Reducing dimension 0 results in a { 1, 3, 4 } dimension tensor
    keepDims = true;
    std::set<unsigned int> axisData4 = { 0 };

    TensorInfo outputTensorInfo4;

    CalculateReducedOutputTensoInfo(inputTensorInfo, axisData4, keepDims, outputTensorInfo4);

    BOOST_ASSERT(outputTensorInfo4.GetNumDimensions() == 3);
    BOOST_ASSERT(outputTensorInfo4.GetShape()[0] == 1);
    BOOST_ASSERT(outputTensorInfo4.GetShape()[1] == 3);
    BOOST_ASSERT(outputTensorInfo4.GetShape()[2] == 4);

    // Reducing dimension 1, 2 results in a { 2, 1, 1 } dimension tensor
    keepDims = true;
    std::set<unsigned int> axisData5 = { 1, 2 };

    TensorInfo outputTensorInfo5;

    CalculateReducedOutputTensoInfo(inputTensorInfo, axisData5,  keepDims, outputTensorInfo5);

    BOOST_ASSERT(outputTensorInfo5.GetNumDimensions() == 3);
    BOOST_ASSERT(outputTensorInfo5.GetShape()[0] == 2);
    BOOST_ASSERT(outputTensorInfo5.GetShape()[1] == 1);
    BOOST_ASSERT(outputTensorInfo5.GetShape()[2] == 1);

}

BOOST_AUTO_TEST_SUITE_END()


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <cmath>
#include <Eigen/Eigen>
#include "covariance.h"

namespace snark{

squared_exponential_covariance::squared_exponential_covariance( double length_scale
                                                              , double signal_variance
                                                              , double data_variance )
    : signal_variance_( signal_variance )
    , data_variance_( data_variance )
    , factor_( -1.0 / ( 2.0 * std::sqrt( length_scale ) ) )
    , self_covariance_( signal_variance + data_variance )
{
}

double squared_exponential_covariance::covariance( const Eigen::VectorXd& v, const Eigen::VectorXd& w ) const
{
    const Eigen::VectorXd& diff = v - w;
    return signal_variance_ * std::exp( factor_ * diff.dot( diff ) );
}

double squared_exponential_covariance::self_covariance() const { return self_covariance_; }

} // namespace snark{ { namespace Robotics {


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-arithmetic
    Function object implementing itrunc capabilities

    Convert to integer by saturated truncation.

    This is an alias of saturated_(toint) (See @ref toint)

    @par Header <boost/simd/function/itrunc.hpp>

    @see ifix
  **/
  Value itrunc(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/itrunc.hpp>
#include <boost/simd/function/simd/itrunc.hpp>

#endif


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

    @Copyright 2016 Numscale SAS
    @copyright 2016 J.T.Lapreste

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE4_1_SIMD_FUNCTION_SPLIT_HIGH_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE4_1_SIMD_FUNCTION_SPLIT_HIGH_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/dispatch/meta/upgrade.hpp>
#include <boost/dispatch/meta/cardinal_of.hpp>
#include <boost/simd/function/simd/slide.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( split_high_
                          , (typename A0)
                          , bs::sse4_1_
                          , bs::pack_<bd::int8_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE upgrade_t<A0> operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return _mm_cvtepi8_epi16(slide<bd::cardinal_of<A0>::value/2>(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( split_high_
                          , (typename A0)
                          , bs::sse4_1_
                          , bs::pack_<bd::int16_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE upgrade_t<A0> operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return _mm_cvtepi16_epi32(slide<bd::cardinal_of<A0>::value/2>(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( split_high_
                          , (typename A0)
                          , bs::sse4_1_
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE upgrade_t<A0> operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return _mm_cvtepi32_epi64(slide<bd::cardinal_of<A0>::value/2>(a0));
    }
  };
} } }

#endif