//
// Created by david on 28/12/2019.
//

#include "testbench.h"
#include <cmath>
#include <iostream>
#include <unordered_map>
#include <utility>
#include <vector>
#include <list>
#include <set>
#include <random>
#include <chrono>
#include <utils/Randoms.h>
#include <utils/BitSet.h>
#include <typing/TypeInfos.h>
#include <typing/iterator.h>
#include "typing/TypeInfo.h"
#include <boost/format.hpp>
#include <boost/any.hpp>

using std::cout, std::unordered_map, std::vector, std::set, std::list, std::string;

int getFIndex(unsigned long a) {
    for (int i = 0; i < 64; i++) {
        if (a & 1) return i;
        a = a >> 1;
    }
    return -1;
}

int getCIndex(unsigned long a) {
    return (int) log2(a);
}

unordered_map<unsigned long, int> init_map() {
    unordered_map<unsigned long, int> res;
    unsigned long a = 1;
    for (int i = 0; i < 64; i++) {
        res[a] = i;
        a = a << 1;
    }
    return res;
}

int getLIndex(unsigned long a) {
    static auto lookup_table = init_map();
    return lookup_table[a];
}

void algo() {
    const int COMPONENTS = 10000000;
    const int CLICKS = 1000;
    const int W = 1920;
    const int H = 1080;

    vector<BitSet> ys;
    vector<BitSet> xs;

    std::random_device dev;
    std::mt19937 rng(dev());
    std::uniform_int_distribution<std::mt19937::result_type> distx(0, W); // distribution in range [1, 6]
    std::uniform_int_distribution<std::mt19937::result_type> disty(0, H); // distribution in range [1, 6]
    std::uniform_int_distribution<std::mt19937::result_type> distw(10, 10); // distribution in range [1, 6]
    std::uniform_int_distribution<std::mt19937::result_type> disth(10, 10); // distribution in range [1, 6]

    cout << "Initializing sets\n";

    for (int ix = 0; ix <= W; ix++)
        xs.emplace_back(COMPONENTS);

    for (int iy = 0; iy <= H; iy++)
        ys.emplace_back(COMPONENTS);

    cout << "Populating sets\n";
    /*vector<Context *> contexts;
    for (int i = 0; i < COMPONENTS; i++) {
        contexts.push_back(new Context(Randoms::random_string()));
    }*/
    int area = 1;
    auto start = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < COMPONENTS; i++) {
        int x = distx(rng);
        int y = disty(rng);
        int w = distw(rng);
        int h = disth(rng);

        area += w + h;

        for (int ix = x; ix < x + w && ix < W; ix++)
            xs[ix].set(i);
        for (int iy = y; iy < y + h && iy < H; iy++)
            ys[iy].set(i);

    }

    auto finish = std::chrono::high_resolution_clock::now();

    std::cout << "Area: " << area << " Time/100px: "
              << std::chrono::duration_cast<std::chrono::microseconds>(finish - start).count() / (1000.0f * area / 100)
              << " in "
              << std::chrono::duration_cast<std::chrono::microseconds>(finish - start).count() / 1000.0f
              << " ms\n";

    int avg_fill = 0;
    for (int ix = 0; ix < W; ix++)
        avg_fill += xs[ix].count();
    for (int iy = 0; iy < H; iy++)
        avg_fill += ys[iy].count();

    cout << "avg_fill:" << avg_fill / (W + H) << "\n";

    vector<int> txs;
    vector<int> tys;

    for (int i = 0; i < CLICKS; i++) {
        txs.push_back(distx(rng));
        tys.push_back(disty(rng));
    }

    int intersections = 0;

    cout << "Intersecting sets\n";
    start = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < CLICKS; i++) {
        intersections += xs[txs[i]].fintersect(ys[tys[i]]);
    }

    finish = std::chrono::high_resolution_clock::now();

    std::cout << "Intersections: " << intersections / (1.0f * CLICKS) << " in "
              << std::chrono::duration_cast<std::chrono::microseconds>(finish - start).count() / (1000.0f * CLICKS)
              << " ms\n";
}

void test_indexing() {
    int SAMPLES;
    std::cin >> SAMPLES;
    vector<unsigned long> l;

    std::random_device dev;
    std::mt19937 rng(dev());
    std::uniform_int_distribution<std::mt19937::result_type> dist6(0, 63); // distribution in range [1, 6]

    unsigned long one = 1;
    for (int i = 0; i < SAMPLES; i++)
        l.push_back(one << dist6(rng));

    // Record start time
    auto start = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < SAMPLES; i++) {
        if (getFIndex(l[i]) < 0)
            cout << "negative\n";
    }

    auto finish = std::chrono::high_resolution_clock::now();

    std::cout << "For: " << std::chrono::duration_cast<std::chrono::microseconds>(finish - start).count() / 1000.0f
              << " ms\n";

    start = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < SAMPLES; i++) {
        if (getCIndex(l[i]) < 0)
            cout << "negative\n";
    }

    finish = std::chrono::high_resolution_clock::now();

    std::cout << "Calculated: "
              << std::chrono::duration_cast<std::chrono::microseconds>(finish - start).count() / 1000.0f << " ms\n";

    start = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < SAMPLES; i++) {
        if (getLIndex(l[i]) < 0)
            cout << "negative\n";
    }

    finish = std::chrono::high_resolution_clock::now();

    std::cout << "Lookup: " << std::chrono::duration_cast<std::chrono::microseconds>(finish - start).count() / 1000.0f
              << " ms\n";

    start = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < SAMPLES; i++) {
        if (SAMPLES < 200) {
            if (getFIndex(l[i]) < 0)
                cout << "negative\n";
        } else {
            if (getCIndex(l[i]) < 0)
                cout << "negative\n";
        }
    }

    finish = std::chrono::high_resolution_clock::now();

    std::cout << "Best: " << std::chrono::duration_cast<std::chrono::microseconds>(finish - start).count() / 1000.0f
              << " ms\n";

}

void indirect_tb() {
    vector<int> original;
    vector<int> first;
    vector<int> second;
    vector<int> third;

    int N = 100000000;

    for (int i = 0; i < N; i++) {
        original.push_back(i);
        first.push_back(original[i]);
        second.push_back(first[i]);
        third.push_back(second[i]);
    }

    int total = 0;
    auto t0 = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < N; i++) {
        total += original[first[second[third[i]]]];

    }

    auto t1 = std::chrono::high_resolution_clock::now();

    cout << "Avg time:"
         << static_cast<int>(std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count()) / 1000
         << ",    ans: " << total << "\n";

    total = 0;
    t0 = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < N; i++) {
        total += original[first[i]];
    }

    t1 = std::chrono::high_resolution_clock::now();

    cout << "Avg time:"
         << static_cast<int>(std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count()) / 1000
         << ",    ans: " << total << "\n";

    total = 0;
    t0 = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < N; i++) {
        total += original[first[second[i]]];
    }

    t1 = std::chrono::high_resolution_clock::now();

    cout << "Avg time:"
         << static_cast<int>(std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count()) / 1000
         << ",    ans: " << total << "\n";

    total = 0;
    t0 = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < N; i++) {
        total += original[i];
    }

    t1 = std::chrono::high_resolution_clock::now();

    cout << "Avg time:"
         << static_cast<int>(std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count()) / 1000
         << ",    ans: " << total << "\n";

    //fatal_error();
}

void TB::run_windowing() {
    algo();

}

void TB::run_anyvector() {
    struct banana {
        int a;
        int b;
        int c;
    };
    using std::vector;
    using boost::any, boost::any_cast;

    vector<any> myvec;
    myvec.reserve(10);

    printf("\n first:  %p \n second: %p \n 10th:   %p \n", &myvec[0], &myvec[1], &myvec[2]);

    struct banana ban = {1, 2, 3};

    myvec[0] = (char) 5;
    myvec[1] = ban;
    myvec[2] = (double) 5.0;

    printf("\n first:  %p \n second: %p \n 10th:   %p \n", &myvec[0], &myvec[1], &myvec[2]);
    void *p = &myvec[1];
    printf("\n a: %d \n b: %p \n c: %p \n", any_cast<struct banana>(myvec[1]).c, &myvec[1], &myvec[2]);

}

void TB::deallocating() {
    using namespace std;
    const int size = 100000000;
    double *buffy = new double[size];

    for (int i = 0; i < size; i++)
        buffy[i] = i * 1.2;

    cout << "deallocate?\n";

    double sum = 0.0;
    for (int i = 0; i < size; i++)
        sum += buffy[i];
    cout << sum << endl;
    int a;
    cin >> a;

    void *p = buffy;

    delete[] p;
    cin >> a;
}

template<typename T, size_t offset>
T *access(void *pointy) {
    return (T *) ((char *) pointy + offset);
}

void TB::fancy_types() {
    void *mymem = new char[sizeof(int) + sizeof(double) + 10];
    auto myint = &access<int, 0>;
    auto mydouble = &access<double, sizeof(int)>;
    auto mystring = &access<char, sizeof(int) + sizeof(double)>;

    *myint(mymem) = 10;
    *mydouble(mymem) = 10.0 / 7;
    memset(mystring(mymem), 0, *myint(mymem));
    strcpy(mystring(mymem), "hello");

    cout << *myint(mymem) << " " << *mydouble(mymem) << " " << mystring(mymem);

}

namespace FancyTyping {
    template<typename... Types>
    size_t size_sum() {
        return (sizeof(Types)+...);
    }

    template<>
    size_t size_sum() {
        return 0;
    }

    template<typename T, typename... Types>
    T *access(void *mem) {
        return (T *) ((char *) mem + size_sum<Types...>());
    }

    template<void *mem, typename T, typename... Types>
    T *access() {
        return (T *) ((char *) mem + size_sum<Types...>());
    }

    template<typename... Types>
    void *allocate() {
        return new char[size_sum<Types...>()];
    }

    template<typename... Types>
    void copy(void *dest, void *src) {
        memcpy(dest, src, size_sum<Types...>());
    }
}

class FancyNode {
public:
    static constexpr auto create = &FancyTyping::allocate<int, double>;
    static constexpr auto copy = &FancyTyping::copy<int, double>;
    static constexpr auto age = &FancyTyping::access<int>;
    static constexpr auto kg = &FancyTyping::access<double, int>;

    void *mem;

    operator void *() {
        return this;
    }

    FancyNode() {
        mem = create();
    }

    FancyNode(void *mem) {
        this->mem = create();
        copy(this->mem, mem);
    }
};

void TB::fancy_access() {
    void *mem = FancyNode::create();
    *FancyNode::age(mem) = 11;
    *FancyNode::kg(mem) = 60.15 + *FancyNode::age(mem);

    cout << *FancyNode::age(mem) << " " << *FancyNode::kg(mem) << "\n";

    auto not_pointy = (FancyNode) mem;
    cout << *not_pointy.age(not_pointy) << " " << *not_pointy.kg(not_pointy) << "\n";
    cout << *not_pointy.age(not_pointy) << " " << *not_pointy.kg(not_pointy) << "\n";

    //FancyNode dupl1(not_pointy);
    FancyNode dupl2(mem);
/*
    //*dupl1.age() = 7;
    *dupl2.kg() = 30.2;

    //cout << *dupl1.age() << " " << *dupl1.kg() << "\n";
    cout << *dupl2.age() << " " << *dupl2.kg() << "\n";
    not_pointy._age(&not_pointy);
    */
}

class FancierNode {
public:

    void *mem;

    FancyTypes::TypeInfo *t;

    FancierNode(FancyTypes::TypeInfo &type) {
        this->t = &type;
        mem = t->create();
        t->initialize(mem);
    }

    template<typename A>
    A access() {
        return (A) mem;
    }

    /*template<typename T, FancyTypes::t_iterator<T> f_iterator, FancyTypes::t_access<T> f_access, FancyTypes::TypeInfo &typeinfo>
    operator FancyTypes::TypeAccess<T, f_iterator, f_access, typeinfo>() {
        return (FancyTypes::TypeAccess<T, f_iterator, f_access, typeinfo>) mem;
    }*/

};

