//--------------------------------------------------------------------
//
// LinearRegression.cpp
//
// Created on: Feb 1, 2018
// Author: Max
//
//--------------------------------------------------------------------

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <fstream>

#include <Launcher.h>
#include <LinearRegression.h>

static const char COMMENT_CHAR = '#';
static const char NUMBER_SEPARATOR_CHAR = ',';
static const char ATTRIBUTE_NAME_CHAR = ':';

using namespace std;

using namespace multifaq::params;
using namespace multifaq::application;
using namespace multifaq::config;
using namespace multifaq::dir;

namespace phoenix = boost::phoenix;
using namespace boost::spirit;

struct PartialGradient
{
    Query* query;
    std::vector<std::pair<size_t,size_t>> inputVariables;
    // vector<size_t> ouputVaribales;
};

std::vector<PartialGradient> partialGradients;

LinearRegression::LinearRegression(shared_ptr<Launcher> launcher)
{
    _compiler = launcher->getCompiler();
    _td = launcher->getTreeDecomposition();
}

LinearRegression::~LinearRegression()
{
    delete[] _queryRootIndex;
}

var_bitset LinearRegression::getCategoricalFeatures()
{
    return _categoricalFeatures;
}

void LinearRegression::run()
{
    loadFeatures();
    modelToQueries();
    _compiler->compile();
}

void LinearRegression::modelToQueries()
{
    size_t numberOfFunctions = 0;
    size_t featureToFunction[NUM_OF_VARIABLES],
        quadFeatureToFunction[NUM_OF_VARIABLES];

    parameterQueries = new Query*[NUM_OF_VARIABLES];

    // std::vector<PartialGradient> partialGradients;

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (_features.test(var) && !_categoricalFeatures.test(var))
        {
            _compiler->addFunction(
                new Function({var}, Operation::sum));
            featureToFunction[var] = numberOfFunctions;
            numberOfFunctions++;
        }
    }

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (_features.test(var) && !_categoricalFeatures.test(var))
        {
            _compiler->addFunction(
                new Function({var}, Operation::quadratic_sum));
            quadFeatureToFunction[var] = numberOfFunctions;
            numberOfFunctions++;
        }
    }

    // --- TODO: TODO: add count query for intercept!! TODO: TODO: --- //
    Aggregate* agg_count = new Aggregate();
    agg_count->_agg.push_back(prod_bitset());

    Query* count = new Query();
    count->_aggregates.push_back(agg_count);
    count->_rootID = _td->_root->_id;
    _compiler->addQuery(count);

    PartialGradient countGradient;
    countGradient.query = count;
    countGradient.inputVariables =
        {{NUM_OF_VARIABLES,NUM_OF_VARIABLES}};
    partialGradients.push_back(countGradient);

#ifndef DEGREE_TWO

    size_t numberOfQueries = 0;

    size_t cont_var_root = _td->_root->_id;

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (!_features[var])
            continue;

        prod_bitset prod_linear_v1;
        prod_bitset prod_quad_v1;

        if (!_categoricalFeatures[var])
        {
            // Linear function per Feature
            prod_linear_v1.set(featureToFunction[var]);

            Aggregate* agg_linear = new Aggregate();
            agg_linear->_agg.push_back(prod_linear_v1);

            Query* linear = new Query();
            linear->_aggregates.push_back(agg_linear);
            linear->_rootID = cont_var_root;

            _compiler->addQuery(linear);

            parameterQueries[var] = linear;

            //TODO: THIS ONE WOULD NEED TO BE MULTIPLIED BY THE PARAM FOR INTERCEPT
            PartialGradient linGradient;
            linGradient.query = linear;
            linGradient.inputVariables =
                {{var,NUM_OF_VARIABLES},{NUM_OF_VARIABLES,var}};
            partialGradients.push_back(linGradient);

            ++numberOfQueries;

            // TODO: What is the queryID ?
            // TODO: what is the variable of the intercept? NUM_OF_VARS + 1?
            // TODO: We should also add this to the NUM_OF_VARS + 1 gradient for
            // the intercept multiplied by param for this var!

            // Quadratic function for each feature
            prod_quad_v1.set(quadFeatureToFunction[var]);

            Aggregate* agg_quad = new Aggregate();
            agg_quad->_agg.push_back(prod_quad_v1);

            Query* quad = new Query();
            quad->_aggregates.push_back(agg_quad);
            quad->_rootID = _td->_root->_id; // cont_var_root;

            _compiler->addQuery(quad);

            // TODO: THIS ONE WOULD NEED TO BE MULTIPLIED BY THE PARAM FOR THIS var!!
            PartialGradient quadGradient;
            quadGradient.query = quad;
            quadGradient.inputVariables = {{var,var}};
            partialGradients.push_back(quadGradient);

            ++numberOfQueries;
        }
        else
        {
            // v1_root = _queryRootIndex[var];

            Aggregate* agg_linear = new Aggregate();
            agg_linear->_agg.push_back(prod_linear_v1);

            Query* linear = new Query();
            linear->_aggregates.push_back(agg_linear);
            linear->_fVars.set(var);
            linear->_rootID = _queryRootIndex[var];
            _compiler->addQuery(linear);

            parameterQueries[var] = linear;

            // THIS ONE WOULD NEED TO BE MULTIPLIED BY THE PARAM FOR INTERCEPT

            //TODO: THIS ONE WOULD NEED TO BE MULTIPLIED BY THE PARAM FOR INTERCEPT
            PartialGradient linGradient;
            linGradient.query = linear;
            linGradient.inputVariables =
                {{var,NUM_OF_VARIABLES},{NUM_OF_VARIABLES,var}};
            partialGradients.push_back(linGradient);

            ++numberOfQueries;
        }

        for (size_t var2 = var+1; var2 < NUM_OF_VARIABLES; ++var2)
        {
            if (_features[var2])
            {
                //*****************************************//
                prod_bitset prod_quad_v2 = prod_linear_v1;

                Aggregate* agg_quad_v2 = new Aggregate();

                Query* quad_v2 = new Query();
                quad_v2->_rootID = _td->_root->_id;

                // if (_categoricalFeatures[var])
                // {
                //     quad_v2->_rootID = _queryRootIndex[var];
                //     quad_v2->_fVars.set(var);
                //     if (_categoricalFeatures[var2])
                //     {
                //         quad_v2->_fVars.set(var2);
                //         // If both varaibles are categoricalVars - we choose the
                //         // var2 as the root
                //         // TODO: We should use the root that is highest to the
                //         // original root
                //         if (_queryRootIndex[var2] <= _queryRootIndex[var])
                //             quad_v2->_rootID = _queryRootIndex[var2];
                //     }
                //     else
                //     {
                //         prod_quad_v2.set(featureToFunction[var2]);
                //     }
                // }
                // else if (_categoricalFeatures[var2])
                // {
                //     quad_v2->_fVars.set(var2);
                //     // If both varaibles are categoricalVars - we choose the
                //     // var2 as the root
                //     // TODO: We should use the root that is highest to the
                //     // original root
                //     quad_v2->_rootID = _queryRootIndex[var2];
                // }
                // else
                // {
                //     // create prod_quad_v2 of both var and var2
                //     prod_quad_v2.set(featureToFunction[var2]);
                // }
                if (_categoricalFeatures[var])
                {
                    quad_v2->_rootID = _queryRootIndex[var];
                    quad_v2->_fVars.set(var);
                }

                if (_categoricalFeatures[var2])
                {
                    quad_v2->_fVars.set(var2);
                    // If both varaibles are categoricalVars - we choose the
                    // var2 as the root
                    quad_v2->_rootID = _queryRootIndex[var2];
                }
                else
                {
                    // create prod_quad_v2 of both var and var2
                    prod_quad_v2.set(featureToFunction[var2]);
                }

                agg_quad_v2->_agg.push_back(prod_quad_v2);

                quad_v2->_aggregates.push_back(agg_quad_v2);
                _compiler->addQuery(quad_v2);

                // TODO: Here we add both sides! multiply var times param of
                // var2 -- add that to gradient of var. And then multiply var2
                // times param of var and add to gradient of var2! -- this can
                // be done togther in one pass over the vector!
                PartialGradient quadGradient;
                quadGradient.query = quad_v2;
                quadGradient.inputVariables = {{var,var2},{var2,var}};
                partialGradients.push_back(quadGradient);

                ++numberOfQueries;
                // TODO: once we have identified the queries and how they are
                // multiplied, we then need to identify the actual views that
                // are computed over and use the information group computation
                // over this view!
            }
        }
    }

