#include <iostream>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Main
#include <boost/test/unit_test.hpp>
#include <vector>

#include "storage_engine/compression.h"
#include "storage_engine/volume.h"

using namespace Akumuli;

static const u64
    EXPECTED[] = {
    0ul, 1ul, 10ul,
    67ul, 127ul, 128ul,
    1024ul, 10000ul,
    100000ul, 420000000ul,
    420000001ul
};

static const size_t
        EXPECTED_SIZE = sizeof(EXPECTED)/sizeof(u64);

template<class TStreamWriter>
void test_stream_write(TStreamWriter& writer) {
    // Encode
    for (auto i = 0u; i < EXPECTED_SIZE; i++) {
        if (!writer.put(EXPECTED[i])) {
            BOOST_FAIL("Buffer is too small");
        }
    }
    writer.commit();

    const size_t
            USED_SIZE = writer.size();
    BOOST_REQUIRE_LT(USED_SIZE, sizeof(EXPECTED));
    BOOST_REQUIRE_GT(USED_SIZE, EXPECTED_SIZE);
}

template<class TVal, class TStreamWriter, class TStreamReader>
void test_stream_chunked_op(TStreamWriter& writer, TStreamReader& reader, size_t nsteps, bool sort_input=false, bool fixed_step=false) {
    std::vector<TVal> input;
    const size_t step_size = 16;
    const size_t input_size = step_size*nsteps;
    TVal value = 100000;

    // Generate
    if (!fixed_step) {
        input.push_back(0);
        for (u32 i = 0; i < (input_size-1); i++) {
            int delta = rand() % 1000 - 500;
            value += TVal(delta);
            input.push_back(value);
        }
    } else {
        for (u32 i = 0; i < nsteps; i++) {
            int delta = rand() % 1000;  // all positive
            for (u32 j = 0; j < step_size; j++) {
                value += TVal(delta);
                input.push_back(value);
            }
        }
    }

    if (sort_input) {
        std::sort(input.begin(), input.end());
    }

    // Encode
    for (auto offset = 0u; offset < input_size; offset += step_size) {
        auto success = writer.tput(input.data() + offset, step_size);
        if (!success) {
            BOOST_REQUIRE(success);
        }
    }

    // Decode and compare results
    std::vector<TVal> results;
    for (auto offset = 0ul; offset < input_size; offset++) {
        auto next = reader.next();
        results.push_back(next);
    }

    BOOST_REQUIRE_EQUAL_COLLECTIONS(input.begin(), input.end(),
                                    results.begin(), results.end());
}

template<class TStreamReader>
void test_stream_read(TStreamReader& reader) {
    // Read it back
    u64 actual[EXPECTED_SIZE];
    for (auto i = 0u; i < EXPECTED_SIZE; i++) {
        actual[i] = reader.next();
    }
    BOOST_REQUIRE_EQUAL_COLLECTIONS(EXPECTED, EXPECTED + EXPECTED_SIZE,
            actual, actual + EXPECTED_SIZE);
}

//! Base128StreamReader::next is a template, so we need to specialize this function.
template<>
void test_stream_read(Base128StreamReader& reader) {
    // Read it back
    u64 actual[EXPECTED_SIZE];
    for (auto i = 0u; i < EXPECTED_SIZE; i++) {
        actual[i] = reader.template next<u64>();
    }
    BOOST_REQUIRE_EQUAL_COLLECTIONS(EXPECTED, EXPECTED + EXPECTED_SIZE,
            actual, actual + EXPECTED_SIZE);
}

//! VByteStreamReader::next is a template, so we need to specialize this function.
template<>
void test_stream_read(VByteStreamReader& reader) {
    // Read it back
    u64 actual[EXPECTED_SIZE];
    for (auto i = 0u; i < EXPECTED_SIZE; i++) {
        actual[i] = reader.template next<u64>();
    }
    BOOST_REQUIRE_EQUAL_COLLECTIONS(EXPECTED, EXPECTED + EXPECTED_SIZE,
            actual, actual + EXPECTED_SIZE);
}

BOOST_AUTO_TEST_CASE(Test_base128) {

    std::vector<unsigned char> data;
    data.resize(1000);

    Base128StreamWriter writer(data.data(), data.data() + data.size());
    test_stream_write(writer);

    Base128StreamReader reader(data.data(), data.data() + data.size());
    test_stream_read(reader);
}

BOOST_AUTO_TEST_CASE(Test_vbyte) {

    std::vector<unsigned char> data;
    data.resize(1000);

    VByteStreamWriter writer(data.data(), data.data() + data.size());
    test_stream_write(writer);

    VByteStreamReader reader(data.data(), data.data() + data.size());
    test_stream_read(reader);
}

BOOST_AUTO_TEST_CASE(Test_chunked_delta_delta_vbyte_0) {
    std::vector<unsigned char> data;
    data.resize(4*1024);  // 4KB of storage

    {   // variable step
        VByteStreamWriter wstream(data.data(), data.data() + data.size());
        DeltaDeltaStreamWriter<16, u64> delta_writer(wstream);
        VByteStreamReader rstream(data.data(), data.data() + data.size());
        DeltaDeltaStreamReader<16, u64> delta_reader(rstream);

        test_stream_chunked_op<u64>(delta_writer, delta_reader, 100, true, false);
    }
    {   // fixed step
        VByteStreamWriter wstream(data.data(), data.data() + data.size());
        DeltaDeltaStreamWriter<16, u64> delta_writer(wstream);
        VByteStreamReader rstream(data.data(), data.data() + data.size());
        DeltaDeltaStreamReader<16, u64> delta_reader(rstream);

        test_stream_chunked_op<u64>(delta_writer, delta_reader, 100, true, true);
    }
}

//! Generate time-series from random walk
struct RandomWalk {
    std::random_device                  randdev;
    std::mt19937                        generator;
    std::normal_distribution<double>    distribution;
    double                              value;

    RandomWalk(double start, double mean, double stddev)
        : generator(randdev())
        , distribution(mean, stddev)
        , value(start)
    {
    }

    double generate() {
        value += distribution(generator);
        return value;
    }
};

void test_float_compression(double start, std::vector<double>* psrc=nullptr) {
    RandomWalk rwalk(start, 1., .11);
    int N = 10000;
    std::vector<double> samples;
    std::vector<u8> block;
    block.resize(N*9, 0);

    // Compress
    VByteStreamWriter wstream(block.data(), block.data() + block.size());
    FcmStreamWriter<> writer(wstream);
    if (psrc == nullptr) {
        double val = rwalk.generate();
        samples.push_back(val);
    } else {
        samples = *psrc;
    }

    for (size_t ix = 0; ix < samples.size(); ix++) {
        auto val = samples.at(ix);
        writer.put(val);
    }
    writer.commit();

    // Decompress
    VByteStreamReader rstream(block.data(), block.data() + block.size());
    FcmStreamReader<> reader(rstream);
    for (size_t ix = 0; ix < samples.size(); ix++) {
        double val = reader.next();
        if (val != samples.at(ix)) {
            BOOST_REQUIRE(val == samples.at(ix));
        }
    }
}

BOOST_AUTO_TEST_CASE(Test_float_compression_0) {
    test_float_compression(0);
}

BOOST_AUTO_TEST_CASE(Test_float_compression_1) {
    test_float_compression(1E-100);
}

BOOST_AUTO_TEST_CASE(Test_float_compression_2) {
    test_float_compression(1E100);
}

BOOST_AUTO_TEST_CASE(Test_float_compression_3) {
    test_float_compression(-1E-100);
}

BOOST_AUTO_TEST_CASE(Test_float_compression_4) {
    test_float_compression(-1E100);
}

BOOST_AUTO_TEST_CASE(Test_float_compression_5) {
    std::vector<double> samples(998, 3.14159);
    samples.push_back(111.222);
    samples.push_back(222.333);
    test_float_compression(0, &samples);
}

void test_block_compression(double start, unsigned N=10000, bool regullar=false) {
    RandomWalk rwalk(start, 1., .11);
    std::vector<aku_Timestamp> timestamps;
    std::vector<double> values;
    std::vector<u8> block;
    block.resize(4096);

    if (regullar) {
        aku_Timestamp its = static_cast<aku_Timestamp>(rand());
        aku_Timestamp stp = static_cast<aku_Timestamp>(rand() % 1000);
        for (unsigned i = 0; i < N; i++) {
            values.push_back(rwalk.generate());
            its += stp;
            timestamps.push_back(its);
        }
    } else {
        aku_Timestamp its = static_cast<aku_Timestamp>(rand());
        for (unsigned i = 0; i < N; i++) {
            values.push_back(rwalk.generate());
            u32 skew = rand() % 100;
            its += skew;
            timestamps.push_back(its);
        }
    }

    // compress

    StorageEngine::DataBlockWriter writer(42, block.data(), block.size());

    size_t actual_nelements = 0ull;
    bool writer_overflow = false;
    for (size_t ix = 0; ix < N; ix++) {
        aku_Status status = writer.put(timestamps.at(ix), values.at(ix));
        if (status == AKU_EOVERFLOW) {
            // Block is full
            actual_nelements = ix;
            writer_overflow = true;
            break;
        }
        BOOST_REQUIRE_EQUAL(status, AKU_SUCCESS);
    }
    if (!writer_overflow) {
        actual_nelements = N;
    }
    size_t size_used = writer.commit();

    // decompress
    StorageEngine::DataBlockReader reader(block.data(), size_used);

    std::vector<aku_Timestamp> out_timestamps;
    std::vector<double> out_values;

    // gen number of elements stored in block
    u32 nelem = reader.nelements();
    BOOST_REQUIRE_EQUAL(nelem, actual_nelements);
    BOOST_REQUIRE_NE(nelem, 0);

    BOOST_REQUIRE_EQUAL(reader.get_id(), 42);
    for (size_t ix = 0ull; ix < reader.nelements(); ix++) {
        aku_Status status;
        aku_Timestamp  ts;
        double      value;
        std::tie(status, ts, value) = reader.next();
        BOOST_REQUIRE_EQUAL(status, AKU_SUCCESS);
        out_timestamps.push_back(ts);
        out_values.push_back(value);
    }

    // nelements() + 1 call should result in error
    aku_Status status;
    aku_Timestamp  ts;
    double      value;
    std::tie(status, ts, value) = reader.next();
    BOOST_REQUIRE_EQUAL(status, AKU_ENO_DATA);

    for (size_t i = 0; i < nelem; i++) {
        if (timestamps.at(i) != out_timestamps.at(i)) {
            BOOST_FAIL("Bad timestamp at " << i << ", expected: " << timestamps.at(i) <<
                       ", actual: " << out_timestamps.at(i));
        }
        if (values.at(i) != out_values.at(i)) {
            BOOST_FAIL("Bad value at " << i << ", expected: " << values.at(i) <<
                       ", actual: " << out_values.at(i));
        }
    }
}

BOOST_AUTO_TEST_CASE(Test_block_compression_00) {
    test_block_compression(0);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_01) {
    test_block_compression(1E-100);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_02) {
    test_block_compression(1E100);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_03) {
    test_block_compression(-1E-100);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_04) {
    test_block_compression(-1E100);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_05) {
    test_block_compression(0, 1);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_06) {
    test_block_compression(0, 16);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_07) {
    test_block_compression(0, 100);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_08) {
    test_block_compression(0, 0x100);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_09) {
    test_block_compression(0, 0x111);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_10) {
    test_block_compression(0, 10000, true);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_11) {
    test_block_compression(1E-100, 10000, true);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_12) {
    test_block_compression(1E100, 10000, true);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_13) {
    test_block_compression(-1E-100, 10000, true);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_14) {
    test_block_compression(-1E100, 10000, true);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_15) {
    test_block_compression(0, 1, true);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_16) {
    test_block_compression(0, 16, true);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_17) {
    test_block_compression(0, 100, true);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_18) {
    test_block_compression(0, 0x100, true);
}

BOOST_AUTO_TEST_CASE(Test_block_compression_19) {
    test_block_compression(0, 0x111, true);
}

struct CheckedBlock {
    enum {
        NCOMPONENTS = 4,
        COMPONENT_SIZE = StorageEngine::AKU_BLOCK_SIZE / NCOMPONENTS,
    };

    std::vector<u8> provided_;
    u32 pos_;
    u8 pod_[1024]; //! all space is allocated here (and not checked)

    CheckedBlock(const std::vector<u8>& p)
        : provided_(p)
        , pos_(0)
    {
    }

    u8* allocate(u32 size) {
        u8* result = provided_.data() + pos_;
        pos_ += size;
        return result;
    }