void TB::typeinfo() {
    using namespace FancyTypes;

#if 0
    FancierNode node(int_i);

    int_t myint = node;

    myint = 5;

    cout << "myint: " << myint << "\n";

    auto myotherint = (int_t) node;

    cout << "myotherint: " << myotherint << "\n";
    myotherint = 7;
    cout << "myint: " << myint << "\n";
#endif

    FancierNode node(uninfo_i);

    string_t st = string("hello");

    cout << st << "\n";

    uninfo_t info = node.mem;

    info = {"Bernadette", 4, 1, 3};
    uninfo_t info2 = {"Bernadette", 2, 1, 3};

    uninfo_t info3 = *info + *info2;

    vector<float> myfile(1000);

    for (int i = 0; i < 1000; i++)
        myfile[i] = i / 3.0f;

    for (int y = 0; y < 10; y++)
        for (int x = 0; x < 10; x++)
            cout << myfile[y * 10 + x] << " ";

    cout << "\n";
    iterator dim2(myfile, 10, 10, 3);

    for (int y = 0; y < 10; y++)
        for (int x = 0; x < 10; x++)
            cout << dim2(1, y, x) << " ";

    /*auto l = I(info).length;

    cout << info->length << " = " << l << "\n";*/
    cout << info.toString() << "\n";
    cout << info3.toString() << "\n";

    cout << "glm tests \n";

    vec3_t a({1.0, 0.5, 0.5});
    vec3_t b({1.0, 0.5, 1.33});

    vec3_t c = *a + *b;

    cout << c;

    cout << "\nDone.";
}

// Oh the lag..
/*
#define EVAL0(...) __VA_ARGS__
#define EVAL1(...) EVAL0(EVAL0(EVAL0(__VA_ARGS__)))
#define EVAL2(...) EVAL1(EVAL1(EVAL1(__VA_ARGS__)))
#define EVAL3(...) EVAL2(EVAL2(EVAL2(__VA_ARGS__)))
#define EVAL4(...) EVAL3(EVAL3(EVAL3(__VA_ARGS__)))
#define EVAL(...)  EVAL4(EVAL4(EVAL4(__VA_ARGS__)))

#define MAP_END(...)
#define MAP_OUT
#define MAP_COMMA ,

#define MAP_GET_END2() 0, MAP_END
#define MAP_GET_END1(...) MAP_GET_END2
#define MAP_GET_END(...) MAP_GET_END1
#define MAP_NEXT0(test, next, ...) next MAP_OUT
#define MAP_NEXT1(test, next) MAP_NEXT0(test, next, 0)

#define MAP_NEXT(test, next)  MAP_NEXT1(MAP_GET_END test, next)

#define MAP0(f, x, peek, ...) f(x) MAP_NEXT(peek, MAP1)(f, peek, __VA_ARGS__)
#define MAP1(f, x, peek, ...) f(x) MAP_NEXT(peek, MAP0)(f, peek, __VA_ARGS__)

#define MAP_LIST_NEXT1(test, next) MAP_NEXT0(test, MAP_COMMA next, 0)
#define MAP_LIST_NEXT(test, next)  MAP_LIST_NEXT1(MAP_GET_END test, next)

#define MAP_LIST0(f, x, peek, ...) f(x) MAP_LIST_NEXT(peek, MAP_LIST1)(f, peek, __VA_ARGS__)
#define MAP_LIST1(f, x, peek, ...) f(x) MAP_LIST_NEXT(peek, MAP_LIST0)(f, peek, __VA_ARGS__)

#define MAP_2P0(f, x, y, peek, ...) f(x, y) MAP_NEXT(peek, MAP_2P1)(f, peek, __VA_ARGS__)
#define MAP_2P1(f, x, y, peek, ...) f(x, y) MAP_NEXT(peek, MAP_2P0)(f, peek, __VA_ARGS__)

#define MAP_2P_LIST0(f, x, y, peek, ...) f(x, y) MAP_LIST_NEXT(peek, MAP_2P_LIST1)(f, peek, __VA_ARGS__)
#define MAP_2P_LIST1(f, x, y, peek, ...) f(x, y) MAP_LIST_NEXT(peek, MAP_2P_LIST0)(f, peek, __VA_ARGS__)

#define MAP_2P(f, ...) EVAL(MAP_2P1(f, __VA_ARGS__, ()()(), ()()(), ()()(), 0))

#define MAP_2P_LIST(f, ...)  EVAL(MAP_2P_LIST1(f, __VA_ARGS__, ()()(), ()()(), ()()(), 0))

#define MAP(f, ...) EVAL(MAP1(f, __VA_ARGS__, ()()(), ()()(), ()()(), 0))

#define MAP_LIST(f, ...) EVAL(MAP_LIST1(f, __VA_ARGS__, ()()(), ()()(), ()()(), 0))

#define STRINGIFY(x) #x
#define CALL(x, y) printf(STRINGIFY(x))
*/

void TB::macro_parenthesis() {
    //MAP_2P_LIST(CALL, a, b, c, d);
}

template<typename T, template<typename...> typename Template>
struct is_specialization : std::false_type {
};

template<template<typename...> typename Template, typename... Args>
struct is_specialization<Template<Args...>, Template> : std::true_type {
};

//template <typename T, template <typename...> typename Template>
//using is_specialization_v = typename is_specialization<T, Template>::value;

template<int N, typename... Args>
struct nth_arg;

template<typename Head, typename... Rest>
struct nth_arg<0, Head, Rest...> {
    typedef Head type;
};

template<int N, typename Head, typename... Rest>
struct nth_arg<N, Head, Rest...> {
    typedef typename nth_arg<N - 1, Rest...>::type type;
};

template<typename S, template<typename...> typename T>
struct argument_extractor;

template<template<typename...> typename C, typename... Args>
struct argument_extractor<C<Args...>, C> {
    template<int N>
    using nth_arg = typename nth_arg<N, Args...>::type;
};

/*
inline typename std::enable_if<(not std::is_same<T, int>::value)
                               and (not std::is_same<T, float>::value), void>::type
*/
/*
template <template <typename> typename C, typename T>
constexpr inline typename std::enable_if<std::is_pointer<C<T>>::value, int>::type
gimme_info(){
    auto content = gimme_info<std::remove_pointer_t<C<T>>>();
    return 200 + content;
}

template <template <typename> typename C, typename T>
constexpr inline typename std::enable_if<not std::is_pointer<C<T>>::value, int>::type
gimme_info(){}
*/

template<typename, typename Condition=void>
struct gimme_info;

template<>
struct gimme_info<int> {
    static constexpr int value = 1;
};

template<>
struct gimme_info<float> {
    static constexpr int value = 2;
};

template<typename A, typename B, typename C>
struct mycomplexthing {
};

//std::enable_if_t<std::is_pointer<T>::value, int> = 0
template<typename T>
struct gimme_info<T, typename std::enable_if_t<std::is_pointer_v<T>>> {
    static constexpr int value = 10 * gimme_info<std::remove_pointer_t<T>>::value;
};

template<typename C>
struct gimme_info<C, typename std::enable_if_t<is_specialization<C, std::vector>::value>> {
    static constexpr int value =
            5 + 10 * gimme_info<typename argument_extractor<C, std::vector>::template nth_arg<0>>::value;
};

template<typename C>
struct gimme_info<C, typename std::enable_if_t<is_specialization<C, mycomplexthing>::value>> {
    static constexpr int value =
            (((gimme_info<typename argument_extractor<C, mycomplexthing>::template nth_arg<0>>::value) * 10
              + gimme_info<typename argument_extractor<C, mycomplexthing>::template nth_arg<1>>::value) * 10
             + gimme_info<typename argument_extractor<C, mycomplexthing>::template nth_arg<2>>::value) * 10 + 6;
};

/*
// Value of <T>
template <typename T>
constexpr inline typename std::enable_if<not std::is_pointer<T>::value, int>::type
gimme_info();

template <>
constexpr int gimme_info<int>(){
    return 1;
}

template <>
constexpr int gimme_info<float>(){
    return 2;
}

// Value of <T*>
template <typename T>
constexpr inline typename std::enable_if<std::is_pointer<T>::value, int>::type
gimme_info(){
    auto content = gimme_info<std::remove_pointer_t<T>>();
    return 10 + content;
}
*/
// Value of <C<T>>
//template <typename C, typename T>
//constexpr inline typename std::enable_if<not std::is_pointer<C<T>>::value, int>::type
//constexpr inline int
//gimme_info();

/*
template <template <typename...> typename C, typename T>
constexpr inline typename std::enable_if<(not std::is_pointer<C<T>>::value) and std::is_same<C<T>, vector<T>>::value, int>::type
gimme_info(){
    //auto content = gimme_info<T>();
    return 100;// + content;
}
*/
/*
template <typename C>
constexpr inline typename std::enable_if_t<is_specialization<C, std::vector>::value, int>
gimme_info(){
    auto content = gimme_info<argument_extractor<C, std::vector>::template nth_arg<1>>();
    return 100 + content;
}
*/
/*
template<typename T>
struct banana{
    template <typename C>
    banana(){

    }
}
 */

//template<typename C>
//using extract_args = argument_extractor<C>;

/*
template <template <typename> typename C, typename T>
constexpr inline typename std::enable_if<(not std::is_pointer<C<T>>::value) and std::is_same<C<T>, vector<T>>::value, int>::type
gimme_info(){
    auto content = gimme_info<T>();
    return 10 + content;
}
*/

void TB::templating() {
    using boost::format;
    cout << format("1 = %1%\n") % gimme_info<int>::value;
    cout << format("2 = %1%\n") % gimme_info<float>::value;
    cout << format("10 = %1%\n") % gimme_info<int *>::value;
    cout << format("10 = %1%\n") % gimme_info<float *>::value;
    //cout << format("err = %1%\n") % gimme_info<double>::value;
    //cout << format("err = %1%\n") % gimme_info<double*>::value;
    cout << format("15 = %1%\n") % gimme_info<vector<int>>::value;
    cout << format("25 = %1%\n") % gimme_info<vector<float>>::value;
    cout << format("105 = %1%\n") % gimme_info<vector<int *>>::value;
    cout << format("205 = %1%\n") % gimme_info<vector<float *>>::value;
    cout << format("150 = %1%\n") % gimme_info<vector<int> *>::value;
    cout << format("250 = %1%\n") % gimme_info<vector<float> *>::value;
    cout << format("1050 = %1%\n") % gimme_info<vector<int *> *>::value;
    cout << format("2050 = %1%\n") % gimme_info<vector<float *> *>::value;
    cout << format("115265 = %1%\n") % gimme_info<vector<mycomplexthing<int, vector<int>, float>>>::value;
}

void TB::birthday() {
    using namespace std;
    using boost::format;

    long double k = 3.141592653589793238462643383279502884L;

    long double best_error = k;
    uint64_t start = 7;
    for (uint64_t i = 1; i < 1000000; i++) {
        long double cn = k * i;
        long double err = 0.5L - abs(cn - floor(cn) - 0.5L);
        if (err < best_error) {
            best_error = err;
            cout << i << " " << err << "\n";
            cout << (i - 2) % 113 << "\n";
        }
    }
    glm::vec3(0, 0, 0);
}

void fun(int *&myptr) {
    cout << "Hello from &\n";
}

void fun(int *&&myptr) {
    cout << "Hello from &&\n";
}

void TB::movingpointers() {
    int *p = new int(5);

    fun(p);
    fun(std::move(p));

    fun(new int(5));
    fun(std::move(new int(5)));

    //I love successful tests
}

#define _A(k, a, b) k::a + k::b
#define _B(k, a, b) k::a * k::b

#define MCAT(a, b, c) a ## b ## C
#define A(a, b) MCAT(k, ::, a) + MCAT(k, ::, b)

#define C(k, m) m

struct mystruct_macro_order {
    constexpr static int k1 = 1;
    constexpr static int k2 = 2;
};

void TB::mystruct_macro_order() {
    //cout<<C(mystruct, C(mystruct, A(k1,k2)));
}

struct structuring_vector_struct {
    int a;
    int b;
    int c;
    int d;
    int e;
};