#else

    size_t cubicFeatureToFunction[NUM_OF_VARIABLES];
    size_t quarticFeatureToFunction[NUM_OF_VARIABLES];

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (_features.test(var) && !_categoricalFeatures.test(var))
        {
            _compiler->addFunction(new Function({var}, Operation::cubic_sum));
            cubicFeatureToFunction[var] = numberOfFunctions;
            numberOfFunctions++;
        }
    }

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (_features.test(var) && !_categoricalFeatures.test(var))
        {
            _compiler->addFunction(new Function({var}, Operation::quartic_sum));
            quarticFeatureToFunction[var] = numberOfFunctions;
            numberOfFunctions++;
        }
    }

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (!_features[var])
            continue;

        Query* linear = new Query();
        Aggregate* agg_linear = new Aggregate();

        prod_bitset prod_linear;

        // add linear
        if (!_categoricalFeatures[var])
            prod_linear.set(featureToFunction[var]);
        else
            linear->_fVars.set(var);

        agg_linear->_agg.push_back(prod_linear);
        // agg_linear->_m[0] = 1;

        linear->_rootID = _queryRootIndex[var];
        linear->_aggregates.push_back(agg_linear);
        _compiler->addQuery(linear);

        /********** PRINT OUT ********/
        // std::cout << _td->getAttribute(var)->_name << std::endl;
        // std::cout << linear->_rootID << std::endl;
        /********** PRINT OUT ********/
    }

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (!_features[var])
            continue;

        if (!_categoricalFeatures[var])
        {
            // add quad
            Query* quad = new Query();
            Aggregate* agg_quad = new Aggregate();
            prod_bitset prod_quad;

            // add quad
            prod_quad.set(quadFeatureToFunction[var]);

            agg_quad->_agg.push_back(prod_quad);
            // agg_quad->_m[0] = 1;

            quad->_rootID = _queryRootIndex[var];
            quad->_aggregates.push_back(agg_quad);
            _compiler->addQuery(quad);

            /********** PRINT OUT ********/
            // std::cout << _td->getAttribute(var)->_name + "*" +
            //     _td->getAttribute(var)->_name  << std::endl;
            // std::cout << quad->_rootID << std::endl;
            /********** PRINT OUT ********/
        }

        for (size_t var2 = var+1; var2 < NUM_OF_VARIABLES; ++var2)
        {
            if (!_features[var2])
                continue;

            // add combo linear, linear
            Query* combo = new Query();
            Aggregate* agg_combo = new Aggregate();
            prod_bitset prod_combo_v1;

            // add combo
            if (!_categoricalFeatures[var])
                prod_combo_v1.set(featureToFunction[var]);
            else
                combo->_fVars.set(var);

            combo->_rootID = _queryRootIndex[var];

            if (!_categoricalFeatures[var2])
                prod_combo_v1.set(featureToFunction[var2]);
            else
            {
                combo->_fVars.set(var2);
                combo->_rootID = _queryRootIndex[var2];
            }

            agg_combo->_agg.push_back(prod_combo_v1);
            // agg_combo->_m[0] = 1;

            combo->_aggregates.push_back(agg_combo);
            _compiler->addQuery(combo);

            /********** PRINT OUT ********/
            // std::cout << _td->getAttribute(var)->_name + "*" +
            //     _td->getAttribute(var2)->_name  << std::endl;
            // std::cout << combo->_rootID << std::endl;
            /********** PRINT OUT ********/
        }
    }

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (!_features[var])
            continue;

        if (!_categoricalFeatures[var])
        {
            // add cubic (if degree 2)
            Query* cubic = new Query();
            Aggregate* agg_cubic = new Aggregate();
            prod_bitset prod_cubic_v1;

            // add cubic
            prod_cubic_v1.set(cubicFeatureToFunction[var]);

            agg_cubic->_agg.push_back(prod_cubic_v1);
            // agg_cubic->_m[0] = 1;

            cubic->_rootID = _queryRootIndex[var];
            cubic->_aggregates.push_back(agg_cubic);
            _compiler->addQuery(cubic);

            /********** PRINT OUT ********/
            // std::cout << "cubic: "<< _td->getAttribute(var)->_name + "*"
            //     +_td->getAttribute(var)->_name + "*" +
            //     _td->getAttribute(var)->_name << std::endl;
            // std::cout << cubic->_rootID << std::endl;
            /********** PRINT OUT ********/

            for (size_t var2 = var+1; var2 < NUM_OF_VARIABLES; ++var2)
            {
                if (!_features[var2] || _categoricalFeatures[var2])
                    continue;

                // add combo quad, linear (if degree 2)
                Query* combo = new Query();
                Aggregate* agg_combo = new Aggregate();
                prod_bitset prod_combo_v1;

                // add quad feature
                prod_combo_v1.set(quadFeatureToFunction[var]);
                combo->_rootID = _queryRootIndex[var];

                if (!_categoricalFeatures[var2])
                    prod_combo_v1.set(featureToFunction[var2]);
                else
                {
                    combo->_fVars.set(var2);
                    combo->_rootID = _queryRootIndex[var2];
                }

                agg_combo->_agg.push_back(prod_combo_v1);
                // agg_combo->_m[0] = 1;

                combo->_aggregates.push_back(agg_combo);
                _compiler->addQuery(combo);

                /********** PRINT OUT ********/
                // std::cout << _td->getAttribute(var)->_name + "*" +
                //     _td->getAttribute(var)->_name + "*" +
                //     _td->getAttribute(var2)->_name << std::endl;
                // std::cout << combo->_rootID << std::endl;
                /********** PRINT OUT ********/
            }
        }

        for (size_t var2 = var+1; var2 < NUM_OF_VARIABLES; ++var2)
        {
            if (!_features[var2])
                continue;

            if (!_categoricalFeatures[var2])
            {
                // add combo of linear, quad
                Query* combo = new Query();
                Aggregate* agg_combo = new Aggregate();
                prod_bitset prod_combo_v1;

                // add linear
                if (!_categoricalFeatures[var])
                    prod_combo_v1.set(featureToFunction[var]);
                else
                    combo->_fVars.set(var);

                combo->_rootID = _queryRootIndex[var];

                // add combo
                prod_combo_v1.set(quadFeatureToFunction[var2]);

                agg_combo->_agg.push_back(prod_combo_v1);
                // agg_combo->_m[0] = 1;

                combo->_aggregates.push_back(agg_combo);
                _compiler->addQuery(combo);

                /********** PRINT OUT ********/
                // std::cout << _td->getAttribute(var)->_name + "*" +
                //     _td->getAttribute(var2)->_name + "*" +
                //     _td->getAttribute(var2)->_name << std::endl;
                // std::cout << combo->_rootID << std::endl;
                /********** PRINT OUT ********/
            }

            // add combinations of linear, linear, linear
            for (size_t var3 = var2+1; var3 < NUM_OF_VARIABLES; ++var3)
            {
                if (!_features[var3])
                    continue;

                // add combo of linear, linear, linear
                Query* combo = new Query();
                Aggregate* agg_combo = new Aggregate();
                prod_bitset prod_combo_v1;

                // add linear
                if (!_categoricalFeatures[var])
                    prod_combo_v1.set(featureToFunction[var]);
                else
                    combo->_fVars.set(var);

                combo->_rootID = _queryRootIndex[var];

                // add linear
                if (!_categoricalFeatures[var2])
                    prod_combo_v1.set(featureToFunction[var2]);
                else
                {
                    combo->_fVars.set(var2);
                    combo->_rootID = _queryRootIndex[var2];
                }

                // add linear
                if (!_categoricalFeatures[var3])
                    prod_combo_v1.set(featureToFunction[var3]);
                else
                {
                    combo->_fVars.set(var3);
                    combo->_rootID = _queryRootIndex[var3];
                }

                agg_combo->_agg.push_back(prod_combo_v1);
                // agg_combo->_m[0] = 1;

                combo->_aggregates.push_back(agg_combo);
                _compiler->addQuery(combo);

                /********** PRINT OUT ********/
                // std::cout << _td->getAttribute(var)->_name + "*" +
                //     _td->getAttribute(var2)->_name + "*" +
                //     _td->getAttribute(var3)->_name << std::endl;
                // std::cout << combo->_rootID << std::endl;
                /********** PRINT OUT ********/
            }
        }
    }

    // do the same again for each combo of variables ...
    for (size_t var = 0; var < NUM_OF_VARIABLES; var++)
    {
        if (!_features[var])
            continue;

        if(!_categoricalFeatures[var])
        {

            // add quartic (if degree 2)
            Query* quartic = new Query();
            Aggregate* agg_quartic = new Aggregate();
            prod_bitset prod_quartic_v1;

            // add quartic
            prod_quartic_v1.set(quarticFeatureToFunction[var]);

            agg_quartic->_agg.push_back(prod_quartic_v1);
            // agg_quartic->_m[0] = 1;

            quartic->_rootID = _queryRootIndex[var];
            quartic->_aggregates.push_back(agg_quartic);
            _compiler->addQuery(quartic);

            /********** PRINT OUT ********/
            std::cout << _td->getAttribute(var)->_name + "*" +
                _td->getAttribute(var)->_name + "*" +_td->getAttribute(var)->_name +
                "*" +_td->getAttribute(var)->_name << std::endl;
            // std::cout << quartic->_rootID << std::endl;
            /********** PRINT OUT ********/

            // add combo cubic and linear (if degree 2)
            for (size_t var2 = var+1; var2 < NUM_OF_VARIABLES; ++var2)
            {
                if (!_features[var2] || _categoricalFeatures[var2])
                    continue;

                // add combo cubic, linear (if degree 2)
                Query* combo = new Query();
                Aggregate* agg_combo = new Aggregate();
                prod_bitset prod_combo_v1;

                // add cubic feature
                prod_combo_v1.set(cubicFeatureToFunction[var]);
                combo->_rootID = _queryRootIndex[var];

                if (!_categoricalFeatures[var2])
                    prod_combo_v1.set(featureToFunction[var2]);
                else
                {
                    combo->_fVars.set(var2);
                    combo->_rootID = _queryRootIndex[var2];
                }

                agg_combo->_agg.push_back(prod_combo_v1);
                // agg_combo->_m[0] = 1;

                combo->_aggregates.push_back(agg_combo);
                _compiler->addQuery(combo);

                /********** PRINT OUT ********/
                // std::cout << _td->getAttribute(var)->_name + "*" +
                //   _td->getAttribute(var)->_name + "*" +_td->getAttribute(var)->_name+
                //     "*" + _td->getAttribute(var2)->_name << std::endl;
                // std::cout << combo->_rootID << std::endl;
                /********** PRINT OUT ********/

            }

            for (size_t var2 = var+1; var2 < NUM_OF_VARIABLES; ++var2)
            {
                if (!_features[var2])
                    continue;

                if (!_categoricalFeatures[var2])
                {
                    // add combo of quad, quad
                    Query* combo = new Query();
                    Aggregate* agg_combo = new Aggregate();
                    prod_bitset prod_combo_v1;

                    // add quad
                    prod_combo_v1.set(quadFeatureToFunction[var]);
                    combo->_rootID = _queryRootIndex[var];

                    // add quad
                    prod_combo_v1.set(quadFeatureToFunction[var2]);

                    if ( _queryRootIndex[var] != _queryRootIndex[var2])
                    {
                        std::cout << "there is a mistake here \n";
                        exit(0);
                    }

                    agg_combo->_agg.push_back(prod_combo_v1);
                    // agg_combo->_m[0] = 1;

                    combo->_aggregates.push_back(agg_combo);
                    _compiler->addQuery(combo);

                    /********** PRINT OUT ********/
                    // std::cout << _td->getAttribute(var)->_name + "*" +
                    //     _td->getAttribute(var)->_name + "*" +
                    //     _td->getAttribute(var2)->_name + "*" <<
                    //     _td->getAttribute(var2)->_name << std::endl;
                    // std::cout << combo->_rootID << std::endl;
                    /********** PRINT OUT ********/

                }

                // add combinations of quad, linear, linear
                for (size_t var3 = var2+1; var3 < NUM_OF_VARIABLES; ++var3)
                {
                    if (!_features[var3])
                        continue;

                    // add combo of quad, linear, linear
                    Query* combo = new Query();
                    Aggregate* agg_combo = new Aggregate();
                    prod_bitset prod_combo_v1;

                    // add quad
                    prod_combo_v1.set(quadFeatureToFunction[var]);
                    combo->_rootID = _queryRootIndex[var];

                    // add linear
                    if (!_categoricalFeatures[var2])
                        prod_combo_v1.set(featureToFunction[var2]);
                    else
                    {
                        combo->_fVars.set(var2);
                        combo->_rootID = _queryRootIndex[var2];
                    }

                    // add linear
                    if (!_categoricalFeatures[var3])
                        prod_combo_v1.set(featureToFunction[var3]);
                    else
                    {
                        combo->_fVars.set(var3);
                        combo->_rootID = _queryRootIndex[var3];
                    }

                    agg_combo->_agg.push_back(prod_combo_v1);
                    // agg_combo->_m[0] = 1;

                    combo->_aggregates.push_back(agg_combo);
                    _compiler->addQuery(combo);

                    /********** PRINT OUT ********/
                    // std::cout << _td->getAttribute(var)->_name + "*" +
                    //     _td->getAttribute(var)->_name + "*" +
                    //     _td->getAttribute(var2)->_name + "*" <<
                    //     _td->getAttribute(var3)->_name << std::endl;
                    // std::cout << combo->_rootID << std::endl;
                    /********** PRINT OUT ********/
                }
            }
        }

        // add combo linear and cubic (if degree 2)
        for (size_t var2 = var+1; var2 < NUM_OF_VARIABLES; ++var2)
        {
            if (!_features[var2] || _categoricalFeatures[var2])
                continue;

            // add combo linear, cubic (if degree 2)
            Query* combo = new Query();
            Aggregate* agg_combo = new Aggregate();
            prod_bitset prod_combo_v1;

            if (!_categoricalFeatures[var])
                prod_combo_v1.set(featureToFunction[var]);
            else
            {
                combo->_fVars.set(var);
            }
            combo->_rootID = _queryRootIndex[var];

            // add cubic feature
            prod_combo_v1.set(cubicFeatureToFunction[var2]);

            agg_combo->_agg.push_back(prod_combo_v1);
            // agg_combo->_m[0] = 1;

            combo->_aggregates.push_back(agg_combo);
            _compiler->addQuery(combo);

            /********** PRINT OUT ********/
            // std::cout << _td->getAttribute(var)->_name + "*" +
            //     _td->getAttribute(var2)->_name + "*" +_td->getAttribute(var2)->_name +
            //     "*" << _td->getAttribute(var2)->_name << std::endl;
            // std::cout << combo->_rootID << std::endl;
            /********** PRINT OUT ********/

            // for (size_t var2 = var+1; var2 < NUM_OF_VARIABLES; ++var2)
            // {
            //     if (!_features[var2])
            //         continue;

            //     if (!_categoricalFeatures[var2])
            //     {
            //         // add combo of quad, quad
            //         Query* combo = new Query();
            //         Aggregate* agg_combo = new Aggregate(1);
            //         // add quad
            //         prod_combo_v1.set(quadFeatureToFunction[var]);
            //         combo->_rootID = _queryRootIndex[var];
            //         // add quad
            //         prod_combo_v1.set(quadFeatureToFunction[var2]);
            //         agg_combo->_agg.push_back(prod_combo_v1);
            //         agg_combo->_m[0] = 1;
            //         combo->_aggregates.push_back(agg_combo);
            //         _compiler->addQuery(combo);
            //     }

            // add combinations of  linear, quad, linear
            for (size_t var3 = var2+1; var3 < NUM_OF_VARIABLES; ++var3)
            {
                if (!_features[var3])
                    continue;

                // add combo of linear, quad, linear
                Query* combo = new Query();
                Aggregate* agg_combo = new Aggregate();
                prod_bitset prod_combo_v1;

                // add linear
                if (!_categoricalFeatures[var])
                    prod_combo_v1.set(featureToFunction[var]);
                else
                    combo->_fVars.set(var);

                combo->_rootID = _queryRootIndex[var];

                // add quad
                prod_combo_v1.set(quadFeatureToFunction[var2]);

                // add linear
                if (!_categoricalFeatures[var3])
                    prod_combo_v1.set(featureToFunction[var3]);
                else
                {
                    combo->_fVars.set(var3);
                    combo->_rootID = _queryRootIndex[var3];
                }

                agg_combo->_agg.push_back(prod_combo_v1);
                // agg_combo->_m[0] = 1;

                combo->_aggregates.push_back(agg_combo);
                _compiler->addQuery(combo);

                /********** PRINT OUT ********/
                // std::cout << _td->getAttribute(var)->_name + "*" +
                //     _td->getAttribute(var2)->_name + "*" +
                //     _td->getAttribute(var2)->_name + "*" <<
                //     _td->getAttribute(var3)->_name << std::endl;
                // std::cout << combo->_rootID << std::endl;
                /********** PRINT OUT ********/
            }

            for (size_t var3 = var2+1; var3 < NUM_OF_VARIABLES; ++var3)
            {
                if (!_features[var3])
                    continue;

                // add combinations of linear, linear, quad
                if (!_categoricalFeatures[var3])
                {
                    // add combo of linear, linear, quad
                    Query* combo = new Query();
                    Aggregate* agg_combo = new Aggregate();
                    prod_bitset prod_combo_v1;

                    // add linear
                    if (!_categoricalFeatures[var])
                        prod_combo_v1.set(featureToFunction[var]);
                    else
                        combo->_fVars.set(var);

                    combo->_rootID = _queryRootIndex[var];

                    // add linear
                    if (!_categoricalFeatures[var2])
                        prod_combo_v1.set(featureToFunction[var2]);
                    else
                    {
                        combo->_fVars.set(var2);
                        combo->_rootID = _queryRootIndex[var2];
                    }

                    // add quad
                    prod_combo_v1.set(quadFeatureToFunction[var3]);

                    agg_combo->_agg.push_back(prod_combo_v1);
                    // agg_combo->_m[0] = 1;

                    combo->_aggregates.push_back(agg_combo);
                    _compiler->addQuery(combo);

                    /********** PRINT OUT ********/
                    // std::cout << _td->getAttribute(var)->_name + "*" +
                    //     _td->getAttribute(var2)->_name + "*" +
                    //     _td->getAttribute(var3)->_name + "*" <<
                    //     _td->getAttribute(var3)->_name << std::endl;
                    // std::cout << combo->_rootID << std::endl;
                    /********** PRINT OUT ********/
                }

                // add combo linear, linear, linear, linear
                for (size_t var4 = var3+1; var4 < NUM_OF_VARIABLES; ++var4)
                {
                    if (!_features[var4])
                        continue;

                    // add combo of linear, linear, linear, linear
                    Query* combo = new Query();
                    Aggregate* agg_combo = new Aggregate();
                    prod_bitset prod_combo_v1;

                    // add linear
                    if (!_categoricalFeatures[var])
                        prod_combo_v1.set(featureToFunction[var]);
                    else
                        combo->_fVars.set(var);

                    combo->_rootID = _queryRootIndex[var];

                    // add linear
                    if (!_categoricalFeatures[var2])
                        prod_combo_v1.set(featureToFunction[var2]);
                    else
                    {
                        combo->_fVars.set(var2);
                        combo->_rootID = _queryRootIndex[var2];
                    }

                    // add linear
                    if (!_categoricalFeatures[var3])
                        prod_combo_v1.set(featureToFunction[var3]);
                    else
                    {
                        combo->_fVars.set(var3);
                        combo->_rootID = _queryRootIndex[var3];
                    }

                    // add linear
                    if (!_categoricalFeatures[var4])
                        prod_combo_v1.set(featureToFunction[var4]);
                    else
                    {
                        combo->_fVars.set(var4);
                        combo->_rootID = _queryRootIndex[var4];
                    }

                    agg_combo->_agg.push_back(prod_combo_v1);
                    // agg_combo->_m[0] = 1;

                    combo->_aggregates.push_back(agg_combo);
                    _compiler->addQuery(combo);

                    /********** PRINT OUT ********/
                    // std::cout << _td->getAttribute(var)->_name + "*" +
                    //     _td->getAttribute(var2)->_name + "*" +
                    //     _td->getAttribute(var3)->_name + "*" <<
                    //     _td->getAttribute(var4)->_name << std::endl;
                    // std::cout << combo->_rootID << std::endl;
                    /********** PRINT OUT ********/

                }
            }
        }
    }