    /** Add component if block is less than NCOMPONENTS in size.
     *  Return index of the component or -1 if block is full.
     */
    int add() { return 0; }

    void set_addr(u64) {}

    u64 get_addr() const { return 0; }

    int space_left() const { return static_cast<int>(StorageEngine::AKU_BLOCK_SIZE - pos_); }

    int size() const { return StorageEngine::AKU_BLOCK_SIZE; }

    void put(u8 val) {
        if (val != provided_.at(pos_)) {
            BOOST_REQUIRE_EQUAL(val, provided_.at(pos_));
        }
        pos_++;
    }

    bool safe_put(u8 val) {
        if (val != provided_.at(pos_)) {
            BOOST_REQUIRE_EQUAL(val, provided_.at(pos_));
        }
        pos_++;
        return true;
    }

    int get_write_pos() const {
        return static_cast<int>(pos_);
    }

    void set_write_pos(int pos) {
        pos_ = static_cast<u32>(pos);
    }

    template<class POD>
    void put(const POD& data) {
        const u8* it = reinterpret_cast<const u8*>(&data);
        for (u32 i = 0; i < sizeof(POD); i++) {
            put(it[i]);
        }
    }

    template<class POD>
    POD* allocate() {
        POD* result = reinterpret_cast<POD*>(pod_ + pos_);
        pos_ += sizeof(POD);
        return result;
    }

    bool is_readonly() const {
        return false;
    }

    const u8* get_data(int) const {
        return nullptr;
    }

    const u8* get_cdata(int) const {
        return nullptr;
    }

    u8* get_data(int) {
        return nullptr;
    }

    size_t get_size(int) const {
        return 1024;
    }

};

void test_block_iovec_compression(double start, unsigned N=10000, bool regullar=false) {
    RandomWalk rwalk(start, 1., .11);
    std::vector<aku_Timestamp> timestamps;
    std::vector<double> values;

    if (regullar) {
        aku_Timestamp its = static_cast<aku_Timestamp>(rand());
        aku_Timestamp stp = static_cast<aku_Timestamp>(rand() % 1000);
        for (unsigned i = 0; i < N; i++) {
            values.push_back(rwalk.generate());
            its += stp;
            timestamps.push_back(its);
        }
    } else {
        aku_Timestamp its = static_cast<aku_Timestamp>(rand());
        for (unsigned i = 0; i < N; i++) {
            values.push_back(rwalk.generate());
            u32 skew = rand() % 100;
            its += skew;
            timestamps.push_back(its);
        }
    }

    // compress using normal block
    std::vector<u8> dbdata;
    dbdata.resize(4096);
    StorageEngine::DataBlockWriter dbwriter(42, dbdata.data(), dbdata.size());
    for (size_t ix = 0; ix < N; ix++) {
        aku_Status status = dbwriter.put(timestamps.at(ix), values.at(ix));
        if (status == AKU_EOVERFLOW) {
            // Block is full
            break;
        }
        BOOST_REQUIRE_EQUAL(status, AKU_SUCCESS);
    }

    // compress
    CheckedBlock chkblock(dbdata);
    StorageEngine::IOVecBlockWriter<CheckedBlock> chkwriter(&chkblock);
    chkwriter.init(42);

    StorageEngine::IOVecBlock block;
    StorageEngine::IOVecBlockWriter<StorageEngine::IOVecBlock> writer(&block);
    writer.init(42);

    size_t actual_nelements = 0ull;
    bool writer_overflow = false;
    for (size_t ix = 0; ix < N; ix++) {
        aku_Status chkstatus = chkwriter.put(timestamps.at(ix), values.at(ix));
        aku_Status status = writer.put(timestamps.at(ix), values.at(ix));
        BOOST_REQUIRE_EQUAL(chkstatus, status);
        if (status == AKU_EOVERFLOW) {
            // Block is full
            actual_nelements = ix;
            writer_overflow = true;
            break;
        }
        BOOST_REQUIRE_EQUAL(status, AKU_SUCCESS);
    }
    if (!writer_overflow) {
        actual_nelements = N;
    }
    size_t size_used = writer.commit();
    AKU_UNUSED(size_used);

    // decompress using normal procedure
    std::vector<u8> cblock;
    cblock.reserve(4096);
    for (int i = 0; i < StorageEngine::IOVecBlock::NCOMPONENTS; i++) {
        const int sz = StorageEngine::IOVecBlock::COMPONENT_SIZE;
        if(block.get_size(i) == 0) {
            break;
        }
        std::copy(block.get_cdata(i), block.get_cdata(i) + sz, std::back_inserter(cblock));
    }

    // This iovec block contains data in once continous region
    StorageEngine::IOVecBlock cont_block(true);
    std::copy(cblock.begin(), cblock.end(), cont_block.get_data(0));

    StorageEngine::DataBlockReader reader(cblock.data(), cblock.size());
    StorageEngine::IOVecBlockReader<StorageEngine::IOVecBlock> iovecreader(&block);
    StorageEngine::IOVecBlockReader<StorageEngine::IOVecBlock> iovecreader_cont(&cont_block);

    std::vector<aku_Timestamp> out_timestamps;
    std::vector<double> out_values;

    // gen number of elements stored in block
    auto nelem = reader.nelements();
    BOOST_REQUIRE_EQUAL(nelem, actual_nelements);
    BOOST_REQUIRE_EQUAL(nelem, iovecreader.nelements());
    BOOST_REQUIRE_EQUAL(nelem, iovecreader_cont.nelements());
    BOOST_REQUIRE_NE(nelem, 0);

    BOOST_REQUIRE_EQUAL(reader.get_id(), 42);
    BOOST_REQUIRE_EQUAL(iovecreader.get_id(), 42);
    BOOST_REQUIRE_EQUAL(iovecreader_cont.get_id(), 42);
    for (size_t ix = 0ull; ix < reader.nelements(); ix++) {
        aku_Status status;
        aku_Timestamp  ts;
        double      value;
        std::tie(status, ts, value) = reader.next();
        BOOST_REQUIRE_EQUAL(status, AKU_SUCCESS);
        out_timestamps.push_back(ts);
        out_values.push_back(value);
        aku_Timestamp  iovects;
        double      iovecvalue;
        // Check io-vec block with four components
        std::tie(status, iovects, iovecvalue) = iovecreader.next();
        BOOST_REQUIRE_EQUAL(value, iovecvalue);
        BOOST_REQUIRE_EQUAL(ts, iovects);
        // Check continous io-vec block
        std::tie(status, iovects, iovecvalue) = iovecreader_cont.next();
        BOOST_REQUIRE_EQUAL(value, iovecvalue);
        BOOST_REQUIRE_EQUAL(ts, iovects);
    }

    // nelements() + 1 call should result in error
    aku_Status status;
    aku_Timestamp  ts;
    double      value;
    std::tie(status, ts, value) = reader.next();
    BOOST_REQUIRE_EQUAL(status, AKU_ENO_DATA);

    for (size_t i = 0; i < nelem; i++) {
        if (timestamps.at(i) != out_timestamps.at(i)) {
            BOOST_FAIL("Bad timestamp at " << i << ", expected: " << timestamps.at(i) <<
                       ", actual: " << out_timestamps.at(i));
        }
        if (values.at(i) != out_values.at(i)) {
            BOOST_FAIL("Bad value at " << i << ", expected: " << values.at(i) <<
                       ", actual: " << out_values.at(i));
        }
    }
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_00) {
    test_block_iovec_compression(0);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_01) {
    test_block_iovec_compression(1E-100);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_02) {
    test_block_iovec_compression(1E100);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_03) {
    test_block_iovec_compression(-1E-100);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_04) {
    test_block_iovec_compression(-1E100);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_05) {
    test_block_iovec_compression(0, 1);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_06) {
    test_block_iovec_compression(0, 16);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_07) {
    test_block_iovec_compression(0, 100);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_08) {
    test_block_iovec_compression(0, 0x100);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_09) {
    test_block_iovec_compression(0, 0x111);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_10) {
    test_block_iovec_compression(0, 10000, true);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_11) {
    test_block_iovec_compression(1E-100, 10000, true);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_12) {
    test_block_iovec_compression(1E100, 10000, true);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_13) {
    test_block_iovec_compression(-1E-100, 10000, true);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_14) {
    test_block_iovec_compression(-1E100, 10000, true);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_15) {
    test_block_iovec_compression(0, 1, true);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_16) {
    test_block_iovec_compression(0, 16, true);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_17) {
    test_block_iovec_compression(0, 100, true);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_18) {
    test_block_iovec_compression(0, 0x100, true);
}