void TB::structuring_vector() {
    vector<int> v = {1, 2, 3, 4, 5};

    auto &s = *(structuring_vector_struct *) v.data();

    cout << s.a << s.b << s.c << s.d << s.e;
}

void isit(int *&lvalue) {
    cout << "nope\n";
}

void isit(int *&&rvalue) {
    cout << "yep\n";
}

template<typename T>
void isit_ref(T &&lr_ref) {
    // static_assert(std::is_same_v<std::remove_reference_t<T>, int *>,
    //         "This function does not support the specified type.");
    isit(std::forward<T>(lr_ref));
}

void TB::isRValue() {
    int *k = new int(3);
    isit(new int(5));
    isit(k);
    isit_ref(new int(10));
    isit_ref(k);
    //isit_ref(5);
}

struct scope_node {
    vector<void *> inputs;
    vector<void *> outputs;
};

struct scope_escape {

    typedef scope_escape this_t;
    scope_node *value;

    void link(size_t *what, scope_node *to, size_t where) {

    }

    enum class Inputs : size_t {
        a = 2, b, c
    };

    typedef struct {
        typedef size_t a;
        typedef int b;
        typedef double c;
    } Inputs_t;

    enum class Outputs : size_t {
        a = 2, b, c
    };

    typedef struct {
        typedef size_t a;
        typedef int b;
        typedef double c;
    } Outputs_t;

};

#define link_scope(dest, dest_where, src, src_where) \
dest.value->inputs[(size_t) decltype(dest)::Inputs::dest_where] = \
src.value->outputs[(size_t) decltype(src)::Outputs::src_where]

void TB::scope_escape_tb() {
    typedef int myint;
    scope_escape::this_t::this_t::Inputs_t::a a = (size_t) scope_escape::Inputs::a;
    cout << (myint) a;

    scope_escape n1, n2;
    typedef decltype(n1)::Inputs n1_ii;
    typedef decltype(n1)::Outputs n1_oi;
    typedef decltype(n2)::Inputs n2_ii;
    typedef decltype(n2)::Outputs n2_oi;

    auto x = new scope_escape::Outputs_t::b();

    //n1.value->inputs[n1.Inputs.a] = n2.value->outputs[n2.Outputs.b];
    //n1.value->inputs[(size_t) decltype(n1)::input_indexes::a] =
    //        n2.value->outputs[(size_t) decltype(n2)::input_indexes::b];

    link_scope(n1, a, n2, b);

}

struct SynSugar {
    int a;
    enum class idx : std::size_t {
        a, b, c, __end
    };
    int r[(size_t) idx::__end] = {1, 2, 3};

    SynSugar(int x) {
        a = x;
    }

    void operator()() {
        cout << "Hello world! " << (size_t) idx::a;
    }
};

void TB::pointy_parant() {
    SynSugar *a = new SynSugar(1);

    (*a)();
    SynSugar &b = *a;

    b();
}

template<size_t index, size_t... values>
struct ct_lut;

template<size_t cur, size_t... values>
struct ct_lut<0, cur, values...> {
    static constexpr size_t value = cur;
};

template<size_t index, size_t cur, size_t... values>
struct ct_lut<index, cur, values...> {
    static constexpr size_t value = ct_lut<index - 1, values...>::value;
};

struct abstract_banana {
    virtual void do_the_flop() = 0;
};

struct banana2 : abstract_banana {

    int k = 0;
    template<size_t N>
    static constexpr size_t lut = ct_lut<N, 1, 2, 3, 4, 5>::value;

    static constexpr size_t arr[5] = {1, 2, 3, 4, 5};
    static const size_t *arr2[3];

    static void print_bananas(bool &var) {
        cout << arr[0];
        var = false;
    }

    void do_the_flop() {
        cout << "|_";
    }

    banana2 &do_the_self_flop() {
        cout << "k: " << k << "\n";
        k++;
        return *this;
    }

    typedef struct {
        typedef float z;
        typedef int start;
        typedef int end;
    } Inputs_t;
};

typedef banana2 banana2;

void TB::lut() {
    bool IamTRUE = true;
    banana2::print_bananas(IamTRUE);
    cout << "I am false: " << IamTRUE;
    abstract_banana **arr = new abstract_banana *[2];
    arr[0] = new banana2();
    arr[0]->do_the_flop();

    banana2().do_the_self_flop().do_the_self_flop().do_the_self_flop();

    banana2::Inputs_t::end a = 2;
}

typedef void (*bad_lambda_func)(vector<int *> &inputs, vector<int *> &outputs, bool &return_control_flow);

bad_lambda_func gimme_good_func() {
    return [](vector<int *> &inputs, vector<int *> &outputs, bool &return_control_flow) {
        static int data = 5;
        cout << "Hello func!\n";
        cout << data++ << "\n";
        return_control_flow = true;
    };
}

void TB::bad_lambda() {
    bad_lambda_func func1 = gimme_good_func();
    bad_lambda_func func2 = gimme_good_func();

    bool ok = false;
    vector<int *> ins = {new int(2)};
    vector<int *> outs = {new int(3)};
    func1(ins, outs, ok);
    func2(ins, outs, ok);
    func1(ins, outs, ok);
    func2(ins, outs, ok);
    cout << "ok: " << ok << "\n";
};

template<typename T>
struct pika {
    T data;

    pika(const T &val) : data(val) {
        cout << "is it l? " << std::is_lvalue_reference_v<T> << "\n";
        cout << "is it r? " << std::is_rvalue_reference_v<T> << "\n";
//        cout << "ref\n";
        if (std::is_same_v<T, int>) {
            cout << "spec\n";
        } else {
            cout << "tmp\n";
        }
    }

    pika(const int *val) : data(val) {
        cout << "int*ss\n";
    }

    operator int *() {
        return new int(5);
    }

    int &gimme_data() &{
        return *new int(7);
    }

    int &&gimme_data() &&{
        return std::move(*new int(7));
    }
};

template<typename T>
void lr_info(T &data) {
    cout << "data is lvalue\n";
}

template<typename T>
void lr_info(T &&data) {
    cout << "data is rvalue\n";
}

void TB::pikapika() {
    auto grr = pika(string("a"));
    string x = "Hellooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo\n";
    auto grrr = pika(x);
    cout << x;
    auto nyaa = pika(5);
    auto nya = pika(new int(5));

    cout << *(int *) nya;

    lr_info(x);
    lr_info(new int(5));
    cout << "member tests:\n";
    lr_info(nya.gimme_data());
    lr_info(pika(new int(5)).gimme_data());
}

void dosth_opt(std::optional<bool *> dosth = {}) {
    if (dosth.has_value()) {
        if (*dosth != nullptr && **dosth == true)
            cout << "I received: true\n";
        else if (*dosth != nullptr && **dosth == false)
            cout << "I received: false\n";
        else
            cout << "What is this fakery\n";
    } else {
        cout << "Nothing mate \n";
    }
}

void TB::optional() {
    dosth_opt();
    dosth_opt(new bool(true));
    dosth_opt(new bool(false));
    dosth_opt(nullptr);
}

int cr_func(int &x) {
    return x + 5;
}

constexpr int cr_func(const int &x) {
    return x + 1;
}

void TB::cr_test() {
    cout << cr_func(5);
    int k = time(nullptr) % 3;
    cout << cr_func(k);
}

void TB::vector_init() {
    vector<int *> myvec = vector<int *>(5);

    myvec.push_back(new int(3));
    cout << "inserted at: " << myvec.size() - 1 << "\n";
    for (int i = 0; i < myvec.size(); i++) {
        if (myvec[i] != nullptr)
            cout << "[" << i << "]: " << *myvec[i] << "\n";
    }

    vector<int> z;
    //R.I.P. last ounce of consistency
    z.push_back(0);
    //z.push_back({1,2,3});
    z.insert(z.end(), 4);
    z.insert(z.end(), {5, 6, 7});
    z = {1, 2, 3};
    for (auto x : z) {
        cout << x << " ";
    }
}

template<typename T, typename Cond = void>
struct is_const_char_array : std::false_type {
};

template<typename T>
struct is_const_char_array<T,
        std::enable_if_t<std::is_array_v<std::remove_reference_t<T>> and
                         std::is_same_v<const char, std::remove_extent_t<std::remove_reference_t<T>>>
        >> : std::true_type {
};

template<typename T>
inline constexpr bool is_const_char_array_v = is_const_char_array<T>::value;

template<typename T, int N = 3>
struct fwd_tag {
};

template<typename T, int N = 0>
struct forwarder {
    typedef T type;
    static const int n = N;
};

template<typename T, int N>
struct forwarder<fwd_tag<T, N>> {
    typedef T type;
    static const int n = N;
};

template<typename T>
using forwarder_t = typename forwarder<T>::type;
template<typename T>
inline const int forwarder_n = forwarder<T>::n;

void TB::is_T() {
    const char (&var)[9] = "position";
    cout << std::is_array_v<std::remove_reference_t<decltype(var)>>;
    cout << std::is_lvalue_reference_v<decltype(var)>;
    cout << std::is_same_v<const char, std::remove_extent_t<std::remove_reference_t<decltype(var)>>>;
    cout << "\n...\n";
    cout << is_const_char_array_v<const char (&)[9]>;
    cout << is_const_char_array_v<const char>;
    cout << is_const_char_array_v<char *>;
    cout << is_const_char_array_v<char *&&>;
    cout << is_const_char_array_v<const char *>;
    cout << is_const_char_array_v<const char *[10]>;
    cout << is_const_char_array_v<const char[10]>;
    cout << is_const_char_array_v<const char[]>;
    cout << is_const_char_array_v<decltype("ana")>;
    cout << "\n...\n";
    cout << is_const_char_array_v<forwarder_t<const char (&)[9]>>;
    cout << is_const_char_array_v<forwarder_t<fwd_tag<const char (&)[9]>>>;
    cout << "\n n: " << forwarder_n<fwd_tag<const char (&)[9], 7>> << "\n";
    cout << "\n n: " << forwarder_n<fwd_tag<const char (&)[9]>> << "\n";
    cout << "\n n: " << forwarder_n<const char (&)[9]> << "\n";

    int data = 5;
    int *dataptr = &data;

    lr_info(&data);
    lr_info(dataptr);
}

template<typename T1>
struct two_con {
    bool value1, value2;

    two_con() {
        value1 = std::is_pointer_v<T1>;
    }

    template<typename _T = T1, typename std::enable_if_t<std::is_same_v<_T, int>, int> = 0>
    void do_the_int_thing() {
        cout << "the int thing\n";
    }
};

void TB::implicit_two_con() {
    vector<int> banana;

    //two_con<int*>().do_the_int_thing();
}

struct jail_struct {
    static constexpr char *name = "Bob";
    vector<int> years_left;

};

void TB::struct_scope_escape() {
    cout << jail_struct::name;
}

// my_template.h
template<typename T>
struct my_template {
    static int brain_size;

    my_template() {
        brain_size++;
    }
};

template<typename T>
int my_template<T>::brain_size = 0;

template<typename T>
struct my_templ {
    // Genius logic: define it in the same file -> "must be defined out of line" -> define it in a separate file
    // -> "undefined reference"
    // Solution: define it the same file BUT LOWER *big brain stuff*
    // Logical conclusion: The C++ Standard eats memes and sometimes they pass undigested
    inline static int instance = 0;

    my_templ() {
        cout << "This is the most brain dead undefined reference I've ever seen: << " << instance++ << " \n";
    }
};

//template<typename T>
//int my_templ<T>::instance = 0;

/*
template<typename T>
struct my_templ_macro_ready;
template<typename T>
int my_templ_macro_ready<T>::instance = 0;
template<typename T>
struct my_templ_macro_ready{
    static int instance;
    my_templ_macro_ready(){
        cout << "This is the most brain dead undefined reference I've ever seen: << " <<  instance++ << " \n";
    }
};
 */

void TB::template_inconsitencies() {
    my_templ<int> i1;
    my_templ<int> i2;
    my_templ<char> c1;
    my_templ<int> i3;
    my_templ<char> c2;
};

size_t tunc_2p_down(size_t x) {
    size_t res = x >> 1;
    res |= res >> 1;
    res |= res >> 2;
    res |= res >> 4;
    res |= res >> 8;
    res |= res >> 16;
    res |= res >> 32;
    return x & ~res;
}