#endif

    // for (size_t q = 0; q < _compiler->numberOfQueries(); q++)
    // {
    //     Query* query = _compiler->getQuery(q);

    //     if (query->_fVars.none())
    //     {
    //         std::cout << q << " : " << std::to_string(query->_rootID) << std::endl;
    //     }
    // }

    // exit(1);

}

void LinearRegression::loadFeatures()
{
    _queryRootIndex = new size_t[NUM_OF_VARIABLES]();

    /* Load the two-pass variables config file into an input stream. */
    ifstream input(FEATURE_CONF);

    if (!input)
    {
        ERROR(FEATURE_CONF+" does not exist. \n");
        exit(1);
    }

    /* String and associated stream to receive lines from the file. */
    string line;
    stringstream ssLine;

    int numOfFeatures = 0;
    int degreeOfInteractions = 0;

    /* Ignore comment and empty lines at the top */
    while (getline(input, line))
    {
        if (line[0] == COMMENT_CHAR || line == "")
            continue;

        break;
    }

    /*
     * Extract number of labels, features and interactions from the config.
     * Parse the line with the three numbers; ignore spaces.
     */
    bool parsingSuccess =
        qi::phrase_parse(line.begin(), line.end(),

                         /* Begin Boost Spirit grammar. */
                         ((qi::int_[phoenix::ref(numOfFeatures) = qi::_1])
                          >> NUMBER_SEPARATOR_CHAR
                          >> (qi::int_[phoenix::ref(degreeOfInteractions) =
                                       qi::_1])),
                         /*  End grammar. */
                         ascii::space);

    assert(parsingSuccess && "The parsing of the features file has failed.");

    /* Read in the features. */
    for (int featureNo = 0; featureNo < numOfFeatures; ++featureNo)
    {
        getline(input, line);
        if (line[0] == COMMENT_CHAR || line == "")
        {
            --featureNo;
            continue;
        }

        ssLine << line;

        string attrName;
        /* Extract the name of the attribute in the current line. */
        getline(ssLine, attrName, ATTRIBUTE_NAME_CHAR);

        string typeOfFeature;
        /* Extract the dimension of the current attribute. */
        getline(ssLine, typeOfFeature, ATTRIBUTE_NAME_CHAR);

        string rootName;
        /* Extract the dimension of the current attribute. */
        getline(ssLine, rootName, ATTRIBUTE_NAME_CHAR);

        int attributeID = _td->getAttributeIndex(attrName);
        int categorical = stoi(typeOfFeature);
        int rootID = _td->getRelationIndex(rootName);

        if (attributeID == -1)
        {
            ERROR("Attribute |"+attrName+"| does not exist.");
            exit(1);
        }

        if (rootID == -1)
        {
            ERROR("Relation |"+rootName+"| does not exist.");
            exit(1);
        }

        if (featureNo == 0 && categorical == 1)
        {
            ERROR("The label needs to be continuous! ");
            exit(1);
        }

        _isFeature.set(attributeID);
        _features.set(attributeID);

        Feature f;

        if (categorical)
        {
            _categoricalFeatures.set(attributeID);
            _queryRootIndex[attributeID] = rootID;

            f.head.set(attributeID);
            _isCategoricalFeature.set(attributeID);
        }
        else
        {
            _queryRootIndex[attributeID] = _td->_root->_id;
            ++f.body[attributeID];
        }

        _listOfFeatures.push_back(f);

        if (featureNo == 0)
            labelID = attributeID;

        /* Clear string stream. */
        ssLine.clear();
    }
}

// TODO: Can we ensure that the continuous parameters are in the order in which
// they are needed for the computation for continuous aggregates?

std::string LinearRegression::generateParameters()
{
    _parameterIndex.resize(NUM_OF_VARIABLES+1);

    std::string constructParam = offset(1)+"size_t numberOfParameters;\n"+
        offset(1)+"double stepSize, error, *params = nullptr, *grad = nullptr, "+
        "*lambda = nullptr, *prevParams = nullptr, *prevGrad = nullptr;\n";

    std::string constructGrad = "";

    std::string numOfParameters = "numberOfParameters = ";
    size_t numOfContParameters = 1;

    // Computing the number of parameters we have
    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (!_features.test(var))
            continue;
        if (!_categoricalFeatures.test(var))
        {
            _parameterIndex[var] = numOfContParameters;
            ++numOfContParameters;
        }
        else
        {
            size_t& viewID = parameterQueries[var]->_aggregates[0]->_incoming[0].first;
            numOfParameters += "V"+std::to_string(viewID)+".size() + ";
        }
    }
    _parameterIndex[NUM_OF_VARIABLES] = 0;
    numOfParameters += std::to_string(numOfContParameters+1)+";\n";

    std::string initParam = offset(2)+numOfParameters+
        offset(2)+"params = new double[numberOfParameters];\n"+
        offset(2)+"grad = new double[numberOfParameters]();\n"+
        offset(2)+"prevParams = new double[numberOfParameters];\n"+
        offset(2)+"prevGrad = new double[numberOfParameters];\n"+
        offset(2)+"lambda = new double[numberOfParameters];\n";

    // TODO: make sure you initialize the params and grad arrays
    if (_categoricalFeatures.any())
        initParam += offset(2)+"size_t categIndex = "+
            std::to_string(numOfContParameters+1)+";\n";

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (!_features.test(var))
            continue;

        if (_categoricalFeatures.test(var))
        {
            size_t& viewID = parameterQueries[var]->_aggregates[0]->_incoming[0].first;

            constructGrad += offset(1)+"std::unordered_map<int,size_t> index_"+
                getAttributeName(var)+";\n";

            initParam += offset(2)+"index_"+getAttributeName(var)+".reserve(V"+
                std::to_string(viewID)+".size());\n";
            // offset(2)+"firstindex_"+getAttributeName(var)+" = categIndex;\n";

            initParam += offset(2)+"for (V"+std::to_string(viewID)+
                "_tuple& tuple : V"+
                std::to_string(viewID)+")\n"+
                offset(3)+"index_"+getAttributeName(var)+
                "[tuple."+getAttributeName(var)+"] = categIndex++;\n";
        }
    }

    initParam += offset(2)+"for(size_t j = 0; j < numberOfParameters; ++j)\n"+
        offset(2)+"{\n"+
        offset(3)+"params[j] = ((double) (rand() % 800 + 1) - 400) / 100;\n"+
        offset(3)+"lambda[j] = 0.1;\n"+
        offset(2)+"}\n"+
        offset(2)+"params["+std::to_string(_parameterIndex[labelID])+"] = -1;\n"+
        offset(2)+"lambda[0] = 0.0;\n";

    std::string outString = constructParam+constructGrad+"\n"+
        offset(1)+"void initParametersGradients()\n"+
        offset(1)+"{\n"+initParam+offset(1)+"}\n";

    return outString;
}

struct ActualGradient
{
    size_t aggNum;
    std::vector<std::pair<size_t,size_t>> incVars;
};

std::string LinearRegression::getAttributeName(size_t attID)
{
    if (attID == NUM_OF_VARIABLES)
        return "int";
    return _td->getAttribute(attID)->_name;
}