BOOST_AUTO_TEST_CASE(Test_iovec_compression_19) {
    test_block_iovec_compression(0, 0x111, true);
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

#include "Lagrangian2d1DR.hpp"
#include <boost/math/quaternion.hpp>
#include "NewtonEulerDS.hpp"
#include "Interaction.hpp"
#include "BlockVector.hpp"
#include <boost/math/quaternion.hpp>

// #define DEBUG_NOCOLOR
// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include "siconos_debug.h"

void Lagrangian2d1DR::initialize(Interaction& inter)
{
  LagrangianR::initialize(inter);
  //proj_with_q  _jachqProj.reset(new SimpleMatrix(_jachq->size(0),_jachq->size(1)));

  if((inter.getSizeOfDS() !=3) and (inter.getSizeOfDS() !=6))
  {
    THROW_EXCEPTION("Lagrangian2d1DR::initialize(Interaction& inter). The size of ds must of size 3");
  }
  unsigned int qSize = 3 * (inter.getSizeOfDS() / 3);
  _jachq.reset(new SimpleMatrix(1, qSize));
}

void Lagrangian2d1DR::computeJachq(const BlockVector& q, BlockVector& z)
{
  DEBUG_BEGIN("Lagrangian2d1DR::computeJachq(Interaction& inter, SP::BlockVector q0 \n");

  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double G1x = q.getValue(0);
  double G1y = q.getValue(1);

  _jachq->setValue(0,0,Nx);
  _jachq->setValue(0,1,Ny);
  _jachq->setValue(0,2,(G1y-Py)*Nx - (G1x-Px)*Ny);

  if(q.size() ==6)
  {
    DEBUG_PRINT("take into account second ds\n");
    double G2x = q.getValue(3);
    double G2y = q.getValue(4);

    _jachq->setValue(0,3,-Nx);
    _jachq->setValue(0,4,-Ny);
    _jachq->setValue(0,5,- ((G2y-Py)*Nx - (G2x-Px)*Ny));
  }
  DEBUG_EXPR(_jachq->display(););
  DEBUG_END("Lagrangian2d1DR::computeJachq(Interaction& inter, SP::BlockVector q0) \n");

}

double Lagrangian2d1DR::distance() const
{
  DEBUG_BEGIN("Lagrangian2d1DR::distance(...)\n")
  SiconosVector dpc(*_Pc2 - *_Pc1);
  DEBUG_END("Lagrangian2d1DR::distance(...)\n")
  return dpc.norm2() * (inner_prod(*_Nc, dpc) >= 0 ? -1 : 1);

}

void Lagrangian2d1DR::computeh(const BlockVector& q, BlockVector& z, SiconosVector& y)
{
  DEBUG_BEGIN("Lagrangian2d1DR::computeh(...)\n");
  DEBUG_EXPR(q.display());
  // Contact points and normal are stored as relative to q1 and q2, if
  // no q2 then pc2 and normal are absolute.

  // Update pc1 based on q and relPc1

  double angle= q(2);
  (*_Pc1)(0) = q(0) + cos(angle) * (*_relPc1)(0)+ sin(angle) * (*_relPc1)(1);
  (*_Pc1)(1) = q(1) + sin(angle) * (*_relPc1)(0)- cos(angle) * (*_relPc1)(1);
  if(q.size() == 6)
  {
    // To be checked
    DEBUG_PRINT("take into account second ds\n");
    angle = q(5);
    (*_Pc2)(0) = q(3) + cos(angle) * (*_relPc2)(0)+ sin(angle) * (*_relPc2)(1);
    (*_Pc2)(1) = q(4) + sin(angle) * (*_relPc2)(0)- cos(angle) * (*_relPc2)(1);
    (*_Nc)(0) =  cos(angle) * (*_relNc)(0)+ sin(angle) * (*_relNc)(1);
    (*_Nc)(1) =  sin(angle) * (*_relNc)(0)+ cos(angle) * (*_relNc)(1);
  }
  else
  {
    *_Pc2 = *_relPc2;
    *_Nc = *_relNc;
  }

  // SP::SiconosVector q1 = (q0.getAllVect())[0];
  // ::boost::math::quaternion<double> qq1((*q1)(3), (*q1)(4), (*q1)(5), (*q1)(6));
  // ::boost::math::quaternion<double> qpc1(0,(*_relPc1)(0),(*_relPc1)(1),(*_relPc1)(2));

  // // apply q1 rotation and add
  // qpc1 = qq1 * qpc1 / qq1;
  // (*_Pc1)(0) = qpc1.R_component_2() + (*q1)(0);
  // (*_Pc1)(1) = qpc1.R_component_3() + (*q1)(1);
  // (*_Pc1)(2) = qpc1.R_component_4() + (*q1)(2);

  // if (q0.numberOfBlocks() > 1)
  // {
  //   // Update pc2 based on q0 and relPc2
  //   SP::SiconosVector q2 = (q0.getAllVect())[1];
  //   ::boost::math::quaternion<double> qq2((*q2)(3), (*q2)(4), (*q2)(5), (*q2)(6));
  //   ::boost::math::quaternion<double> qpc2(0,(*_relPc2)(0),(*_relPc2)(1),(*_relPc2)(2));

  //   // apply q2 rotation and add
  //   qpc2 = qq2 * qpc2 / qq2;
  //   (*_Pc2)(0) = qpc2.R_component_2() + (*q2)(0);
  //   (*_Pc2)(1) = qpc2.R_component_3() + (*q2)(1);
  //   (*_Pc2)(2) = qpc2.R_component_4() + (*q2)(2);

  //   // same for normal
  //   ::boost::math::quaternion<double> qnc(0, (*_relNc)(0), (*_relNc)(1), (*_relNc)(2));
  //   qnc = qq2 * qnc / qq2;
  //   (*_Nc)(0) = qnc.R_component_2();
  //   (*_Nc)(1) = qnc.R_component_3();
  //   (*_Nc)(2) = qnc.R_component_4();
  // }
  // else
  // {
  //   *_Pc2 = *_relPc2;
  //   *_Nc = *_relNc;
  // }

  LagrangianScleronomousR::computeh(q, z, y);
  y.setValue(0, distance());
  DEBUG_EXPR(y.display(););
  DEBUG_EXPR(display(););
  DEBUG_END("Lagrangian2d1DR::computeh(...)\n")
}
void Lagrangian2d1DR::display() const
{
  LagrangianR::display();

  std::cout << " _Pc1 :" << std::endl;
  if(_Pc1)
    _Pc1->display();
  else
    std::cout << " nullptr :" << std::endl;

  std::cout << " _Pc2 :" << std::endl;
  if(_Pc2)
    _Pc2->display();
  else
    std::cout << " nullptr :" << std::endl;

  std::cout << " _relPc1 :" << std::endl;
  if(_relPc1)
    _relPc1->display();
  else
    std::cout << " nullptr :" << std::endl;

  std::cout << " _relPc2 :" << std::endl;
  if(_relPc2)
    _relPc2->display();
  else
    std::cout << " nullptr :" << std::endl;

  std::cout << " _Nc :" << std::endl;
  if(_Nc)
    _Nc->display();
  else
    std::cout << " nullptr :" << std::endl;
  std::cout << " _relNc :" << std::endl;
  if(_relNc)
    _relNc->display();
  else
    std::cout << " nullptr :" << std::endl;

}


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

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 "itkTractsToDWIImageFilter.h"
#include <boost/progress.hpp>
#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkPolyLine.h>
#include <itkImageRegionIteratorWithIndex.h>
#include <itkResampleImageFilter.h>
#include <itkNearestNeighborInterpolateImageFunction.h>
#include <itkBSplineInterpolateImageFunction.h>
#include <itkCastImageFilter.h>
#include <itkImageFileWriter.h>
#include <itkRescaleIntensityImageFilter.h>
#include <itkWindowedSincInterpolateImageFunction.h>
#include <itkResampleDwiImageFilter.h>
#include <itkKspaceImageFilter.h>
#include <itkDftImageFilter.h>
#include <itkAddImageFilter.h>
#include <itkConstantPadImageFilter.h>
#include <itkCropImageFilter.h>
#include <mitkAstroStickModel.h>
#include <vtkTransform.h>
#include <iostream>
#include <fstream>
#include <itkImageDuplicator.h>
#include <itksys/SystemTools.hxx>
#include <mitkIOUtil.h>
#include <itkDiffusionTensor3DReconstructionImageFilter.h>
#include <itkDiffusionTensor3D.h>
#include <itkTractDensityImageFilter.h>
#include <boost/lexical_cast.hpp>
#include <itkTractsToVectorImageFilter.h>

namespace itk
{

template< class PixelType >
TractsToDWIImageFilter< PixelType >::TractsToDWIImageFilter()
    : m_FiberBundle(NULL)
    , m_StatusText("")
    , m_UseConstantRandSeed(false)
    , m_RandGen(itk::Statistics::MersenneTwisterRandomVariateGenerator::New())
{
    m_RandGen->SetSeed();
}

template< class PixelType >
TractsToDWIImageFilter< PixelType >::~TractsToDWIImageFilter()
{

}

template< class PixelType >
TractsToDWIImageFilter< PixelType >::DoubleDwiType::Pointer TractsToDWIImageFilter< PixelType >::DoKspaceStuff( std::vector< DoubleDwiType::Pointer >& images )
{
    int numFiberCompartments = m_Parameters.m_FiberModelList.size();
    // create slice object
    ImageRegion<2> sliceRegion;
    sliceRegion.SetSize(0, m_UpsampledImageRegion.GetSize()[0]);
    sliceRegion.SetSize(1, m_UpsampledImageRegion.GetSize()[1]);
    Vector< double, 2 > sliceSpacing;
    sliceSpacing[0] = m_UpsampledSpacing[0];
    sliceSpacing[1] = m_UpsampledSpacing[1];

    // frequency map slice
    SliceType::Pointer fMapSlice = NULL;
    if (m_Parameters.m_SignalGen.m_FrequencyMap.IsNotNull())
    {
        fMapSlice = SliceType::New();
        ImageRegion<2> region;
        region.SetSize(0, m_UpsampledImageRegion.GetSize()[0]);
        region.SetSize(1, m_UpsampledImageRegion.GetSize()[1]);
        fMapSlice->SetLargestPossibleRegion( region );
        fMapSlice->SetBufferedRegion( region );
        fMapSlice->SetRequestedRegion( region );
        fMapSlice->Allocate();
        fMapSlice->FillBuffer(0.0);
    }

    DoubleDwiType::Pointer newImage = DoubleDwiType::New();
    newImage->SetSpacing( m_Parameters.m_SignalGen.m_ImageSpacing );
    newImage->SetOrigin( m_Parameters.m_SignalGen.m_ImageOrigin );
    newImage->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
    newImage->SetLargestPossibleRegion( m_Parameters.m_SignalGen.m_ImageRegion );
    newImage->SetBufferedRegion( m_Parameters.m_SignalGen.m_ImageRegion );
    newImage->SetRequestedRegion( m_Parameters.m_SignalGen.m_ImageRegion );
    newImage->SetVectorLength( images.at(0)->GetVectorLength() );
    newImage->Allocate();

    std::vector< unsigned int > spikeVolume;
    for (unsigned int i=0; i<m_Parameters.m_SignalGen.m_Spikes; i++)
        spikeVolume.push_back(m_RandGen->GetIntegerVariate()%images.at(0)->GetVectorLength());
    std::sort (spikeVolume.begin(), spikeVolume.end());
    std::reverse (spikeVolume.begin(), spikeVolume.end());

    m_StatusText += "0%   10   20   30   40   50   60   70   80   90   100%\n";
    m_StatusText += "|----|----|----|----|----|----|----|----|----|----|\n*";
    unsigned long lastTick = 0;

    boost::progress_display disp(2*images.at(0)->GetVectorLength()*images.at(0)->GetLargestPossibleRegion().GetSize(2));
    for (unsigned int g=0; g<images.at(0)->GetVectorLength(); g++)
    {
        std::vector< unsigned int > spikeSlice;
        while (!spikeVolume.empty() && spikeVolume.back()==g)
        {
            spikeSlice.push_back(m_RandGen->GetIntegerVariate()%images.at(0)->GetLargestPossibleRegion().GetSize(2));
            spikeVolume.pop_back();
        }
        std::sort (spikeSlice.begin(), spikeSlice.end());
        std::reverse (spikeSlice.begin(), spikeSlice.end());

        for (unsigned int z=0; z<images.at(0)->GetLargestPossibleRegion().GetSize(2); z++)
        {
            std::vector< SliceType::Pointer > compartmentSlices;
            std::vector< double > t2Vector;

            for (unsigned int i=0; i<images.size(); i++)
            {
                DiffusionSignalModel<double>* signalModel;
                if (i<numFiberCompartments)
                    signalModel = m_Parameters.m_FiberModelList.at(i);
                else
                    signalModel = m_Parameters.m_NonFiberModelList.at(i-numFiberCompartments);

                SliceType::Pointer slice = SliceType::New();
                slice->SetLargestPossibleRegion( sliceRegion );
                slice->SetBufferedRegion( sliceRegion );
                slice->SetRequestedRegion( sliceRegion );
                slice->SetSpacing(sliceSpacing);
                slice->Allocate();
                slice->FillBuffer(0.0);

                // extract slice from channel g
                for (unsigned int y=0; y<images.at(0)->GetLargestPossibleRegion().GetSize(1); y++)
                    for (unsigned int x=0; x<images.at(0)->GetLargestPossibleRegion().GetSize(0); x++)
                    {
                        SliceType::IndexType index2D; index2D[0]=x; index2D[1]=y;
                        DoubleDwiType::IndexType index3D; index3D[0]=x; index3D[1]=y; index3D[2]=z;

                        slice->SetPixel(index2D, images.at(i)->GetPixel(index3D)[g]);

                        if (fMapSlice.IsNotNull() && i==0)
                            fMapSlice->SetPixel(index2D, m_Parameters.m_SignalGen.m_FrequencyMap->GetPixel(index3D));
                    }

                compartmentSlices.push_back(slice);
                t2Vector.push_back(signalModel->GetT2());
            }

            if (this->GetAbortGenerateData())
                return NULL;

            // create k-sapce (inverse fourier transform slices)
            itk::Size<2> outSize; outSize.SetElement(0, m_Parameters.m_SignalGen.m_ImageRegion.GetSize(0)); outSize.SetElement(1, m_Parameters.m_SignalGen.m_ImageRegion.GetSize(1));
            itk::KspaceImageFilter< SliceType::PixelType >::Pointer idft = itk::KspaceImageFilter< SliceType::PixelType >::New();
            idft->SetCompartmentImages(compartmentSlices);
            idft->SetT2(t2Vector);
            idft->SetUseConstantRandSeed(m_UseConstantRandSeed);
            idft->SetParameters(m_Parameters);
            idft->SetZ((double)z-(double)images.at(0)->GetLargestPossibleRegion().GetSize(2)/2.0);
            idft->SetDiffusionGradientDirection(m_Parameters.m_SignalGen.GetGradientDirection(g));
            idft->SetFrequencyMapSlice(fMapSlice);
            idft->SetOutSize(outSize);
            int numSpikes = 0;
            while (!spikeSlice.empty() && spikeSlice.back()==z)
            {
                numSpikes++;
                spikeSlice.pop_back();
            }
            idft->SetSpikesPerSlice(numSpikes);
            idft->Update();

            ComplexSliceType::Pointer fSlice;
            fSlice = idft->GetOutput();

            ++disp;
            unsigned long newTick = 50*disp.count()/disp.expected_count();
            for (unsigned  long tick = 0; tick<(newTick-lastTick); tick++)
                m_StatusText += "*";
            lastTick = newTick;

            // fourier transform slice
            SliceType::Pointer newSlice;
            itk::DftImageFilter< SliceType::PixelType >::Pointer dft = itk::DftImageFilter< SliceType::PixelType >::New();
            dft->SetInput(fSlice);
            dft->Update();
            newSlice = dft->GetOutput();

            // put slice back into channel g
            for (unsigned int y=0; y<fSlice->GetLargestPossibleRegion().GetSize(1); y++)
                for (unsigned int x=0; x<fSlice->GetLargestPossibleRegion().GetSize(0); x++)
                {
                    DoubleDwiType::IndexType index3D; index3D[0]=x; index3D[1]=y; index3D[2]=z;
                    SliceType::IndexType index2D; index2D[0]=x; index2D[1]=y;

                    DoubleDwiType::PixelType pix3D = newImage->GetPixel(index3D);
                    pix3D[g] = newSlice->GetPixel(index2D);
                    newImage->SetPixel(index3D, pix3D);
                }
            ++disp;
            newTick = 50*disp.count()/disp.expected_count();
            for (unsigned long tick = 0; tick<(newTick-lastTick); tick++)
                m_StatusText += "*";
            lastTick = newTick;
        }
    }
    m_StatusText += "\n\n";
    return newImage;
}

template< class PixelType >
void TractsToDWIImageFilter< PixelType >::GenerateData()
{
    m_TimeProbe.Start();
    m_StatusText = "Starting simulation\n";

    // check input data
    if (m_FiberBundle.IsNull())
        itkExceptionMacro("Input fiber bundle is NULL!");

    if (m_Parameters.m_FiberModelList.empty())
        itkExceptionMacro("No diffusion model for fiber compartments defined!");

    if (m_Parameters.m_NonFiberModelList.empty())
        itkExceptionMacro("No diffusion model for non-fiber compartments defined!");

    int baselineIndex = m_Parameters.m_SignalGen.GetFirstBaselineIndex();
    if (baselineIndex<0)
        itkExceptionMacro("No baseline index found!");

    if (!m_Parameters.m_SignalGen.m_SimulateKspaceAcquisition)
        m_Parameters.m_SignalGen.m_DoAddGibbsRinging = false;

    if (m_UseConstantRandSeed)  // always generate the same random numbers?
        m_RandGen->SetSeed(0);
    else
        m_RandGen->SetSeed();

    // initialize output dwi image
    ImageRegion<3> croppedRegion = m_Parameters.m_SignalGen.m_ImageRegion; croppedRegion.SetSize(1, croppedRegion.GetSize(1)*m_Parameters.m_SignalGen.m_CroppingFactor);
    itk::Point<double,3> shiftedOrigin = m_Parameters.m_SignalGen.m_ImageOrigin; shiftedOrigin[1] += (m_Parameters.m_SignalGen.m_ImageRegion.GetSize(1)-croppedRegion.GetSize(1))*m_Parameters.m_SignalGen.m_ImageSpacing[1]/2;
    typename OutputImageType::Pointer outImage = OutputImageType::New();
    outImage->SetSpacing( m_Parameters.m_SignalGen.m_ImageSpacing );
    outImage->SetOrigin( shiftedOrigin );
    outImage->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
    outImage->SetLargestPossibleRegion( croppedRegion );
    outImage->SetBufferedRegion( croppedRegion );
    outImage->SetRequestedRegion( croppedRegion );
    outImage->SetVectorLength( m_Parameters.m_SignalGen.GetNumVolumes() );
    outImage->Allocate();
    typename OutputImageType::PixelType temp;
    temp.SetSize(m_Parameters.m_SignalGen.GetNumVolumes());
    temp.Fill(0.0);
    outImage->FillBuffer(temp);

    // ADJUST GEOMETRY FOR FURTHER PROCESSING
    // is input slize size a power of two?
    unsigned int x=m_Parameters.m_SignalGen.m_ImageRegion.GetSize(0); unsigned int y=m_Parameters.m_SignalGen.m_ImageRegion.GetSize(1);
    ItkDoubleImgType::SizeType pad; pad[0]=x%2; pad[1]=y%2; pad[2]=0;
    m_Parameters.m_SignalGen.m_ImageRegion.SetSize(0, x+pad[0]);
    m_Parameters.m_SignalGen.m_ImageRegion.SetSize(1, y+pad[1]);
    if (m_Parameters.m_SignalGen.m_FrequencyMap.IsNotNull() && (pad[0]>0 || pad[1]>0))
    {
        itk::ConstantPadImageFilter<ItkDoubleImgType, ItkDoubleImgType>::Pointer zeroPadder = itk::ConstantPadImageFilter<ItkDoubleImgType, ItkDoubleImgType>::New();
        zeroPadder->SetInput(m_Parameters.m_SignalGen.m_FrequencyMap);
        zeroPadder->SetConstant(0);
        zeroPadder->SetPadUpperBound(pad);
        zeroPadder->Update();
        m_Parameters.m_SignalGen.m_FrequencyMap = zeroPadder->GetOutput();
    }
    if (m_Parameters.m_SignalGen.m_MaskImage.IsNotNull() && (pad[0]>0 || pad[1]>0))
    {
        itk::ConstantPadImageFilter<ItkUcharImgType, ItkUcharImgType>::Pointer zeroPadder = itk::ConstantPadImageFilter<ItkUcharImgType, ItkUcharImgType>::New();
        zeroPadder->SetInput(m_Parameters.m_SignalGen.m_MaskImage);
        zeroPadder->SetConstant(0);
        zeroPadder->SetPadUpperBound(pad);
        zeroPadder->Update();
        m_Parameters.m_SignalGen.m_MaskImage = zeroPadder->GetOutput();
    }

    // Apply in-plane upsampling for Gibbs ringing artifact
    double upsampling = 1;
    if (m_Parameters.m_SignalGen.m_DoAddGibbsRinging)
        upsampling = 2;
    m_UpsampledSpacing = m_Parameters.m_SignalGen.m_ImageSpacing;
    m_UpsampledSpacing[0] /= upsampling;
    m_UpsampledSpacing[1] /= upsampling;
    m_UpsampledImageRegion = m_Parameters.m_SignalGen.m_ImageRegion;
    m_UpsampledImageRegion.SetSize(0, m_Parameters.m_SignalGen.m_ImageRegion.GetSize()[0]*upsampling);
    m_UpsampledImageRegion.SetSize(1, m_Parameters.m_SignalGen.m_ImageRegion.GetSize()[1]*upsampling);
    m_UpsampledOrigin = m_Parameters.m_SignalGen.m_ImageOrigin;
    m_UpsampledOrigin[0] -= m_Parameters.m_SignalGen.m_ImageSpacing[0]/2; m_UpsampledOrigin[0] += m_UpsampledSpacing[0]/2;
    m_UpsampledOrigin[1] -= m_Parameters.m_SignalGen.m_ImageSpacing[1]/2; m_UpsampledOrigin[1] += m_UpsampledSpacing[1]/2;
    m_UpsampledOrigin[2] -= m_Parameters.m_SignalGen.m_ImageSpacing[2]/2; m_UpsampledOrigin[2] += m_UpsampledSpacing[2]/2;

    // generate double images to store the individual compartment signals
    m_CompartmentImages.clear();
    int numFiberCompartments = m_Parameters.m_FiberModelList.size();
    int numNonFiberCompartments = m_Parameters.m_NonFiberModelList.size();

    for (int i=0; i<numFiberCompartments+numNonFiberCompartments; i++)
    {
        DoubleDwiType::Pointer doubleDwi = DoubleDwiType::New();
        doubleDwi->SetSpacing( m_UpsampledSpacing );
        doubleDwi->SetOrigin( m_UpsampledOrigin );
        doubleDwi->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
        doubleDwi->SetLargestPossibleRegion( m_UpsampledImageRegion );
        doubleDwi->SetBufferedRegion( m_UpsampledImageRegion );
        doubleDwi->SetRequestedRegion( m_UpsampledImageRegion );
        doubleDwi->SetVectorLength( m_Parameters.m_SignalGen.GetNumVolumes() );
        doubleDwi->Allocate();
        DoubleDwiType::PixelType pix;
        pix.SetSize(m_Parameters.m_SignalGen.GetNumVolumes());
        pix.Fill(0.0);
        doubleDwi->FillBuffer(pix);
        m_CompartmentImages.push_back(doubleDwi);
    }

    // initialize output volume fraction images
    m_VolumeFractions.clear();
    for (int i=0; i<numFiberCompartments+numNonFiberCompartments; i++)
    {
        ItkDoubleImgType::Pointer doubleImg = ItkDoubleImgType::New();
        doubleImg->SetSpacing( m_UpsampledSpacing );
        doubleImg->SetOrigin( m_UpsampledOrigin );
        doubleImg->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
        doubleImg->SetLargestPossibleRegion( m_UpsampledImageRegion );
        doubleImg->SetBufferedRegion( m_UpsampledImageRegion );
        doubleImg->SetRequestedRegion( m_UpsampledImageRegion );
        doubleImg->Allocate();
        doubleImg->FillBuffer(0);
        m_VolumeFractions.push_back(doubleImg);
    }

    // get volume fraction images
    ItkDoubleImgType::Pointer sumImage = ItkDoubleImgType::New();
    bool foundVolumeFractionImage = false;

    for (int i=0; i<numNonFiberCompartments; i++)  // look if a volume fraction image is set
    {
        if (m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage().IsNotNull())
        {
            foundVolumeFractionImage = true;

            itk::ConstantPadImageFilter<ItkDoubleImgType, ItkDoubleImgType>::Pointer zeroPadder = itk::ConstantPadImageFilter<ItkDoubleImgType, ItkDoubleImgType>::New();
            zeroPadder->SetInput(m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage());
            zeroPadder->SetConstant(0);
            zeroPadder->SetPadUpperBound(pad);
            zeroPadder->Update();
            m_Parameters.m_NonFiberModelList[i]->SetVolumeFractionImage(zeroPadder->GetOutput());

            sumImage->SetSpacing( m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->GetSpacing() );
            sumImage->SetOrigin( m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->GetOrigin() );
            sumImage->SetDirection( m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->GetDirection() );
            sumImage->SetLargestPossibleRegion( m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->GetLargestPossibleRegion() );
            sumImage->SetBufferedRegion( m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->GetLargestPossibleRegion() );
            sumImage->SetRequestedRegion( m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->GetLargestPossibleRegion() );
            sumImage->Allocate();
            sumImage->FillBuffer(0);
            break;
        }
    }
    if (!foundVolumeFractionImage)
    {
        sumImage->SetSpacing( m_UpsampledSpacing );
        sumImage->SetOrigin( m_UpsampledOrigin );
        sumImage->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
        sumImage->SetLargestPossibleRegion( m_UpsampledImageRegion );
        sumImage->SetBufferedRegion( m_UpsampledImageRegion );
        sumImage->SetRequestedRegion( m_UpsampledImageRegion );
        sumImage->Allocate();
        sumImage->FillBuffer(0.0);
    }
    for (int i=0; i<numNonFiberCompartments; i++)
    {
        if (m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage().IsNull())
        {
            ItkDoubleImgType::Pointer doubleImg = ItkDoubleImgType::New();
            doubleImg->SetSpacing( sumImage->GetSpacing() );
            doubleImg->SetOrigin( sumImage->GetOrigin() );
            doubleImg->SetDirection( sumImage->GetDirection() );
            doubleImg->SetLargestPossibleRegion( sumImage->GetLargestPossibleRegion() );
            doubleImg->SetBufferedRegion( sumImage->GetLargestPossibleRegion() );
            doubleImg->SetRequestedRegion( sumImage->GetLargestPossibleRegion() );
            doubleImg->Allocate();
            doubleImg->FillBuffer(1.0/numNonFiberCompartments);
            m_Parameters.m_NonFiberModelList[i]->SetVolumeFractionImage(doubleImg);
        }
        ImageRegionIterator<ItkDoubleImgType> it(m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage(), m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->GetLargestPossibleRegion());
        while(!it.IsAtEnd())
        {
            sumImage->SetPixel(it.GetIndex(), sumImage->GetPixel(it.GetIndex())+it.Get());
            ++it;
        }
    }
    for (int i=0; i<numNonFiberCompartments; i++)
    {
        ImageRegionIterator<ItkDoubleImgType> it(m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage(), m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->GetLargestPossibleRegion());
        while(!it.IsAtEnd())
        {
            if (sumImage->GetPixel(it.GetIndex())>0)
                it.Set(it.Get()/sumImage->GetPixel(it.GetIndex()));
            ++it;
        }
    }

    // resample mask image and frequency map to fit upsampled geometry
    if (m_Parameters.m_SignalGen.m_DoAddGibbsRinging)
    {
        if (m_Parameters.m_SignalGen.m_MaskImage.IsNotNull())
        {
            // rescale mask image (otherwise there are problems with the resampling)
            itk::RescaleIntensityImageFilter<ItkUcharImgType,ItkUcharImgType>::Pointer rescaler = itk::RescaleIntensityImageFilter<ItkUcharImgType,ItkUcharImgType>::New();
            rescaler->SetInput(0,m_Parameters.m_SignalGen.m_MaskImage);
            rescaler->SetOutputMaximum(100);
            rescaler->SetOutputMinimum(0);
            rescaler->Update();

            // resample mask image
            itk::ResampleImageFilter<ItkUcharImgType, ItkUcharImgType>::Pointer resampler = itk::ResampleImageFilter<ItkUcharImgType, ItkUcharImgType>::New();
            resampler->SetInput(rescaler->GetOutput());
            resampler->SetOutputParametersFromImage(m_Parameters.m_SignalGen.m_MaskImage);
            resampler->SetSize(m_UpsampledImageRegion.GetSize());
            resampler->SetOutputSpacing(m_UpsampledSpacing);
            resampler->SetOutputOrigin(m_UpsampledOrigin);
            itk::NearestNeighborInterpolateImageFunction<ItkUcharImgType, double>::Pointer nn_interpolator
                    = itk::NearestNeighborInterpolateImageFunction<ItkUcharImgType, double>::New();
            resampler->SetInterpolator(nn_interpolator);
            resampler->Update();
            m_Parameters.m_SignalGen.m_MaskImage = resampler->GetOutput();

            itk::ImageFileWriter<ItkUcharImgType>::Pointer w = itk::ImageFileWriter<ItkUcharImgType>::New();
            w->SetFileName("/local/mask_ups.nrrd");
            w->SetInput(m_Parameters.m_SignalGen.m_MaskImage);
            w->Update();
        }
        // resample frequency map
        if (m_Parameters.m_SignalGen.m_FrequencyMap.IsNotNull())
        {
            itk::ResampleImageFilter<ItkDoubleImgType, ItkDoubleImgType>::Pointer resampler = itk::ResampleImageFilter<ItkDoubleImgType, ItkDoubleImgType>::New();
            resampler->SetInput(m_Parameters.m_SignalGen.m_FrequencyMap);
            resampler->SetOutputParametersFromImage(m_Parameters.m_SignalGen.m_FrequencyMap);
            resampler->SetSize(m_UpsampledImageRegion.GetSize());
            resampler->SetOutputSpacing(m_UpsampledSpacing);
            resampler->SetOutputOrigin(m_UpsampledOrigin);
            itk::NearestNeighborInterpolateImageFunction<ItkDoubleImgType, double>::Pointer nn_interpolator
                    = itk::NearestNeighborInterpolateImageFunction<ItkDoubleImgType, double>::New();
            resampler->SetInterpolator(nn_interpolator);
            resampler->Update();
            m_Parameters.m_SignalGen.m_FrequencyMap = resampler->GetOutput();
        }
    }

    // no input tissue mask is set -> create default
    m_MaskImageSet = true;
    if (m_Parameters.m_SignalGen.m_MaskImage.IsNull())
    {
        m_StatusText += "No tissue mask set\n";
        MITK_INFO << "No tissue mask set";
        m_Parameters.m_SignalGen.m_MaskImage = ItkUcharImgType::New();
        m_Parameters.m_SignalGen.m_MaskImage->SetSpacing( m_UpsampledSpacing );
        m_Parameters.m_SignalGen.m_MaskImage->SetOrigin( m_UpsampledOrigin );
        m_Parameters.m_SignalGen.m_MaskImage->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
        m_Parameters.m_SignalGen.m_MaskImage->SetLargestPossibleRegion( m_UpsampledImageRegion );
        m_Parameters.m_SignalGen.m_MaskImage->SetBufferedRegion( m_UpsampledImageRegion );
        m_Parameters.m_SignalGen.m_MaskImage->SetRequestedRegion( m_UpsampledImageRegion );
        m_Parameters.m_SignalGen.m_MaskImage->Allocate();
        m_Parameters.m_SignalGen.m_MaskImage->FillBuffer(1);
        m_MaskImageSet = false;
    }
    else
    {
        m_StatusText += "Using tissue mask\n";
        MITK_INFO << "Using tissue mask";
    }

    m_Parameters.m_SignalGen.m_ImageRegion = croppedRegion;
    x=m_Parameters.m_SignalGen.m_ImageRegion.GetSize(0); y=m_Parameters.m_SignalGen.m_ImageRegion.GetSize(1);
    if ( x%2 == 1 )
        m_Parameters.m_SignalGen.m_ImageRegion.SetSize(0, x+1);
    if ( y%2 == 1 )
        m_Parameters.m_SignalGen.m_ImageRegion.SetSize(1, y+1);

    // resample fiber bundle for sufficient voxel coverage
    m_StatusText += "\n"+this->GetTime()+" > Resampling fibers ...\n";
    double segmentVolume = 0.0001;
    float minSpacing = 1;
    if(m_UpsampledSpacing[0]<m_UpsampledSpacing[1] && m_UpsampledSpacing[0]<m_UpsampledSpacing[2])
        minSpacing = m_UpsampledSpacing[0];
    else if (m_UpsampledSpacing[1] < m_UpsampledSpacing[2])
        minSpacing = m_UpsampledSpacing[1];
    else
        minSpacing = m_UpsampledSpacing[2];
    m_FiberBundleWorkingCopy = m_FiberBundle->GetDeepCopy();
    double volumeAccuracy = 10;
    m_FiberBundleWorkingCopy->ResampleFibers(minSpacing/volumeAccuracy);
    double mmRadius = m_Parameters.m_SignalGen.m_AxonRadius/1000;
    if (mmRadius>0)
        segmentVolume = M_PI*mmRadius*mmRadius*minSpacing/volumeAccuracy;

    double maxVolume = 0;
    m_VoxelVolume = m_UpsampledSpacing[0]*m_UpsampledSpacing[1]*m_UpsampledSpacing[2];

    if (m_Parameters.m_SignalGen.m_DoAddMotion)
    {
        std::string fileName = "fiberfox_motion_0.log";
        std::string filePath = mitk::IOUtil::GetTempPath();
        if (m_Parameters.m_Misc.m_OutputPath.size()>0)
            filePath = m_Parameters.m_Misc.m_OutputPath;

        int c = 1;

        while (itksys::SystemTools::FileExists((filePath+fileName).c_str()))
        {
            fileName = "fiberfox_motion_";
            fileName += boost::lexical_cast<std::string>(c);
            fileName += ".log";
            c++;
        }

        m_Logfile.open((filePath+fileName).c_str());
        m_Logfile << "0 rotation: 0,0,0; translation: 0,0,0\n";

        if (m_Parameters.m_SignalGen.m_DoRandomizeMotion)
        {
            m_StatusText += "Adding random motion artifacts:\n";
            m_StatusText += "Maximum rotation: +/-" + boost::lexical_cast<std::string>(m_Parameters.m_SignalGen.m_Rotation) + "°\n";
            m_StatusText += "Maximum translation: +/-" + boost::lexical_cast<std::string>(m_Parameters.m_SignalGen.m_Translation) + "mm\n";
        }
        else
        {
            m_StatusText += "Adding linear motion artifacts:\n";
            m_StatusText += "Maximum rotation: " + boost::lexical_cast<std::string>(m_Parameters.m_SignalGen.m_Rotation) + "°\n";
            m_StatusText += "Maximum translation: " + boost::lexical_cast<std::string>(m_Parameters.m_SignalGen.m_Translation) + "mm\n";
        }
        m_StatusText += "Motion logfile: " + (filePath+fileName) + "\n";
        MITK_INFO << "Adding motion artifacts";
        MITK_INFO << "Maximum rotation: " << m_Parameters.m_SignalGen.m_Rotation;
        MITK_INFO << "Maxmimum translation: " << m_Parameters.m_SignalGen.m_Translation;
    }
    maxVolume = 0;

    m_StatusText += "\n"+this->GetTime()+" > Generating " + boost::lexical_cast<std::string>(numFiberCompartments+numNonFiberCompartments) + "-compartment diffusion-weighted signal.\n";
    MITK_INFO << "Generating " << numFiberCompartments+numNonFiberCompartments << "-compartment diffusion-weighted signal.";
    int numFibers = m_FiberBundle->GetNumFibers();
    boost::progress_display disp(numFibers*m_Parameters.m_SignalGen.GetNumVolumes());

    // get transform for motion artifacts
    m_FiberBundleTransformed = m_FiberBundleWorkingCopy;
    m_Rotation = m_Parameters.m_SignalGen.m_Rotation/m_Parameters.m_SignalGen.GetNumVolumes();
    m_Translation = m_Parameters.m_SignalGen.m_Translation/m_Parameters.m_SignalGen.GetNumVolumes();

    // creat image to hold transformed mask (motion artifact)
    m_MaskImage = ItkUcharImgType::New();
    itk::ImageDuplicator<ItkUcharImgType>::Pointer duplicator = itk::ImageDuplicator<ItkUcharImgType>::New();
    duplicator->SetInputImage(m_Parameters.m_SignalGen.m_MaskImage);
    duplicator->Update();
    m_MaskImage = duplicator->GetOutput();

    // second upsampling needed for motion artifacts
    ImageRegion<3>      upsampledImageRegion = m_UpsampledImageRegion;
    DoubleVectorType    upsampledSpacing = m_UpsampledSpacing;
    upsampledSpacing[0] /= 4;
    upsampledSpacing[1] /= 4;
    upsampledSpacing[2] /= 4;
    upsampledImageRegion.SetSize(0, m_UpsampledImageRegion.GetSize()[0]*4);
    upsampledImageRegion.SetSize(1, m_UpsampledImageRegion.GetSize()[1]*4);
    upsampledImageRegion.SetSize(2, m_UpsampledImageRegion.GetSize()[2]*4);
    itk::Point<double,3> upsampledOrigin = m_UpsampledOrigin;
    upsampledOrigin[0] -= m_UpsampledSpacing[0]/2; upsampledOrigin[0] += upsampledSpacing[0]/2;
    upsampledOrigin[1] -= m_UpsampledSpacing[1]/2; upsampledOrigin[1] += upsampledSpacing[1]/2;
    upsampledOrigin[2] -= m_UpsampledSpacing[2]/2; upsampledOrigin[2] += upsampledSpacing[2]/2;
    m_UpsampledMaskImage = ItkUcharImgType::New();
    itk::ResampleImageFilter<ItkUcharImgType, ItkUcharImgType>::Pointer upsampler = itk::ResampleImageFilter<ItkUcharImgType, ItkUcharImgType>::New();
    upsampler->SetInput(m_Parameters.m_SignalGen.m_MaskImage);
    upsampler->SetOutputParametersFromImage(m_Parameters.m_SignalGen.m_MaskImage);
    upsampler->SetSize(upsampledImageRegion.GetSize());
    upsampler->SetOutputSpacing(upsampledSpacing);
    upsampler->SetOutputOrigin(upsampledOrigin);
    itk::NearestNeighborInterpolateImageFunction<ItkUcharImgType, double>::Pointer nn_interpolator
            = itk::NearestNeighborInterpolateImageFunction<ItkUcharImgType, double>::New();
    upsampler->SetInterpolator(nn_interpolator);
    upsampler->Update();
    m_UpsampledMaskImage = upsampler->GetOutput();

    unsigned long lastTick = 0;
    int signalModelSeed = m_RandGen->GetIntegerVariate();
    switch (m_Parameters.m_SignalGen.m_DiffusionDirectionMode)
    {
    case(SignalGenerationParameters::FIBER_TANGENT_DIRECTIONS):   // use fiber tangent directions to determine diffusion direction
    {
        m_StatusText += "0%   10   20   30   40   50   60   70   80   90   100%\n";
        m_StatusText += "|----|----|----|----|----|----|----|----|----|----|\n*";

        for (unsigned int g=0; g<m_Parameters.m_SignalGen.GetNumVolumes(); g++)
        {
            // Set signal model random generator seeds to get same configuration in each voxel
            for (int i=0; i<m_Parameters.m_FiberModelList.size(); i++)
                m_Parameters.m_FiberModelList.at(i)->SetSeed(signalModelSeed);
            for (int i=0; i<m_Parameters.m_NonFiberModelList.size(); i++)
                m_Parameters.m_NonFiberModelList.at(i)->SetSeed(signalModelSeed);

            ItkDoubleImgType::Pointer intraAxonalVolumeImage = ItkDoubleImgType::New();
            intraAxonalVolumeImage->SetSpacing( m_UpsampledSpacing );
            intraAxonalVolumeImage->SetOrigin( m_UpsampledOrigin );
            intraAxonalVolumeImage->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
            intraAxonalVolumeImage->SetLargestPossibleRegion( m_UpsampledImageRegion );
            intraAxonalVolumeImage->SetBufferedRegion( m_UpsampledImageRegion );
            intraAxonalVolumeImage->SetRequestedRegion( m_UpsampledImageRegion );
            intraAxonalVolumeImage->Allocate();
            intraAxonalVolumeImage->FillBuffer(0);

            vtkPolyData* fiberPolyData = m_FiberBundleTransformed->GetFiberPolyData();

            // generate fiber signal (if there are any fiber models present)
            if (!m_Parameters.m_FiberModelList.empty())
                for( int i=0; i<numFibers; i++ )
                {
                    vtkCell* cell = fiberPolyData->GetCell(i);
                    int numPoints = cell->GetNumberOfPoints();
                    vtkPoints* points = cell->GetPoints();

                    if (numPoints<2)
                        continue;

                    for( int j=0; j<numPoints; j++)
                    {
                        if (this->GetAbortGenerateData())
                        {
                            m_StatusText += "\n"+this->GetTime()+" > Simulation aborted\n";
                            return;
                        }

                        double* temp = points->GetPoint(j);
                        itk::Point<float, 3> vertex = GetItkPoint(temp);
                        itk::Vector<double> v = GetItkVector(temp);

                        itk::Vector<double, 3> dir(3);
                        if (j<numPoints-1)
                            dir = GetItkVector(points->GetPoint(j+1))-v;
                        else
                            dir = v-GetItkVector(points->GetPoint(j-1));

                        if (dir.GetSquaredNorm()<0.0001 || dir[0]!=dir[0] || dir[1]!=dir[1] || dir[2]!=dir[2])
                            continue;

                        itk::Index<3> idx;
                        itk::ContinuousIndex<float, 3> contIndex;
                        m_MaskImage->TransformPhysicalPointToIndex(vertex, idx);
                        m_MaskImage->TransformPhysicalPointToContinuousIndex(vertex, contIndex);

                        if (!m_MaskImage->GetLargestPossibleRegion().IsInside(idx) || m_MaskImage->GetPixel(idx)<=0)
                            continue;

                        // generate signal for each fiber compartment
                        for (int k=0; k<numFiberCompartments; k++)
                        {
                            m_Parameters.m_FiberModelList[k]->SetFiberDirection(dir);
                            DoubleDwiType::PixelType pix = m_CompartmentImages.at(k)->GetPixel(idx);
                            pix[g] += segmentVolume*m_Parameters.m_FiberModelList[k]->SimulateMeasurement(g);

                            m_CompartmentImages.at(k)->SetPixel(idx, pix);
                        }

                        // update fiber volume image
                        double vol = intraAxonalVolumeImage->GetPixel(idx) + segmentVolume;
                        intraAxonalVolumeImage->SetPixel(idx, vol);
                        if (g==0 && vol>maxVolume)
                            maxVolume = vol;
                    }

                    // progress report
                    ++disp;
                    unsigned long newTick = 50*disp.count()/disp.expected_count();
                    for (unsigned int tick = 0; tick<(newTick-lastTick); tick++)
                        m_StatusText += "*";
                    lastTick = newTick;
                }

            // generate non-fiber signal
            ImageRegionIterator<ItkUcharImgType> it3(m_MaskImage, m_MaskImage->GetLargestPossibleRegion());
            double fact = 1;
            if (m_Parameters.m_SignalGen.m_AxonRadius<0.0001 || maxVolume>m_VoxelVolume)
                fact = m_VoxelVolume/maxVolume;
            while(!it3.IsAtEnd())
            {
                if (it3.Get()>0)
                {
                    DoubleDwiType::IndexType index = it3.GetIndex();

                    // adjust intra-axonal signal to abtain an only-fiber voxel
                    if (fabs(fact-1.0)>0.0001)
                        for (int i=0; i<numFiberCompartments; i++)
                        {
                            DoubleDwiType::PixelType pix = m_CompartmentImages.at(i)->GetPixel(index);
                            pix[g] *= fact;
                            m_CompartmentImages.at(i)->SetPixel(index, pix);
                        }

                    // simulate other compartments
                    SimulateNonFiberSignal(index, intraAxonalVolumeImage->GetPixel(index)*fact, g);
                }
                ++it3;
            }

            // move fibers
            SimulateMotion(g);
        }
        break;
    }
    case (SignalGenerationParameters::MAIN_FIBER_DIRECTIONS): // use main fiber directions to determine voxel-wise diffusion directions
    {
        typedef itk::Image< itk::Vector< float, 3>, 3 >                                 ItkDirectionImage3DType;
        typedef itk::VectorContainer< unsigned int, ItkDirectionImage3DType::Pointer >  ItkDirectionImageContainerType;

        // calculate main fiber directions
        itk::TractsToVectorImageFilter<float>::Pointer fOdfFilter = itk::TractsToVectorImageFilter<float>::New();
        fOdfFilter->SetFiberBundle(m_FiberBundleTransformed);
        fOdfFilter->SetMaskImage(m_MaskImage);
        fOdfFilter->SetAngularThreshold(cos(m_Parameters.m_SignalGen.m_FiberSeparationThreshold*M_PI/180.0));
        fOdfFilter->SetNormalizeVectors(false);
        fOdfFilter->SetUseWorkingCopy(true);
        fOdfFilter->SetSizeThreshold(0);
        fOdfFilter->SetMaxNumDirections(3);
        fOdfFilter->Update();
        ItkDirectionImageContainerType::Pointer directionImageContainer = fOdfFilter->GetDirectionImageContainer();

        // allocate image storing intra-axonal volume fraction information
        ItkDoubleImgType::Pointer intraAxonalVolumeImage = ItkDoubleImgType::New();
        intraAxonalVolumeImage->SetSpacing( m_UpsampledSpacing );
        intraAxonalVolumeImage->SetOrigin( m_UpsampledOrigin );
        intraAxonalVolumeImage->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
        intraAxonalVolumeImage->SetLargestPossibleRegion( m_UpsampledImageRegion );
        intraAxonalVolumeImage->SetBufferedRegion( m_UpsampledImageRegion );
        intraAxonalVolumeImage->SetRequestedRegion( m_UpsampledImageRegion );
        intraAxonalVolumeImage->Allocate();
        intraAxonalVolumeImage->FillBuffer(0);

        // determine intra-axonal volume fraction using the tract density
        itk::TractDensityImageFilter< ItkDoubleImgType >::Pointer tdiFilter = itk::TractDensityImageFilter< ItkDoubleImgType >::New();
        tdiFilter->SetFiberBundle(m_FiberBundleTransformed);
        tdiFilter->SetBinaryOutput(false);
        tdiFilter->SetOutputAbsoluteValues(false);
        tdiFilter->SetInputImage(intraAxonalVolumeImage);
        tdiFilter->SetUseImageGeometry(true);
        tdiFilter->Update();
        intraAxonalVolumeImage = tdiFilter->GetOutput();

        m_StatusText += "0%   10   20   30   40   50   60   70   80   90   100%\n";
        m_StatusText += "|----|----|----|----|----|----|----|----|----|----|\n*";
        boost::progress_display disp(m_MaskImage->GetLargestPossibleRegion().GetNumberOfPixels()*m_Parameters.m_SignalGen.GetNumVolumes());

        for (unsigned int g=0; g<m_Parameters.m_SignalGen.GetNumVolumes(); g++)
        {
            // Set signal model random generator seeds to get same configuration in each voxel
            for (int i=0; i<m_Parameters.m_FiberModelList.size(); i++)
                m_Parameters.m_FiberModelList.at(i)->SetSeed(signalModelSeed);
            for (int i=0; i<m_Parameters.m_NonFiberModelList.size(); i++)
                m_Parameters.m_NonFiberModelList.at(i)->SetSeed(signalModelSeed);

            if (m_Parameters.m_SignalGen.m_DoAddMotion && g>0)  // if fibers have moved we need a new TDI and new directions
            {
                fOdfFilter->SetFiberBundle(m_FiberBundleTransformed);
                fOdfFilter->SetMaskImage(m_MaskImage);
                fOdfFilter->Update();
                directionImageContainer = fOdfFilter->GetDirectionImageContainer();

                tdiFilter->SetFiberBundle(m_FiberBundleTransformed);
                tdiFilter->Update();
                intraAxonalVolumeImage = tdiFilter->GetOutput();
            }

            ImageRegionIterator< ItkUcharImgType > it(m_MaskImage, m_MaskImage->GetLargestPossibleRegion());
            while(!it.IsAtEnd())
            {
                ++disp;
                unsigned long newTick = 50*disp.count()/disp.expected_count();
                for (unsigned int tick = 0; tick<(newTick-lastTick); tick++)
                    m_StatusText += "*";
                lastTick = newTick;

                if (this->GetAbortGenerateData())
                {
                    m_StatusText += "\n"+this->GetTime()+" > Simulation aborted\n";
                    return;
                }

                if (it.Get()>0)
                {
                    // generate fiber signal
                    for (int c=0; c<m_Parameters.m_FiberModelList.size(); c++)
                    {
                        int count = 0;
                        DoubleDwiType::PixelType pix = m_CompartmentImages.at(c)->GetPixel(it.GetIndex());
                        for (unsigned int i=0; i<directionImageContainer->Size(); i++)
                        {
                            itk::Vector< double, 3> dir;
                            dir.CastFrom(directionImageContainer->GetElement(i)->GetPixel(it.GetIndex()));
                            double norm = dir.GetNorm();
                            if (norm>0.0001)
                            {
                                m_Parameters.m_FiberModelList.at(c)->SetFiberDirection(dir);
                                pix[g] += m_Parameters.m_FiberModelList.at(c)->SimulateMeasurement(g)*norm;
                                count++;
                            }
                        }
                        if (count>0)
                            pix[g] /= count;
                        pix[g] *= intraAxonalVolumeImage->GetPixel(it.GetIndex())*m_VoxelVolume;
                        m_CompartmentImages.at(c)->SetPixel(it.GetIndex(), pix);
                    }

                    // simulate other compartments
                    SimulateNonFiberSignal(it.GetIndex(), intraAxonalVolumeImage->GetPixel(it.GetIndex())*m_VoxelVolume, g);
                }
                ++it;
            }

            SimulateMotion(g);
        }

        itk::ImageFileWriter< ItkUcharImgType >::Pointer wr = itk::ImageFileWriter< ItkUcharImgType >::New();
        wr->SetInput(fOdfFilter->GetNumDirectionsImage());
        wr->SetFileName(mitk::IOUtil::GetTempPath()+"/NumDirections_MainFiberDirections.nrrd");
        wr->Update();
        break;
    }
    case (SignalGenerationParameters::RANDOM_DIRECTIONS):
    {
        ItkUcharImgType::Pointer numDirectionsImage = ItkUcharImgType::New();
        numDirectionsImage->SetSpacing( m_UpsampledSpacing );
        numDirectionsImage->SetOrigin( m_UpsampledOrigin );
        numDirectionsImage->SetDirection( m_Parameters.m_SignalGen.m_ImageDirection );
        numDirectionsImage->SetLargestPossibleRegion( m_UpsampledImageRegion );
        numDirectionsImage->SetBufferedRegion( m_UpsampledImageRegion );
        numDirectionsImage->SetRequestedRegion( m_UpsampledImageRegion );
        numDirectionsImage->Allocate();
        numDirectionsImage->FillBuffer(0);
        double sepAngle = cos(m_Parameters.m_SignalGen.m_FiberSeparationThreshold*M_PI/180.0);

        m_StatusText += "0%   10   20   30   40   50   60   70   80   90   100%\n";
        m_StatusText += "|----|----|----|----|----|----|----|----|----|----|\n*";
        boost::progress_display disp(m_MaskImage->GetLargestPossibleRegion().GetNumberOfPixels());
        ImageRegionIterator<ItkUcharImgType> it(m_MaskImage, m_MaskImage->GetLargestPossibleRegion());
        while(!it.IsAtEnd())
        {
            ++disp;
            unsigned long newTick = 50*disp.count()/disp.expected_count();
            for (unsigned int tick = 0; tick<(newTick-lastTick); tick++)
                m_StatusText += "*";
            lastTick = newTick;

            if (this->GetAbortGenerateData())
            {
                m_StatusText += "\n"+this->GetTime()+" > Simulation aborted\n";
                return;
            }

            if (it.Get()>0)
            {
                int numFibs = m_RandGen->GetIntegerVariate(2)+1;
                DoubleDwiType::PixelType pix = m_CompartmentImages.at(0)->GetPixel(it.GetIndex());
                double volume = m_RandGen->GetVariateWithClosedRange(0.3);

                //                double sum = 0;
                std::vector< double > fractions;
                for (int i=0; i<numFibs; i++)
                {
                    //                    fractions.push_back(1);
                    fractions.push_back(0.5+m_RandGen->GetVariateWithClosedRange(0.5));
                    //                    sum += fractions.at(i);
                }
                //                for (int i=0; i<numFibs; i++)
                //                    fractions[i] /= sum;

                std::vector< itk::Vector<double, 3> > directions;
                for (int i=0; i<numFibs; i++)
                {
                    DoubleVectorType fib;
                    fib[0] = m_RandGen->GetVariateWithClosedRange(2)-1.0;
                    fib[1] = m_RandGen->GetVariateWithClosedRange(2)-1.0;
                    fib[2] = m_RandGen->GetVariateWithClosedRange(2)-1.0;
                    fib.Normalize();

                    double min = 0;
                    for (unsigned int d=0; d<directions.size(); d++)
                    {
                        double angle = fabs(fib*directions[d]);
                        if (angle>min)
                            min = angle;
                    }
                    if (min<sepAngle)
                    {
                        m_Parameters.m_FiberModelList.at(0)->SetFiberDirection(fib);
                        pix += m_Parameters.m_FiberModelList.at(0)->SimulateMeasurement()*fractions[i];
                        directions.push_back(fib);
                    }
                    else
                        i--;
                }
                pix *= (1-volume);
                m_CompartmentImages.at(0)->SetPixel(it.GetIndex(), pix);

                // CSF/GM
                {
                    pix += volume*m_Parameters.m_NonFiberModelList.at(0)->SimulateMeasurement();
                }

                numDirectionsImage->SetPixel(it.GetIndex(), numFibs);
            }
            ++it;
        }

        itk::ImageFileWriter< ItkUcharImgType >::Pointer wr = itk::ImageFileWriter< ItkUcharImgType >::New();
        wr->SetInput(numDirectionsImage);
        wr->SetFileName(mitk::IOUtil::GetTempPath()+"/NumDirections_RandomDirections.nrrd");
        wr->Update();
    }
    }

    if (m_Logfile.is_open())
    {
        m_Logfile << "DONE";
        m_Logfile.close();
    }
    m_StatusText += "\n\n";
    if (this->GetAbortGenerateData())
    {
        m_StatusText += "\n"+this->GetTime()+" > Simulation aborted\n";
        return;
    }

    DoubleDwiType::Pointer doubleOutImage;
    if ( m_Parameters.m_SignalGen.m_SimulateKspaceAcquisition ) // do k-space stuff
    {
        m_StatusText += this->GetTime()+" > Adjusting complex signal\n";
        MITK_INFO << "Adjusting complex signal:";

        if (m_Parameters.m_SignalGen.m_DoSimulateRelaxation)
            m_StatusText += "Simulating signal relaxation\n";
        if (m_Parameters.m_SignalGen.m_FrequencyMap.IsNotNull())
            m_StatusText += "Simulating distortions\n";
        if (m_Parameters.m_SignalGen.m_DoAddGibbsRinging)
            m_StatusText += "Simulating ringing artifacts\n";
        if (m_Parameters.m_SignalGen.m_EddyStrength>0)
            m_StatusText += "Simulating eddy currents\n";
        if (m_Parameters.m_SignalGen.m_Spikes>0)
            m_StatusText += "Simulating spikes\n";
        if (m_Parameters.m_SignalGen.m_CroppingFactor<1.0)
            m_StatusText += "Simulating aliasing artifacts\n";
        if (m_Parameters.m_SignalGen.m_KspaceLineOffset>0)
            m_StatusText += "Simulating ghosts\n";

        doubleOutImage = DoKspaceStuff(m_CompartmentImages);
        m_Parameters.m_SignalGen.m_SignalScale = 1; // already scaled in DoKspaceStuff
    }
    else    // don't do k-space stuff, just sum compartments
    {
        m_StatusText += this->GetTime()+" > Summing compartments\n";
        MITK_INFO << "Summing compartments";
        doubleOutImage = m_CompartmentImages.at(0);

        for (unsigned int i=1; i<m_CompartmentImages.size(); i++)
        {
            itk::AddImageFilter< DoubleDwiType, DoubleDwiType, DoubleDwiType>::Pointer adder = itk::AddImageFilter< DoubleDwiType, DoubleDwiType, DoubleDwiType>::New();
            adder->SetInput1(doubleOutImage);
            adder->SetInput2(m_CompartmentImages.at(i));
            adder->Update();
            doubleOutImage = adder->GetOutput();
        }
    }
    if (this->GetAbortGenerateData())
    {
        m_StatusText += "\n"+this->GetTime()+" > Simulation aborted\n";
        return;
    }

    m_StatusText += this->GetTime()+" > Finalizing image\n";
    MITK_INFO << "Finalizing image";
    if (m_Parameters.m_SignalGen.m_SignalScale>1)
        m_StatusText += " Scaling signal\n";
    if (m_Parameters.m_NoiseModel!=NULL)
        m_StatusText += " Adding noise\n";
    unsigned int window = 0;
    unsigned int min = itk::NumericTraits<unsigned int>::max();
    ImageRegionIterator<OutputImageType> it4 (outImage, outImage->GetLargestPossibleRegion());
    DoubleDwiType::PixelType signal; signal.SetSize(m_Parameters.m_SignalGen.GetNumVolumes());
    boost::progress_display disp2(outImage->GetLargestPossibleRegion().GetNumberOfPixels());

    m_StatusText += "0%   10   20   30   40   50   60   70   80   90   100%\n";
    m_StatusText += "|----|----|----|----|----|----|----|----|----|----|\n*";
    lastTick = 0;

    while(!it4.IsAtEnd())
    {
        if (this->GetAbortGenerateData())
        {
            m_StatusText += "\n"+this->GetTime()+" > Simulation aborted\n";
            return;
        }

        ++disp2;
        unsigned long newTick = 50*disp2.count()/disp2.expected_count();
        for (unsigned long tick = 0; tick<(newTick-lastTick); tick++)
            m_StatusText += "*";
        lastTick = newTick;

        typename OutputImageType::IndexType index = it4.GetIndex();
        signal = doubleOutImage->GetPixel(index)*m_Parameters.m_SignalGen.m_SignalScale;

        if (m_Parameters.m_NoiseModel!=NULL)
            m_Parameters.m_NoiseModel->AddNoise(signal);

        for (unsigned int i=0; i<signal.Size(); i++)
        {
            if (signal[i]>0)
                signal[i] = floor(signal[i]+0.5);
            else
                signal[i] = ceil(signal[i]-0.5);

            if ( (!m_Parameters.m_SignalGen.IsBaselineIndex(i) || signal.Size()==1) && signal[i]>window)
                window = signal[i];
            if ( (!m_Parameters.m_SignalGen.IsBaselineIndex(i) || signal.Size()==1) && signal[i]<min)
                min = signal[i];
        }
        it4.Set(signal);
        ++it4;
    }
    window -= min;
    unsigned int level = window/2 + min;
    m_LevelWindow.SetLevelWindow(level, window);
    this->SetNthOutput(0, outImage);

    m_StatusText += "\n\n";
    m_StatusText += "Finished simulation\n";
    m_StatusText += "Simulation time: "+GetTime();
    m_TimeProbe.Stop();
}

template< class PixelType >
void TractsToDWIImageFilter< PixelType >::SimulateMotion(int g)
{
    if (m_Parameters.m_SignalGen.m_DoAddMotion && g<m_Parameters.m_SignalGen.GetNumVolumes()-1)
    {
        if (m_Parameters.m_SignalGen.m_DoRandomizeMotion)
        {
            m_FiberBundleTransformed = m_FiberBundleWorkingCopy->GetDeepCopy();
            m_Rotation[0] = m_RandGen->GetVariateWithClosedRange(m_Parameters.m_SignalGen.m_Rotation[0]*2)-m_Parameters.m_SignalGen.m_Rotation[0];
            m_Rotation[1] = m_RandGen->GetVariateWithClosedRange(m_Parameters.m_SignalGen.m_Rotation[1]*2)-m_Parameters.m_SignalGen.m_Rotation[1];
            m_Rotation[2] = m_RandGen->GetVariateWithClosedRange(m_Parameters.m_SignalGen.m_Rotation[2]*2)-m_Parameters.m_SignalGen.m_Rotation[2];
            m_Translation[0] = m_RandGen->GetVariateWithClosedRange(m_Parameters.m_SignalGen.m_Translation[0]*2)-m_Parameters.m_SignalGen.m_Translation[0];
            m_Translation[1] = m_RandGen->GetVariateWithClosedRange(m_Parameters.m_SignalGen.m_Translation[1]*2)-m_Parameters.m_SignalGen.m_Translation[1];
            m_Translation[2] = m_RandGen->GetVariateWithClosedRange(m_Parameters.m_SignalGen.m_Translation[2]*2)-m_Parameters.m_SignalGen.m_Translation[2];
        }

        // rotate mask image
        if (m_MaskImageSet)
        {
            ImageRegionIterator<ItkUcharImgType> maskIt(m_UpsampledMaskImage, m_UpsampledMaskImage->GetLargestPossibleRegion());
            m_MaskImage->FillBuffer(0);

            while(!maskIt.IsAtEnd())
            {
                if (maskIt.Get()<=0)
                {
                    ++maskIt;
                    continue;
                }

                DoubleDwiType::IndexType index = maskIt.GetIndex();
                itk::Point<double, 3> point;
                m_UpsampledMaskImage->TransformIndexToPhysicalPoint(index, point);
                if (m_Parameters.m_SignalGen.m_DoRandomizeMotion)
                    point = m_FiberBundleWorkingCopy->TransformPoint(point.GetVnlVector(), m_Rotation[0],m_Rotation[1],m_Rotation[2],m_Translation[0],m_Translation[1],m_Translation[2]);
                else
                    point = m_FiberBundleWorkingCopy->TransformPoint(point.GetVnlVector(), m_Rotation[0]*(g+1),m_Rotation[1]*(g+1),m_Rotation[2]*(g+1),m_Translation[0]*(g+1),m_Translation[1]*(g+1),m_Translation[2]*(g+1));

                m_MaskImage->TransformPhysicalPointToIndex(point, index);
                if (m_MaskImage->GetLargestPossibleRegion().IsInside(index))
                    m_MaskImage->SetPixel(index,100);
                ++maskIt;
            }
        }

        // rotate fibers
        if (m_Logfile.is_open())
        {
            m_Logfile << g+1 << " rotation: " << m_Rotation[0] << "," << m_Rotation[1] << "," << m_Rotation[2] << ";";
            m_Logfile << " translation: " << m_Translation[0] << "," << m_Translation[1] << "," << m_Translation[2] << "\n";
        }
        m_FiberBundleTransformed->TransformFibers(m_Rotation[0],m_Rotation[1],m_Rotation[2],m_Translation[0],m_Translation[1],m_Translation[2]);
    }
}

template< class PixelType >
void TractsToDWIImageFilter< PixelType >::SimulateNonFiberSignal(ItkUcharImgType::IndexType index, double intraAxonalVolume, int g)
{
    int numFiberCompartments = m_Parameters.m_FiberModelList.size();
    int numNonFiberCompartments = m_Parameters.m_NonFiberModelList.size();

    if (intraAxonalVolume>0.0001 && m_Parameters.m_SignalGen.m_DoDisablePartialVolume)  // only fiber in voxel
    {
        DoubleDwiType::PixelType pix = m_CompartmentImages.at(0)->GetPixel(index);
        if (g>=0)
            pix[g] *= m_VoxelVolume/intraAxonalVolume;
        else
            pix *= m_VoxelVolume/intraAxonalVolume;
        m_CompartmentImages.at(0)->SetPixel(index, pix);
        m_VolumeFractions.at(0)->SetPixel(index, 1);
        for (int i=1; i<numFiberCompartments; i++)
        {
            DoubleDwiType::PixelType pix = m_CompartmentImages.at(i)->GetPixel(index);
            if (g>=0)
                pix[g] = 0.0;
            else
                pix.Fill(0.0);
            m_CompartmentImages.at(i)->SetPixel(index, pix);
        }
    }
    else
    {
        m_VolumeFractions.at(0)->SetPixel(index, intraAxonalVolume/m_VoxelVolume);

        itk::Point<double, 3> point;
        m_MaskImage->TransformIndexToPhysicalPoint(index, point);
        if (m_Parameters.m_SignalGen.m_DoAddMotion)
        {
            if (m_Parameters.m_SignalGen.m_DoRandomizeMotion && g>0)
                point = m_FiberBundleWorkingCopy->TransformPoint(point.GetVnlVector(), -m_Rotation[0],-m_Rotation[1],-m_Rotation[2],-m_Translation[0],-m_Translation[1],-m_Translation[2]);
            else if (g>=0)
                point = m_FiberBundleWorkingCopy->TransformPoint(point.GetVnlVector(), -m_Rotation[0]*g,-m_Rotation[1]*g,-m_Rotation[2]*g,-m_Translation[0]*g,-m_Translation[1]*g,-m_Translation[2]*g);
        }

        if (m_Parameters.m_SignalGen.m_DoDisablePartialVolume)
        {
            int maxVolumeIndex = 0;
            double maxWeight = 0;
            for (int i=0; i<numNonFiberCompartments; i++)
            {
                double weight = 0;
                if (numNonFiberCompartments>1)
                {
                    DoubleDwiType::IndexType newIndex;
                    m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->TransformPhysicalPointToIndex(point, newIndex);
                    if (!m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->GetLargestPossibleRegion().IsInside(newIndex))
                        continue;
                    weight = m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->GetPixel(newIndex);
                }

                if (weight>maxWeight)
                {
                    maxWeight = weight;
                    maxVolumeIndex = i;
                }
            }
            DoubleDwiType::Pointer doubleDwi = m_CompartmentImages.at(maxVolumeIndex+numFiberCompartments);
            DoubleDwiType::PixelType pix = doubleDwi->GetPixel(index);

            if (g>=0)
                pix[g] += m_Parameters.m_NonFiberModelList[maxVolumeIndex]->SimulateMeasurement(g);
            else
                pix += m_Parameters.m_NonFiberModelList[maxVolumeIndex]->SimulateMeasurement();
            doubleDwi->SetPixel(index, pix);
            m_VolumeFractions.at(maxVolumeIndex+numFiberCompartments)->SetPixel(index, 1);
        }
        else
        {
            double extraAxonalVolume = m_VoxelVolume-intraAxonalVolume;    // non-fiber volume
            double interAxonalVolume = 0;
            if (numFiberCompartments>1)
                interAxonalVolume = extraAxonalVolume * intraAxonalVolume/m_VoxelVolume;   // inter-axonal fraction of non fiber compartment scales linearly with f
            double other = extraAxonalVolume - interAxonalVolume;        // rest of compartment
            double singleinter = interAxonalVolume/(numFiberCompartments-1);

            // adjust non-fiber and intra-axonal signal
            for (int i=1; i<numFiberCompartments; i++)
            {
                DoubleDwiType::PixelType pix = m_CompartmentImages.at(i)->GetPixel(index);
                if (intraAxonalVolume>0)    // remove scaling by intra-axonal volume from inter-axonal compartment
                {
                    if (g>=0)
                        pix[g] /= intraAxonalVolume;
                    else
                        pix /= intraAxonalVolume;
                }
                if (g>=0)
                    pix[g] *= singleinter;
                else
                    pix *= singleinter;
                m_CompartmentImages.at(i)->SetPixel(index, pix);
                m_VolumeFractions.at(i)->SetPixel(index, singleinter/m_VoxelVolume);
            }

            for (int i=0; i<numNonFiberCompartments; i++)
            {
                double weight = 1;
                if (numNonFiberCompartments>1)
                {
                    DoubleDwiType::IndexType newIndex;
                    m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->TransformPhysicalPointToIndex(point, newIndex);
                    if (!m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->GetLargestPossibleRegion().IsInside(newIndex))
                        continue;
                    weight = m_Parameters.m_NonFiberModelList[i]->GetVolumeFractionImage()->GetPixel(newIndex);
                }

                DoubleDwiType::Pointer doubleDwi = m_CompartmentImages.at(i+numFiberCompartments);
                DoubleDwiType::PixelType pix = doubleDwi->GetPixel(index);

                if (g>=0)
                    pix[g] += m_Parameters.m_NonFiberModelList[i]->SimulateMeasurement(g)*other*weight;
                else
                    pix += m_Parameters.m_NonFiberModelList[i]->SimulateMeasurement()*other*weight;
                doubleDwi->SetPixel(index, pix);
                m_VolumeFractions.at(i+numFiberCompartments)->SetPixel(index, other/m_VoxelVolume*weight);
            }
        }
    }
}

template< class PixelType >
itk::Point<float, 3> TractsToDWIImageFilter< PixelType >::GetItkPoint(double point[3])
{
    itk::Point<float, 3> itkPoint;
    itkPoint[0] = point[0];
    itkPoint[1] = point[1];
    itkPoint[2] = point[2];
    return itkPoint;
}

template< class PixelType >
itk::Vector<double, 3> TractsToDWIImageFilter< PixelType >::GetItkVector(double point[3])
{
    itk::Vector<double, 3> itkVector;
    itkVector[0] = point[0];
    itkVector[1] = point[1];
    itkVector[2] = point[2];
    return itkVector;
}

template< class PixelType >
vnl_vector_fixed<double, 3> TractsToDWIImageFilter< PixelType >::GetVnlVector(double point[3])
{
    vnl_vector_fixed<double, 3> vnlVector;
    vnlVector[0] = point[0];
    vnlVector[1] = point[1];
    vnlVector[2] = point[2];
    return vnlVector;
}

template< class PixelType >
vnl_vector_fixed<double, 3> TractsToDWIImageFilter< PixelType >::GetVnlVector(Vector<float,3>& vector)
{
    vnl_vector_fixed<double, 3> vnlVector;
    vnlVector[0] = vector[0];
    vnlVector[1] = vector[1];
    vnlVector[2] = vector[2];
    return vnlVector;
}

template< class PixelType >
double TractsToDWIImageFilter< PixelType >::RoundToNearest(double num) {
    return (num > 0.0) ? floor(num + 0.5) : ceil(num - 0.5);
}

template< class PixelType >
std::string TractsToDWIImageFilter< PixelType >::GetTime()
{
    m_TimeProbe.Stop();
    unsigned long total = RoundToNearest(m_TimeProbe.GetTotal());
    unsigned long hours = total/3600;
    unsigned long minutes = (total%3600)/60;
    unsigned long seconds = total%60;
    std::string out = "";
    out.append(boost::lexical_cast<std::string>(hours));
    out.append(":");
    out.append(boost::lexical_cast<std::string>(minutes));
    out.append(":");
    out.append(boost::lexical_cast<std::string>(seconds));
    m_TimeProbe.Start();
    return out;
}

}


/*
This example shows how to utilize the result of Eye-in-Hand calibration to transform either a (picking) point
coordinates or the entire point cloud from the camera frame to the robot base frame. The YAML files for this sample can
be found under the main instructions for Zivid samples.
*/

#include <Zivid/Zivid.h>

#include <Eigen/Core>
#include <opencv2/core/core.hpp>

#include <cmath>
#include <iostream>

namespace
{
    enum class CommandType
    {
        cmdTransformSinglePoint,
        cmdTransformPointCloud,
        cmdUnknown
    };

    std::string getInput()
    {
        std::string command;
        std::getline(std::cin, command);
        return command;
    }

    CommandType enterCommand()
    {
        std::cout << "Enter command, s (to transform single point) or p (to transform point cloud): ";
        const auto command = getInput();

        if(command == "S" || command == "s")
        {
            return CommandType::cmdTransformSinglePoint;
        }
        if(command == "P" || command == "p")
        {
            return CommandType::cmdTransformPointCloud;
        }
        return CommandType::cmdUnknown;
    }

    Eigen::MatrixXf cvToEigen(const cv::Mat &cvMat)
    {
        if(cvMat.dims > 2)
        {
            throw std::invalid_argument("Invalid matrix dimensions. Expected 2D.");
        }

        Eigen::MatrixXf eigenMat(cvMat.rows, cvMat.cols);
        for(int i = 0; i < cvMat.rows; i++)
        {
            for(int j = 0; j < cvMat.cols; j++)
            {
                eigenMat(i, j) = cvMat.at<float>(i, j);
            }
        }
        return eigenMat;
    }

    Zivid::Matrix4x4 cvToZivid(const cv::Mat &cvMat)
    {
        const auto zividMat = Zivid::Matrix4x4(cvMat.ptr<float>(0), cvMat.ptr<float>(0) + 16);
        return zividMat;
    }

    cv::Mat readTransform(const std::string &transformFile)
    {
        auto fileStorage = cv::FileStorage();

        if(!fileStorage.open(transformFile, cv::FileStorage::Mode::READ))
        {
            throw std::invalid_argument("Could not open " + transformFile);
        }
        try
        {
            const auto poseStateNode = fileStorage["PoseState"];

            if(poseStateNode.empty())
            {
                throw std::invalid_argument("PoseState not found in file " + transformFile);
            }

            const auto rows = poseStateNode.mat().rows;
            const auto cols = poseStateNode.mat().cols;
            if(rows != 4 || cols != 4)
            {
                throw std::invalid_argument("Expected 4x4 matrix in " + transformFile + ", but got "
                                            + std::to_string(cols) + "x" + std::to_string(rows));
            }

            auto poseState = poseStateNode.mat();
            fileStorage.release();
            return poseState;
        }
        catch(...)
        {
            fileStorage.release();
            throw;
        }
    }
} // namespace

int main()
{
    try
    {
        Zivid::Application zivid;

        std::cout << "Reading camera pose in end-effector frame (result of eye-in-hand calibration)" << std::endl;
        const cv::Mat eyeInHandTransformation =
            readTransform(std::string(ZIVID_SAMPLE_DATA_DIR) + "/EyeInHandTransform.yaml");

        std::cout << "Reading end-effector pose in robot base frame" << std::endl;
        const cv::Mat endEffectorPose = readTransform(std::string(ZIVID_SAMPLE_DATA_DIR) + "/RobotTransform.yaml");

        const auto dataFile = std::string(ZIVID_SAMPLE_DATA_DIR) + "/ZividGem.zdf";
        std::cout << "Reading ZDF frame from file: " << dataFile << std::endl;
        const Zivid::Frame frame = Zivid::Frame(dataFile);
        auto pointCloud = frame.pointCloud();

        std::cout << "Computing camera pose in robot base frame" << std::endl;
        const cv::Mat transformBaseToCameraCV = endEffectorPose * eyeInHandTransformation;

        switch(enterCommand())
        {
            case CommandType::cmdTransformSinglePoint:
            {
                std::cout << "Transforming single point" << std::endl;

                // The (picking) point is defined as image coordinates in camera frame. It is hard-coded for the
                // ZividGem.zdf
                const size_t imageCoordinateX = 1357;
                const size_t imageCoordinateY = 666;
                const auto xyz = pointCloud.copyPointsXYZW();

                const Eigen::Vector4f pointInCameraFrame(xyz(imageCoordinateY, imageCoordinateX).x,
                                                         xyz(imageCoordinateY, imageCoordinateX).y,
                                                         xyz(imageCoordinateY, imageCoordinateX).z,
                                                         xyz(imageCoordinateY, imageCoordinateX).w);

                std::cout << "Point coordinates in camera frame: " << pointInCameraFrame.x() << " "
                          << pointInCameraFrame.y() << " " << pointInCameraFrame.z() << std::endl;

                // Converting to Eigen matrix for easier computation
                const Eigen::MatrixXf transformBaseToCamera = cvToEigen(transformBaseToCameraCV);

                std::cout << "Transforming (picking) point from camera to robot base frame" << std::endl;
                const Eigen::Vector4f pointInBaseFrame = transformBaseToCamera * pointInCameraFrame;

                std::cout << "Point coordinates in robot base frame: " << pointInBaseFrame.x() << " "
                          << pointInBaseFrame.y() << " " << pointInBaseFrame.z() << std::endl;
                break;
            }
            case CommandType::cmdTransformPointCloud:
            {
                std::cout << "Transforming point cloud" << std::endl;
                const auto transformationMatrix = cvToZivid(transformBaseToCameraCV);
                pointCloud.transform(transformationMatrix);

                const auto *saveFile = "ZividGemTransformed.zdf";
                std::cout << "Saving frame to file: " << saveFile << std::endl;
                frame.save(saveFile);
                break;
            }
            case CommandType::cmdUnknown:
            {
                std::cout << "Error: Unknown command" << std::endl;
                break;
            }
        }
    }

    catch(const std::exception &e)
    {
        std::cerr << "Error: " << e.what() << std::endl;
        std::cout << "Press enter to exit." << std::endl;
        std::cin.get();
        return EXIT_FAILURE;
    }
}