void TB::trunc_pow2() {
    for (size_t i = 2; i < 2048;) {
        cout << (tunc_2p_down(i) >> 1) << " < " << i << "\n";
        i += tunc_2p_down(i) >> 1;
    }
}

void TB::dynamic_queue() {
    std::list<int> queue;
    queue.push_back(1);
    for (auto &cur : queue) {
        if (cur < 1000)
            queue.push_back(cur * 2);
        cout << cur << "\n";
    }
}


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

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 <vnl/vnl_cross.h>
#include <vnl/vnl_quaternion.h>
#include <mitkRawShModel.h>
#include <boost/math/special_functions.hpp>
#include <itkOrientationDistributionFunction.h>
#include <mitkFiberfoxParameters.h>
#include <itkDiffusionTensor3DReconstructionImageFilter.h>
#include <itkAdcImageFilter.h>
#include <itkAnalyticalDiffusionQballReconstructionImageFilter.h>
#include <mitkDiffusionPropertyHelper.h>
#include <mitkImageCast.h>
#include <mitkProperties.h>

using namespace mitk;
using namespace boost::math;

template< class ScalarType >
RawShModel< ScalarType >::RawShModel()
    : m_ShOrder(0)
    , m_ModelIndex(-1)
    , m_MaxNumKernels(1000)
{
    this->m_RandGen = itk::Statistics::MersenneTwisterRandomVariateGenerator::New();
    this->m_RandGen->SetSeed();
    m_AdcRange.first = 0;
    m_AdcRange.second = 0.004;
    m_FaRange.first = 0;
    m_FaRange.second = 1;
}

template< class ScalarType >
RawShModel< ScalarType >::~RawShModel()
{

}

template< class ScalarType >
void RawShModel< ScalarType >::Clear()
{
    m_ShCoefficients.clear();
    m_PrototypeMaxDirection.clear();
    m_B0Signal.clear();
}

template< class ScalarType >
void RawShModel< ScalarType >::RandomModel()
{
    m_ModelIndex = this->m_RandGen->GetIntegerVariate(m_B0Signal.size()-1);
}

template< class ScalarType >
unsigned int RawShModel< ScalarType >::GetNumberOfKernels()
{
    return m_B0Signal.size();
}

template< class ScalarType >
bool RawShModel< ScalarType >::SampleKernels(Image::Pointer diffImg, ItkUcharImageType::Pointer maskImage, TensorImageType::Pointer tensorImage, QballFilterType::CoefficientImageType::Pointer itkFeatureImage, ItkDoubleImageType::Pointer adcImage)
{
    if (diffImg.IsNull())
        return false;

    typedef itk::VectorImage<short, 3> ITKDiffusionImageType;
        ITKDiffusionImageType::Pointer itkVectorImagePointer = ITKDiffusionImageType::New();
  mitk::CastToItkImage(diffImg, itkVectorImagePointer);

    const int shOrder = 2;

    if (tensorImage.IsNull())
    {
        typedef itk::DiffusionTensor3DReconstructionImageFilter< short, short, double > TensorReconstructionImageFilterType;

        TensorReconstructionImageFilterType::Pointer filter = TensorReconstructionImageFilterType::New();
        filter->SetGradientImage( static_cast<mitk::GradientDirectionsProperty*>( diffImg->GetProperty(mitk::DiffusionPropertyHelper::GRADIENTCONTAINERPROPERTYNAME.c_str()).GetPointer() )->GetGradientDirectionsContainer(), itkVectorImagePointer );
        filter->SetBValue(static_cast<mitk::FloatProperty*>( diffImg->GetProperty(mitk::DiffusionPropertyHelper::REFERENCEBVALUEPROPERTYNAME.c_str()).GetPointer() )->GetValue());
        filter->Update();
        tensorImage = filter->GetOutput();
    }

    const int NumCoeffs = (shOrder*shOrder + shOrder + 2)/2 + shOrder;
    if (itkFeatureImage.IsNull())
    {
        QballFilterType::Pointer qballfilter = QballFilterType::New();
        qballfilter->SetGradientImage( static_cast<mitk::GradientDirectionsProperty*>( diffImg->GetProperty(mitk::DiffusionPropertyHelper::GRADIENTCONTAINERPROPERTYNAME.c_str()).GetPointer() )->GetGradientDirectionsContainer(), itkVectorImagePointer );
        qballfilter->SetBValue(static_cast<mitk::FloatProperty*>( diffImg->GetProperty(mitk::DiffusionPropertyHelper::REFERENCEBVALUEPROPERTYNAME.c_str()).GetPointer() )->GetValue());
        qballfilter->SetLambda(0.006);
        qballfilter->SetNormalizationMethod(QballFilterType::QBAR_RAW_SIGNAL);
        qballfilter->Update();
        itkFeatureImage = qballfilter->GetCoefficientImage();
    }

    if (adcImage.IsNull())
    {
        itk::AdcImageFilter< short, double >::Pointer adcFilter = itk::AdcImageFilter< short, double >::New();
        adcFilter->SetInput(itkVectorImagePointer);
        adcFilter->SetGradientDirections(static_cast<mitk::GradientDirectionsProperty*>( diffImg->GetProperty(mitk::DiffusionPropertyHelper::GRADIENTCONTAINERPROPERTYNAME.c_str()).GetPointer() )->GetGradientDirectionsContainer());
        adcFilter->SetB_value(static_cast<mitk::FloatProperty*>( diffImg->GetProperty(mitk::DiffusionPropertyHelper::REFERENCEBVALUEPROPERTYNAME.c_str()).GetPointer() )->GetValue());
        adcFilter->Update();
        adcImage = adcFilter->GetOutput();
    }

    int b0Index;
    for (unsigned int i=0; i<static_cast<mitk::GradientDirectionsProperty*>( diffImg->GetProperty(mitk::DiffusionPropertyHelper::ORIGINALGRADIENTCONTAINERPROPERTYNAME.c_str()).GetPointer() )->GetGradientDirectionsContainer()->Size(); i++)
        if ( static_cast<mitk::GradientDirectionsProperty*>( diffImg->GetProperty(mitk::DiffusionPropertyHelper::ORIGINALGRADIENTCONTAINERPROPERTYNAME.c_str()).GetPointer() )->GetGradientDirectionsContainer()->GetElement(i).magnitude()<0.001 )
        {
            b0Index = i;
            break;
        }

    double max = 0;
    {
        itk::ImageRegionIterator< itk::VectorImage< short, 3 > >  it(itkVectorImagePointer, itkVectorImagePointer->GetLargestPossibleRegion());
        while(!it.IsAtEnd())
        {
            if (maskImage.IsNotNull() && maskImage->GetPixel(it.GetIndex())<=0)
            {
                ++it;
                continue;
            }
            if (it.Get()[b0Index]>max)
                max = it.Get()[b0Index];
            ++it;
        }
    }

    MITK_INFO << "Sampling signal kernels.";
    unsigned int count = 0;
    itk::ImageRegionIterator< itk::Image< itk::DiffusionTensor3D< double >, 3 > >  it(tensorImage, tensorImage->GetLargestPossibleRegion());
    while(!it.IsAtEnd())
    {
        bool skipPixel = false;
        if (maskImage.IsNotNull() && maskImage->GetPixel(it.GetIndex())<=0)
        {
            ++it;
            continue;
        }
        for (unsigned int i=0; i<static_cast<mitk::GradientDirectionsProperty*>( diffImg->GetProperty(mitk::DiffusionPropertyHelper::GRADIENTCONTAINERPROPERTYNAME.c_str()).GetPointer() )->GetGradientDirectionsContainer()->Size(); i++)
        {
            if (itkVectorImagePointer->GetPixel(it.GetIndex())[i]!=itkVectorImagePointer->GetPixel(it.GetIndex())[i] || itkVectorImagePointer->GetPixel(it.GetIndex())[i]<=0 || itkVectorImagePointer->GetPixel(it.GetIndex())[i]>itkVectorImagePointer->GetPixel(it.GetIndex())[b0Index])
            {
                skipPixel = true;
                break;
            }
        }
        if (skipPixel)
        {
            ++it;
            continue;
        }

        typedef itk::DiffusionTensor3D<double>    TensorType;
        TensorType::EigenValuesArrayType eigenvalues;
        TensorType::EigenVectorsMatrixType eigenvectors;
        TensorType tensor = it.Get();
        double FA = tensor.GetFractionalAnisotropy();
        double ADC = adcImage->GetPixel(it.GetIndex());
        QballFilterType::CoefficientImageType::PixelType itkv = itkFeatureImage->GetPixel(it.GetIndex());
        vnl_vector_fixed< double, NumCoeffs > coeffs;
        for (unsigned int c=0; c<itkv.Size(); c++)
            coeffs[c] = itkv[c]/max;

        if ( this->GetMaxNumKernels()>this->GetNumberOfKernels() && FA>m_FaRange.first && FA<m_FaRange.second && ADC>m_AdcRange.first && ADC<m_AdcRange.second)
        {
            if (this->SetShCoefficients( coeffs, (double)itkVectorImagePointer->GetPixel(it.GetIndex())[b0Index]/max ))
            {
                tensor.ComputeEigenAnalysis(eigenvalues, eigenvectors);
                itk::Vector<double,3> dir;
                dir[0] = eigenvectors(2, 0);
                dir[1] = eigenvectors(2, 1);
                dir[2] = eigenvectors(2, 2);
                m_PrototypeMaxDirection.push_back(dir);
                count++;
                MITK_INFO << "KERNEL: " << it.GetIndex() << " (" << count << ")";
            }
        }
        ++it;
    }

    if (m_ShCoefficients.size()>0)
        return true;
    return false;
}

// convert cartesian to spherical coordinates
template< class ScalarType >
void RawShModel< ScalarType >::Cart2Sph( GradientListType gradients )
{
    m_SphCoords.set_size(gradients.size(), 2);
    m_SphCoords.fill(0.0);

    for (unsigned int i=0; i<gradients.size(); i++)
    {
        GradientType g = gradients[i];
        if( g.GetNorm() > 0.0001 )
        {
            gradients[i].Normalize();
            m_SphCoords(i,0) = acos(gradients[i][2]); // theta
            m_SphCoords(i,1) = atan2(gradients[i][1], gradients[i][0]); // phi
        }
    }
}

template< class ScalarType >
void RawShModel< ScalarType >::SetFiberDirection(GradientType fiberDirection)
{
    this->m_FiberDirection = fiberDirection;
    this->m_FiberDirection.Normalize();

    RandomModel();
    GradientType axis = itk::CrossProduct(this->m_FiberDirection, m_PrototypeMaxDirection.at(m_ModelIndex));
    axis.Normalize();

    vnl_quaternion<double> rotation(axis.GetVnlVector(), acos(dot_product(this->m_FiberDirection.GetVnlVector(), m_PrototypeMaxDirection.at(m_ModelIndex).GetVnlVector())));
    rotation.normalize();

    GradientListType gradients;
    for (unsigned int i=0; i<this->m_GradientList.size(); i++)
    {
        GradientType dir = this->m_GradientList.at(i);
        if( dir.GetNorm() > 0.0001 )
        {
            dir.Normalize();
            vnl_vector_fixed< double, 3 > vnlDir = rotation.rotate(dir.GetVnlVector());
            dir[0] = vnlDir[0]; dir[1] = vnlDir[1]; dir[2] = vnlDir[2];
            dir.Normalize();
        }
        gradients.push_back(dir);
    }

    Cart2Sph( gradients );
}