std::string LinearRegression::generateGradients()
{
    size_t numberOfViews = _compiler->numberOfViews();

    std::vector<std::vector<ActualGradient>> actualGradients(numberOfViews);

    for(PartialGradient& pGrad : partialGradients)
    {
        // get view and aggregate corresponding to the query
        std::pair<size_t,size_t>& viewAggPair =
            pGrad.query->_aggregates[0]->_incoming[0];

        // Assign to view, the aggregate and the corresponding param, grad pair
        ActualGradient ag;
        ag.aggNum = viewAggPair.second;
        ag.incVars = pGrad.inputVariables;

        actualGradients[viewAggPair.first].push_back(ag);
    }

    // TODO: *Observations:* the view with all continuous aggregates only has one tuple
    // Ideally, we would have the corresponding params in an array so this
    // computation can be vectorized ! Ideally also the gradients should be a vector !

    std::string gradString =  offset(1)+"void computeGradient()\n"+offset(1)+"{\n"+
        offset(2)+"memset(grad, 0, numberOfParameters * "+
        std::to_string(sizeof(double))+");\n";

    std::string errorString =  offset(1)+"void computeError()\n"+offset(1)+"{\n"+
        offset(2)+"error = 0.0;\n";

    for (size_t viewID = 0; viewID < numberOfViews; ++viewID)
    {
        // TODO: check if this view has at least one free variable
        // If it does not then avoid the for loop, get the tuple reference and
        // adjust the offset
        bool viewHasFreeVars = _compiler->getView(viewID)->_fVars.any();
        size_t off = 3;

        if (!actualGradients[viewID].empty())
        {
            if (viewHasFreeVars)
            {
                gradString += offset(2)+"for (V"+std::to_string(viewID)+
                    "_tuple& tuple : V"+std::to_string(viewID)+")\n"+offset(2)+"{\n";
                errorString += offset(2)+"for (V"+std::to_string(viewID)+
                    "_tuple& tuple : V"+std::to_string(viewID)+")\n"+offset(2)+"{\n";
            }
            else
            {
                gradString += offset(2)+"V"+std::to_string(viewID)+
                    "_tuple& tuple = V"+std::to_string(viewID)+"[0];\n";
                errorString += offset(2)+"V"+std::to_string(viewID)+
                    "_tuple& tuple = V"+std::to_string(viewID)+"[0];\n";
                off = 2;
            }
        }

        for (ActualGradient& ag : actualGradients[viewID])
        {
            size_t prevGradID = 1000;
            size_t prevParamID = 1000;

            // TODO: TODO: We go at great length to avoid adding errors twice
            // here, this could surely be simplified by adding each combination
            // to actualGradients only once!

            for (size_t i = 0; i < ag.incVars.size(); i++)
            {
                size_t& gradID = ag.incVars[i].first;
                size_t& paramID = ag.incVars[i].second;

                if (gradID == labelID)
                    continue;

                if (prevGradID != paramID || prevParamID != gradID)
                    errorString += offset(off)+ "error += ";

                if (gradID == paramID &&
                    (prevGradID != paramID || prevParamID != gradID))
                    errorString += "0.5 * ";

                if (_categoricalFeatures[gradID])
                {
                    gradString += offset(off)+"grad[index_"+getAttributeName(gradID)+
                        "[tuple."+getAttributeName(gradID)+"]] += ";

                    if (prevGradID != paramID || prevParamID != gradID)
                        errorString += "params[index_"+getAttributeName(gradID)+
                            "[tuple."+getAttributeName(gradID)+"]] * ";
                }
                else
                {
                    gradString += offset(off)+"grad["+
                        std::to_string(_parameterIndex[gradID])+"] += ";

                    if (prevGradID != paramID || prevParamID != gradID)
                        errorString += "params["+
                            std::to_string(_parameterIndex[gradID])+"] * ";
                }

                gradString += "tuple.aggregates["+std::to_string(ag.aggNum)+"] * ";

                if (prevGradID != paramID || prevParamID != gradID)
                    errorString += "tuple.aggregates["+std::to_string(ag.aggNum)+"] * ";

                if (_categoricalFeatures[paramID])
                {
                    gradString += "params[index_"+getAttributeName(paramID)+
                        "[tuple."+getAttributeName(paramID)+"]];\n";

                    if (prevGradID != paramID || prevParamID != gradID)
                        errorString += "params[index_"+getAttributeName(paramID)+
                            "[tuple."+getAttributeName(paramID)+"]];\n";
                }
                else
                {
                    // gradString += "param_"+getAttributeName(paramID)+";\n";
                    gradString += "params["+
                        std::to_string(_parameterIndex[paramID])+"];\n";

                    if (prevGradID != paramID || prevParamID != gradID)
                        errorString += "params["+
                            std::to_string(_parameterIndex[paramID])+"];\n";
                }

                prevGradID = gradID;
                prevParamID = paramID;
            }
        }
        if (!actualGradients[viewID].empty() && viewHasFreeVars)
        {
            gradString += offset(2)+"}\n";
            errorString += offset(2)+"}\n";
        }

    }
    gradString += offset(2)+"for (size_t j = 0; j < numberOfParameters; ++j)\n"+
        offset(3)+"grad[j] /= tuple.aggregates[0];\n"+
        offset(1)+"}\n\n";

    std::string labelStr = std::to_string(_parameterIndex[labelID]);

    errorString +=  offset(2)+"error /= tuple.aggregates[0];\n\n"+
        offset(2)+"double paramNorm = 0.0;\n"+
        offset(2)+"/* Adding the regulariser to the error */\n"+
        offset(2)+"for (size_t j = 1; j < numberOfParameters; ++j)\n"+
        offset(3)+"paramNorm += params[j] * params[j];\n"+
        offset(2)+"paramNorm -= params["+labelStr+"] * params["+labelStr+"];\n\n"+
        offset(2)+"error += lambda[1] * paramNorm;\n"+
        offset(1)+"}\n";

    return gradString + errorString;
}

inline std::string LinearRegression::offset(size_t off)
{
    return std::string(off*3, ' ');
}

std::string LinearRegression::typeToStr(Type t)
{
    switch(t)
    {
    case Type::Integer : return "int";
    case Type::Double : return "double";
    case Type::Short : return "short";
    case Type::U_Integer : return "size_t";
    default :
        ERROR("This type does not exist \n");
        exit(1);
    }
}

void LinearRegression::generateCode()
{
    // This is necessary here, because it constructs the firstEntry array used below
    std::string parameterGeneration = generateParameters();

    std::string stepSizeFunction = offset(1)+"inline double computeStepSize()\n"+
        offset(1)+"{\n"+
        offset(2)+"double DSS = 0.0, GSS = 0.0, DGS = 0.0, paramDiff, gradDiff;\n"+
        offset(2)+"for(size_t j = 0; j < numberOfParameters; ++j)\n"+offset(2)+"{\n"+
        offset(3)+"if(j == "+std::to_string(_parameterIndex[labelID])+") continue;\n\n"+
        offset(3)+"paramDiff = params[j] - prevParams[j];\n"+
        offset(3)+"gradDiff = grad[j] - prevGrad[j];\n\n"+
        offset(3)+"DSS += paramDiff * paramDiff;\n"+
        offset(3)+"GSS += gradDiff * gradDiff;\n"+
        offset(3)+"DGS += paramDiff * gradDiff;\n"+
        offset(2)+"}\n"+
        offset(2)+"if (DGS == 0.0 || GSS == 0.0) return stepSize;\n"+
        offset(2)+"double Ts = DSS / DGS;\n"+
        offset(2)+"double Tm = DGS / GSS;\n"+
        offset(2)+"if (Tm < 0.0 || Ts < 0.0) return stepSize;\n"+
        offset(2)+"return (Tm / Ts > 0.5)? Tm : Ts - 0.5 * Tm;\n"+
        offset(1)+"}\n";

    std::string runFunction = offset(1)+"void runApplication()\n"+offset(1)+"{\n"+
         offset(2)+"int64_t startProcess = duration_cast<milliseconds>("+
        "system_clock::now().time_since_epoch()).count();\n"+
        offset(2)+"initParametersGradients();\n"+
        generateConvergenceLoop()+
        "\n"+offset(2)+"int64_t endProcess = duration_cast<milliseconds>("+
        "system_clock::now().time_since_epoch()).count()-startProcess;\n"+
        offset(2)+"std::cout << \"Run Application: \"+"+
        "std::to_string(endProcess)+\"ms.\\n\";\n\n"+
        offset(2)+"std::ofstream ofs(\"times.txt\",std::ofstream::out | " +
        "std::ofstream::app);\n"+
        offset(2)+"ofs << \"\\t\" << endProcess << std::endl;\n"+
        offset(2)+"ofs.close();\n\n"+
        offset(2)+"printOutput();\n"+
        offset(2)+"std::cout << \"numberOfIterations: \" << iteration << \"\\n\";\n"+
        offset(2)+"delete[] update;\n"+
        offset(2)+"evaluateModel();\n"+
        offset(1)+"}\n";

    std::ofstream ofs(multifaq::dir::OUTPUT_DIRECTORY+"ApplicationHandler.h", std::ofstream::out);
    ofs << "#ifndef INCLUDE_APPLICATIONHANDLER_HPP_\n"<<
        "#define INCLUDE_APPLICATIONHANDLER_HPP_\n\n"<<
        "#include \"DataHandler.h\"\n\n"<<
        "namespace lmfao\n{\n"<<
        offset(1)+"void runApplication();\n"<<
        "}\n\n#endif /* INCLUDE_APPLICATIONHANDLER_HPP_*/\n";
    ofs.close();

    ofs.open(multifaq::dir::OUTPUT_DIRECTORY+"ApplicationHandler.cpp", std::ofstream::out);
    ofs << "#include \"ApplicationHandler.h\"\n"
        << "#include <boost/spirit/include/qi.hpp>\n"
        << "#include <boost/spirit/include/phoenix_operator.hpp>\n"
        << "#include <cmath>\n\n"
        << "namespace qi = boost::spirit::qi;\n"
        << "namespace phoenix = boost::phoenix;\n\n"
        << "namespace lmfao\n{\n";
    ofs << parameterGeneration << std::endl;
    ofs << generateGradients() << std::endl;
    ofs << generatePrintFunction() << std::endl;
    ofs << stepSizeFunction << std::endl;
    ofs << generateTestDataEvaluation() << std::endl;
    ofs << runFunction << std::endl;
    ofs << "}\n";
    ofs.close();
}

std::string LinearRegression::generateConvergenceLoop()
{
    std::string convergenceCheck =
        offset(3)+"/* Normalized residual stopping condition */\n"+
	offset(3)+"if (sqrt(gradientNorm) / (firstGradientNorm + 0.001) < 0.00001)\n"+
        offset(3)+"{\n"+
        // offset(4)+"converged = true;\n"+
        offset(4)+"std::cout << \"We have converged!! \\n\";\n"+
        offset(4)+"break;\n"+
        offset(3)+"}\n";

    std::string labelStr = std::to_string(_parameterIndex[labelID]);

    std::string updateParams = offset(3)+"gradientNorm = 0.0;\n"+
        offset(3)+"for (int j = 0; j < numberOfParameters; ++j)\n"+
        offset(3)+"{\n"+
        offset(4)+"update[j] = grad[j] + lambda[j] * params[j];\n"+
        offset(4)+"gradientNorm += update[j] * update[j];\n"+
        offset(4)+"prevParams[j] = params[j];\n"+
        offset(4)+"prevGrad[j] = grad[j];\n"+
        offset(4)+"params[j] = params[j] - stepSize * update[j];\n"+
        offset(3)+"}\n"+
        offset(3)+"params["+labelStr+"] = -1;\n"+
        offset(3)+"prevParams["+labelStr+"] = -1;\n"+
        offset(3)+"gradientNorm -= update["+labelStr+"] * update["+labelStr+"];\n\n"+
        offset(3)+"computeError();\n\n";

    std::string backtracking =
        offset(3)+"/* Backtracking Line Search: Decrease stepSize until condition"+
        " is satisfied */\n"+offset(3)+"backtrackingSteps = 0;\n"+
        offset(3)+"while(error > previousError - (stepSize/2) * gradientNorm &&"+
        " backtrackingSteps < 500)\n"+
        offset(3)+"{\n"+
        offset(4)+"stepSize /= 2;"+
        offset(4)+"// Update parameters based on the new stepSize.\n"+
        offset(4)+"for (int j = 0; j < numberOfParameters; ++j)\n"+
        offset(5)+"params[j] = prevParams[j] - stepSize * update[j];\n"+
        offset(4)+"params["+std::to_string(_parameterIndex[labelID])+"] = -1;\n"+
        offset(4)+"computeError();\n"+
        offset(4)+"++backtrackingSteps;\n"+
        offset(3)+"}\n";

    // TODO: TODO: TODO: SHOULD THE GRADIENT NORM INCLUDE THE REGULARIZER!!?!?
    return offset(2)+"double stepSize = 0.1, gradientNorm = 0.0, "+
        "*update = new double[numberOfParameters];\n"+
        offset(2)+"size_t iteration = 0, backtrackingSteps = 0;\n\n"+
        offset(2)+"computeGradient();\n"+
        offset(2)+"for (int j = 0; j < numberOfParameters; ++j)\n"+offset(2)+"{\n"+
        offset(3)+"update[j] = grad[j] + lambda[j] * params[j];\n"+
        offset(3)+"gradientNorm += update[j] * update[j];\n"+offset(2)+"}\n"+
        offset(2)+"gradientNorm -= update["+labelStr+"] * update["+labelStr+"];\n"+
        offset(2)+"double firstGradientNorm = sqrt(gradientNorm);\n\n"+
        offset(2)+"computeError();\n"+
        offset(2)+"double previousError = error;\n\n"+
        offset(2)+"do\n"+offset(2)+"{\n"+
        updateParams+backtracking+convergenceCheck+
        offset(3)+"computeGradient();\n"+
        offset(3)+"stepSize = computeStepSize();\n"+
        offset(3)+"previousError = error;\n"+
        offset(3)+"++iteration;\n"+
        offset(2)+"} while (iteration < 1000);\n\n";
}