template< class ScalarType >
bool RawShModel< ScalarType >::SetShCoefficients(vnl_vector< double > shCoefficients, double b0 )
{
    m_ShOrder = 2;
    while ( (m_ShOrder*m_ShOrder + m_ShOrder + 2)/2 + m_ShOrder <= shCoefficients.size() )
        m_ShOrder += 2;
    m_ShOrder -= 2;

    m_ModelIndex = m_B0Signal.size();
    m_B0Signal.push_back(b0);
    m_ShCoefficients.push_back(shCoefficients);

    //    itk::OrientationDistributionFunction<double, 10000> odf;
    //    GradientListType gradients;
    //    for (unsigned int i=0; i<odf.GetNumberOfComponents(); i++)
    //    {
    //        GradientType dir; dir[0]=odf.GetDirection(i)[0]; dir[1]=odf.GetDirection(i)[1]; dir[2]=odf.GetDirection(i)[2];
    //        dir.Normalize();
    //        gradients.push_back(dir);
    //    }
    //    Cart2Sph( this->m_GradientList );
    //    PixelType signal = SimulateMeasurement();

    //    int minDirIdx = 0;
    //    double min = itk::NumericTraits<double>::max();
    //    for (unsigned int i=0; i<signal.GetSize(); i++)
    //    {
    //        if (signal[i]>b0 || signal[i]<0)
    //        {
    //            MITK_INFO << "Corrupted signal value detected. Kernel rejected.";
    //            m_B0Signal.pop_back();
    //            m_ShCoefficients.pop_back();
    //            return false;
    //        }
    //        if (signal[i]<min)
    //        {
    //            min = signal[i];
    //            minDirIdx = i;
    //        }
    //    }
    //    GradientType maxDir = this->m_GradientList.at(minDirIdx);
    //    maxDir.Normalize();
    //    m_PrototypeMaxDirection.push_back(maxDir);

    Cart2Sph( this->m_GradientList );
    m_ModelIndex = -1;

    return true;
}

template< class ScalarType >
ScalarType RawShModel< ScalarType >::SimulateMeasurement(unsigned int dir)
{
    ScalarType signal = 0;

    if (m_ModelIndex==-1)
        RandomModel();

    if (dir>=this->m_GradientList.size())
        return signal;

    int j, m; double mag, plm;

    if (this->m_GradientList[dir].GetNorm()>0.001)
    {
        j=0;
        for (int l=0; l<=m_ShOrder; l=l+2)
            for (m=-l; m<=l; m++)
            {
                plm = legendre_p<double>(l,abs(m),cos(m_SphCoords(dir,0)));
                mag = sqrt((double)(2*l+1)/(4.0*M_PI)*factorial<double>(l-abs(m))/factorial<double>(l+abs(m)))*plm;
                double basis;

                if (m<0)
                    basis = sqrt(2.0)*mag*cos(fabs((double)m)*m_SphCoords(dir,1));
                else if (m==0)
                    basis = mag;
                else
                    basis = pow(-1.0, m)*sqrt(2.0)*mag*sin(m*m_SphCoords(dir,1));

                signal += m_ShCoefficients.at(m_ModelIndex)[j]*basis;
                j++;
            }
    }
    else
        signal = m_B0Signal.at(m_ModelIndex);

    m_ModelIndex = -1;

    return signal;
}

template< class ScalarType >
typename RawShModel< ScalarType >::PixelType RawShModel< ScalarType >::SimulateMeasurement()
{
    if (m_ModelIndex==-1)
        RandomModel();

    PixelType signal;
    signal.SetSize(this->m_GradientList.size());

    int M = this->m_GradientList.size();
    int j, m; double mag, plm;

    vnl_matrix< double > shBasis;
    shBasis.set_size(M, m_ShCoefficients.at(m_ModelIndex).size());
    shBasis.fill(0.0);

    for (int p=0; p<M; p++)
    {
        if (this->m_GradientList[p].GetNorm()>0.001)
        {
            j=0;
            for (int l=0; l<=m_ShOrder; l=l+2)
                for (m=-l; m<=l; m++)
                {
                    plm = legendre_p<double>(l,abs(m),cos(m_SphCoords(p,0)));
                    mag = sqrt((double)(2*l+1)/(4.0*M_PI)*factorial<double>(l-abs(m))/factorial<double>(l+abs(m)))*plm;

                    if (m<0)
                        shBasis(p,j) = sqrt(2.0)*mag*cos(fabs((double)m)*m_SphCoords(p,1));
                    else if (m==0)
                        shBasis(p,j) = mag;
                    else
                        shBasis(p,j) = pow(-1.0, m)*sqrt(2.0)*mag*sin(m*m_SphCoords(p,1));

                    j++;
                }

            double val = 0;
            for (unsigned int cidx=0; cidx<m_ShCoefficients.at(m_ModelIndex).size(); cidx++)
                val += m_ShCoefficients.at(m_ModelIndex)[cidx]*shBasis(p,cidx);
            signal[p] = val;
        }
        else
            signal[p] = m_B0Signal.at(m_ModelIndex);
    }

    m_ModelIndex = -1;

    return signal;
}


#include <boost/graph/is_kuratowski_subgraph.hpp>


/******************************************************************************
**
** Copyright (C) 2009-2011 Kyle Lutz <kyle.r.lutz@gmail.com>
** All rights reserved.
**
** This file is a part of the chemkit project. For more information
** see <http://www.chemkit.org>.
**
** Redistribution and use in source and binary forms, with or without
** modification, are permitted provided that the following conditions
** are met:
**
**   * Redistributions of source code must retain the above copyright
**     notice, this list of conditions and the following disclaimer.
**   * Redistributions in binary form must reproduce the above copyright
**     notice, this list of conditions and the following disclaimer in the
**     documentation and/or other materials provided with the distribution.
**   * Neither the name of the chemkit project nor the names of its
**     contributors may be used to endorse or promote products derived
**     from this software without specific prior written permission.
**
** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**
******************************************************************************/

#include "oplscalculation.h"

#include <boost/lexical_cast.hpp>

#include <chemkit/topology.h>
#include <chemkit/constants.h>
#include <chemkit/cartesiancoordinates.h>

// === OplsCalculation ===================================================== //
OplsCalculation::OplsCalculation(int type, int atomCount, int parameterCount)
    : chemkit::ForceFieldCalculation(type, atomCount, parameterCount)
{
}

// === OplsBondStrechCalculation =========================================== //
OplsBondStrechCalculation::OplsBondStrechCalculation(size_t a, size_t b)
    : OplsCalculation(BondStrech, 2, 2)
{
    setAtom(0, a);
    setAtom(1, b);
}

bool OplsBondStrechCalculation::setup(const OplsParameters *parameters)
{
    int typeA = boost::lexical_cast<int>(atomType(0));
    int typeB = boost::lexical_cast<int>(atomType(1));

    const OplsBondStrechParameters *p = parameters->bondStrechParameters(typeA, typeB);
    if(!p){
        return false;
    }

    setParameter(0, p->kb);
    setParameter(1, p->r0);

    return true;
}

chemkit::Real OplsBondStrechCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real kb = parameter(0);
    chemkit::Real r0 = parameter(1);

    chemkit::Real r = coordinates->distance(a, b);

    return kb * pow(r - r0, 2);
}

std::vector<chemkit::Vector3> OplsBondStrechCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real kb = parameter(0);
    chemkit::Real r0 = parameter(1);

    chemkit::Real r = coordinates->distance(a, b);

    boost::array<chemkit::Vector3, 2> gradient = coordinates->distanceGradient(a, b);

    // dE/dr
    chemkit::Real de_dr = 2.0 * kb * (r - r0);

    gradient[0] *= de_dr;
    gradient[1] *= de_dr;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}

// === OplsAngleBendCalculation ============================================ //
OplsAngleBendCalculation::OplsAngleBendCalculation(size_t a, size_t b, size_t c)
    : OplsCalculation(AngleBend, 3, 2)
{
    setAtom(0, a);
    setAtom(1, b);
    setAtom(2, c);
}

bool OplsAngleBendCalculation::setup(const OplsParameters *parameters)
{
    int typeA = boost::lexical_cast<int>(atomType(0));
    int typeB = boost::lexical_cast<int>(atomType(1));
    int typeC = boost::lexical_cast<int>(atomType(2));

    const OplsAngleBendParameters *p = parameters->angleBendParameters(typeA, typeB, typeC);
    if(!p){
        return false;
    }

    setParameter(0, p->ka);
    setParameter(1, p->theta0 * chemkit::constants::DegreesToRadians);

    return true;
}

chemkit::Real OplsAngleBendCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    chemkit::Real ka = parameter(0);
    chemkit::Real theta0 = parameter(1);

    chemkit::Real theta = coordinates->angleRadians(a, b, c);

    return ka * pow(theta - theta0, 2);
}

std::vector<chemkit::Vector3> OplsAngleBendCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);

    chemkit::Real ka = parameter(0);
    chemkit::Real theta0 = parameter(1);

    chemkit::Real theta = coordinates->angleRadians(a, b, c);

    boost::array<chemkit::Vector3, 3> gradient = coordinates->angleGradientRadians(a, b, c);

    // dE/dtheta
    chemkit::Real de_dtheta = (2.0 * ka * (theta - theta0));

    gradient[0] *= de_dtheta;
    gradient[1] *= de_dtheta;
    gradient[2] *= de_dtheta;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}

// === OplsTorsionCalculation ============================================== //
OplsTorsionCalculation::OplsTorsionCalculation(size_t a, size_t b, size_t c, size_t d)
    : OplsCalculation(Torsion, 4, 3)
{
    setAtom(0, a);
    setAtom(1, b);
    setAtom(2, c);
    setAtom(3, d);
}

bool OplsTorsionCalculation::setup(const OplsParameters *parameters)
{
    int typeA = boost::lexical_cast<int>(atomType(0));
    int typeB = boost::lexical_cast<int>(atomType(1));
    int typeC = boost::lexical_cast<int>(atomType(2));
    int typeD = boost::lexical_cast<int>(atomType(3));

    const OplsTorsionParameters *p = parameters->torsionParameters(typeA, typeB, typeC, typeD);
    if(!p){
        return false;
    }

    setParameter(0, p->v1);
    setParameter(1, p->v2);
    setParameter(2, p->v3);

    return true;
}

chemkit::Real OplsTorsionCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    chemkit::Real v1 = parameter(0);
    chemkit::Real v2 = parameter(1);
    chemkit::Real v3 = parameter(2);

    chemkit::Real phi = coordinates->torsionAngleRadians(a, b, c, d);

    return (1.0/2.0) * (v1 * (1.0 + cos(phi)) + v2 * (1.0 - cos(2.0 * phi)) + v3 * (1.0 + cos(3.0 * phi)));
}

std::vector<chemkit::Vector3> OplsTorsionCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);
    size_t c = atom(2);
    size_t d = atom(3);

    chemkit::Real v1 = parameter(0);
    chemkit::Real v2 = parameter(1);
    chemkit::Real v3 = parameter(2);

    chemkit::Real phi = coordinates->torsionAngleRadians(a, b, c, d);

    // dE/dphi
    chemkit::Real de_dphi = (1.0/2.0) * (-v1 * sin(phi) + 2.0 * v2 * sin(2.0 * phi) - 3.0 * v3 * sin(3.0 * phi));

    boost::array<chemkit::Vector3, 4> gradient = coordinates->torsionAngleGradientRadians(a, b, c, d);

    gradient[0] *= de_dphi;
    gradient[1] *= de_dphi;
    gradient[2] *= de_dphi;
    gradient[3] *= de_dphi;

    return std::vector<chemkit::Vector3>(gradient.begin(), gradient.end());
}

// === OplsNonbondedCalculation ============================================ //
OplsNonbondedCalculation::OplsNonbondedCalculation(size_t a, size_t b)
    : OplsCalculation(VanDerWaals | Electrostatic, 2, 5)
{
    setAtom(0, a);
    setAtom(1, b);
}

bool OplsNonbondedCalculation::setup(const OplsParameters *parameters)
{
    int typeA = boost::lexical_cast<int>(atomType(0));
    int typeB = boost::lexical_cast<int>(atomType(1));

    const OplsVanDerWaalsParameters *pa = parameters->vanDerWaalsParameters(typeA);
    const OplsVanDerWaalsParameters *pb = parameters->vanDerWaalsParameters(typeB);
    if(!pa || !pb){
        return false;
    }

    chemkit::Real qa = parameters->partialCharge(typeA);
    chemkit::Real qb = parameters->partialCharge(typeB);
    chemkit::Real sigma = sqrt(pa->sigma * pb->sigma);
    chemkit::Real epsilon = sqrt(pa->epsilon * pb->epsilon);

    setParameter(0, qa);
    setParameter(1, qb);
    setParameter(2, sigma);
    setParameter(3, epsilon);

    // one-four scaling
    if(topology()->isOneFour(atom(0), atom(1))){
        setParameter(4, 0.5);
    }
    else{
        setParameter(4, 1.0);
    }

    return true;
}