std::string LinearRegression::generatePrintFunction()
{
    std::string printParam = offset(2)+"std::cout << \"param_int = \" << "+
        "params["+std::to_string(_parameterIndex[NUM_OF_VARIABLES])+"] << \";\\n\";\n";

    std::string printGrad = offset(2)+"std::cout << \"grad_int = \" << "+
        "grad["+std::to_string(_parameterIndex[NUM_OF_VARIABLES])+"] << \";\\n\";\n";

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (!_features.test(var))
            continue;

        if (!_categoricalFeatures.test(var))
        {
            printParam += offset(2)+"std::cout << \"param_"+getAttributeName(var)+" = "
                "\" << params["+std::to_string(_parameterIndex[var])+"] << \";\\n\";\n";
            printGrad += offset(2)+"std::cout << \"grad_"+getAttributeName(var)+" = "
                "\" << grad["+std::to_string(_parameterIndex[var])+"] <<\";\\n\";\n";
        }
        else
        {
            printParam += offset(2)+"for (std::pair<int,size_t> tuple : index_"+
                getAttributeName(var)+")\n"+
                offset(3)+"std::cout << \"param_"+getAttributeName(var)+
                "[\"<< tuple.first <<\"] = \"<< params[tuple.second] << \";\\n\";\n";
            printGrad += offset(2)+"for (std::pair<int,size_t> tuple : index_"+
                getAttributeName(var)+")\n"+
                offset(3)+"std::cout << \"grad_"+getAttributeName(var)+
                "[\"<< tuple.first <<\"] = \"<< grad[tuple.second] << \";\\n\";\n";
        }
    }

    return offset(1)+"void printOutput()\n"+offset(1)+"{\n"+printParam+"\n"+
        printGrad+offset(1)+"}\n";
}

std::string LinearRegression::generateTestDataEvaluation()
{
    // TODO: This should only output the features not all variables.
    // If changed it should be changed in the SQL generator as well.
    std::string attributeString = "",
        attrConstruct = offset(4)+"qi::phrase_parse(tuple.begin(),tuple.end(),";

    for (size_t var = 0; var < _td->numberOfAttributes(); ++var)
    {
        Attribute* att = _td->getAttribute(var);
        attrConstruct += "\n"+offset(5)+"qi::"+typeToStr(att->_type)+
            "_[phoenix::ref("+att->_name+") = qi::_1]>>";
        attributeString += offset(2)+typeToStr(att->_type) + " "+
            att->_name + ";\n";
    }

    // attrConstruct.pop_back();
    attrConstruct.pop_back();
    attrConstruct.pop_back();
    attrConstruct += ",\n"+offset(5)+"\'|\');\n";

    std::string testTuple = offset(1)+"struct Test_tuple\n"+offset(1)+"{\n"+
        attributeString+offset(2)+"Test_tuple(const std::string& tuple)\n"+
        offset(2)+"{\n"+attrConstruct+offset(2)+"}\n"+offset(1)+"};\n\n";

    std::string loadFunction = offset(1)+"void loadTestDataset("+
        "std::vector<Test_tuple>& TestDataset)\n"+offset(1)+"{\n"+
        offset(2)+"std::ifstream input;\n"+offset(2)+"std::string line;\n"+
        offset(2)+"input.open(PATH_TO_DATA + \"test_data.tbl\");\n"+
        offset(2)+"if (!input)\n"+offset(2)+"{\n"+
        offset(3)+"std::cerr << \"TestDataset.tbl does is not exist.\\n\";\n"+
        offset(3)+"exit(1);\n"+offset(2)+"}\n"+
        offset(2)+"while(getline(input, line))\n"+
        offset(3)+"TestDataset.push_back(Test_tuple(line));\n"+
        offset(2)+"input.close();\n"+offset(1)+"}\n\n";

    std::string prediction = "";
    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (!_features[var])
            continue;

        Attribute* att = _td->getAttribute(var);

        if (_categoricalFeatures[var])
            prediction += "params[index_"+att->_name+"[tuple."+att->_name+"]]+";
        else
            prediction += "params["+std::to_string(_parameterIndex[var])+
                "]*tuple."+att->_name+"+";
    }
    prediction.pop_back();

    std::string evalFunction = offset(1)+"void evaluateModel()\n"+offset(1)+"{\n"+
        offset(2)+"std::vector<Test_tuple> TestDataset;\n"+
        offset(2)+"loadTestDataset(TestDataset);\n"+
        offset(2)+"double diff, error = 0.0;\n"+
        offset(2)+"for (Test_tuple& tuple : TestDataset)\n"+offset(2)+"{\n"+
        offset(3)+"diff = params[0]+"+prediction+";\n"+
        offset(3)+"error += diff * diff;\n"+
        offset(2)+"}\n"+
        offset(2)+"error /= TestDataset.size();\n"+
        offset(2)+"std::cout << \"RMSE: \" << sqrt(error) << std::endl;\n"+
        offset(1)+"}\n";

    return testTuple + loadFunction + evalFunction;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#define NT2_UNIT_MODULE "boost.simd swar toolbox - comparator/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of swar components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 24/02/2011
///
#include <boost/simd/toolbox/swar/include/functions/comparator.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/simd/include/functions/all.hpp>
#include <boost/simd/include/functions/max.hpp>
#include <boost/simd/include/functions/min.hpp>

#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/real.hpp>
#include <boost/simd/include/constants/infinites.hpp>
#include <boost/simd/sdk/memory/is_aligned.hpp>
#include <boost/simd/sdk/memory/aligned_type.hpp>
#include <boost/simd/include/functions/load.hpp>

//COMMENTED
NT2_TEST_CASE_TPL ( comparator_real__3_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::comparator;
  using boost::simd::tag::comparator_;
  using boost::simd::load;
  using boost::simd::native;
  using boost::simd::meta::cardinal_of;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef typename boost::dispatch::meta::upgrade<T>::type   u_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                       ivT;
  typedef typename boost::dispatch::meta::call<comparator_(vT,vT,int)>::type r_t;
  typedef typename boost::dispatch::meta::call<comparator_(T,T,int)>::type sr_t;
  typedef typename boost::dispatch::meta::scalar_of<r_t>::type ssr_t;

//   // specific values tests
  typedef typename boost::dispatch::meta::strip<typename boost::fusion::result_of::at_c<r_t,0>::type>::type r_t0;
  typedef typename boost::dispatch::meta::strip<typename boost::fusion::result_of::at_c<r_t,1>::type>::type r_t1;
  typedef typename boost::dispatch::meta::strip<typename boost::fusion::result_of::at_c<r_t,2>::type>::type r_t2;
  {
    r_t res = comparator(boost::simd::Inf<vT>(), boost::simd::Inf<vT>(), 0);
    NT2_TEST_EQUAL( boost::fusion::get<0>(res), boost::simd::Inf<r_t0>());
    NT2_TEST_EQUAL( boost::fusion::get<1>(res), boost::simd::Inf<r_t1>());
    NT2_TEST_EQUAL( boost::fusion::get<2>(res), r_t2(0) );
  }
  {
    r_t res = comparator(boost::simd::Minf<vT>(), boost::simd::Minf<vT>(), 0);
    NT2_TEST_EQUAL( boost::fusion::get<0>(res), boost::simd::Minf<r_t0>());
    NT2_TEST_EQUAL( boost::fusion::get<1>(res), boost::simd::Minf<r_t0>());
    NT2_TEST_EQUAL( boost::fusion::get<2>(res), r_t2(0) );
  }
  {
    r_t res = comparator(boost::simd::Mone<vT>(), boost::simd::Mone<vT>(), 0);
    NT2_TEST_EQUAL( boost::fusion::get<0>(res), boost::simd::Mone<r_t0>());
    NT2_TEST_EQUAL( boost::fusion::get<1>(res), boost::simd::Mone<r_t0>());
    NT2_TEST_EQUAL( boost::fusion::get<2>(res), r_t2(0) );
  }
  {
    r_t res = comparator(boost::simd::Nan<vT>(), boost::simd::Nan<vT>(), 0);
    NT2_TEST_EQUAL( boost::fusion::get<0>(res), boost::simd::Nan<r_t0>());
    NT2_TEST_EQUAL( boost::fusion::get<1>(res), boost::simd::Nan<r_t0>());
    NT2_TEST_EQUAL( boost::fusion::get<2>(res), r_t2(0) );
  }
  {
    r_t res = comparator(boost::simd::One<vT>(), boost::simd::One<vT>(), 0);
    NT2_TEST_EQUAL( boost::fusion::get<0>(res), boost::simd::One<r_t0>());
    NT2_TEST_EQUAL( boost::fusion::get<1>(res), boost::simd::One<r_t0>());
    NT2_TEST_EQUAL( boost::fusion::get<2>(res), r_t2(0) );
  }
  {
    r_t res = comparator(boost::simd::Zero<vT>(), boost::simd::Zero<vT>(), 0);
    NT2_TEST_EQUAL( boost::fusion::get<0>(res), boost::simd::Zero<r_t0>());
    NT2_TEST_EQUAL( boost::fusion::get<1>(res), boost::simd::Zero<r_t0>());
    NT2_TEST_EQUAL( boost::fusion::get<2>(res), r_t2(0) );
  }
} // end of test for floating_


#include <boost/units/base_units/information/nat.hpp>


#include <boost/geometry/policies/relate/intersection_points.hpp>


#include <iostream>
#include <string>
#include <cstring>
#include <cmath>
#include <ctime>
#include <fstream>
#include <sstream>
#include <exception>
#include <cstring>
//#include <boost/tokenizer.hpp>
#include <boost/circular_buffer.hpp>
#include <boost/filesystem.hpp>
#include <map>
//#include <set>
//#include <unordered_map>
#include <cstdarg>
#include "options.hpp"
#include "basic_utils/stream_reader.hpp"
#include "array_of_trees.hpp"

typedef int64_t Index;
std::string provenance;

#include "basic_utils/utils.hpp"
//#include "basic_utils/stream_reader.hpp"
#include "vocabulary.hpp"

Index cnt_words_processed;
//Index cnt_words_processed_old;

Index cnt_bigrams;
Vocabulary vocab;
#include "basic_utils/file_io.hpp"

void load_bigrams(std::string str_path_in, const Options & options, ArrayOfTrees & counters)
{
    DirReader dr(str_path_in);
    auto size_window = options.size_window;
    boost::circular_buffer<int64_t> cb(size_window); //- size of n-grams
    for (uint64_t i = 0 ; i < size_window; i++) cb.push_back(-1);
    wchar_t * word;
    while ((word = dr.get_word()) != NULL )
    {
        if (vocab.is_word_valid(std::wstring(word)))
        {
            //std::cerr<<wstring_to_utf8(std::wstring(word))<<"\n";
            cnt_words_processed++;
            Index id_current = vocab.get_id(word);
            if ( (word[0] == L'.') ||  (word == std::wstring(L"!")) ||  (word == std::wstring(L"?")) )
            {
                if (options.obey_sentence_bounds)
                {
                    id_current = -1;
                    for (size_t j = 1; j < cb.size(); j++)
                        cb.push_back(id_current);
                }
                continue;
            }
            //freq_per_id[id_current]++;
            if ((cnt_words_processed % 500000) == 0)
                std::cerr << "bgrams " << cnt_words_processed / 1000 << "k of " << vocab.cnt_words_processed / 1000 << "k (" << 100.0 * cnt_words_processed / vocab.cnt_words_processed << "%)\n";
            cb.push_back(id_current);
            //auto i = cb.begin();
            auto last = cb[cb.size() - 1];
            for (size_t j = 0; j < cb.size() - 1; j++)
            {
                //std::cerr<<"accing "<<last<<" - "<<cb[j]<<"\n";
                //std::cerr<<"accing "<<wstring_to_utf8(lst_id2word[last])<<" - "<<wstring_to_utf8(lst_id2word[cb[j]])<<"\n";
                counters.accumulate(last, cb[j]);
                counters.accumulate(cb[j], last);
            }
        }
    }
}

struct window_params
{
    uint64_t size_window;
    bool symmetric;
};

std::string get_str_time()
{
    std::time_t result = std::time(nullptr);
    return std::asctime(std::localtime(&result));
}