chemkit::Real OplsNonbondedCalculation::energy(const chemkit::CartesianCoordinates *coordinates) const
{
    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real qa = parameter(0);
    chemkit::Real qb = parameter(1);
    chemkit::Real e = 332.06; // vacuum permitivity
    chemkit::Real sigma = parameter(2);
    chemkit::Real epsilon = parameter(3);
    chemkit::Real scale = parameter(4);

    chemkit::Real r = coordinates->distance(a, b);

    return scale * ((qa * qb * e) / r + 4.0 * epsilon * (pow(sigma / r, 12) - pow(sigma / r, 6)));
}

std::vector<chemkit::Vector3> OplsNonbondedCalculation::gradient(const chemkit::CartesianCoordinates *coordinates) const
{
    std::vector<chemkit::Vector3> gradient(2);

    size_t a = atom(0);
    size_t b = atom(1);

    chemkit::Real qa = parameter(0);
    chemkit::Real qb = parameter(1);
    chemkit::Real e = 332.06; // vacuum permitivity
    chemkit::Real sigma = parameter(2);
    chemkit::Real epsilon = parameter(3);
    chemkit::Real scale = parameter(4);

    chemkit::Real r = coordinates->distance(a, b);
    chemkit::Real sr = sigma / r;

    // dE/dr
    chemkit::Real de_dr = scale * ((1.0 / pow(r, 3)) * (-qa * qb * e + -4.0 * epsilon * sigma * (12.0 * pow(sr, 11) - 6.0 * pow(sr, 5))));

    // dE/da
    chemkit::Vector3 de_da = (coordinates->position(a) - coordinates->position(b)) * de_dr;

    gradient[0] = de_da;
    gradient[1] = -de_da;

    return gradient;
}


#include <lwr_controllers/one_task_inverse_dynamics_jl.h>
#include <utils/pseudo_inversion.h>
#include <utils/skew_symmetric.h>

#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <Eigen/LU>

#include <math.h>

namespace lwr_controllers
{
	OneTaskInverseDynamicsJL::OneTaskInverseDynamicsJL() {}
	OneTaskInverseDynamicsJL::~OneTaskInverseDynamicsJL() {}

	bool OneTaskInverseDynamicsJL::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
	{
        KinematicChainControllerBase<hardware_interface::EffortJointInterface>::init(robot, n);

		jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
		id_solver_.reset(new KDL::ChainDynParam(kdl_chain_,gravity_));
		fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));

		qdot_last_.resize(kdl_chain_.getNrOfJoints());
		tau_.resize(kdl_chain_.getNrOfJoints());
		J_.resize(kdl_chain_.getNrOfJoints());
		J_dot_.resize(kdl_chain_.getNrOfJoints());
		J_star_.resize(kdl_chain_.getNrOfJoints());
		Kp_.resize(kdl_chain_.getNrOfJoints());
		Kd_.resize(kdl_chain_.getNrOfJoints());
		M_.resize(kdl_chain_.getNrOfJoints());
		C_.resize(kdl_chain_.getNrOfJoints());
		G_.resize(kdl_chain_.getNrOfJoints());

		J_last_.resize(kdl_chain_.getNrOfJoints());

		sub_command_ = nh_.subscribe("command", 1, &OneTaskInverseDynamicsJL::command, this);
		pub_error_ = nh_.advertise<std_msgs::Float64MultiArray>("error", 1000);
		pub_pose_ = nh_.advertise<std_msgs::Float64MultiArray>("pose", 1000);
		pub_marker_ = nh_.advertise<visualization_msgs::Marker>("marker",1000);

		return true;
	}

	void OneTaskInverseDynamicsJL::starting(const ros::Time& time)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    		joint_des_states_.q(i) = joint_msr_states_.q(i);
    		joint_des_states_.qdot(i) = joint_msr_states_.qdot(i);
    		Kp_(i) = 100;
  			Kd_(i) = 20;
    	}

    	I_ = Eigen::Matrix<double,7,7>::Identity(7,7);
    	e_ref_ = Eigen::Matrix<double,6,1>::Zero();

    	first_step_ = 0;
    	cmd_flag_ = 0;
    	step_ = 0;

	}

	void OneTaskInverseDynamicsJL::update(const ros::Time& time, const ros::Duration& period)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    	}

    	// clearing msgs before publishing
    	msg_err_.data.clear();
    	msg_pose_.data.clear();

    	if (cmd_flag_)
    	{
    		// resetting N and tau(t=0) for the highest priority task
    		N_trans_ = I_;
    		SetToZero(tau_);

    		// computing Inertia, Coriolis and Gravity matrices
		    id_solver_->JntToMass(joint_msr_states_.q, M_);
		    id_solver_->JntToCoriolis(joint_msr_states_.q, joint_msr_states_.qdot, C_);
		    id_solver_->JntToGravity(joint_msr_states_.q, G_);
		    G_.data.setZero();

		    // computing the inverse of M_ now, since it will be used often
		    pseudo_inverse(M_.data,M_inv_,false); //M_inv_ = M_.data.inverse();

	    	// computing Jacobian J(q)
	    	jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);

	    	// computing the distance from the mid points of the joint ranges as objective function to be minimized
	    	phi_ = task_objective_function(joint_msr_states_.q);

	    	// using the first step to compute jacobian of the tasks
	    	if (first_step_)
	    	{
	    		J_last_ = J_;
	    		phi_last_ = phi_;
	    		first_step_ = 0;
	    		return;
	    	}

	    	// computing the derivative of Jacobian J_dot(q) through numerical differentiation
	    	J_dot_.data = (J_.data - J_last_.data)/period.toSec();

	    	// computing forward kinematics
	    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

	    	if (Equal(x_,x_des_,0.05))
	    	{
	    		ROS_INFO("On target");
	    		cmd_flag_ = 0;
	    		return;
	    	}

	    	// pushing x to the pose msg
	    	for (int i = 0; i < 3; i++)
	    		msg_pose_.data.push_back(x_.p(i));

	    	// setting marker parameters
	    	set_marker(x_,msg_id_);

	    	// computing end-effector position/orientation error w.r.t. desired frame
	    	x_err_ = diff(x_,x_des_);

	    	x_dot_ = J_.data*joint_msr_states_.qdot.data;

	    	// setting error reference
	    	for(int i = 0; i < e_ref_.size(); i++)
		    {
	    		// e = x_des_dotdot + Kd*(x_des_dot - x_dot) + Kp*(x_des - x)
	    		e_ref_(i) =  -Kd_(i)*(x_dot_(i)) + Kp_(i)*x_err_(i);
    			msg_err_.data.push_back(e_ref_(i));
	    	}

    		// computing b = J*M^-1*(c+g) - J_dot*q_dot
    		b_ = J_.data*M_inv_*(C_.data + G_.data) - J_dot_.data*joint_msr_states_.qdot.data;

	    	// computing omega = J*M^-1*N^T*J
	    	omega_ = J_.data*M_inv_*N_trans_*J_.data.transpose();

	    	// computing lambda = omega^-1
	    	pseudo_inverse(omega_,lambda_);
	    	//lambda_ = omega_.inverse();

	    	// computing nullspace
	    	N_trans_ = N_trans_ - J_.data.transpose()*lambda_*J_.data*M_inv_;

	    	// finally, computing the torque tau
	    	tau_.data = J_.data.transpose()*lambda_*(e_ref_ + b_) + N_trans_*(Eigen::Matrix<double,7,1>::Identity(7,1)*(phi_ - phi_last_)/(period.toSec()));

	    	// saving J_ and phi of the last iteration
	    	J_last_ = J_;
	    	phi_last_ = phi_;

    	}

    	// set controls for joints
    	for (int i = 0; i < joint_handles_.size(); i++)
    	{
    		if(cmd_flag_)
    			joint_handles_[i].setCommand(tau_(i));
    		else
       			joint_handles_[i].setCommand(PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period));
    	}

    	// publishing markers for visualization in rviz
    	pub_marker_.publish(msg_marker_);
    	msg_id_++;

	    // publishing error
	    pub_error_.publish(msg_err_);
	    // publishing pose
	    pub_pose_.publish(msg_pose_);
	    ros::spinOnce();

	}

	void OneTaskInverseDynamicsJL::command(const lwr_controllers::PoseRPY::ConstPtr &msg)
	{
		KDL::Frame frame_des_;

		switch(msg->id)
		{
			case 0:
			frame_des_ = KDL::Frame(
					KDL::Rotation::RPY(msg->orientation.roll,
						 			  msg->orientation.pitch,
								 	  msg->orientation.yaw),
					KDL::Vector(msg->position.x,
								msg->position.y,
								msg->position.z));
			break;

			case 1: // position only
			frame_des_ = KDL::Frame(
				KDL::Vector(msg->position.x,
							msg->position.y,
							msg->position.z));
			break;

			case 2: // orientation only
			frame_des_ = KDL::Frame(
				KDL::Rotation::RPY(msg->orientation.roll,
				   	 			   msg->orientation.pitch,
								   msg->orientation.yaw));
			break;

			default:
			ROS_INFO("Wrong message ID");
			return;
		}

		x_des_ = frame_des_;
		cmd_flag_ = 1;
	}

	void OneTaskInverseDynamicsJL::set_marker(KDL::Frame x, int id)
	{
				msg_marker_.header.frame_id = "world";
				msg_marker_.header.stamp = ros::Time();
				msg_marker_.ns = "end_effector";
				msg_marker_.id = id;
				msg_marker_.type = visualization_msgs::Marker::SPHERE;
				msg_marker_.action = visualization_msgs::Marker::ADD;
				msg_marker_.pose.position.x = x.p(0);
				msg_marker_.pose.position.y = x.p(1);
				msg_marker_.pose.position.z = x.p(2);
				msg_marker_.pose.orientation.x = 0.0;
				msg_marker_.pose.orientation.y = 0.0;
				msg_marker_.pose.orientation.z = 0.0;
				msg_marker_.pose.orientation.w = 1.0;
				msg_marker_.scale.x = 0.005;
				msg_marker_.scale.y = 0.005;
				msg_marker_.scale.z = 0.005;
				msg_marker_.color.a = 1.0;
				msg_marker_.color.r = 0.0;
				msg_marker_.color.g = 1.0;
				msg_marker_.color.b = 0.0;
	}

	double OneTaskInverseDynamicsJL::task_objective_function(KDL::JntArray q)
	{
		double sum = 0;
		double temp;
		int N = q.data.size();

		for (int i = 0; i < N; i++)
		{
			temp = ((q(i) - joint_limits_.center(i))/(joint_limits_.max(i) - joint_limits_.min(i)));
			sum += temp*temp;
		}

		sum /= 2*N;

		return -sum;
	}
}

PLUGINLIB_EXPORT_CLASS(lwr_controllers::OneTaskInverseDynamicsJL, controller_interface::ControllerBase)


// Author: Riley Raschke
// © 2016 rrappsdev.com
//

#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <thread>

#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>

#include <PBnumbers.h>
#include <PBgenerator.h>
#include <BigInt.h>

namespace po = boost::program_options;

using namespace std;

void RunMonteCarlo(
      PBgenerator* gen,
      const vector<PBnumbers>& myLines,
      string threadName,
      int* drawCnt,
      PBnumbers* matchedNums,
      bool reseedFlag
   ){

   boost::timer::cpu_timer t;
   boost::timer::cpu_times times;
   PBnumbers result;
   cout << threadName << " Checking Numbers\n";
   *drawCnt=1;
   t.start();
   do {
      if( *drawCnt % 1000000 == 0 ){
         times = t.elapsed();
         cout << threadName
            << " " << setw(11) << *drawCnt
            << " drawings occured. Last one million in " << boost::timer::format(times, 3, "%w") << " wall clock seconds\n";
            t.start();
         if( reseedFlag ) {
            gen->ReSeed();
         }
      }
      result = gen->Generate();
      ++*drawCnt;
   } while( ! result.in(myLines) );
   *matchedNums = result;

   cout << threadName << " Match found on drawing number " << *drawCnt << " !!WINNER!! " << result << "\n";
}