int main(int argc, char * argv[])
{
    Options options = ProcessOptions(argc, argv);
    auto str_path_in = options.path_in.string();
    auto path_out = options.path_out;

    std::cerr << "loading vocabulary\n";
    vocab.read_from_precomputed(path_out.string());
    std::ifstream f_prov((path_out / boost::filesystem::path("provenance.txt")).string());
    provenance = std::string((std::istreambuf_iterator<char>(f_prov)), std::istreambuf_iterator<char>());

    if (options.size_window < 2)
    {
        std::cerr << "window size <=1 - nothing else to be done! quiting...\n";
        return 0;
    }

    provenance += "cooccurrences collected on " + get_str_time();
    provenance += "source corpus : " + str_path_in + "\n";

    std::cerr << "extracting bigrams\n";
    ArrayOfTrees counters(vocab.cnt_words);
    //counters.rows.resize(vocab.cnt_words);
    provenance += "windows size : " + FormatHelper::ConvertToStr(options.size_window);
    provenance += "\nobey sentence boundaries : ";
    provenance += options.obey_sentence_bounds ? "yes" : "no";
    provenance += "\nfrequency weightening : PMI\n";
    load_bigrams(str_path_in, options, counters);

    std::cerr << "dumping results to disk\n";

    //dump_crs(path_out.string(),counters.rows,vocab,true);
    counters.dump_csr(path_out.string().c_str(), vocab.freq_per_id.data(), 0);
    write_value_to_file((path_out / boost::filesystem::path("provenance.txt")).string(), provenance);
    if (options.debug)
    {
        //dump_crs(path_out.string(),counters.rows,vocab,false);
        write_cooccurrence_text((path_out / boost::filesystem::path("bigrams_list")).string(), counters.rows , vocab);
    }
    return 0;
}


#ifndef STAN_MATH_PRIM_MAT_FUN_COMMON_TYPE_HPP
#define STAN_MATH_PRIM_MAT_FUN_COMMON_TYPE_HPP

#include <stan/math/prim/arr/fun/common_type.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <boost/math/tools/promotion.hpp>

namespace stan {
  namespace math {
    /**
     * Struct which calculates type promotion over two types.
     *
     * <p>This specialization is for matrix types.
     *
     * @tparam T1 type of elements contained in Eigen::Matrix<T1>
     * @tparam T2 type of elements contained in Eigen::Matrix<T2>
     * @tparam R number of rows
     * @tparam C number of columns
     */
    template <typename T1, typename T2, int R, int C>
    struct common_type<Eigen::Matrix<T1, R, C>, Eigen::Matrix<T2, R, C> > {
      typedef Eigen::Matrix<typename common_type<T1, T2>::type, R, C> type;
    };

  }
}

#endif


/*=========================================================================
 *
 *  Copyright Insight Software Consortium
 *
 *  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.txt
 *
 *  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 "itkImageFileReader.h"
#include "itkImageFileWriter.h"
#include "itkImage.h"
#include "itkMetaDataObject.h"
#include "itkKretzImageIO.h"
#include "itkKretzImageIOFactory.h"
#include "itkToroidalToCartesianTransform.h"
#include <itkCastImageFilter.h>
#include <itkNormalizeImageFilter.h>
#include <itkResampleImageFilter.h>
#include "itkNearestNeighborInterpolateImageFunction.h"
#include <list>
#include <fstream>
#include <boost/program_options.hpp>

namespace po = boost::program_options;

int execute(std::string filename, std::string filename_out, std::string filename_cartesian, bool flagNormalise, bool flagDoppler)
{
  typedef unsigned char InputPixelType;
  const unsigned int   Dimension = 3;
  typedef itk::Image< InputPixelType, Dimension > InputImageType;
  typedef itk::Image< double, Dimension > DoubleImageType;
  typedef itk::ImageFileReader< InputImageType > ReaderType;
  ReaderType::Pointer reader = ReaderType::New();
  reader->SetFileName( filename );
  typedef itk::KretzImageIO           ImageIOType;
  ImageIOType::Pointer kretzImageIO = ImageIOType::New();
  reader->SetImageIO( kretzImageIO );
  kretzImageIO->SetIsDoppler(flagDoppler);
  try
  {
    reader->Update();
  }
  catch (itk::ExceptionObject & e)
  {
    std::cerr << "exception in file reader " << std::endl;
    std::cerr << e.GetDescription() << std::endl;
    std::cerr << e.GetLocation() << std::endl;
    return EXIT_FAILURE;
  }
  InputImageType::Pointer inputImage = reader->GetOutput();
  //typedef itk::MetaDataDictionary   DictionaryType;
  //DictionaryType & dictionary = inputImage->GetMetaDataDictionary();

  //If a cartesian file is supplied transform it back to toroidal
  //space using the geometry of the supplied kretz file
  if(!filename_cartesian.empty())
  {
    typedef itk::ImageFileReader< DoubleImageType > ReaderType;
    ReaderType::Pointer reader_cartesian = ReaderType::New();
    reader_cartesian->SetFileName( filename_cartesian );
    reader_cartesian->Update();
    std::cout << filename_cartesian << std::endl;
    typedef itk::ToroidalToCartesianTransform<double,Dimension> T2CTransformType;
    T2CTransformType::Pointer t2c = T2CTransformType::New();
    t2c->SetBModeRadius(kretzImageIO->GetRadiusD());
    t2c->SetSweepRadius(kretzImageIO->GetRadiusBStart());
    t2c->SetResolution(kretzImageIO->GetResolution());
    t2c->SetTablePhi(kretzImageIO->m_TableAnglesPhi);
    t2c->SetTableTheta(kretzImageIO->m_TableAnglesTheta);

    typedef itk::ResampleImageFilter<DoubleImageType,DoubleImageType> ResampleFilterType;
    typename ResampleFilterType::Pointer resampleFilter = ResampleFilterType::New();

    resampleFilter->SetTransform(t2c);
    resampleFilter->SetOutputOrigin(inputImage->GetOrigin());
    resampleFilter->SetOutputDirection(inputImage->GetDirection());
    resampleFilter->SetOutputSpacing(inputImage->GetSpacing());

    typedef itk::NearestNeighborInterpolateImageFunction<DoubleImageType, double >  NearestNeighbourInterpolatorType;
    auto interpolator = NearestNeighbourInterpolatorType::New();
    resampleFilter->SetInterpolator(interpolator);
    resampleFilter->SetSize(inputImage->GetLargestPossibleRegion().GetSize());

    resampleFilter->SetInput(reader_cartesian->GetOutput());
    resampleFilter->Update();
    typedef itk::ImageFileWriter< DoubleImageType >  WriterType;
    WriterType::Pointer writer = WriterType::New();
    writer->SetInput( resampleFilter->GetOutput() );
    writer->SetFileName( filename_out );
    try
    {
      writer->Update();
    }
    catch (itk::ExceptionObject & e)
    {
      std::cerr << "exception in file writer " << std::endl;
      std::cerr << e.GetDescription() << std::endl;
      std::cerr << e.GetLocation() << std::endl;
      return EXIT_FAILURE;
    }
  }
  else if(flagNormalise){ //if normalised images wanted outpu double image type

    typedef itk::ImageFileWriter<DoubleImageType> ImageWriterType;
    typedef itk::CastImageFilter<InputImageType,DoubleImageType> CastImageFilterType;
    typedef itk::NormalizeImageFilter<DoubleImageType, DoubleImageType> NormaliseImageFilterType;

    CastImageFilterType::Pointer castImgFilter = CastImageFilterType::New();
    castImgFilter->SetInput(inputImage);
    castImgFilter->Update();

    DoubleImageType::Pointer castImage = castImgFilter->GetOutput();

    NormaliseImageFilterType::Pointer normaliseFilter = NormaliseImageFilterType::New();
    normaliseFilter->SetInput(castImage);
    normaliseFilter->Update();
    DoubleImageType::Pointer normImage = normaliseFilter->GetOutput();

    ImageWriterType::Pointer ITKImageWriter = ImageWriterType::New();

    ITKImageWriter->SetFileName(filename_out.c_str());
    ITKImageWriter->SetInput(normImage);
    ITKImageWriter->Write();

  }
  else //otherwise write the toroidal volume to the output file
  {
    typedef itk::ImageFileWriter< InputImageType >  WriterType;
    WriterType::Pointer writer = WriterType::New();
    writer->SetInput( reader->GetOutput() );
    writer->SetFileName( filename_out );
    try
    {
      writer->Update();
    }
    catch (itk::ExceptionObject & e)
    {
      std::cerr << "exception in file writer " << std::endl;
      std::cerr << e.GetDescription() << std::endl;
      std::cerr << e.GetLocation() << std::endl;
      return EXIT_FAILURE;
    }
  }
  return EXIT_SUCCESS;
}

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

  std::string filename, filename_cartesian, filename_out;
  po::options_description desc("Allowed options");

  //bool flagMask = false;
  bool flagNormalise = false;
  bool flagDoppler = false;
  //bool flagThreshold = false;

  desc.add_options()
    ("help,h", "produce help message")
    ("inputfile,i", po::value<std::string>(&filename)->required(), "set input file")
    ("filename_cartesian,c", po::value<std::string>(&filename_cartesian), "set cartesian file")
    ("outputfile,o", po::value<std::string>(&filename_out)->required(), "set output file")
    ("normalise,n", po::bool_switch(&flagNormalise), "normalise volume")
    ("doppler,d", po::bool_switch(&flagDoppler), "do doppler")
    ;

  try
  {

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);

    if (vm.count("help") || vm.empty() || vm.count("i") || vm.count("o") ) {
      std::cout << desc << "\n";
      return 1;
    }

    po::notify(vm);

    return execute(filename, filename_out, filename_cartesian, flagNormalise, flagDoppler);

  }
  catch(std::exception& e)
  {

    std::cerr << "Error: " << e.what() << "\n";

    std::cout << desc << "\n";

    return 1;
  }
  catch(...)
  {
    std::cerr << "Unknown error!" << "\n";
    return 1;
  }

}


#include <stan/math/rev.hpp>
#include <boost/mp11.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/functor/test_fixture_ode.hpp>
#include <test/unit/math/rev/functor/test_fixture_dae_chem.hpp>
#include <test/unit/math/rev/functor/dae_test_functors.hpp>

/**
 *
 * Use same solver functor type for both w & w/o tolerance control
 */
template <typename solve_type, typename... Ts>
using ode_test_tuple = std::tuple<solve_type, solve_type, Ts...>;

/**
 * Outer product of test types
 */
using chemical_kinetics_test_types
    = boost::mp11::mp_product<ode_test_tuple, ::testing::Types<dae_functor>,
                              ::testing::Types<double>,  // t
                              ::testing::Types<double>,  // yy0
                              ::testing::Types<double>,  // yp0
                              ::testing::Types<double>   // theta
                              >;

TYPED_TEST_SUITE_P(chemical_kinetics_test);
TYPED_TEST_P(chemical_kinetics_test, param_and_data_finite_diff) {
  // params that gives extreme gradients
  this->test_fd_dv(1.e-3, 2e-3);

  // params that gives moderate gradients
  this->theta = {0.4, 0.5, 0.5};
  this->test_fd_dv(1.e-3, 5e-6);
}
TYPED_TEST_P(chemical_kinetics_test, value) {
  this->test_value(0.0);
  this->test_value(1.0);
  this->test_value(-1.0);
}

TYPED_TEST_P(chemical_kinetics_test, error) { this->test_bad(); }

REGISTER_TYPED_TEST_SUITE_P(chemical_kinetics_test, value, error,
                            param_and_data_finite_diff);
INSTANTIATE_TYPED_TEST_SUITE_P(StanOde, chemical_kinetics_test,
                               chemical_kinetics_test_types);

TYPED_TEST_SUITE_P(chemical_kinetics_data_test);
TYPED_TEST_P(chemical_kinetics_data_test, value) {
  this->test_value(0.0);
  this->test_value(1.0);
  this->test_value(-1.0);
}
TYPED_TEST_P(chemical_kinetics_data_test, param_and_data_finite_diff) {
  // params that gives extreme gradients
  this->test_fd_dv(1.e-3, 2e-3);

  // params that gives moderate gradients
  this->theta = {0.4, 0.5, 0.5};
  this->x_r = {0.5, 0.5};
  this->test_fd_dv(1.e-3, 2e-6);
}
REGISTER_TYPED_TEST_SUITE_P(chemical_kinetics_data_test, value,
                            param_and_data_finite_diff);
INSTANTIATE_TYPED_TEST_SUITE_P(StanDAE, chemical_kinetics_data_test,
                               chemical_kinetics_test_types);


/*=========================================================================
 *
 *  Copyright David Doria 2012 daviddoria@gmail.com
 *
 *  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.txt
 *
 *  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.
 *
 *=========================================================================*/

// Pixel descriptors
#include "PixelDescriptors/ImagePatchPixelDescriptor.h"

// Visitors
#include "Visitors/NearestNeighborsDefaultVisitor.hpp"

// Descriptor visitors
#include "Visitors/DescriptorVisitors/ImagePatchDescriptorVisitor.hpp"

// Inpainting visitors
#include "Visitors/InpaintingVisitor.hpp"
#include "Visitors/AcceptanceVisitors/DefaultAcceptanceVisitor.hpp"

// Nearest neighbors functions
#include "NearestNeighbor/LinearSearchBest/HistogramCorrelation.hpp"
#include "NearestNeighbor/LinearSearchBest/HistogramDifference.hpp"
#include "NearestNeighbor/LinearSearchBest/HoleHistogramDifference.hpp"
#include "NearestNeighbor/LinearSearchBest/DualHistogramDifference.hpp"
#include "NearestNeighbor/LinearSearchBest/AdaptiveDualHistogramDifference.hpp"
#include "NearestNeighbor/LinearSearchBest/AdaptiveDualQuadrantHistogramDifference.hpp"
#include "NearestNeighbor/LinearSearchBest/IntroducedEnergy.hpp"
#include "NearestNeighbor/LinearSearchBest/First.hpp"
#include "NearestNeighbor/LinearSearchBest/FirstAndWrite.hpp"
#include "NearestNeighbor/LinearSearchBest/StrategySelection.hpp"
#include "NearestNeighbor/LinearSearchKNNProperty.hpp"
#include "NearestNeighbor/TwoStepNearestNeighbor.hpp"

// Initializers
#include "Initializers/InitializeFromMaskImage.hpp"
#include "Initializers/InitializePriority.hpp"

// Inpainters
#include "Inpainters/PatchInpainter.hpp"
#include "Inpainters/CompositePatchInpainter.hpp"

// Difference functions
#include "DifferenceFunctions/ImagePatchDifference.hpp"
#include "DifferenceFunctions/SumAbsolutePixelDifference.hpp"
#include "DifferenceFunctions/SumSquaredPixelDifference.hpp"

// Utilities
#include "Utilities/PatchHelpers.h"
#include "Utilities/IndirectPriorityQueue.h"

// Inpainting
#include "Algorithms/InpaintingAlgorithm.hpp"

// Priority
#include "Priority/PriorityRandom.h"
#include "Priority/PriorityCriminisi.h"

// ITK
#include "itkImageFileReader.h"

// Submodules
#include <Helpers/Helpers.h>
#include <ITKVTKHelpers/ITKVTKHelpers.h>
#include <Mask/MaskOperations.h>

// Boost
#include <boost/graph/grid_graph.hpp>
#include <boost/property_map/property_map.hpp>

// Run with: Data/trashcan.png Data/trashcan.mask 15 filled.png
int main(int argc, char *argv[])
{
  // Verify arguments
  if(argc != 7)
  {
    std::cerr << "Required arguments: image.png imageMask.mask patchHalfWidth numberOfKNN binsPerChannel output.png" << std::endl;
    std::cerr << "Input arguments: ";
    for(int i = 1; i < argc; ++i)
    {
      std::cerr << argv[i] << " ";
    }
    return EXIT_FAILURE;
  }

  std::stringstream ssArguments;
  for(int i = 1; i < argc; ++i)
  {
    ssArguments << argv[i] << " ";
    std::cout << argv[i] << " ";
  }

  // Parse arguments
  std::string imageFileName;
  std::string maskFileName;

  unsigned int patchHalfWidth = 0;

  unsigned int numberOfKNN = 0;

  unsigned int binsPerChannel = 0;

  std::string outputFileName;

  ssArguments >> imageFileName >> maskFileName >> patchHalfWidth >> numberOfKNN >> binsPerChannel >> outputFileName;

  // Output arguments
  std::cout << "Reading image: " << imageFileName << std::endl;
  std::cout << "Reading mask: " << maskFileName << std::endl;
  std::cout << "Patch half width: " << patchHalfWidth << std::endl;
  std::cout << "numberOfKNN: " << numberOfKNN << std::endl;
  std::cout << "Output: " << outputFileName << std::endl;

  // typedef itk::Image<itk::CovariantVector<unsigned char, 3>, 2> OriginalImageType; // This doesn't allow for direct "a - b" pixel comparisons, because (100 - 150) or similar will underflow!
  // typedef itk::Image<itk::CovariantVector<float, 3>, 2> OriginalImageType; // This is quite slow
  typedef itk::Image<itk::CovariantVector<int, 3>, 2> OriginalImageType;
//  typedef itk::Image<itk::CovariantVector<short, 3>, 2> OriginalImageType; // Can't use this (signed range: -32768 to 32767 (not high enough, needs to handle 255*255 = 65025) )

  typedef  itk::ImageFileReader<OriginalImageType> ImageReaderType;
  ImageReaderType::Pointer imageReader = ImageReaderType::New();
  imageReader->SetFileName(imageFileName);
  imageReader->Update();

  OriginalImageType* originalImage = imageReader->GetOutput();
  ITKHelpers::WriteRGBImage(originalImage, "OriginalImage.png");
  ITKHelpers::WriteImage(originalImage, "OriginalImage.mha");

  itk::ImageRegion<2> fullRegion = originalImage->GetLargestPossibleRegion();

//  ImageType::Pointer image = ImageType::New();
//  ITKHelpers::DeepCopy(imageReader->GetOutput(), image.GetPointer());

  // Convert the image to HSV
  typedef itk::Image<itk::CovariantVector<float, 3>, 2> HSVImageType;
  HSVImageType::Pointer hsvImage = HSVImageType::New();
//  ITKVTKHelpers::ConvertRGBtoHSV(image.GetPointer(), hsvImage.GetPointer());
  ITKVTKHelpers::ConvertRGBtoHSV(originalImage, hsvImage.GetPointer());

  ITKHelpers::WriteImage(hsvImage.GetPointer(), "HSVImage.mha");

  // Read the mask
  Mask::Pointer mask = Mask::New();
  mask->Read(maskFileName);
  ITKHelpers::WriteImage(mask.GetPointer(), "Mask.mha");

  std::cout << "hole pixels: " << mask->CountHolePixels() << std::endl;
  std::cout << "valid pixels: " << mask->CountValidPixels() << std::endl;

  // Blur the image
  typedef OriginalImageType BlurredImageType; // Usually the blurred image is the same type as the original image.
  BlurredImageType::Pointer blurredImage = BlurredImageType::New();
  float blurVariance = 2.0f;
  MaskOperations::MaskedBlur(originalImage, mask, blurVariance, blurredImage.GetPointer());

  ITKHelpers::WriteRGBImage(blurredImage.GetPointer(), "BlurredImage.png");

  // Create the graph
  typedef ImagePatchPixelDescriptor<OriginalImageType> ImagePatchPixelDescriptorType;

  typedef boost::grid_graph<2> VertexListGraphType;

  // We can't make this a signed type (size_t versus int) because we allow negative
  boost::array<std::size_t, 2> graphSideLengths = { { fullRegion.GetSize()[0],
                                                      fullRegion.GetSize()[1] } };
  VertexListGraphType graph(graphSideLengths);
  typedef boost::graph_traits<VertexListGraphType>::vertex_descriptor VertexDescriptorType;
  typedef boost::graph_traits<VertexListGraphType>::vertex_iterator VertexIteratorType;

  // Get the index map
  typedef boost::property_map<VertexListGraphType, boost::vertex_index_t>::const_type IndexMapType;
  IndexMapType indexMap(get(boost::vertex_index, graph));

  // Create the descriptor map. This is where the data for each pixel is stored.
  typedef boost::vector_property_map<ImagePatchPixelDescriptorType, IndexMapType> ImagePatchDescriptorMapType;
  ImagePatchDescriptorMapType imagePatchDescriptorMap(num_vertices(graph), indexMap);

  // Create the patch inpainter.
  typedef PatchInpainter<OriginalImageType> OriginalImageInpainterType;
  OriginalImageInpainterType originalImagePatchInpainter(patchHalfWidth, originalImage, mask);
  originalImagePatchInpainter.SetDebugImages(true);
  originalImagePatchInpainter.SetImageName("RGB");

  // Create an inpainter for the HSV image.
  typedef PatchInpainter<HSVImageType> HSVImageInpainterType;
  HSVImageInpainterType hsvImagePatchInpainter(patchHalfWidth, hsvImage, mask);

  // Create an inpainter for the blurred image.
  typedef PatchInpainter<BlurredImageType> BlurredImageInpainterType;
  BlurredImageInpainterType blurredImagePatchInpainter(patchHalfWidth, blurredImage, mask);

  // Create a composite inpainter.
  CompositePatchInpainter inpainter;
  inpainter.AddInpainter(&originalImagePatchInpainter);
  inpainter.AddInpainter(&hsvImagePatchInpainter);
  inpainter.AddInpainter(&blurredImagePatchInpainter);

  // Create the priority function
  typedef PriorityCriminisi<BlurredImageType> PriorityType;
  PriorityType priorityFunction(blurredImage, mask, patchHalfWidth);
//  priorityFunction.SetDebugLevel(1);

  // Queue
  typedef IndirectPriorityQueue<VertexListGraphType> BoundaryNodeQueueType;
  BoundaryNodeQueueType boundaryNodeQueue(graph);

  // Create the descriptor visitor (RGB). This is one thing that would need to be changed to use HSV pixel comparisons
  // (along with the template paramater of the PatchDifferenceType defined further below).
  typedef ImagePatchDescriptorVisitor<VertexListGraphType, OriginalImageType, ImagePatchDescriptorMapType>
      ImagePatchDescriptorVisitorType;
  ImagePatchDescriptorVisitorType
      imagePatchDescriptorVisitor(originalImage, mask,
                                  imagePatchDescriptorMap, patchHalfWidth);

  typedef DefaultAcceptanceVisitor<VertexListGraphType> AcceptanceVisitorType;
  AcceptanceVisitorType acceptanceVisitor;

  // Create the inpainting visitor. (The mask is inpainted in FinishVertex)
  typedef InpaintingVisitor<VertexListGraphType, OriginalImageType, BoundaryNodeQueueType,
      ImagePatchDescriptorVisitorType, AcceptanceVisitorType, PriorityType>
      InpaintingVisitorType;
  InpaintingVisitorType inpaintingVisitor(originalImage, mask, boundaryNodeQueue,
                                          imagePatchDescriptorVisitor, acceptanceVisitor,
                                          &priorityFunction, patchHalfWidth,
                                          outputFileName);
  inpaintingVisitor.SetDebugImages(true);
  inpaintingVisitor.SetAllowNewPatches(true);

  InitializePriority(mask, boundaryNodeQueue, &priorityFunction);

  // Initialize the boundary node queue from the user provided mask image.
  InitializeFromMaskImage<InpaintingVisitorType, VertexDescriptorType>(mask, &inpaintingVisitor);
  std::cout << "PatchBasedInpaintingNonInteractive: There are " << boundaryNodeQueue.CountValidNodes()
            << " nodes in the boundaryNodeQueue" << std::endl;

  // Select squared or absolute pixel error
//  typedef ImagePatchDifference<ImagePatchPixelDescriptorType, SumAbsolutePixelDifference<OriginalImageType::PixelType> > PatchDifferenceType;
  typedef ImagePatchDifference<ImagePatchPixelDescriptorType,
      SumSquaredPixelDifference<OriginalImageType::PixelType> > PatchDifferenceType;

  // Create the first (KNN) neighbor finder
  typedef LinearSearchKNNProperty<ImagePatchDescriptorMapType, PatchDifferenceType> KNNSearchType;
  KNNSearchType linearSearchKNN(imagePatchDescriptorMap, numberOfKNN);

  typedef LinearSearchBestStrategySelection<ImagePatchDescriptorMapType, OriginalImageType> BestSearchType;
  BestSearchType linearSearchBest(imagePatchDescriptorMap, originalImage, mask);

  typedef LinearSearchBestFirstAndWrite<ImagePatchDescriptorMapType,
      OriginalImageType, PatchDifferenceType> TopPatchesWriterType;
  TopPatchesWriterType topPatchesWriter(imagePatchDescriptorMap, originalImage, mask);

  // Setup the two step neighbor finder
//  TwoStepNearestNeighbor<KNNSearchType, BestSearchType>
//      twoStepNearestNeighbor(linearSearchKNN, linearSearchBest);

  TwoStepNearestNeighbor<KNNSearchType, BestSearchType, TopPatchesWriterType>
      twoStepNearestNeighbor(linearSearchKNN, linearSearchBest, topPatchesWriter);

  // For debugging, look at the initial priority queue
//  PatchHelpers::DumpQueue(boundaryNodeQueue, boundaryStatusMap, priorityMap);

  // Perform the inpainting
  InpaintingAlgorithm(graph, inpaintingVisitor, &boundaryNodeQueue,
                      twoStepNearestNeighbor, &inpainter);

  // If the output filename is a png file, then use the RGBImage writer so that it is first
  // casted to unsigned char. Otherwise, write the file directly.
  if(Helpers::GetFileExtension(outputFileName) == "png")
  {
    ITKHelpers::WriteRGBImage(originalImage, outputFileName);
  }
  else
  {
    ITKHelpers::WriteImage(originalImage, outputFileName);
  }

  return EXIT_SUCCESS;
}