int main( int argc, char** argv ) {

   int lineCount = 10;
   int nCount = 5;
   int pCount = 1;
   int nMax = 69;
   int pMax = 26;
   int threadCount = 2;
   bool multiGen = false;
   bool reseed = false;

   /**
    * Get Args
    */
   po::options_description desc("Allowed options");

   desc.add_options()
      ("help,h", "produce this message")
      ("lines,l", po::value<int>(), "Lines to play")
      ("nCount,nc", po::value<int>(), "Number of regular balls")
      ("pCount,pc", po::value<int>(), "Number of power balls")
      ("nMax", po::value<int>(), "Max value of the regular ball(s)")
      ("pMax", po::value<int>(), "Max value of the power ball(s)")
      ("threads,t", po::value<int>(), "Number of threads to create")
      ("generator,g", "Use a unique generator for each thread. (Faster, no generator mutex lock wait), potentially less random")
      ("reseed,r", "NOTE: Requires -g ; Reseed Every 1 million drawings per thread, each thread is idealy offset by 3.14 seconds; potentially less random even more");

   try {
      po::variables_map vm;
      po::store(po::parse_command_line(argc, argv, desc), vm);
      po::notify(vm);
      if( vm.count("help")    ){ cout << desc << "\n"; return 0;        }
      if( vm.count("lines")   ){ lineCount   = vm["lines"].as<int>();   }
      if( vm.count("nCount")  ){ nCount      = vm["nCount"].as<int>();  }
      if( vm.count("pCount")  ){ pCount      = vm["pCount"].as<int>();  }
      if( vm.count("nMax")    ){ nMax        = vm["nMax"].as<int>();    }
      if( vm.count("pMax")    ){ pMax        = vm["pMax"].as<int>();    }
      if( vm.count("threads") ){ threadCount = vm["threads"].as<int>(); }
      if( vm.count("reseed")  ){ reseed = true; }
      if( vm.count("generator") ){ multiGen = true; }
   } catch (int e){
      cout << desc << "\n";
      return 1;
   }

   if( reseed && ! multiGen ){
      cout << desc << "\n";
      return 1;
   }

   // Print runtime info
   cout << "Playing " << lineCount << " lines against " << threadCount << " threads with\n" <<
      nCount << " regular ball(s) and a max value of: " << nMax << "\n" <<
      pCount << " power ball(s) and a max value of: "   << pMax << "\n";

   vector<thread>    threads;
   vector<PBnumbers> myLines;
   PBgenerator lottery( nCount, pCount, nMax, pMax );

   // Generate and print the players lines
   int  i = 1;
   char line[64];
   for(i = 0; i < lineCount; ++i){
      myLines.push_back( lottery.Generate() );
      sprintf( line, "Line %8d: %s\n",i+1, &(myLines[i].toString()[0]) );
      cout << line ;
   }

   // Storage for each threads return value
   int          drawings[threadCount];
   PBnumbers    matches[threadCount];
   PBgenerator generators[threadCount];

   // Kick off all the threads
   for( i = 0; i < threadCount; ++i){
      sprintf( line, "Thread #%-3d ", i+1);
      string x = line;
      drawings[i] = 0;
      usleep(10000); // stagger thread starts
      if( multiGen ){
         usleep(3141592); // Allow time for new seed PI seconds ;)
         generators[i] = lottery;
         generators[i].ReSeed(); // Remove this and results will be the same for every thread!!! (not desired)
         threads.push_back( std::thread( &RunMonteCarlo, &generators[i], myLines, x, &drawings[i], &matches[i], reseed) );
      } else {
         threads.push_back( std::thread( &RunMonteCarlo, &lottery, myLines, x, &drawings[i], &matches[i], reseed) );
      }
   }

   // Wait for all threads to finish
   for( i = 0; i < threadCount; ++i){
      threads[i].join();
   }

   // summarize drawings and calculate average
   BigInt sum = 0;
   for( i = 0; i < threadCount; ++i){
      sum = sum + drawings[i];
      sprintf( line, "Thread #%-3d took %11d drawings to match: %s\n", i+1, drawings[i], &(matches[i].toString()[0]) );
      string resultInfo = line;
      cout << resultInfo;
   }

   cout << "Avg: " << sum / threadCount << "\n";

   // Show the plays lines again
   cout << "You're lines again:\n";
   for(i = 0; i < lineCount; ++i){
      sprintf( line, "Line %8d: %s\n",i+1, &(myLines[i].toString()[0]) );
      cout << line ;
   }

   return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SCALAR_FAST_FREXP_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SCALAR_FAST_FREXP_HPP_INCLUDED
#include <boost/simd/toolbox/ieee/functions/fast_frexp.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/dispatch/meta/adapted_traits.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/at.hpp>
#include <math.h>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::fast_frexp_, tag::cpu_, (A0)(A2)
                            , (scalar_< double_<A0> >)
                              (scalar_< double_<A0> >)
                              (scalar_< int64_<A2> >)
                            )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a0,A0 & a1,A2 & a2) const
    {
      int32_t tmp;
      a1 = ::frexp(a0, &tmp);
      a2 = tmp;
      return 0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::fast_frexp_, tag::cpu_, (A0)(A2)
                            , (scalar_< single_<A0> >)
                              (scalar_< single_<A0> >)
                              (scalar_< int32_<A2> >)
                            )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a0,A0 & a1,A2 & a2) const
    {
      //      int32_t tmp;
      a1 = ::frexpf(a0, &a2);
      return 0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::fast_frexp_, tag::cpu_, (A0)(A2)
                            , (scalar_< double_<A0> >)
                              (scalar_< int64_<A2> >)
                            )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A2 & a2) const
    {
      typedef typename dispatch::meta::as_integer<A0, signed>::type      int_type;
      A0 a1;
      boost::simd::fast_frexp(a0, a1, a2);
      return a1;
    }
  };
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::fast_frexp_, tag::cpu_, (A0)(A2)
                            , (scalar_< single_<A0> >)
                              (scalar_< int32_<A2> >)
                            )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A2 & a2) const
    {
      typedef typename dispatch::meta::as_integer<A0, signed>::type      int_type;
      A0 a1;
      boost::simd::fast_frexp(a0, a1, a2);
      return a1;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::fast_frexp_, tag::cpu_,
                            (A0),
                            (scalar_ < double_<A0> > )
                           )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>::type mantissa;
    typedef typename dispatch::meta::as_integer<A0,signed>::type                   exponent;
    typedef boost::fusion::vector<mantissa,exponent>                               result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type res;
      int r1t;
      boost::fusion::at_c<0>(res) = ::frexp(a0, &r1t);
      boost::fusion::at_c<1>(res) = r1t;
      return res;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::fast_frexp_, tag::cpu_,
                            (A0),
                            (scalar_ < single_<A0> > )
                           )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>::type mantissa;
    typedef typename dispatch::meta::as_integer<A0,signed>::type                   exponent;
    typedef boost::fusion::vector<mantissa,exponent>                            result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type res;
      boost::fusion::at_c<0>(res) = ::frexpf(a0, &boost::fusion::at_c<1>(res));
      return res;
    }
  };
} } }
#endif


#include <boost/assert.hpp>
#include <cmath>
#include <limits>
#include <utility>
#include <algorithm>

#include "mh_moves/mh_moves.hpp"
#include "sampling/simple.hpp"
#include "mh_moves/utility.hpp"
#include "tools/debug.hpp"

namespace biggles {

namespace creation_fun {

    const size_t max_gap = 3;

    typedef std::deque<observation> obs_container;

    bool get_track_obs(const observation& first_obs, const clutter_t& clutter, const size_t max_gap,
        obs_container& track_obs)
    {
        BOOST_ASSERT(max_gap > 1);
        bool done = false;
        track_obs.clear();
        track_obs.push_back(first_obs);
        observation last_obs = first_obs;
        time_stamp final_ts = clutter.last_time_stamp();
        while (not done) {
            bool added_obs = false;
            for (size_t gap = 1; gap <= max_gap; ++ gap) {
                time_stamp current_ts = t(last_obs) + gap;
                if (current_ts == final_ts) { break; }
                float radius = mh_moves::light_years(t(last_obs), current_ts);
                obs_container candidate_obs = clutter.locate_near(current_ts, last_obs, radius);
                if (candidate_obs.empty()) continue;
                last_obs = *sampling::from_range(candidate_obs.begin(), candidate_obs.end());
                track_obs.push_back(last_obs);
                added_obs = true;
            }
            done = not added_obs;
        }
        return track_obs.size() > 1;
    }

    bool get_track_at_ts(const time_stamp ts, const clutter_t& clutter, const size_t max_gap, obs_container& track_obs) {
        obs_container firsts = clutter.observations(ts);
        bool done = false;
        while (firsts.size() > 0 and not done) {
            obs_container::iterator obs_it = sampling::from_range(firsts.begin(), firsts.end());
            firsts.erase(obs_it);
            done = get_track_obs(*obs_it, clutter, max_gap, track_obs);
        }
        return done;
    }

    bool create_track(const partition_ptr_t& start_partition_ptr, partition_ptr_t& end_partition_ptr) {
        const clutter_t& clutter(start_partition_ptr->clutter());
        const track_collection& tracks(start_partition_ptr->tracks());

        time_stamp first_ts = clutter.first_time_stamp();
        time_stamp last_ts = clutter.last_time_stamp();
        obs_container track_obs;
        bool success = false;
        for (time_stamp ts = first_ts; ts < last_ts - 1; ts++) {
            success = get_track_at_ts(ts, clutter, max_gap, track_obs);
            if (success) break;
        }
        if (not success) {
            end_partition_ptr = start_partition_ptr;
            return success;
        }

        // make new track
        shared_track_ptr new_track(new track(t(track_obs.front()), t(track_obs.back())+1,
                                track_obs.begin(), track_obs.end(), 1.f));
        // make new clutter
        clutter_ptr new_clutter(new clutter_t(clutter));
        for (obs_container::iterator obs_it = track_obs.begin(); obs_it != track_obs.end(); obs_it++) {
            new_clutter->erase(*obs_it);
        }
        // make new partition
        mh_moves::shared_track_collection_ptr new_tracks(new track_collection(tracks));
        new_tracks->insert(new_track);
        end_partition_ptr = partition_ptr_t(
            new partition(start_partition_ptr->pool(), new_tracks, new_clutter, start_partition_ptr->expansion())
        );
        return success;
    }

    bool create_track_reverse(const partition_ptr_t& start_partition_ptr, partition_ptr_t& end_partition_ptr) {
        const clutter_t& clutter(start_partition_ptr->clutter());
        const track_collection& tracks(start_partition_ptr->tracks());

        time_stamp first_ts = clutter.first_time_stamp();
        time_stamp last_ts = clutter.last_time_stamp();
        obs_container track_obs;
        bool success = false;
        for (time_stamp ts = first_ts; ts < last_ts - 1; ts++) {
            success = get_track_at_ts(ts, clutter, max_gap, track_obs);
            if (success) break;
        }
        if (not success) {
            end_partition_ptr = start_partition_ptr;
            return success;
        }

        // make new track
        shared_track_ptr new_track(new track(t(track_obs.front()), t(track_obs.back())+1,
                                track_obs.begin(), track_obs.end(), 1.f));
        // make new clutter
        clutter_ptr new_clutter(new clutter_t(clutter));
        for (obs_container::iterator obs_it = track_obs.begin(); obs_it != track_obs.end(); obs_it++) {
            new_clutter->erase(*obs_it);
        }
        // make new partition
        mh_moves::shared_track_collection_ptr new_tracks(new track_collection(tracks));
        new_tracks->insert(new_track);
        end_partition_ptr = partition_ptr_t(
            new partition(start_partition_ptr->pool(), new_tracks, new_clutter, start_partition_ptr->expansion())
        );
        return success;
    }

} // creation_fun

bool max_creation(const partition_ptr_t& start_partition_ptr, partition_ptr_t& end_partition_ptr) {

    partition_ptr_t input_partition = start_partition_ptr;

    float total_obs = input_partition->observation_count();

    bool done = false;
    while (not done) {
        done = not creation_fun::create_track(input_partition, end_partition_ptr)
               or float(end_partition_ptr->clutter().size())/total_obs <= 0.5;
        input_partition = end_partition_ptr;
    }
    return true;
} // max_creation

bool rev_creation(const partition_ptr_t& start_partition_ptr, partition_ptr_t& end_partition_ptr) {

    partition_ptr_t input_partition = start_partition_ptr;

    float total_obs = input_partition->observation_count();

    bool done = false;
    while (not done) {
        done = not creation_fun::create_track_reverse(input_partition, end_partition_ptr)
               or float(end_partition_ptr->clutter().size())/total_obs <= 0.5;
        input_partition = end_partition_ptr;
    }
    return true;
} // rev_creation

} // biggles