// -*- C++ -*-
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
//                                   Jiao Lin
//                      California Institute of Technology
//                      (C) 2005-2013 All Rights Reserved
//
// {LicenseText}
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

#include <boost/python.hpp>

#include "wrap_E_Q_Kernel.h"
#include "wrap_E_vQ_Kernel.h"
#include "wrap_Broadened_E_Q_Kernel.h"

namespace wrap_mccomponents{

  void wrap_basic_containers();

  void wrap_HomogeneousNeutronScatterer();
  void wrap_AbstractScatteringKernel();
  void wrap_absorptioncoefficientcalculators();
  void wrap_CompositeScatteringKernel();
  void wrap_kernelcontainer();

  void wrap_DGSSXResPixel();

  void wrap_IsotropicKernel();
  void wrap_DGSSXResKernel();

  void wrap_He3TubeKernel();
  void wrap_EventModeMCA();
  void wrap_ConstantEnergyTransferKernel();
  void wrap_ConstantQEKernel();
  void wrap_ConstantvQEKernel();
  void wrap_SQEkernel();
  void wrap_SQE_EnergyFocusing_Kernel();
  void wrap_SQkernel();
  void wrap_SvQkernel();
  void wrap_GridSQE();
  void wrap_SQE_fromexpression();
  void wrap_GridSQ();
  void wrap_GridSvQ();
  void wrap_SQ_fromexpression();

  void wrap_NdArray();
  void wrap_AbstractDOS();
  void wrap_LinearlyInterpolatedDOS();
  void wrap_LinearlyInterpolatableAxis();
  void wrap_epsilon_t();
  void wrap_AbstractDebyeWallerFactorCalculator();
  void wrap_DWFromDOS();
  void wrap_AbstractDispersion_3D();
  void wrap_PeriodicDispersion_3D();
  void wrap_ChangeCoordinateSystem_forDispersion_3D();
  void wrap_LinearlyInterpolatedDispersionOnGrid_3D();
  void wrap_AtomicScatterer();
  void wrap_Phonon_IncoherentElastic_kernel();
  void wrap_Phonon_IncoherentInelastic_kernel();
  void wrap_Phonon_IncoherentInelastic_EnergyFocusing_kernel();
  void wrap_Phonon_CoherentInelastic_PolyXtal_kernel();
  void wrap_rootsfinders();
  void wrap_targetregion();
  void wrap_Phonon_CoherentInelastic_SingleXtal_kernel();
  void wrap_RandomNumberGenerator();
  void wrap_SimplePowderDiffractionKernel();
  void wrap_SingleCrystalDiffractionKernel();

  struct Wrap_E_Q_Kernel;
  struct Wrap_Broadened_E_Q_Kernel;
  struct Wrap_E_vQ_Kernel;
}

BOOST_PYTHON_MODULE(mccomponentsbp)
{
  using namespace boost::python;
  using namespace wrap_mccomponents;

  wrap_basic_containers();

  wrap_HomogeneousNeutronScatterer();
  wrap_AbstractScatteringKernel();
  wrap_absorptioncoefficientcalculators();
  wrap_CompositeScatteringKernel();
  wrap_kernelcontainer();

  wrap_DGSSXResPixel();

  wrap_IsotropicKernel();
  wrap_DGSSXResKernel();

  wrap_He3TubeKernel();
  wrap_EventModeMCA();

  wrap_ConstantEnergyTransferKernel();
  wrap_ConstantQEKernel();
  wrap_ConstantvQEKernel();
  wrap_SQEkernel();
  wrap_SQE_EnergyFocusing_Kernel();
  wrap_SQkernel();
  wrap_SvQkernel();

  wrap_GridSQE();
  wrap_SQE_fromexpression();
  wrap_GridSQ();
  wrap_GridSvQ();
  wrap_SQ_fromexpression();

  wrap_NdArray();
  wrap_AbstractDOS();
  wrap_LinearlyInterpolatedDOS();
  wrap_LinearlyInterpolatableAxis();
  wrap_epsilon_t();
  wrap_AbstractDebyeWallerFactorCalculator();
  wrap_DWFromDOS();
  wrap_AbstractDispersion_3D();
  wrap_PeriodicDispersion_3D();
  wrap_ChangeCoordinateSystem_forDispersion_3D();
  wrap_LinearlyInterpolatedDispersionOnGrid_3D();

  wrap_Phonon_IncoherentElastic_kernel();
  wrap_Phonon_IncoherentInelastic_kernel();
  wrap_Phonon_IncoherentInelastic_EnergyFocusing_kernel();

  wrap_AtomicScatterer();
  wrap_Phonon_CoherentInelastic_PolyXtal_kernel();

  wrap_rootsfinders();
  wrap_targetregion();
  wrap_Phonon_CoherentInelastic_SingleXtal_kernel();

  wrap_RandomNumberGenerator();

  wrap_SimplePowderDiffractionKernel();
  wrap_SingleCrystalDiffractionKernel();

  Wrap_E_Q_Kernel wrap_e_q_kernel;
  Wrap_Broadened_E_Q_Kernel wrap_broadened_e_q_kernel;
  Wrap_E_vQ_Kernel wrap_e_vq_kernel;
}

// version
// $Id$

// End of file


#include "Window.h"

#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/SceneObject.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>

#include <Eigen/Geometry>

#include <Inventor/actions/SoLineHighlightRenderAction.h>
#include <Inventor/nodes/SoShapeHints.h>
#include <Inventor/nodes/SoLightModel.h>
#include <Inventor/nodes/SoMaterial.h>

#include <boost/filesystem.hpp>
#include <boost/make_shared.hpp>
#include <boost/assert.hpp>

#include "../../include/ik/DifferentialReferenceIK.h"

float TIMER_MS = 30.0f;

Window::Window(VirtualRobot::RobotPtr robot, Qt::WFlags flags)
:QMainWindow(NULL)
,robot(robot)
,computeIK(true)
{
	frameNames = {
				"LeftLeg_TCP",
				"RightLeg_TCP",
				"TorsoCenter",
				"Waist",
				};

	nodes = robot->getRobotNodeSet("CoMCompensation");

	boost::shared_ptr<DifferentialReferenceIK> diffIK =
		boost::make_shared<DifferentialReferenceIK>(
			nodes,
			robot,
			robot->getRobotNodeSet("ColModelAll"),
			robot->getRobotNode(frameNames[0]),
			robot->getRobotNode(frameNames[1]),
			robot->getRobotNode(frameNames[2]),
			robot->getRobotNode(frameNames[3]));
	referenceIK = boost::dynamic_pointer_cast<ReferenceIK>(diffIK);
	robotInv = diffIK->getInvertedRobot();

	for (auto& s : frameNames)
	{
		refFrames.push_back(robot->getRobotNode(s)->getGlobalPose());
	}

	sceneSep = new SoSeparator();
	sceneSep->ref();
	robotSep = new SoSeparator();
	robotInvSep = new SoSeparator();
	sceneSep->addChild(robotSep);
	sceneSep->addChild(robotInvSep);
    boost::shared_ptr<VirtualRobot::CoinVisualization> visualization =
		robot->getVisualization<VirtualRobot::CoinVisualization>(VirtualRobot::SceneObject::Full);
	robotSep->addChild(visualization->getCoinVisualization());

    visualization =
		robotInv->getVisualization<VirtualRobot::CoinVisualization>(VirtualRobot::SceneObject::Full);
	SoMaterial* mat = new SoMaterial();
	mat->diffuseColor.setValue(0.2, 1.0, 0.2);
	mat->ambientColor.setValue(0.1, 0.8, 0.1);
	mat->setOverride(true);
	robotInvSep->addChild(mat);
	robotInvSep->addChild(visualization->getCoinVisualization());

	setupUI();

	viewer->viewAll();
}

Window::~Window()
{
	sceneSep->unref();
}

void Window::setupUI()
{
	UI.setupUi(this);
	viewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP);

	// setup
	viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
	viewer->setAccumulationBuffer(true);
	viewer->setGLRenderAction(new SoLineHighlightRenderAction());
	viewer->setTransparencyType(SoGLRenderAction::BLEND);
	viewer->setFeedbackVisibility(true);
	viewer->setSceneGraph(sceneSep);
	viewer->viewAll();

	for (unsigned i = 0; i < frameNames.size(); i++)
	{
		UI.comboBoxFrames->insertItem(i, frameNames[i].c_str());
	}

	connect(UI.spinBoxX, SIGNAL(valueChanged(double)), this, SLOT(frameChanged(double)));
	connect(UI.spinBoxY, SIGNAL(valueChanged(double)), this, SLOT(frameChanged(double)));
	connect(UI.spinBoxZ, SIGNAL(valueChanged(double)), this, SLOT(frameChanged(double)));
	connect(UI.spinBoxRoll, SIGNAL(valueChanged(double)), this, SLOT(frameChanged(double)));
	connect(UI.spinBoxPitch, SIGNAL(valueChanged(double)), this, SLOT(frameChanged(double)));
	connect(UI.spinBoxYaw, SIGNAL(valueChanged(double)), this, SLOT(frameChanged(double)));
	connect(UI.comboBoxFrames, SIGNAL(currentIndexChanged(int)), this, SLOT(updateCurrentRef()));
}

int Window::main()
{
	SoQt::show(this);
	SoQt::mainLoop();
	return 0;
}

void Window::quit()
{
	this->close();
	SoQt::exitMainLoop();
}

void Window::closeEvent(QCloseEvent* event)
{
    quit();
    QMainWindow::closeEvent(event);
}

void Window::frameChanged(double)
{
	unsigned i = UI.comboBoxFrames->currentIndex();
	BOOST_ASSERT(i < refFrames.size());

	VirtualRobot::MathTools::rpy2eigen4f(
		UI.spinBoxRoll->value(),
		UI.spinBoxPitch->value(),
		UI.spinBoxYaw->value(),
		refFrames[i]);
	refFrames[i](0, 3) = UI.spinBoxX->value();
	refFrames[i](1, 3) = UI.spinBoxY->value();
	refFrames[i](2, 3) = UI.spinBoxZ->value();

	updateIK();
}

void Window::updateIK()
{
	if (!computeIK)
		return;

	std::cout << "Using the following frames for IK:" << std::endl;
	for (auto& f : refFrames)
	{
		std::cout << f << std::endl;
	}

	Eigen::VectorXf result;
	Kinematics::SupportPhase phase;
	if (UI.radioButtonLeftSupport->isChecked())
	{
		phase = Kinematics::SUPPORT_LEFT;
	}
	else if (UI.radioButtonRightSupport->isChecked())
	{
		phase = Kinematics::SUPPORT_RIGHT;
	}
	else if (UI.radioButtonDualSupport->isChecked())
	{
		phase = Kinematics::SUPPORT_BOTH;
	}
	referenceIK->computeStep(refFrames[0], refFrames[1], refFrames[2], refFrames[3], Eigen::Vector3f::Zero(), phase, result);

	for (unsigned i = 0; i < frameNames.size(); i++)
	{
		refFrames[i] = robot->getRobotNode(frameNames[i])->getGlobalPose();
	}
}

void Window::updateCurrentRef()
{
	unsigned i = UI.comboBoxFrames->currentIndex();

	float xyzrpy[6];

	VirtualRobot::MathTools::eigen4f2rpy(refFrames[i], xyzrpy);

	computeIK = false;
	UI.spinBoxX->setValue(xyzrpy[0]);
	UI.spinBoxY->setValue(xyzrpy[1]);
	UI.spinBoxZ->setValue(xyzrpy[2]);
	UI.spinBoxRoll->setValue(xyzrpy[3]);
	UI.spinBoxPitch->setValue(xyzrpy[4]);
	UI.spinBoxYaw->setValue(xyzrpy[5]);
	computeIK = true;
}