#include <iostream>
#include <fstream>
#include <mist/mist.h>
#include <Eigen/Core>
#include "ItkImageIO.h"
#include "dataIO.h"
#include "configs.h"
#include "maximum_a_posteriori.h"
#include "evaluations.h"

typedef unsigned char mask_t;
typedef unsigned char label_t;
typedef double feat_t;
typedef double atlas_t;

int main(int argc, char **argv) {

	if (argc != 8 && argc != 9) {
		std::cerr << "Usage:" << std::endl;
		std::cerr << argv[0] << " <input feature directory> <input abdominal cavity mask directory> "
			<< " <input param directory> <input true label directory> "
			<< " <output directory> <test filename list> <feature name list> "
			<< " <input atlas directory>"
			<< std::endl;
		return EXIT_FAILURE;
	}

	std::cout << "----- Read data list -----" << std::endl;
	const std::string input_feat_dir = std::string(argv[1]) + "/";
	const std::string input_abd_mask_dir = std::string(argv[2]) + "/";
	const std::string input_param_dir = std::string(argv[3]) + "/";
	const std::string input_true_label_dir = std::string(argv[4]) + "/";
	const std::string output_dir = std::string(argv[5]) + "/";
	const std::string test_filename_list_path = argv[6];
	const std::string feature_name_list_path = argv[7];
	std::string input_atlas_dir;
	if (argc == 9) {
		input_atlas_dir = std::string(argv[8]) + "/";
	}

	std::list<std::string> test_filename_list;
	std::list<std::string> feature_name_list;
	if (!get_data_list(test_filename_list_path, test_filename_list) ||
		!get_data_list(feature_name_list_path, feature_name_list)) {
		return EXIT_FAILURE;
	}
	const int num_cases = test_filename_list.size();
	const int num_features = feature_name_list.size();

	std::cout << "----- Load estimated param -----" << std::endl;
	std::vector<Eigen::MatrixXd> mean(TESTTIME_NUM_LABELS, Eigen::MatrixXd(num_features, 1));
	std::vector<Eigen::MatrixXd> covariance(TESTTIME_NUM_LABELS, Eigen::MatrixXd(num_features, num_features));
	for (int l = 0; l < TESTTIME_NUM_LABELS; l++) {
		read_raw_to_eigen(mean[l], input_param_dir + TESTTIME_LABEL_NAMES[l] + "/estimated_mean_param.raw", num_features);
		read_raw_to_eigen(covariance[l], input_param_dir + TESTTIME_LABEL_NAMES[l] + "/estimated_covariance_param.raw", num_features);
	}

	std::cout << "----- Apply maximum a posteriori -----" << std::endl;
	for (auto case_itr = test_filename_list.begin(); case_itr != test_filename_list.end(); ++case_itr) {
		std::cout << "Case: " << *case_itr << std::endl;
		std::vector<mask_t> abd_mask_img;
		ImageIO<NDIMS> abd_mask_mhd;
		abd_mask_mhd.Read(abd_mask_img, input_abd_mask_dir + *case_itr);
		const int xe = abd_mask_mhd.Size(0);
		const int ye = abd_mask_mhd.Size(1);
		const int ze = abd_mask_mhd.Size(2);
		const int se = xe*ye*ze;
		const double x_spacing = abd_mask_mhd.Spacing(0);
		const double y_spacing = abd_mask_mhd.Spacing(1);
		const double z_spacing = abd_mask_mhd.Spacing(2);

		size_t num_pixel = 0;
		for (int z = 0; z < ze; z++) {
			for (int y = 0; y < ye; y++) {
				for (int x = 0; x < xe; x++) {
					if (abd_mask_img.at(z * xe * ye + y * xe + x)) num_pixel++;
				}
			}
		}

		std::cout << "---- Load feature ----" << std::endl;
		std::vector<feat_t> feature_array(num_pixel * num_features);
		for (auto feat_itr = feature_name_list.begin(); feat_itr != feature_name_list.end(); ++feat_itr) {
			std::vector<feat_t> feat_img;
			ImageIO<NDIMS> feature_mhd;
			feature_mhd.Read(feat_img, input_feat_dir + *feat_itr + "/" + *case_itr);

			size_t tmp = 0;
			size_t i = std::distance(feature_name_list.begin(), feat_itr);
			for (int z = 0; z < ze; z++) {
				for (int y = 0; y < ye; y++) {
					for (int x = 0; x < xe; x++) {
						if (abd_mask_img.at(z * xe * ye + y * xe + x)) {
							feature_array.at(i*num_pixel + tmp) = feat_img.at(z * xe * ye + y * xe + x);
							tmp++;
						}
					}
				}
			}
		}

		std::vector<feat_t> atlas_array(num_pixel * TESTTIME_NUM_LABELS);
		if (argc == 9) { // Atlas distribution
			std::cout << "---- Load atlas ----" << std::endl;
			for (int l = 0; l < TESTTIME_NUM_LABELS; l++) {
				std::vector<atlas_t> atlas_img;
				ImageIO<NDIMS> atlas_mhd;
				atlas_mhd.Read(atlas_img, input_atlas_dir + TESTTIME_LABEL_NAMES[l] + "/atlas.mhd");

				size_t tmp = 0;
				for (int z = 0; z < ze; z++) {
					for (int y = 0; y < ye; y++) {
						for (int x = 0; x < xe; x++) {
							if (abd_mask_img.at(z * xe * ye + y * xe + x)) {
								atlas_array.at(l*num_pixel + tmp) = atlas_img.at(z * xe * ye + y * xe + x);
								tmp++;
							}
						}
					}
				}
			}/* Label loop */
			 // If atlas is 0, label is assigned others class
			for (int n = 0; n < num_pixel; n++) {
				double tmp = 0.;
				for (int l = 0; l < TESTTIME_NUM_LABELS; l++) {
					tmp += atlas_array.at(l*num_pixel + n);
				}
				if (tmp <= 0) {
					atlas_array.at((TESTTIME_NUM_LABELS - 1)*num_pixel + n) = 1.;
				}
			}

		}
		else {
			// Uniform distribution
			for (int s = 0; s < num_pixel*TESTTIME_NUM_LABELS; s++) {
				atlas_array.at(s) = 1.0 / (atlas_t)TESTTIME_NUM_LABELS;
			}
		}

		// Normalize atlas
		std::cout << "----- Normalize atlas probability -----" << std::endl;
		maximum_a_posteriori::normalize_probability(atlas_array, num_pixel, TESTTIME_NUM_LABELS);

		// Calc posterior probability
		std::vector<double> posterior(TESTTIME_NUM_LABELS*num_pixel);
		for (int l = 0; l < TESTTIME_NUM_LABELS; l++) {
			std::cout << "Calc poterior probability at: " << TESTTIME_LABEL_NAMES[l] << std::endl;
			atlas_t *atlas_tmp = atlas_array.data() + l*num_pixel;
			double *posterior_tmp = posterior.data() + l*num_pixel;
			maximum_a_posteriori::calculate_posterior(feature_array,
				mean[l],
				covariance[l],
				atlas_tmp,
				num_pixel,
				num_features,
				posterior_tmp);
		} /* Label loop */

		  // Normalize posterior
		std::cout << "----- Normalize posterior probability -----" << std::endl;
		maximum_a_posteriori::normalize_probability(posterior, num_pixel, TESTTIME_NUM_LABELS);

		for (int l = 0; l < TESTTIME_NUM_LABELS; l++) {
			std::cout << "Save posterior: " << TESTTIME_LABEL_NAMES[l] << std::endl;
			double *posterior_tmp = posterior.data() + l*num_pixel;
			std::vector<double> posterior_img(abd_mask_img.size(), 0);

			size_t tmp = 0;
			for (int s = 0; s < se; s++) {
				if (abd_mask_img.at(s)) {
					posterior_img.at(s) = posterior_tmp[tmp++];
				}
			}
			std::string result_dir = output_dir + *case_itr + "/posterior/";
			make_dir(result_dir);
			save_vector(result_dir + TESTTIME_LABEL_NAMES[l] + ".mhd",
				posterior_img, xe, ye, ze,
				x_spacing, y_spacing, z_spacing);
		} /* Label loop */

		std::cout << "----- MAP Segmentation -----" << std::endl;
		std::vector<label_t> label_array(num_pixel);
		maximum_a_posteriori::segmentation(posterior, num_pixel, TESTTIME_NUM_LABELS, label_array);

		std::vector<label_t> label_img(abd_mask_img.size(), 0);
		size_t tmp = 0;
		for (int s = 0; s < se; s++) {
			if (abd_mask_img.at(s)) {
				label_img.at(s) = label_array.at(tmp++);
			}
		}
		// correct label misalignment
		for (int s = 0; s < se; s++) {
			if (label_img.at(s) >= IGNORED_LABEL_NUM) {
				label_img.at(s) += 1;
			}
		}

		std::string result_dir = output_dir + *case_itr;
		make_dir(result_dir);
		save_vector(result_dir + "/map_segmentation.mhd",
			label_img, xe, ye, ze,
			x_spacing, y_spacing, z_spacing);

		std::cout << "----- Start evaluations -----" << std::endl;
		std::ofstream ofs(result_dir + "/results.csv");
		ofs << "organs, JI, predict ratio, true ratio, cross, join, fp, fn,"
			<< "total num pixel in mask, num predicted pixel,"
			<< "num gt pixel" << std::endl;
		std::vector<label_t> gt_img;
		ImageIO<NDIMS> gt_mhd;
		gt_mhd.Read(gt_img, input_true_label_dir + *case_itr);
		for (int l = 0; l < NUM_LABELS; l++) {
			if (l == IGNORED_LABEL_NUM-1) continue;
			std::vector<label_t> predict_tmp(se, 0);
			std::vector<label_t> gt_tmp(se, 0);
			if (l != NUM_LABELS - 1) { // for organs
				for (int s = 0; s < se; s++) {
					if (label_img.at(s) == l + 1) {
						predict_tmp.at(s) = 1;
					}
					if (gt_img.at(s) == l + 1) {
						gt_tmp.at(s) = 1;
					}
				}
			}
			else { // for others
				for (int s = 0; s < se; s++) {
					if (abd_mask_img.at(s)) {
						if (label_img.at(s) == l + 1) { // l+1 = NUM_LABELS
							predict_tmp.at(s) = 1;
						}
						if (gt_img.at(s) == 0) {
							gt_tmp.at(s) = 1;
						}
					}
				}
			}

			std::vector<size_t> results;
			double ji = calc_jaccard_index(predict_tmp, gt_tmp, abd_mask_img, results);
			std::cout << "Label: " << LABEL_NAMES[l] << std::endl;
			std::cout << "JI: " << ji << std::endl;
			ofs << LABEL_NAMES[l] << "," << ji << ","
				<< (double)results[5] / (double)results[4] << ","
				<< (double)results[6] / (double)results[4] << ",";
			for (auto itr = results.begin(); itr != results.end(); itr++) {
				ofs << *itr << ",";
			}
			ofs << std::endl;
		} /* Label loop */
		ofs.close();

	} /* Case loop */

	return EXIT_SUCCESS;
}