// Copyright (C) 2011  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include "tester.h"
#include <dlib/svm_threaded.h>
#include <dlib/rand.h>

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.sequence_labeler");

// ----------------------------------------------------------------------------------------

    const unsigned long num_label_states = 3; // the "hidden" states
    const unsigned long num_sample_states = 3;

// ----------------------------------------------------------------------------------------

    struct funny_sequence
    {
        std::vector<unsigned long> item;
        unsigned long size() const { return item.size(); }
    };
    funny_sequence make_funny_sequence(const std::vector<unsigned long>& item)
    {
        funny_sequence temp;
        temp.item = item;
        return temp;
    }

// ----------------------------------------------------------------------------------------

    class feature_extractor
    {
    public:
        typedef funny_sequence sequence_type;

        unsigned long num_features() const
        {
            return num_label_states*num_label_states + num_label_states*num_sample_states;
        }

        unsigned long order() const
        {
            return 1;
        }

        unsigned long num_labels() const
        {
            return num_label_states;
        }

        template <typename feature_setter, typename EXP>
        void get_features (
            feature_setter& set_feature,
            const sequence_type& x,
            const matrix_exp<EXP>& y,
            unsigned long position
        ) const
        {
            if (y.size() > 1)
                set_feature(y(1)*num_label_states + y(0));

            set_feature(num_label_states*num_label_states +
                        y(0)*num_sample_states + x.item[position]);
        }
    };

    class feature_extractor_partial
    {
    public:
        typedef funny_sequence sequence_type;

        unsigned long num_features() const
        {
            return num_label_states*num_label_states + num_label_states*num_sample_states;
        }

        unsigned long order() const
        {
            return 1;
        }

        unsigned long num_labels() const
        {
            return num_label_states;
        }

        template <typename feature_setter, typename EXP>
        void get_features (
            feature_setter& set_feature,
            const sequence_type& x,
            const matrix_exp<EXP>& y,
            unsigned long position
        ) const
        {
            if (y.size() > 1)
            {
                set_feature(y(1)*num_label_states + y(0), 0.5);
                set_feature(y(1)*num_label_states + y(0), 0.5);
            }

            set_feature(num_label_states*num_label_states +
                        y(0)*num_sample_states + x.item[position],0.4);
            set_feature(num_label_states*num_label_states +
                        y(0)*num_sample_states + x.item[position],0.6);
        }
    };

    bool called_rejct_labeling = false;
    class feature_extractor2
    {
    public:
        typedef funny_sequence sequence_type;

        unsigned long num_features() const
        {
            return num_label_states*num_label_states + num_label_states*num_sample_states;
        }

        unsigned long order() const
        {
            return 1;
        }

        unsigned long num_labels() const
        {
            return num_label_states;
        }

        template <typename EXP>
        bool reject_labeling (
            const sequence_type& ,
            const matrix_exp<EXP>& ,
            unsigned long
        ) const
        {
            called_rejct_labeling = true;
            return false;
        }

        template <typename feature_setter, typename EXP>
        void get_features (
            feature_setter& set_feature,
            const sequence_type& x,
            const matrix_exp<EXP>& y,
            unsigned long position
        ) const
        {
            if (y.size() > 1)
                set_feature(y(1)*num_label_states + y(0));

            set_feature(num_label_states*num_label_states +
                        y(0)*num_sample_states + x.item[position]);
        }
    };

    void serialize(const feature_extractor&, std::ostream&) {}
    void deserialize(feature_extractor&, std::istream&) {}
    void serialize(const feature_extractor2&, std::ostream&) {}
    void deserialize(feature_extractor2&, std::istream&) {}

// ----------------------------------------------------------------------------------------

    void sample_hmm (
        dlib::rand& rnd,
        const matrix<double>& transition_probabilities,
        const matrix<double>& emission_probabilities,
        unsigned long previous_label,
        unsigned long& next_label,
        unsigned long& next_sample
    )
    /*!
        requires
            - previous_label < transition_probabilities.nr()
            - transition_probabilities.nr() == transition_probabilities.nc()
            - transition_probabilities.nr() == emission_probabilities.nr()
            - The rows of transition_probabilities and emission_probabilities must sum to 1.
              (i.e. sum_cols(transition_probabilities) and sum_cols(emission_probabilities)
              must evaluate to vectors of all 1s.)
        ensures
            - This function randomly samples the HMM defined by transition_probabilities
              and emission_probabilities assuming that the previous hidden state
              was previous_label.
            - The HMM is defined by:
                - P(next_label |previous_label) == transition_probabilities(previous_label, next_label)
                - P(next_sample|next_label)     == emission_probabilities  (next_label,     next_sample)
            - #next_label == the sampled value of the hidden state
            - #next_sample == the sampled value of the observed state
    !*/
    {
        // sample next_label
        double p = rnd.get_random_double();
        for (long c = 0; p >= 0 && c < transition_probabilities.nc(); ++c)
        {
            next_label = c;
            p -= transition_probabilities(previous_label, c);
        }

        // now sample next_sample
        p = rnd.get_random_double();
        for (long c = 0; p >= 0 && c < emission_probabilities.nc(); ++c)
        {
            next_sample = c;
            p -= emission_probabilities(next_label, c);
        }
    }

// ----------------------------------------------------------------------------------------

    void make_dataset (
        const matrix<double>& transition_probabilities,
        const matrix<double>& emission_probabilities,
        std::vector<funny_sequence>& samples,
        std::vector<std::vector<unsigned long> >& labels,
        unsigned long dataset_size
    )
    /*!
        requires
            - transition_probabilities.nr() == transition_probabilities.nc()
            - transition_probabilities.nr() == emission_probabilities.nr()
            - The rows of transition_probabilities and emission_probabilities must sum to 1.
              (i.e. sum_cols(transition_probabilities) and sum_cols(emission_probabilities)
              must evaluate to vectors of all 1s.)
        ensures
            - This function randomly samples a bunch of sequences from the HMM defined by
              transition_probabilities and emission_probabilities.
            - The HMM is defined by:
                - The probability of transitioning from hidden state H1 to H2
                  is given by transition_probabilities(H1,H2).
                - The probability of a hidden state H producing an observed state
                  O is given by emission_probabilities(H,O).
            - #samples.size() == labels.size() == dataset_size
            - for all valid i:
                - #labels[i] is a randomly sampled sequence of hidden states from the
                  given HMM.  #samples[i] is its corresponding randomly sampled sequence
                  of observed states.
    !*/
    {
        samples.clear();
        labels.clear();

        dlib::rand rnd;

        // now randomly sample some labeled sequences from our Hidden Markov Model
        for (unsigned long iter = 0; iter < dataset_size; ++iter)
        {
            const unsigned long sequence_size = rnd.get_random_32bit_number()%20+3;
            std::vector<unsigned long> sample(sequence_size);
            std::vector<unsigned long> label(sequence_size);

            unsigned long previous_label = rnd.get_random_32bit_number()%num_label_states;
            for (unsigned long i = 0; i < sample.size(); ++i)
            {
                unsigned long next_label=0, next_sample=0;
                sample_hmm(rnd, transition_probabilities, emission_probabilities,
                           previous_label, next_label, next_sample);

                label[i] = next_label;
                sample[i] = next_sample;

                previous_label = next_label;
            }

            samples.push_back(make_funny_sequence(sample));
            labels.push_back(label);
        }
    }

// ----------------------------------------------------------------------------------------

    template <typename fe_type>
    void do_test()
    {
        called_rejct_labeling = false;

        matrix<double> transition_probabilities(num_label_states, num_label_states);
        transition_probabilities = 0.05, 0.90, 0.05,
        0.05, 0.05, 0.90,
        0.90, 0.05, 0.05;

        matrix<double> emission_probabilities(num_label_states,num_sample_states);
        emission_probabilities = 0.5, 0.5, 0.0,
        0.0, 0.5, 0.5,
        0.5, 0.0, 0.5;

        print_spinner();

        std::vector<funny_sequence> samples;
        std::vector<std::vector<unsigned long> > labels;
        make_dataset(transition_probabilities,emission_probabilities,
                     samples, labels, 1000);

        dlog << LINFO << "samples.size(): "<< samples.size();

        // print out some of the randomly sampled sequences
        for (int i = 0; i < 10; ++i)
        {
            dlog << LINFO << "hidden states:   " << trans(vector_to_matrix(labels[i]));
            dlog << LINFO << "observed states: " << trans(vector_to_matrix(samples[i].item));
            dlog << LINFO << "******************************";
        }

        print_spinner();
        structural_sequence_labeling_trainer<fe_type> trainer;
        trainer.set_c(4);
        DLIB_TEST(trainer.get_c() == 4);
        trainer.set_num_threads(4);
        DLIB_TEST(trainer.get_num_threads() == 4);

        // Learn to do sequence labeling from the dataset
        sequence_labeler<fe_type> labeler = trainer.train(samples, labels);

        std::vector<unsigned long> predicted_labels = labeler(samples[0]);
        dlog << LINFO << "true hidden states:      "<< trans(vector_to_matrix(labels[0]));
        dlog << LINFO << "predicted hidden states: "<< trans(vector_to_matrix(predicted_labels));

        DLIB_TEST(vector_to_matrix(labels[0]) == vector_to_matrix(predicted_labels));

        print_spinner();

        // We can also do cross-validation
        matrix<double> confusion_matrix;
        confusion_matrix = cross_validate_sequence_labeler(trainer, samples, labels, 4);
        dlog << LINFO << "cross-validation: ";
        dlog << LINFO << confusion_matrix;
        double accuracy = sum(diag(confusion_matrix))/sum(confusion_matrix);
        dlog << LINFO << "label accuracy: "<< accuracy;
        DLIB_TEST(std::abs(accuracy - 0.882) < 0.01);

        print_spinner();

        matrix<double,0,1> true_hmm_model_weights = log(join_cols(reshape_to_column_vector(transition_probabilities),
                                                                  reshape_to_column_vector(emission_probabilities)));

        sequence_labeler<fe_type> labeler_true(true_hmm_model_weights);

        confusion_matrix = test_sequence_labeler(labeler_true, samples, labels);
        dlog << LINFO << "True HMM model: ";
        dlog << LINFO << confusion_matrix;
        accuracy = sum(diag(confusion_matrix))/sum(confusion_matrix);
        dlog << LINFO << "label accuracy: "<< accuracy;
        DLIB_TEST(std::abs(accuracy - 0.882) < 0.01);

        print_spinner();

        // Finally, the labeler can be serialized to disk just like most dlib objects.
        ostringstream sout;
        serialize(labeler, sout);

        sequence_labeler<fe_type> labeler2;
        // recall from disk
        istringstream sin(sout.str());
        deserialize(labeler2, sin);
        confusion_matrix = test_sequence_labeler(labeler2, samples, labels);
        dlog << LINFO << "deserialized labeler: ";
        dlog << LINFO << confusion_matrix;
        accuracy = sum(diag(confusion_matrix))/sum(confusion_matrix);
        dlog << LINFO << "label accuracy: "<< accuracy;
        DLIB_TEST(std::abs(accuracy - 0.882) < 0.01);
    }

// ----------------------------------------------------------------------------------------

    void test2()
    {
        /*
            The point of this test is to make sure calling set_feature() multiple
            times works the way it is supposed to.
        */

        print_spinner();
        std::vector<funny_sequence> samples;
        std::vector<std::vector<unsigned long> > labels;

        matrix<double> transition_probabilities(num_label_states, num_label_states);
        transition_probabilities = 0.05, 0.90, 0.05,
        0.05, 0.05, 0.90,
        0.90, 0.05, 0.05;

        matrix<double> emission_probabilities(num_label_states,num_sample_states);
        emission_probabilities = 0.5, 0.5, 0.0,
        0.0, 0.5, 0.5,
        0.5, 0.0, 0.5;

        make_dataset(transition_probabilities,emission_probabilities,
                     samples, labels, 1000);

        dlog << LINFO << "samples.size(): "<< samples.size();

        structural_sequence_labeling_trainer<feature_extractor> trainer;
        structural_sequence_labeling_trainer<feature_extractor_partial> trainer_part;
        trainer.set_c(4);
        trainer_part.set_c(4);
        trainer.set_num_threads(4);
        trainer_part.set_num_threads(4);

        // Learn to do sequence labeling from the dataset
        sequence_labeler<feature_extractor> labeler = trainer.train(samples, labels);
        sequence_labeler<feature_extractor_partial> labeler_part = trainer_part.train(samples, labels);

        // Both feature extractors should be equivalent.
        DLIB_TEST(length(labeler.get_weights() - labeler_part.get_weights()) < 1e-10);

    }

// ----------------------------------------------------------------------------------------

    class sequence_labeler_tester : public tester
    {
    public:
        sequence_labeler_tester (
        ) :
            tester ("test_sequence_labeler",
                    "Runs tests on the sequence labeling code.")
        {}

        void perform_test (
        )
        {
            do_test<feature_extractor>();
            DLIB_TEST(called_rejct_labeling == false);
            do_test<feature_extractor2>();
            DLIB_TEST(called_rejct_labeling == true);

            test2();
        }
    } a;

}


#ifndef DART_PROTO_EIGEN
#define DART_PROTO_EIGEN

#include <Eigen/Dense>

#include "dart/math/MathTypes.hpp"
#include "dart/proto/Eigen.pb.h"

namespace dart {
namespace proto {

void serializeVector(proto::VectorXs& proto, const Eigen::VectorXs& vec);
Eigen::VectorXs deserializeVector(const proto::VectorXs& proto);

void serializeMatrix(proto::MatrixXs& proto, const Eigen::MatrixXs& mat);
Eigen::MatrixXs deserializeMatrix(const proto::MatrixXs& proto);

} // namespace proto
} // namespace dart

#endif


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

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

 * See LICENSE for the license information

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

/**
 * @file    Errors.cpp
 * @brief   Factor Graph Values
 * @brief   Errors
 * @author  Carlos Nieto
 * @author  Christian Potthast
 */

#include <boost/range/adaptor/map.hpp>
#include <gtsam/linear/Errors.h>
#include <gtsam/linear/VectorValues.h>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
Errors::Errors(){}

/* ************************************************************************* */
Errors::Errors(const VectorValues& V) {
  for(const Vector& e: V | boost::adaptors::map_values) {
    push_back(e);
  }
}

/* ************************************************************************* */
void Errors::print(const std::string& s) const {
  cout << s << endl;
  for(const Vector& v: *this)
    gtsam::print(v);
}

/* ************************************************************************* */
struct equalsVector : public std::function<bool(const Vector&, const Vector&)> {
  double tol_;
  equalsVector(double tol = 1e-9) : tol_(tol) {}
  bool operator()(const Vector& expected, const Vector& actual) {
    return equal_with_abs_tol(expected, actual,tol_);
  }
};

bool Errors::equals(const Errors& expected, double tol) const {
  if( size() != expected.size() ) return false;
  return equal(begin(),end(),expected.begin(),equalsVector(tol));
}

/* ************************************************************************* */
Errors Errors::operator+(const Errors& b) const {
#ifndef NDEBUG
  size_t m = size();
  if (b.size()!=m)
    throw(std::invalid_argument("Errors::operator+: incompatible sizes"));
#endif
  Errors result;
  Errors::const_iterator it = b.begin();
    for(const Vector& ai: *this)
    result.push_back(ai + *(it++));
  return result;
}

/* ************************************************************************* */
Errors Errors::operator-(const Errors& b) const {
#ifndef NDEBUG
  size_t m = size();
  if (b.size()!=m)
    throw(std::invalid_argument("Errors::operator-: incompatible sizes"));
#endif
  Errors result;
  Errors::const_iterator it = b.begin();
  for(const Vector& ai: *this)
    result.push_back(ai - *(it++));
  return result;
}

/* ************************************************************************* */
Errors Errors::operator-() const {
  Errors result;
  for(const Vector& ai: *this)
    result.push_back(-ai);
  return result;
}

/* ************************************************************************* */
double dot(const Errors& a, const Errors& b) {
#ifndef NDEBUG
  size_t m = a.size();
  if (b.size()!=m)
    throw(std::invalid_argument("Errors::dot: incompatible sizes"));
#endif
  double result = 0.0;
  Errors::const_iterator it = b.begin();
  for(const Vector& ai: a)
    result += gtsam::dot(ai, *(it++));
  return result;
}

/* ************************************************************************* */
template<>
void axpy(double alpha, const Errors& x, Errors& y) {
  Errors::const_iterator it = x.begin();
  for(Vector& yi: y)
    yi += alpha * (*(it++));
}

/* ************************************************************************* */
void print(const Errors& a, const string& s) {
  a.print(s);
}

/* ************************************************************************* */

} // gtsam


//=======================================================================
// Copyright (c) 2014 Piotr Smulewicz
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file in_balls.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-01-29
 */

#ifndef PAAL_IN_BALLS_HPP
#define PAAL_IN_BALLS_HPP

#include <boost/test/unit_test.hpp>

namespace paal {

template <typename IT, typename CE, typename Metric, typename Dist>
void in_balls(const IT &items, const CE &centers, const Metric &metric,
              Dist radius) {
    static auto const EPSILON = 0.00001;
    for (auto j : items) {
        bool inBall = false;
        for (auto i : centers) {
            if (metric(j, i) <= radius + EPSILON) {
                inBall = true;
                break;
            }
        }
        if (!inBall) {
            LOGLN("Item " << j << " is not included in any ball");
            ON_LOG(for (auto i
                        : centers) {)
                LOGLN("Distance to " << i << " = " << metric(j, i));
            ON_LOG(
            })
        }
        BOOST_CHECK(inBall);
    }
}
} //! paal
#endif // PAAL_IN_BALLS_HPP


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file sort_traverser_test.cpp
/// \brief Add your file description here.

#define BOOST_TEST_MODULE sort_traverser_test
#include <boost/test/included/unit_test.hpp>
#include "mcrl2/pbes/find.h"
#include "mcrl2/pbes/txt2pbes.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

void test_pbes(const std::string& text)
{
  pbes p = txt2pbes(text);
  std::set<data::sort_expression> sorts;
  pbes_system::find_sort_expressions(p, std::inserter(sorts, sorts.end()));
  std::cerr << "sorts: " << data::pp(data::sort_expression_list(sorts.begin(), sorts.end())) << std::endl;
}

BOOST_AUTO_TEST_CASE(test1)
{
  std::string t1 =
    "% simple test, n is constant \n"
    "                             \n"
    "pbes nu X(n: Nat) =          \n"
    "       X(n);                 \n"
    "                             \n"
    "init X(0);                   \n"
  ;

  std::string t2 =
    "% simple test, n is NOT constant \n"
    "                                 \n"
    "pbes nu X(n: Nat) =              \n"
    "       X(n + 1);                 \n"
    "                                 \n"
    "init X(0);                       \n"
  ;

  std::string t3 =
    "% multiple parameters: n1 is constant, n2 is not \n"
    "                                                 \n"
    "pbes nu X(n1,n2: Nat) =                          \n"
    "       X(n1, n2 + 1);                            \n"
    "                                                 \n"
    "init X(0, 1);                                    \n"
  ;

  std::string t4 =
    "% conditions: n is not constant, even if conditions are enabled\n"
    "                                                               \n"
    "pbes nu X(n: Nat) =                                            \n"
    "       val(2 < n) || X(n + 1);                                 \n"
    "                                                               \n"
    "init X(0);                                                     \n"
  ;

  std::string t5 =
    "% conditions: n is constant if conditions are enabled \n"
    "                                                      \n"
    "pbes nu X(n: Nat) =                                   \n"
    "       val(2 < n) || X(n + 1);                        \n"
    "                                                      \n"
    "init X(5);                                            \n"
  ;

  std::string t6 =
    "% reachability: n1 is not constant, equation Y is not reachable and should be removed \n"
    "                                                                                \n"
    "pbes nu X(n1: Nat) = X(n1+1);                                                         \n"
    "     mu Y(n2: Nat) = Y(n2+1);                                                         \n"
    "                                                                                \n"
    "init X(5);                                                                            \n"
  ;

  std::string t7 =
    "% multiple edges from one vertex, one edge invalidates the assertion of the other: no constants should be found \n"
    "                                                                                \n"
    "pbes                                                                                \n"
    "   mu X1(n1,m1:Nat) = X2(n1) || X2(m1);                                                                         \n"
    "   mu X2(n2:Nat)     = X1(n2,n2);                                                                               \n"
    "init                                                                                \n"
    "   X1(2,1);                                                                                \n"
  ;

  std::string t8 =
    "% conditions: parameters b,c and d will always be removed, with conditions on, parameter \n"
    "% n will NOT be removed. changing b,c or d to false or n to a bigger number              \n"
    "% then 5 WILL result in the removal of n                                                 \n"
    "                                                                                \n"
    "pbes nu X(b,c,d:Bool, n:Nat) =                                                           \n"
    "    val(b) || ( val(c) && ( val(d) && (val(n > 5) || X(b,c,d,n+1))));                    \n"
    "                                                                                \n"
    "init                                                                                \n"
    "X(false,true,true,5);                                                                    \n"
  ;

  std::string t9 =
    "% conditions: universal quantification which can be solved, n1 and n2 are \n"
    "% constants if conditions are enabled                                     \n"
    "                                                                          \n"
    "pbes mu X(n1,n2:Nat) =                                                    \n"
    "    forall m:Nat. (val(n1>n2) && X(n1+1,n2+1));                           \n"
    "                                                                          \n"
    "init X(1,2);                                                              \n"
  ;

  std::string t10 =
    "% conditions: existential quantification which can be solved, n1 and n2 \n"
    "% are constants if conditions are enabled                               \n"
    "                                                                        \n"
    "pbes mu X(n1,n2:Nat) =                                                  \n"
    "    exists m:Nat. (val(n1>n2) && X(n1+1,n2+1));                         \n"
    "                                                                        \n"
    "init X(1,2);                                                            \n"
  ;

  std::string t11 =
    "% conditions: universal quantification which cannot be solved \n"
    "% (by the current rewriter), n1 and n2 are constants, but not \n"
    "% detected as such                                            \n"
    "                                                              \n"
    "pbes mu X(n1,n2:Nat) =                                        \n"
    "    forall m:Nat. (val(m>n2) && X(n1+1,n2+1));                \n"
    "                                                              \n"
    "init X(1,2);                                                  \n"
  ;

  std::string t12 =
    "% example 4.2.1 from \"Tools for PBES\" report                                     \n"
    "% constants without conditions: o4, n2, n4, o4                                     \n"
    "% constants with conditions: everything (equations X1,X2,X3 and X4 are removed)    \n"
    "                                                                                \n"
    "pbes                                                                               \n"
    "   mu X1(n1,m1,o1,p1:Nat) = (val(n1<m1) || X2(o1,p1)) && X3(n1) && X1(n1,m1,4,p1); \n"
    "   mu X2(n2,m2:Nat)       = X5(n2,m2) || X5(m2,n2);                                \n"
    "   nu X3(n3:Nat)          = val(n3<3) || X1(n3,n3,4,n3+1);                         \n"
    "   nu X4(n4,m4,o4:Nat)    = val(n4 <= m4+o4) || (X3(n4) && X4(n4,m4+1,n4));        \n"
    "   mu X5(n5,m5:Nat)       = val(n5>m5) || X3(n5);                                  \n"
    "                                                                                \n"
    "init                                                                               \n"
    "   X4(0,0,0);                                                                      \n"
  ;

  std::string t13 =
    "pbes nu X =           \n"
    "        Y(true);      \n"
    "      mu Y(b: Bool) = \n"
    "        X;            \n"
    "                      \n"
    "init X;               \n"
  ;

  std::string t14 =
    "pbes nu X(m:Nat) =           \n"
    "        forall n:Nat . X(n); \n"
    "                             \n"
    "init X(0);                   \n"
  ;

  test_pbes(t1);
  test_pbes(t2);
  test_pbes(t3);
  test_pbes(t4);
  test_pbes(t5);
  test_pbes(t6);
  test_pbes(t7);
  test_pbes(t8);
  test_pbes(t9);
  test_pbes(t10);
  test_pbes(t11);
  test_pbes(t12);
  test_pbes(t13);
  test_pbes(t14);
}


#define BOOST_TEST_MODULE pcraster geo algorithm
#include <boost/test/unit_test.hpp>
#include <iterator>
#include "geo_algorithm.h"
#include "geo_point.h"
#include "geo_pointvalue.h"

BOOST_AUTO_TEST_CASE(points_in_area)
{
  using namespace geo;

  {
    typedef Point<int, 2> Punt;
    typedef double Value;
    typedef std::vector<PointValue<Punt, Value> > PointValues;

    Value value(0.0);
    PointValues points;
    size_t nrRows = 3;
    size_t nrCols = 3;

    for(size_t row = 0; row < nrRows; ++row) {
      for(size_t col = 0; col < nrCols; ++col) {
        points.push_back(PointValue<Punt, Value>(Punt(row, col), value));
      }
    }

    Punt point(1, 1);
    PointValues subset;
    Punt::CoordinateType radius = 0;

    subset.clear();
    pointsInArea(point, radius, points.begin(), points.end(),
         std::back_inserter(subset));
    BOOST_CHECK(subset.size() == 9);

    radius = 1;
    subset.clear();
    pointsInArea(point, radius, points.begin(), points.end(),
         std::back_inserter(subset));
    BOOST_CHECK(subset.size() == 5);

    radius = 2;
    subset.clear();
    pointsInArea(point, radius, points.begin(), points.end(),
         std::back_inserter(subset));
    BOOST_CHECK(subset.size() == 9);
  }
}

BOOST_AUTO_TEST_CASE(maximum_)
{
  using namespace geo;

  {
    typedef Point<int, 2> Punt;
    typedef int Value;
    typedef std::vector<PointValue<Punt, Value> > PointValues;

    PointValues points;
    size_t nrRows = 3;
    size_t nrCols = 3;

    for(size_t row = 0; row < nrRows; ++row) {
      for(size_t col = 0; col < nrCols; ++col) {
        points.push_back(PointValue<Punt, Value>(Punt(row, col),
              row * nrCols + col));
      }
    }

    Punt point(1, 1);
    PointValues subset;
    Punt::CoordinateType radius;
    bool result;
    Value max;

    radius = 0;
    subset.clear();
    result = maximum(max, point, radius, points.begin(), points.end());
    BOOST_CHECK(result == true);
    BOOST_CHECK(max == 8);

    radius = 1;
    subset.clear();
    result = maximum(max, point, radius, points.begin(), points.end());
    BOOST_CHECK(result == true);
    BOOST_CHECK(max == 7);

    radius = 2;
    subset.clear();
    result = maximum(max, point, radius, points.begin(), points.end());
    BOOST_CHECK(result == true);
    BOOST_CHECK(max == 8);
  }

  {
    typedef Point<double, 2> Point;
    typedef int Value;
    typedef std::vector<PointValue<Point, Value> > PointValues;

    PointValues points;
    size_t nrRows = 3;
    size_t nrCols = 3;

    for(size_t row = 0; row < nrRows; ++row) {
      for(size_t col = 0; col < nrCols; ++col) {
        points.push_back(PointValue<Point, Value>(Point(row, col),
              row * nrCols + col));
      }
    }

    Point point(1, 1);
    PointValues subset;
    Point::CoordinateType radius;
    bool result;
    Value max;

    radius = 0;
    subset.clear();
    result = maximum(max, point, radius, points.begin(), points.end());
    BOOST_CHECK(result == true);
    BOOST_CHECK(max == 8);

    radius = 1;
    subset.clear();
    result = maximum(max, point, radius, points.begin(), points.end());
    BOOST_CHECK(result == true);
    BOOST_CHECK(max == 7);

    radius = 2;
    subset.clear();
    result = maximum(max, point, radius, points.begin(), points.end());
    BOOST_CHECK(result == true);
    BOOST_CHECK(max == 8);
  }
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_COPYSIGN_INCLUDE
#define MTL_COPYSIGN_INCLUDE

#include <complex>
#include <cmath>
#include <boost/numeric/mtl/operation/real.hpp>
#include <boost/numeric/mtl/operation/imag.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl {

namespace sfunctor {

    template <typename Value1, typename Value2>
    struct copysign
    {
	static inline Value1 apply(const Value1& v, const Value2& s)
	{
	    using math::zero; using std::abs;

	    Value1 a(abs(v));
	    return s < zero(s) ? -a : a;
	}
    };

    // This specialization is questionable and thus subject to elimination
    template <typename Value1, typename Value2>
    struct copysign<std::complex<Value1>, Value2>
    {
	static inline Value1 apply(const std::complex<Value1>& v, const Value2& s)
	{
	    using mtl::real; using mtl::imag;
	    return std::complex<Value1>(copysign<Value1, Value2>::apply(real(v), s),
					copysign<Value1, Value2>::apply(imag(v), s));
	}
    };
#if 0 // ndef _MSC_VER doesn't work on BigRed with gcc 4.2.2 (????) and isn't used anyway
    template <>
    struct copysign<float, float>
    {
	static inline float apply(float v, float s)
	{ return ::copysignf(v, s); }
    };

    template <>
    struct copysign<double, double>
    {
	static inline double apply(double v, double s)
	{ return ::copysign(v, s); }
    };

    template <>
    struct copysign<long double, long double>
    {
	static inline long double apply(long double v, long double s)
	{ return copysignl(v, s); }
    };
#endif // _MSC_VER
}

/// sign of scalars; for complex numbers sign of real part
template <typename Value1, typename Value2>
inline Value1 copysign(const Value1& v, const Value2& s)
{
    vampir_trace<1001> tracer;
    return sfunctor::copysign<Value1, Value2>::apply(v, s);
}

} // namespace mtl

#endif // MTL_COPYSIGN_INCLUDE


// Copyright 2017 Peter Dimov.
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/mp11/mpl.hpp>
#include <boost/mp11/list.hpp>

#include <boost/mpl/at.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/distance.hpp>
#include <boost/mpl/clear.hpp>
#include <boost/mpl/empty.hpp>
#include <boost/mpl/erase.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/insert_range.hpp>
#include <boost/mpl/is_sequence.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/push_front.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/reverse.hpp>
#include <boost/mpl/remove.hpp>
#include <boost/mpl/copy.hpp>
#include <boost/mpl/back_inserter.hpp>

#include <boost/core/lightweight_test_trait.hpp>
#include <type_traits>
#include <tuple>

template<class T> using add_pointer_t = typename std::add_pointer<T>::type;

template<class L1> void test()
{
    namespace mpl = boost::mpl;
    using namespace boost::mp11;

    // intrinsics

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::at<L1, mpl::int_<0>>::type, mp_at_c<L1, 0>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::at<L1, mpl::int_<1>>::type, mp_at_c<L1, 1>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::at<L1, mpl::int_<2>>::type, mp_at_c<L1, 2>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::at_c<L1, 0>::type, mp_at_c<L1, 0>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::at_c<L1, 1>::type, mp_at_c<L1, 1>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::at_c<L1, 2>::type, mp_at_c<L1, 2>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::back<L1>::type, float>));

    BOOST_TEST_EQ((mpl::distance<typename mpl::begin<L1>::type, typename mpl::end<L1>::type>::type::value), mp_size<L1>::value);

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::clear<L1>::type, mp_clear<L1>>));

    BOOST_TEST_TRAIT_FALSE((typename mpl::empty<L1>::type));
    BOOST_TEST_TRAIT_TRUE((typename mpl::empty<mp_clear<L1>>::type));

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::erase<L1, typename mpl::begin<L1>::type>::type, mp_pop_front<L1>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::front<L1>::type, mp_front<L1>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::insert<L1, typename mpl::begin<L1>::type, void>::type, mp_push_front<L1, void>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::insert<L1, typename mpl::end<L1>::type, void>::type, mp_push_back<L1, void>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::insert_range<L1, typename mpl::end<L1>::type, L1>::type, mp_append<L1, L1>>));

    BOOST_TEST_TRAIT_TRUE((typename mpl::is_sequence<L1>::type));

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::pop_front<L1>::type, mp_pop_front<L1>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::push_back<L1, char>::type, mp_push_back<L1, char>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::push_front<L1, char>::type, mp_push_front<L1, char>>));

    BOOST_TEST_EQ((mpl::size<L1>::type::value), mp_size<L1>::value);

    // algorithms

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::transform<L1, std::add_pointer<mpl::_1>>::type, mp_transform<add_pointer_t, L1>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::reverse<L1>::type, mp_reverse<L1>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<typename mpl::remove<L1, int>::type, mp_remove<L1, int>>));

    using L2 = typename mpl::copy<L1, mpl::back_inserter<mpl::vector<>>>::type;
    using L3 = typename mpl::copy<L2, mpl::back_inserter<mp_clear<L1>>>::type;

    BOOST_TEST_TRAIT_TRUE((std::is_same<L1, L3>));
}

int main()
{
    using boost::mp11::mp_list;

    test<mp_list<int, void, float>>();
    test<std::tuple<int, long, float>>(); // MPL instantiates the tuple, so no 'void'

    return boost::report_errors();
}


/*
 [auto_generated]
 boost/numeric/odeint/util/ublas_wrapper.hpp

 [begin_description]
 Resizing for ublas::vector and ublas::matrix
 [end_description]

 Copyright 2011-2013 Mario Mulansky
 Copyright 2011-2013 Karsten Ahnert

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_UTIL_UBLAS_WRAPPER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_UTIL_UBLAS_WRAPPER_HPP_INCLUDED

#include <boost/type_traits/integral_constant.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>

#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>

#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>

/* extend ublas by a few operations */

/* map norm_inf onto reduce( v , default_operations::maximum ) */
namespace boost { namespace numeric { namespace odeint {

    template< typename T , typename A >
    struct vector_space_norm_inf< boost::numeric::ublas::vector<T,A> >
    {
        typedef T result_type;

        result_type operator()( const boost::numeric::ublas::vector<T,A> &x ) const
        {
            return boost::numeric::ublas::norm_inf( x );
        }
    };

    template< class T , class L , class A >
    struct vector_space_norm_inf< boost::numeric::ublas::matrix<T,L,A> >
    {
        typedef T result_type;

        result_type operator()( const boost::numeric::ublas::matrix<T,L,A> &x ) const
        {
            return boost::numeric::ublas::norm_inf( x );
        }
    };
} } }

/* additional operations:
 * abs( v )
 * v / w
 * a + v
 */
namespace boost { namespace numeric { namespace ublas {

    // elementwise abs - calculates absolute values of the elements
    template<class T>
    struct scalar_abs: public scalar_unary_functor<T> {
        typedef typename scalar_unary_functor<T>::value_type value_type;
        typedef typename scalar_unary_functor<T>::argument_type argument_type;
        typedef typename scalar_unary_functor<T>::result_type result_type;

        static BOOST_UBLAS_INLINE
        result_type apply (argument_type t) {
            using std::abs;
            return abs (t);
        }
    };

    // (abs v) [i] = abs (v [i])
    template<class E>
    BOOST_UBLAS_INLINE
    typename vector_unary_traits<E, scalar_abs<typename E::value_type> >::result_type
    abs (const vector_expression<E> &e) {
        typedef typename vector_unary_traits<E, scalar_abs<typename E::value_type> >::expression_type expression_type;
        return expression_type (e ());
    }

    // (abs m) [i] = abs (m [i])
    template<class E>
    BOOST_UBLAS_INLINE
    typename matrix_unary1_traits<E, scalar_abs<typename E::value_type> >::result_type
    abs (const matrix_expression<E> &e) {
        typedef typename matrix_unary1_traits<E, scalar_abs<typename E::value_type> >::expression_type expression_type;
        return expression_type (e ());
    }

    // elementwise division (v1 / v2) [i] = v1 [i] / v2 [i]
    template<class E1, class E2>
    BOOST_UBLAS_INLINE
    typename vector_binary_traits<E1, E2, scalar_divides<typename E1::value_type,
                                                         typename E2::value_type> >::result_type
    operator / (const vector_expression<E1> &e1,
                const vector_expression<E2> &e2) {
        typedef typename vector_binary_traits<E1, E2, scalar_divides<typename E1::value_type,
                                                                     typename E2::value_type> >::expression_type expression_type;
        return expression_type (e1 (), e2 ());
    }

    // elementwise division (m1 / m2) [i] = m1 [i] / m2 [i]
    template<class E1, class E2>
    BOOST_UBLAS_INLINE
    typename matrix_binary_traits<E1, E2, scalar_divides<typename E1::value_type,
    typename E2::value_type> >::result_type
    operator / (const matrix_expression<E1> &e1,
                const matrix_expression<E2> &e2) {
        typedef typename matrix_binary_traits<E1, E2, scalar_divides<typename E1::value_type,
                                                                     typename E2::value_type> >::expression_type expression_type;
        return expression_type (e1 (), e2 ());
    }

    // addition with scalar
    // (t + v) [i] = t + v [i]
    template<class T1, class E2>
    BOOST_UBLAS_INLINE
    typename enable_if< is_convertible<T1, typename E2::value_type >,
    typename vector_binary_scalar1_traits<const T1, E2, scalar_plus<T1, typename E2::value_type> >::result_type
    >::type
    operator + (const T1 &e1,
                const vector_expression<E2> &e2) {
        typedef typename vector_binary_scalar1_traits<const T1, E2, scalar_plus<T1, typename E2::value_type> >::expression_type expression_type;
        return expression_type (e1, e2 ());
    }

    // addition with scalar
    // (t + m) [i] = t + m [i]
    template<class T1, class E2>
    BOOST_UBLAS_INLINE
    typename enable_if< is_convertible<T1, typename E2::value_type >,
    typename matrix_binary_scalar1_traits<const T1, E2, scalar_plus<T1, typename E2::value_type> >::result_type
    >::type
    operator + (const T1 &e1,
                const matrix_expression<E2> &e2) {
        typedef typename matrix_binary_scalar1_traits<const T1, E2, scalar_plus<T1, typename E2::value_type> >::expression_type expression_type;
        return expression_type (e1, e2 ());
    }

} } }

/* add resize functionality */
namespace boost {
namespace numeric {
namespace odeint {

/*
 * resizeable specialization for boost::numeric::ublas::vector
 */
template< class T , class A >
struct is_resizeable< boost::numeric::ublas::vector< T , A > >
{
    typedef boost::true_type type;
    const static bool value = type::value;
};

/*
 * resizeable specialization for boost::numeric::ublas::matrix
 */
template< class T , class L , class A >
struct is_resizeable< boost::numeric::ublas::matrix< T , L , A > >
{
    typedef boost::true_type type;
    const static bool value = type::value;
};

/*
 * resizeable specialization for boost::numeric::ublas::permutation_matrix
 */
template< class T , class A >
struct is_resizeable< boost::numeric::ublas::permutation_matrix< T , A > >
{
    typedef boost::true_type type;
    const static bool value = type::value;
};

// specialization for ublas::matrix
// same size and resize specialization for matrix-matrix resizing
template< class T , class L , class A , class T2 , class L2 , class A2 >
struct same_size_impl< boost::numeric::ublas::matrix< T , L , A > , boost::numeric::ublas::matrix< T2 , L2 , A2 > >
{
    static bool same_size( const boost::numeric::ublas::matrix< T , L , A > &m1 ,
                           const boost::numeric::ublas::matrix< T2 , L2 , A2 > &m2 )
    {
        return ( ( m1.size1() == m2.size1() ) && ( m1.size2() == m2.size2() ) );
    }
};

template< class T , class L , class A , class T2 , class L2 , class A2 >
struct resize_impl< boost::numeric::ublas::matrix< T , L , A > , boost::numeric::ublas::matrix< T2 , L2 , A2 > >
{
    static void resize( boost::numeric::ublas::matrix< T , L , A > &m1 ,
                        const boost::numeric::ublas::matrix< T2 , L2 , A2 > &m2 )
    {
        m1.resize( m2.size1() , m2.size2() );
    }
};

// same size and resize specialization for matrix-vector resizing
template< class T , class L , class A , class T_V , class A_V >
struct same_size_impl< boost::numeric::ublas::matrix< T , L , A > , boost::numeric::ublas::vector< T_V , A_V > >
{
    static bool same_size( const boost::numeric::ublas::matrix< T , L , A > &m ,
                           const boost::numeric::ublas::vector< T_V , A_V > &v )
    {
        return ( ( m.size1() == v.size() ) && ( m.size2() == v.size() ) );
    }
};

template< class T , class L , class A , class T_V , class A_V >
struct resize_impl< boost::numeric::ublas::matrix< T , L , A > , boost::numeric::ublas::vector< T_V , A_V > >
{
    static void resize( boost::numeric::ublas::matrix< T , L , A > &m ,
                        const boost::numeric::ublas::vector< T_V , A_V > &v )
    {
        m.resize( v.size() , v.size() );
    }
};

// specialization for ublas::permutation_matrix
// same size and resize specialization for matrix-vector resizing
template< class T , class A , class T_V , class A_V >
struct same_size_impl< boost::numeric::ublas::permutation_matrix< T , A > ,
                       boost::numeric::ublas::vector< T_V , A_V > >
{
    static bool same_size( const boost::numeric::ublas::permutation_matrix< T , A > &m ,
                           const boost::numeric::ublas::vector< T_V , A_V > &v )
    {
        return ( m.size() == v.size() ); // && ( m.size2() == v.size() ) );
    }
};

template< class T , class A , class T_V , class A_V >
struct resize_impl< boost::numeric::ublas::vector< T_V , A_V > ,
                    boost::numeric::ublas::permutation_matrix< T , A > >
{
    static void resize( const boost::numeric::ublas::vector< T_V , A_V > &v,
                        boost::numeric::ublas::permutation_matrix< T , A > &m )
    {
        m.resize( v.size() , v.size() );
    }
};

template< class T , class A >
struct state_wrapper< boost::numeric::ublas::permutation_matrix< T , A > > // with resizing
{
    typedef boost::numeric::ublas::permutation_matrix< T , A > state_type;
    typedef state_wrapper< state_type > state_wrapper_type;

    state_type m_v;

    state_wrapper() : m_v( 1 ) // permutation matrix constructor requires a size, choose 1 as default
    { }

};

} } }

#endif // BOOST_NUMERIC_ODEINT_UTIL_UBLAS_WRAPPER_HPP_INCLUDED


#include <iostream>
#include <vector>
#include <type_traits>
#include <numeric>
#include <string>
#include <gsl/gsl>
#include <concepts>
#include <cosan/data/CosanData.h>
// // Maybe useful to restricted to only CosanBO class.
// template <class T, class U>
// concept Derived = std::is_base_of<U, T>::value;

// // Useful for Metric, Model, Splittor class.
// template <class T, class U>
// concept Derived = std::is_base_of<U, T>::value;

// template<Derived<Base> T>
// void f(T);  // T is constrained by Derived<T, Base>

template<typename NumericType>
concept Numeric = std::is_arithmetic<NumericType>::value ;

// template<Numeric  T>
template<typename NumericType,
             typename = typename std::enable_if<std::is_arithmetic<NumericType>::value,NumericType>::type>
void f(NumericType a){
	std::cout<<a<<std::endl;

}
// template<typename T> using Ref = T&;
// template<typename T> concept C =
// requires {
//     typename T::inner; // required nested member name
//     typename S<T>;     // required class template specialization
//     typename Ref<T>;   // required alias template substitution
// };

// template <class T, class U> using CommonType = std::common_type_t<T, U>;
// template <class T, class U> concept Common =
// requires (T&& t, U&& u) {
//     typename CommonType<T, U>; // CommonType<T, U> is valid and names a type
//     { CommonType<T, U>{std::forward<T>(t)} };
//     { CommonType<T, U>{std::forward<U>(u)} };
// };

int main(){
	f<std::string>(3);
	int
	Cosan::CosanMatrix<double> M(5,4);
	return 0;
}

// template<typename T1>
// class A{
//   T1 a;
//   template<class T2,
//            std::enable_if_t<std::is_same_v<std::decay_t<T2>,std::vector<T1>>,bool> = true>
//   void foo(T2 && a){
//     // stuff
//     }

// }

// int main(){
// 	return 0;
// }

// #include <iostream>
// #include <vector>
// #include <type_traits>
// #include <Eigen/dense>

// template<class T,
//          std::enable_if_t<std::is_same_v<std::decay_t<T>,Eigen::MatrixXd>,bool> = true>
// void foo(T && a){
// 	std::cout<<"Hello"<<std::endl;
// }
// int main(){
// 	Eigen::MatrixXd a;
// 	a.resize(3,3);
// 	a<<1,2,3,
// 	   4,5,6,
// 	   7,8,9;
// 	std::cout<<a(std::vector<int>({0,2}),Eigen::all)<<std::endl;
// 	foo(a(std::vector<int>({0,2}),Eigen::all));

// 	return 0;
// }


#include "tkdCmdParser.h"

#include "vnl/vnl_vector.h"
#include "vnl/vnl_matrix.h"
#include "vnl/vnl_matrix_fixed.h"
#include <vnl/algo/vnl_cholesky.h>
#include <vnl/vnl_transpose.h>

#include <cmath>
#include <ctime>

#include <list>

#include <itkImage.h>
#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>
#include <itkImageRegionIteratorWithIndex.h>
#include <itkImageLinearIteratorWithIndex.h>
#include <itkImageRegionIterator.h>
#include <itkDiffusionTensor3D.h>

#include <boost/tokenizer.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random.hpp>
#include <boost/math/distributions/gamma.hpp>

/**
 * dki namespace.
 */
namespace dki
{

	struct parameters
	{
		std::string inputFileName;
		std::string maskFileName;
		std::string bvecsFileName;
		std::string bvalsFileName;
		std::string outputFileName;
		std::string sep;
		int iterations;
		bool regularize;
	};

	typedef double PixelType;
	typedef unsigned char MaskPixelType;

	typedef itk::Image< PixelType, 4 > ImageType;
	typedef itk::Image< MaskPixelType, 3 > MaskImageType;

	typedef itk::Image< PixelType, 3 > OutputImageType;

	typedef itk::DiffusionTensor3D< PixelType > DTITensorType;
	typedef vnl_vector_fixed< PixelType, 15 > DKITensorType;

	typedef itk::ImageFileReader< ImageType > ReaderType;
	typedef itk::ImageFileReader< MaskImageType > MaskReaderType;

	typedef vnl_vector< PixelType > VectorType;
	typedef vnl_matrix< PixelType > MatrixType;

	typedef itk::ImageLinearConstIteratorWithIndex< ImageType > ConstIterator4DType;
	typedef itk::ImageRegionConstIteratorWithIndex< MaskImageType > ConstIterator3DType;
	typedef itk::ImageRegionIteratorWithIndex< OutputImageType > Iterator3DType;

	typedef itk::ImageFileWriter< OutputImageType > WriterType;

	typedef boost::tokenizer< boost::char_separator< char > > TokType;
	typedef boost::mt19937 random_number_type;

	/**
	 * Return 2-rand DTI tensor.
	 *
	 * ITK tensor layout =>
	 * 					| 0  1  2  |
	 *			        | X  3  4  |
	 *       			| X  X  5  |
	 */
	static DTITensorType GetDTITensor( const VectorType& theta )
	{
		DTITensorType D( 0.0 );

		D( 0, 0 ) = theta( 1 );
		D( 0, 1 ) = theta( 4 );
		D( 0, 2 ) = theta( 6 );
		D( 1, 1 ) = theta( 2 );
		D( 1, 2 ) = theta( 5 );
		D( 2, 2 ) = theta( 3 );

		return D;
	}

	/**
	 * Return 4-rand DKI tensor (for now a fixed vector with 15 elements).
	 *
	 * theta_dki = [W_1111, W_2222, W_3333,
	 * 				W_1112, W_2223, W_2221,
	 * 				W_2223, W_3331, W_3332,
	 * 				W_1122, W_1133, W_2233,
	 * 				W_1123, W_1223, W_1233]
	 */
	DKITensorType GetDKITensor( const VectorType& theta )
	{
		DKITensorType tensor;
		for( unsigned int i = 0; i < 15; i++ )
			tensor( i ) = theta( 7 + i );

		return tensor;
	}

	/**
	 * Predicate to remove non positive semi-definite DTI/DKI tensors.
	 */
	class non_positive_definite
	{
		public:

		/**
		 * Check for non positive semi-definite tensor.
		 */
		bool operator() ( const VectorType& beta, bool dki_tensor = false )
		{
			DTITensorType DT = GetDTITensor( beta );

			DTITensorType::EigenValuesArrayType eigenValues;
			DT.ComputeEigenValues( eigenValues );

			bool non_negative = false;

			for( unsigned int i = 0; i < 3; i++ )
				if( eigenValues[ i ] < 0 )
					non_negative = true;

			if( dki_tensor )
			{
				DKITensorType DK = GetDKITensor( beta );
				// TODO calculate if DK tensor is postive semi-definite ...
			}

			return non_negative;
		}
	};

	/**
	 * Date, 06-01-2011.
	 *
	 * Implement Gibbs sampler for linear DTI fits.
	 *
	 * Ref: "Introduction to Applied Bayesian Statistics and Estimation for Social Scientists, Scott M. Lynch, 2007. Springer"
	 */
	class DKIFit
    {
        public:

		ImageType::Pointer Input;
		MaskImageType::Pointer Mask;

		MatrixType X_dti; // dti design matrix
		MatrixType X_dki; // dki design matrix
		MatrixType Z; // covariance matrix
		VectorType B; // bvals
		MatrixType R; // bvecs

		OutputImageType::Pointer S0_dti;
		OutputImageType::Pointer S0_dki;

		OutputImageType::Pointer FA_dti;
		OutputImageType::Pointer FA_dki;

		OutputImageType::Pointer Trace_dti;
		OutputImageType::Pointer Trace_dki;

		OutputImageType::Pointer AKC;
		OutputImageType::Pointer Kmax;
		OutputImageType::Pointer Kmin;

		/**
		 * Start DKI fit.
		 */
		void Run( const parameters& args )
        {
			SetImage( args.inputFileName );
			SetMask( args.maskFileName, Input.GetPointer() );
			SetBVals( args.bvalsFileName, args.sep );
			SetBVecs( args.bvecsFileName, args.sep );
			SetDTIDesignMatrix();
			SetDKIDesignMatrix();
			AllocateOutput( Input.GetPointer() );
			Fit( args.iterations, args.regularize );
			Write( args.outputFileName );
        }

        protected:

		/**
		 * Return cholesky decomposition for symmetric matrix.
		 *
		 * Sqrt( A ) using cholesky decomposition is twice as fast as svd/qr.
		 *
		 * The cholesky decomposition decomposes symmetric A = LL'
		 * where L is lower triangular.
		 */
		MatrixType Cholesky( const MatrixType& A )
		{
			/* Test Cholesky ...
			MatrixType A( 3, 3 );
			A( 0, 0 ) = 2; A( 0, 1 ) = 0; A( 0, 2 ) = 0.1;
			A( 1, 0 ) = 0; A( 1, 1 ) = 2; A( 1, 2 ) = 0;
			A( 2, 0 ) = 0.1; A( 2, 1 ) = 0; A( 2, 2 ) = 4;
			MatrixType L = Cholesky( A );
			std::cout << "Cholesky: " << L * L.transpose() << std::endl;
			*/
			vnl_cholesky::Operation op = vnl_cholesky::quiet;

			vnl_cholesky chol( A, op );

			return chol.upper_triangle(); // A = U'U
		}

		/**
		 * Normal distribution sampler.
		 */
		VectorType SampleNormal( random_number_type& ran, unsigned int total, double mean, double sigma )
		{
			using namespace boost;

			// select Gaussian probability distribution
			normal_distribution< double > norm_dist( mean, sigma );

			// bind random number generator to distribution, forming a function
			variate_generator< random_number_type&, normal_distribution< double > > sampler( ran, norm_dist );

			VectorType samples( total );

			for( unsigned int i = 0; i < total; i++ )
				samples( i ) = sampler(); // sample from the distribution

			return samples;
		}

		/**
		 * Inverse gamma distribution sampler.
		 *
		 * Implemented as 1 / gamma, with boost library 1.45 switch to inverse_gamma_distribution()
		 *
		 * shape =~ alpha
		 * scale =~ beta
		 */
		VectorType SampleIGamma( random_number_type& ran, unsigned int total, double alpha, double beta )
		{
			using namespace boost;

		    gamma_distribution< double > gamma_dist( alpha );

		    variate_generator< random_number_type&, gamma_distribution< double > > sampler( ran, gamma_dist );

		    VectorType samples( total );

		    for( unsigned int i = 0; i < total; i++ )
		    	samples( i ) = 1 / ( beta * sampler() ); // sample gamma distribution with scale = 1, and invert

		    return samples;
		}

		/**
		 * Design matrix with ln(S0) as constant .
		 *
		 * See: "C.G. Koay et al / Journal of Magnetic Resonance 128 (2006) pag. 116"
		 *
		 * DTI tensor thus: ln(S0), Dxx, Dyy, Dzz, Dxy, Dyz, Dxz.
		 */
		void SetDTIDesignMatrix()
		{
			MatrixType A( R.rows(), 7, 0 ); // 5 x 7

			for ( unsigned int i = 0; i < R.rows(); i++ )
			{
				double b = B( i );
				double Gx = R( i, 0 );
				double Gy = R( i, 1 );
				double Gz = R( i, 2 );

				A( i, 0 ) = 1; // ln(S0 )

				A( i, 1 ) = -b * Gx * Gx;

				A( i, 2 ) = -b * Gy * Gy;

				A( i, 3 ) = -b * Gz * Gz;

				A( i, 4 ) = -2 * b * Gx * Gy;

				A( i, 5 ) = -2 * b * Gy * Gz;

				A( i, 6 ) = -2 * b * Gx * Gz;
			}

			X_dti = A;
		}

		/**
		 * Bayesian Linear least squares fit.
         *
		 * // TODO add covariance matrix W => to get linear 'weighted' regression.
		 * beta = (X' * X)^-1 * X' * ln( y )
		 */
		VectorType FitVoxel( const MatrixType& X, const VectorType& y, unsigned int iterations,
				unsigned int k, unsigned int n, bool regularize )
		{
			// (X' * X)^-1

			vnl_svd< PixelType > svd( X.transpose() * X );
			MatrixType XTXI = svd.inverse();

			// ln( y )

			VectorType Y( y.size() );
			for( unsigned int i = 0; i < y.size(); i++ )
				Y( i ) = log( y( i ) );

			// (X' * X)^-1 * X' * ln( y )

			VectorType beta = XTXI * X.transpose() * Y;

			// sample sigma from it's inverse gamma marginal distribution

			// Create a Mersenne twister random number generator
			random_number_type eng( static_cast< unsigned int > ( std::time( 0 ) ) );

			PixelType shape = 0.5 * ( n - k );
			PixelType scale = 0.5 * dot_product( ( Y - X * beta ), ( Y - X * beta ) );
			VectorType s2 = SampleIGamma( eng, iterations, shape, scale );

			// -----------------------------------
			// sample distribution for beta (MVN):
			// -----------------------------------
			// mean => inv(X'X) * (X'Y)				[fitted beta]
			// variance => sigma^2 * inv(X'X)		[sqrt using cholesky decomp.]

			std::list< VectorType > bs;
			for( unsigned int i = 0; i < iterations; i++ )
				bs.push_back( beta + SampleNormal( eng, k, 0, 1 ) * Cholesky( s2( i ) * XTXI ) );

			// remove non positive semi definite tensors
			if (regularize )
				bs.remove_if( non_positive_definite() );

			// if nothing left, return zero tensor ...

			if( bs.empty() )
			{
				//std::cout << "Voxel found with sampled tensors not positive semi-definite!" << std::endl;
				return VectorType( beta.size(), 0 ); // return empty tensor
			}

			// get mean tensor

			VectorType mean_beta( beta.size(), 0 );

			for( std::list< VectorType >::iterator it = bs.begin(); it != bs.end(); ++it )
				mean_beta += *it;

			return mean_beta /= bs.size();
		}

		/**
		 * Read bvals from file.
		 */
		void SetBVals( const std::string& bvalsFileName, const std::string& sep )
		{
			// init
			VectorType b( GetNumberOfRows( bvalsFileName ), 0 );

			// open data ...
			std::ifstream in( bvalsFileName.c_str() );
			if ( in.fail() )
			{
				std::cerr << "*** ERROR ***: could not read from: " << bvalsFileName << std::endl;
				exit( EXIT_FAILURE );
			}

			std::string line;
			unsigned int rowIndex = 0;

			while ( getline( in, line ) )
			{
				TokType tok( line, boost::char_separator< char >( sep.c_str() ) );
				try
				{
					for ( TokType::iterator id = tok.begin(); id != tok.end(); ++id )
					{
						b( rowIndex ) = boost::lexical_cast< PixelType >( *id );
					}
					rowIndex++;
				}
				catch ( boost::bad_lexical_cast& e )
				{
					std::cout << "*** WARNING ***: could not parse " << bvalsFileName << std::endl;
					e.what();
					exit( EXIT_FAILURE );
				}
			}

			in.close();

			B = b;
		}

		void SetBVecs( const std::string& bvecsFileName, const std::string& sep )
		{
			// init
			MatrixType r( GetNumberOfRows( bvecsFileName ), 3, 0 );

			// open data ...
			std::ifstream in( bvecsFileName.c_str() );
			if ( in.fail() )
			{
				std::cerr << "*** ERROR ***: could not read from: " << bvecsFileName << std::endl;
				exit( EXIT_FAILURE );
			}

			std::string line;
			unsigned int rowIndex = 0;

			while ( getline( in, line ) )
			{
				TokType tok( line, boost::char_separator< char >( sep.c_str() ) );
				try
				{
					unsigned int colIndex = 0;
					for ( TokType::iterator id = tok.begin(); id != tok.end(); ++id )
					{
						r( rowIndex, colIndex ) = boost::lexical_cast< PixelType >( *id );
						colIndex++;
					}
					rowIndex++;
				}
				catch ( boost::bad_lexical_cast& e )
				{
					std::cout << "*** WARNING ***: could not parse " << bvecsFileName << std::endl;
					std::cout << e.what() << std::endl;
					exit( EXIT_FAILURE );
				}
			}

			in.close();

			R = r;
		}

		/**
		 * Build X_dki from bvecs and bvals input files.
		 *
		 * DTI tensor elements:
		 * --------------------
		 * [D_11, D_12, D_13, D_22, D_23, D_33]
		 *
		 * DKI tensor elements:
		 * --------------------
		 * [W_1111, W_2222, W_3333,
		 * 	W_1112, W_2223, W_2221, W_2223, W_3331, W_3332,
		 *  W_1122, W_1133, W_2233,
		 * 	W_1123, W_1223, W_1233]
		 */
		void SetDKIDesignMatrix()
		{
			// check ...
			if( B.size() != R.rows() )
			{
				std::cerr << "*** ERROR ***: bvals and bvecs size does not match!" << std::endl;
				exit( EXIT_FAILURE );
			}

			// init ( e.g. [52 x 22] ) ...
			X_dki = MatrixType( R.rows(), 22, 0 );

			for ( unsigned int i = 0; i < R.rows(); i++ )
			{
				double b = B( i );
				double Gx = R( i, 0 );
				double Gy = R( i, 1 );
				double Gz = R( i, 2 );

				// ln( S0 )

				X_dki( i, 0 ) = 1;

				// DTI tensor

				X_dki( i, 1 ) = -b * Gx * Gx;
				X_dki( i, 2 ) = -b * Gy * Gy;
				X_dki( i, 3 ) = -b * Gz * Gz;
				X_dki( i, 4 ) = -2 * b * Gx * Gy;
				X_dki( i, 5 ) = -2 * b * Gy * Gz;
				X_dki( i, 6 ) = -2 * b * Gx * Gz;

				// DKI tensor

				PixelType c = ( b * b ) / 6;

				X_dki( i, 7 ) = c * Gx * Gx * Gx * Gx; // W_1111
				X_dki( i, 8 ) = c * Gy * Gy * Gy * Gy; // W_2222
				X_dki( i, 9 ) = c * Gz * Gz * Gz * Gz; // W_3333

				X_dki( i, 10 ) = 4 * c * Gx * Gx * Gx * Gy; // W_1112
				X_dki( i, 11 ) = 4 * c * Gx * Gx * Gx * Gy; // W_2223
				X_dki( i, 12 ) = 4 * c * Gx * Gy * Gy * Gy; // W_2221
				X_dki( i, 13 ) = 4 * c * Gy * Gy * Gy * Gz; // W_2223
				X_dki( i, 14 ) = 4 * c * Gx * Gz * Gz * Gz; // W_3331
				X_dki( i, 15 ) = 4 * c * Gy * Gz * Gz * Gz; // W_3332

				X_dki( i, 16 ) = 6 * c * Gx * Gx * Gy * Gy; // W_1122
				X_dki( i, 17 ) = 6 * c * Gx * Gx * Gz * Gz; // W_1133
				X_dki( i, 18 ) = 6 * c * Gy * Gy * Gz * Gz; // W_2233

				X_dki( i, 19 ) = 12 * c * Gx * Gx * Gy * Gz; // W_1123
				X_dki( i, 20 ) = 12 * c * Gx * Gy * Gy * Gz; // W_1223
				X_dki( i, 21 ) = 12 * c * Gx * Gy * Gz * Gz; // W_1233
			}
		}

		/**
		 * Return S0.
		 */
		PixelType GetS0( const VectorType& theta )
		{
			return std::exp( theta( 0 ) );
		}

		/**
		 * Return largest AKC value.
		 *
		 * See:
		 * "Principal invariant and inherent parameters of diffusion kurtosis tensors"
		 * "L. Qi et al / J. Math. Anal. Appl. 349 (2009) 165 - 180."
		 *
		 * return [mean(k), min(k), max(k)]
		 */
		VectorType GetKurtosisValues( const PixelType MD, const VectorType& D_app, const DKITensorType& W )
		{
			// get number of b-zeros ...
			unsigned int b0s = 0;
			for( unsigned int i = 0; i < B.size(); i++ )
				if( B( i ) == 0 )
					b0s++;

			VectorType K( R.rows() - b0s, 0 );

			unsigned int dwi_index = 0;

			for ( unsigned int i = 0; i < R.rows(); i++ )
			{
				if( B( i ) != 0 ) // only dwi ...
				{
					double Gx = R( i, 0 );
					double Gy = R( i, 1 );
					double Gz = R( i, 2 );

					PixelType tmp0 = MD * MD / ( D_app( i ) * D_app( i ) ); // D_app contains b0s ...

					PixelType tmp1 = W( 0 ) * Gx * Gx * Gx * Gx + W( 1 ) * Gy * Gy * Gy * Gy + W( 2 ) * Gz * Gz * Gz * Gz;

					PixelType tmp2 = 4 * ( W( 3 ) * Gx * Gx * Gx * Gy + W( 4 ) * Gx * Gx * Gx * Gy + W( 5 ) * Gx * Gy * Gy * Gy + W( 6 ) * Gy * Gy * Gy * Gz + W( 7 ) * Gx * Gz * Gz * Gz + W( 8 ) * Gy * Gz * Gz * Gz );

					PixelType tmp3 = 6 * ( W( 9 ) * Gx * Gx * Gy * Gy + W( 10 ) * Gx * Gx * Gz * Gz + W( 11 ) * Gy * Gy * Gz * Gz );

					PixelType tmp4 = 12 * ( W( 12 ) * Gx * Gx * Gy * Gz + W( 13 ) * Gx * Gy * Gy * Gz + W( 14 ) * Gx * Gy * Gz * Gz );

					K( dwi_index ) = tmp0 * ( tmp1 + tmp2 + tmp3 + tmp4 );

					dwi_index++;
				}
			}

			VectorType output( 3, 0 ); // average K, min K, max K

			output( 0 ) = K.mean();
			output( 1 ) = K.min_value();
			output( 2 ) = K.max_value();

			return output;
		}

		/**
		 * Return number of training data points.
		 */
		unsigned int GetNumberOfRows( const std::string& inputFile )
		{
			std::ifstream in( inputFile.c_str() );

			if ( in.fail() )
			{
				std::cerr << "*** ERROR ***: could not read from: " << inputFile << std::endl;
				exit( EXIT_FAILURE );
			}
			std::string line;
			unsigned int result = 0;

			while ( getline( in, line ) )
				result++;

			in.close();
			return result;
		}

		/**
		 * MCMC DTI fitting.
		 */
		void Fit( unsigned int iterations, bool regularize )
		{
			ConstIterator4DType it( Input, Input->GetLargestPossibleRegion() );
			ConstIterator3DType mit( Mask, Mask->GetLargestPossibleRegion() );

			Iterator3DType itS0_dti( S0_dti, S0_dti->GetLargestPossibleRegion() );
			Iterator3DType itS0_dki( S0_dki, S0_dki->GetLargestPossibleRegion() );

			Iterator3DType itFA_dti( FA_dti, FA_dti->GetLargestPossibleRegion() );
			Iterator3DType itFA_dki( FA_dki, FA_dki->GetLargestPossibleRegion() );

			Iterator3DType itTrace_dti( Trace_dti, Trace_dti->GetLargestPossibleRegion() );
			Iterator3DType itTrace_dki( Trace_dki, Trace_dki->GetLargestPossibleRegion() );

			Iterator3DType itAKC( AKC, AKC->GetLargestPossibleRegion() );
			Iterator3DType itKmin( Kmin, Kmin->GetLargestPossibleRegion() );
			Iterator3DType itKmax( Kmax, Kmax->GetLargestPossibleRegion() );

			it.SetDirection( 3 );
			it.GoToBegin();
			mit.GoToBegin();

			itS0_dti.GoToBegin();
			itS0_dki.GoToBegin();

			itFA_dti.GoToBegin();
			itFA_dki.GoToBegin();

			itTrace_dti.GoToBegin();
			itTrace_dki.GoToBegin();

			itAKC.GoToBegin();
			itKmin.GoToBegin();
			itKmax.GoToBegin();

			unsigned int totalDWI = ( Input->GetLargestPossibleRegion().GetSize() )[3];

			unsigned int sliceIndex = 0;

			// for each series of DWI ...
			while ( !it.IsAtEnd(), !mit.IsAtEnd() )
			{
				if ( mit.Get() != 0 )
				{
					// slice index
					if( sliceIndex != mit.GetIndex()[ 2 ] )
					{
						std::cout << "Processing slice: " << sliceIndex << std::endl;
						sliceIndex = mit.GetIndex()[ 2 ];
					}

					VectorType y( totalDWI );

					while ( !it.IsAtEndOfLine() )
					{
						y( ( it.GetIndex() )[3] ) = it.Get();
						++it;
					}

					// DTI => 7 elements (ln(S0) = 1, theta_D = 6)
					// DTI fit only ...

					unsigned int k = 7;
					unsigned int n = y.size();
					VectorType theta_dti = FitVoxel( X_dti, y, iterations, k, n, regularize );
					itS0_dti.Set( GetS0( theta_dti ) );
					DTITensorType tensor_simple = GetDTITensor( theta_dti );
					itFA_dti.Set( tensor_simple.GetFractionalAnisotropy() );
					itTrace_dti.Set( tensor_simple.GetTrace() );

					// DKI => 22 elements (ln(S0) = 1, theta_D = 6, theta_K = 15)
					// DTI and DKI fit in one...

					k += 15;

					VectorType theta_dki = FitVoxel( X_dki, y, iterations, k, n, regularize );
					itS0_dki.Set( GetS0( theta_dki ) );

					DTITensorType tensor_dti = GetDTITensor( theta_dki ); // DTI tensor from DKI fit ...
					itFA_dki.Set( tensor_dti.GetFractionalAnisotropy() );
					itTrace_dki.Set( tensor_dti.GetTrace() );

					DKITensorType tensor_dki = GetDKITensor( theta_dki );
					VectorType tmp = GetKurtosisValues( tensor_dti.GetTrace(), FitDapp( y ), tensor_dki );

					itAKC.Set( tmp( 0 ) );
					itKmin.Set( tmp( 1 ) );
					itKmax.Set( tmp( 2 ) );
				}

				it.NextLine();
				++mit;

				++itS0_dti;
				++itS0_dki;

				++itFA_dti;
				++itFA_dki;

				++itTrace_dti;
				++itTrace_dki;

				++itAKC;
				++itKmin;
				++itKmax;
			}
		}

		/**
		 * Return apparant diffusion per direction.
		 *
		 * ln( Sb / S0 ) / -b = D_app
		 */
		VectorType FitDapp( const VectorType& y )
		{
			// average S0 ...

			PixelType S0 = 0;
			PixelType totalS0;

			for( unsigned int i = 0; i < B.size(); i++ )
			{
				if( B( i ) == 0 )
				{
					S0 += y( i );
					totalS0++;
				}
			}
			if( totalS0 == 0 )
			{
				std::cerr << "*** ERROR ***: no b-zeros!" << std::endl;
				exit( EXIT_FAILURE );
			}
			S0 /= totalS0;

			// D_app ...

			VectorType D_app( R.rows(), 0 );

			for( unsigned int i = 0; i < B.size(); i++ )
			{
				if( B( i ) != 0 )
					D_app( i ) = log( y( i )  / S0 ) / - B( i );
			}

			return D_app;
		}

		/**
		 * Return covariate matrix Z.
		 */
		MatrixType BuildCovarianceMatrix( const VectorType& y )
		{
			MatrixType Z( y.size(), y.size(), 0 );

			// fill diagonal
			for( unsigned int i = 0; i < Z.rows(); i++ )
				Z( i, i ) = y( i ) * y( i );

			return Z;
		}

		/**
		 * Allocate all output images to 0.
		 */
		void AllocateOutput( ImageType::ConstPointer input )
		{
			MaskImageType::Pointer output = MaskImageType::New();

			OutputImageType::RegionType region3D;
			OutputImageType::IndexType index3D;
			OutputImageType::SizeType size3D;
			OutputImageType::SpacingType spacing3D;
			OutputImageType::PointType origin3D;

			ImageType::RegionType region = input->GetLargestPossibleRegion();
			ImageType::SizeType size = region.GetSize();
			ImageType::IndexType index = region.GetIndex();
			ImageType::SpacingType spacing = input->GetSpacing();
			ImageType::PointType origin = input->GetOrigin();

			size3D[0] = size[0];
			size3D[1] = size[1];
			size3D[2] = size[2];
			index3D[0] = index[0];
			index3D[1] = index[1];
			index3D[2] = index[2];
			origin3D[0] = origin[0];
			origin3D[1] = origin[1];
			origin3D[2] = origin[2];
			spacing3D[0] = spacing[0];
			spacing3D[1] = spacing[1];
			spacing3D[2] = spacing[2];

			region3D.SetSize( size3D );
			region3D.SetIndex( index3D );

			// create
			S0_dti = OutputImageType::New();
			S0_dki = OutputImageType::New();

			FA_dti = OutputImageType::New();
			FA_dki = OutputImageType::New();

			Trace_dti = OutputImageType::New();
			Trace_dki = OutputImageType::New();

			AKC = OutputImageType::New();
			Kmax = OutputImageType::New();
			Kmin = OutputImageType::New();

			// set
			S0_dti->SetRegions( region3D );
			S0_dti->SetSpacing( spacing3D );
			S0_dti->SetOrigin( origin3D );
			S0_dti->Allocate();
			S0_dti->FillBuffer( 0 );

			S0_dki->SetRegions( region3D );
			S0_dki->SetSpacing( spacing3D );
			S0_dki->SetOrigin( origin3D );
			S0_dki->Allocate();
			S0_dki->FillBuffer( 0 );

			FA_dti->SetRegions( region3D );
			FA_dti->SetSpacing( spacing3D );
			FA_dti->SetOrigin( origin3D );
			FA_dti->Allocate();
			FA_dti->FillBuffer( 0 );

			FA_dki->SetRegions( region3D );
			FA_dki->SetSpacing( spacing3D );
			FA_dki->SetOrigin( origin3D );
			FA_dki->Allocate();
			FA_dki->FillBuffer( 0 );

			Trace_dti->SetRegions( region3D );
			Trace_dti->SetSpacing( spacing3D );
			Trace_dti->SetOrigin( origin3D );
			Trace_dti->Allocate();
			Trace_dti->FillBuffer( 0 );

			Trace_dki->SetRegions( region3D );
			Trace_dki->SetSpacing( spacing3D );
			Trace_dki->SetOrigin( origin3D );
			Trace_dki->Allocate();
			Trace_dki->FillBuffer( 0 );

			AKC->SetRegions( region3D );
			AKC->SetSpacing( spacing3D );
			AKC->SetOrigin( origin3D );
			AKC->Allocate();
			AKC->FillBuffer( 0 );

			Kmax->SetRegions( region3D );
			Kmax->SetSpacing( spacing3D );
			Kmax->SetOrigin( origin3D );
			Kmax->Allocate();
			Kmax->FillBuffer( 0 );

			Kmin->SetRegions( region3D );
			Kmin->SetSpacing( spacing3D );
			Kmin->SetOrigin( origin3D );
			Kmin->Allocate();
			Kmin->FillBuffer( 0 );
		}

		/**
		 * Set input image.
		 */
		void SetImage( const std::string& inputFileName )
		{
			if ( !inputFileName.empty() )
			{
				ReaderType::Pointer reader = ReaderType::New();
				reader->SetFileName( inputFileName );
				reader->Update();
				Input = reader->GetOutput();
			}
			else
			{
				std::cerr << "Could not read input!" << std::endl;
				exit( EXIT_FAILURE );
			}
		}

		/**
		 * Return mask if file given, else create empty mask from input file.
		 */
		void SetMask( const std::string& maskFileName, ImageType::ConstPointer input )
		{
			if ( !maskFileName.empty() )
			{
				MaskReaderType::Pointer reader = MaskReaderType::New();
				reader->SetFileName( maskFileName );
				reader->Update();
				Mask = reader->GetOutput();
			}
			else
			{
				MaskImageType::Pointer output = MaskImageType::New();
				MaskImageType::RegionType region3D;
				MaskImageType::IndexType index3D;
				MaskImageType::SizeType size3D;
				MaskImageType::SpacingType spacing3D;
				MaskImageType::PointType origin3D;

				ImageType::RegionType region = input->GetLargestPossibleRegion();
				ImageType::SizeType size = region.GetSize();
				ImageType::IndexType index = region.GetIndex();
				ImageType::SpacingType spacing = input->GetSpacing();
				ImageType::PointType origin = input->GetOrigin();

				size3D[0] = size[0];
				size3D[1] = size[1];
				size3D[2] = size[2];
				index3D[0] = index[0];
				index3D[1] = index[1];
				index3D[2] = index[2];
				origin3D[0] = origin[0];
				origin3D[1] = origin[1];
				origin3D[2] = origin[2];
				spacing3D[0] = spacing[0];
				spacing3D[1] = spacing[1];
				spacing3D[2] = spacing[2];

				region3D.SetSize( size3D );
				region3D.SetIndex( index3D );

				// set
				output->SetRegions( region3D );
				output->SetSpacing( spacing3D );
				output->SetOrigin( origin3D );
				output->Allocate();
				output->FillBuffer( 1 );

				Mask = output;
			}
		}

		/**
		 * Write output images.
		 */
		void Write( const std::string& outputFileName )
		{
			// Writer
			WriterType::Pointer writer = WriterType::New();

			// S0
			writer->SetFileName( ( outputFileName + std::string( "_S0_dti.nii.gz" ) ).c_str() );
			writer->SetInput( S0_dti );
			writer->Update();

			writer->SetFileName( ( outputFileName + std::string( "_S0_dki.nii.gz" ) ).c_str() );
			writer->SetInput( S0_dki );
			writer->Update();

			// FA
			writer->SetFileName( ( outputFileName + std::string( "_FA_dti.nii.gz" ) ).c_str() );
			writer->SetInput( FA_dti );
			writer->Update();

			writer->SetFileName( ( outputFileName + std::string( "_FA_dki.nii.gz" ) ).c_str() );
			writer->SetInput( FA_dki );
			writer->Update();

			// Trace
			writer->SetFileName( ( outputFileName + std::string( "_Trace_dti.nii.gz" ) ).c_str() );
			writer->SetInput( Trace_dti );
			writer->Update();

			writer->SetFileName( ( outputFileName + std::string( "_Trace_dki.nii.gz" ) ).c_str() );
			writer->SetInput( Trace_dki );
			writer->Update();

			// AKC
			writer->SetFileName( ( outputFileName + std::string( "_AKC.nii.gz" ) ).c_str() );
			writer->SetInput( AKC );
			writer->Update();

			// Kmin
			writer->SetFileName( ( outputFileName + std::string( "_Kmin.nii.gz" ) ).c_str() );
			writer->SetInput( Kmin );
			writer->Update();

			// Kmax
			writer->SetFileName( ( outputFileName + std::string( "_Kmax.nii.gz" ) ).c_str() );
			writer->SetInput( Kmax );
			writer->Update();
		}

    }; // end class DKIFit

} // end namespace dki

/**
 * DKI fitting routine.
 */
int main( int argc, char ** argv )
{
    tkd::CmdParser p( argv[0], "Fit voxel-wise DTI using Gibbs sampling and postitive semi-definite tensor regularization." );

    dki::parameters args;
    args.sep = " ";
    args.iterations = 1000;
    args.regularize = false;

    p.AddArgument( args.inputFileName, "input" )
        ->AddAlias( "i" )
        ->SetDescription( "Input 4D image" )
        ->SetRequired( true );

    p.AddArgument( args.bvecsFileName, "bvecs" )
        ->AddAlias( "r" )
        ->SetDescription( "Diffusion gradient vectors (FSL format)" )
        ->SetRequired( true );

    p.AddArgument( args.bvalsFileName, "bvals" )
        ->AddAlias( "b" )
        ->SetDescription( "Diffusion b-value vector (FSL format)" )
        ->SetRequired( true );

    p.AddArgument( args.outputFileName, "output" )
        ->AddAlias( "o" )
        ->SetDescription( "Output filename base" )
        ->SetRequired( true );

    p.AddArgument( args.maskFileName, "mask" )
        ->AddAlias( "m" )
        ->SetDescription( "Mask 3D image" );

    p.AddArgument( args.sep, "separation" )
        ->AddAlias( "s" )
        ->SetDescription( "Separation string in bvecs file (default: ' ')" );

    p.AddArgument( args.iterations, "iterations" )
        ->AddAlias( "it" )
        ->SetDescription( "Number of MCMC sampling iterations per voxel (default: 1000)" );

    p.AddArgument( args.regularize, "regularize" )
        ->AddAlias( "reg" )
        ->SetDescription( "Regularize posteriors by removal of non postivive semi-definite tensors (default: false)" );

    if ( !p.Parse( argc, argv ) )
    {
        p.PrintUsage( std::cout );
        return EXIT_FAILURE;
    }

    dki::DKIFit fit;
    fit.Run( args );

    return EXIT_SUCCESS;
}


#pragma once
#ifndef OPENGM_MINSTCUTBOOST_HXX
#define OPENGM_MINSTCUTBOOST_HXX

#ifndef BOOST_DISABLE_ASSERTS
#define BOOST_DISABLE_ASSERTS
#define USED_BOOST_DISABLE_ASSERTS
#endif

#include <queue>
#include <cassert>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/typeof/typeof.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>

namespace opengm {

   enum BoostMaxFlowAlgorithm {
      PUSH_RELABEL, EDMONDS_KARP, KOLMOGOROV
   };

   /// \brief Boost solvers for the min st-cut framework GraphCut
   template<class NType, class VType, BoostMaxFlowAlgorithm mfalg>
   class MinSTCutBoost {
   public:
      // Type-Definitions
      typedef NType node_type;
      typedef VType ValueType;
      typedef boost::vecS OutEdgeList;
      typedef boost::vecS VertexList;
      typedef boost::adjacency_list_traits<OutEdgeList, VertexList, boost::directedS> graph_traits;
      typedef graph_traits::edge_descriptor edge_descriptor;
      typedef graph_traits::vertex_descriptor vertex_descriptor;

      /// \cond HIDDEN_SYMBOLS
      struct Edge {
         Edge() : capacity(ValueType()), residual(ValueType()), reverse(edge_descriptor())
            {}
         ValueType capacity;
         ValueType residual;
         edge_descriptor reverse;
      };
      /// \endcond

      typedef boost::adjacency_list<OutEdgeList, VertexList, boost::directedS, size_t, Edge> graph_type;
      typedef typename boost::graph_traits<graph_type>::edge_iterator edge_iterator;
      typedef typename boost::graph_traits<graph_type>::out_edge_iterator out_edge_iterator;

      // Methods
      MinSTCutBoost();
      MinSTCutBoost(size_t numberOfNodes, size_t numberOfEdges);
      void addEdge(node_type, node_type, ValueType);
      void calculateCut(std::vector<bool>&);

   private:
      // Members
      graph_type graph_;
      size_t numberOfNodes_;
      size_t numberOfEdges_;
      static const NType S = 0;
      static const NType T = 1;
   };

   //*********************
   //** Implementation  **
   //*********************

   template<class NType, class VType, BoostMaxFlowAlgorithm mfalg>
   MinSTCutBoost<NType, VType, mfalg>::MinSTCutBoost() {
      numberOfNodes_ = 2;
      numberOfEdges_ = 0;
   }

   template<class NType, class VType, BoostMaxFlowAlgorithm mfalg>
   MinSTCutBoost<NType, VType, mfalg>::MinSTCutBoost(size_t numberOfNodes, size_t numberOfEdges) {
      numberOfNodes_ = numberOfNodes;
      numberOfEdges_ = numberOfEdges;
      graph_ = graph_type(numberOfNodes_);
      //std::cout << "#nodes : " << numberOfNodes_ << std::endl;
   }

   template<class NType, class VType, BoostMaxFlowAlgorithm mfalg>
   void MinSTCutBoost<NType, VType, mfalg>::addEdge(node_type n1, node_type n2, ValueType cost) {
      assert(n1 < numberOfNodes_);
      assert(n2 < numberOfNodes_);
      assert(cost >= 0);
      std::pair<edge_descriptor, bool> e = add_edge(n1, n2, graph_);
      std::pair<edge_descriptor, bool> er = add_edge(n2, n1, graph_);
      graph_[e.first].capacity += cost;
      graph_[e.first].reverse = er.first;
      graph_[er.first].reverse = e.first;
      //std::cout << n1 << "->" << n2 << " : " << cost << std::endl;
   }

   template<class NType, class VType, BoostMaxFlowAlgorithm mfalg>
   void MinSTCutBoost<NType, VType, mfalg>::calculateCut(std::vector<bool>& segmentation) {
      if (mfalg == KOLMOGOROV) {//Kolmogorov
         std::vector<boost::default_color_type> color(num_vertices(graph_));
         std::vector<edge_descriptor> pred(num_vertices(graph_));
         std::vector<vertex_descriptor> dist(num_vertices(graph_));
         boykov_kolmogorov_max_flow(graph_,
            get(&Edge::capacity, graph_),
            get(&Edge::residual, graph_),
            get(&Edge::reverse, graph_),
            &pred[0],
            &color[0],
            &dist[0],
            get(boost::vertex_index, graph_),
            S, T
            );
         // find (s,t)-cut set
         segmentation.resize(num_vertices(graph_));
         for (size_t j = 2; j < num_vertices(graph_); ++j) {
            if (color[j] == boost::black_color || color[j] == boost::gray_color) {
               segmentation[j] = false;
            } else if (color[j] == boost::white_color) {
               segmentation[j] = true;
            }
         }
      }
      else if (mfalg == PUSH_RELABEL) {// PushRelable

         push_relabel_max_flow(graph_, S, T,
            get(&Edge::capacity, graph_),
            get(&Edge::residual, graph_),
            get(&Edge::reverse, graph_),
            get(boost::vertex_index_t(), graph_)
            );
         // find (s,t)-cut set
         segmentation.resize(num_vertices(graph_), true);
         segmentation[S] = false; // source
         segmentation[T] = false; // sink
         typedef typename boost::property_map<graph_type, boost::vertex_index_t>::type VertexIndexMap;
         VertexIndexMap vertexIndexMap = get(boost::vertex_index, graph_);
         std::queue<vertex_descriptor> q;
         q.push(*(vertices(graph_).first)); // source
         while (!q.empty()) {
            out_edge_iterator current, end;
            boost::tie(current, end) = out_edges(q.front(), graph_);
            q.pop();
            while (current != end) {
               if (graph_[*current].residual > 0) {
                  vertex_descriptor v = target(*current, graph_);
                  if (vertexIndexMap[v] > 1 && segmentation[vertexIndexMap[v]] == true) {
                     segmentation[vertexIndexMap[v]] = false;
                     q.push(v);
                  }
               }
               ++current;
            }
         }
      }
      else if (mfalg == EDMONDS_KARP) {//EdmondsKarp
         std::vector<boost::default_color_type> color(num_vertices(graph_));
         std::vector<edge_descriptor> pred(num_vertices(graph_));
         edmonds_karp_max_flow(graph_, S, T,
            get(&Edge::capacity, graph_),
            get(&Edge::residual, graph_),
            get(&Edge::reverse, graph_),
            &color[0], &pred[0]
            );
         // find (s,t)-cut set
         segmentation.resize(num_vertices(graph_));
         for (size_t j = 2; j < num_vertices(graph_); ++j) {
            if (color[j] == boost::black_color) {
               segmentation[j] = false;
            } else if (color[j] == boost::white_color) {
               segmentation[j] = true;
            } else {
               throw std::runtime_error("At least one vertex is labeled neither black nor white.");
            }
         }
      }
      else {//UNKNOWN MaxFlowalgorithm
         throw std::runtime_error("Unknown MaxFlow-algorithm in MinSTCutBoost.hxx");
      }
      return;
   }

} // namespace opengm

#ifdef USED_BOOST_DISABLE_ASSERTS
#undef BOOST_DISABLE_ASSERTS
#undef USED_BOOST_DISABLE_ASSERTS
#endif

#endif // #ifndef OPENGM_MINSTCUTBOOST_HXX


/*
 * CrossSections.cpp
 *
 *  Created on: 13 Aug 2010
 *      Author: kreczko
 */

#include "../interface/EventWeightProvider.h"
#include "../interface/GlobalVariables.h"

#include "TFile.h"

#include <boost/scoped_ptr.hpp>
#include <boost/filesystem.hpp>

#include <iostream>

using namespace std;
namespace BAT {

EventWeightProvider::EventWeightProvider(string datasetInformationFile) :
		datasetInfo_(datasetInformationFile), //
		xsection(datasetInfo_.getCrossSections()), //
		numberOfProcessedEvents(datasetInfo_.getArrayOfProcessedEvents()), //
		estimatedPileUp(Globals::estimatedPileup), //
		DATAdistribution(), //
		pileUpWeights(), //
		numberOfEventsWithTooHighPileUp(0) {

	generate_weights();
}

EventWeightProvider::~EventWeightProvider() {

}

double EventWeightProvider::getWeight(DataType::value type) {
	if (type <= DataType::SingleMuon_Rereco)
		return 1.;
	else
		return xsection[type] * Globals::luminosity / numberOfProcessedEvents[type];
}

double EventWeightProvider::reweightPileUp(unsigned int numberOfVertices) {
	if (numberOfVertices >= pileUpWeights.size()) {
		++numberOfEventsWithTooHighPileUp;
		return 1.;
	}

	return pileUpWeights.at(numberOfVertices);
}

double EventWeightProvider::reweightTopPt(const EventPtr event) {
	const MCParticleCollection genPart(event->GenParticles());

	double topPt = 0.;
	double tbarPt = 0.;
	for (unsigned int i = 0; i < genPart.size(); i++) {

		if (genPart.at(i)->pdgId() == (6))
		  	  topPt = genPart.at(i)->pt();

		if (genPart.at(i)->pdgId() == (-6))
		  	  tbarPt = genPart.at(i)->pt();
	}

	double SFtop = exp(0.159-0.00141*topPt);
	double SFtbar = exp(0.159-0.00141*tbarPt);

	double weight=sqrt(SFtop*SFtbar);
  return weight;
}

//boost::shared_ptr<TH1D> EventWeightProvider::getPileUpHistogram(std::string pileUpEstimationFile) {
//	using namespace std;
//
//	if (!boost::filesystem::exists(pileUpEstimationFile))
//		throw "Exception in EventWeightProvider::getPileUpHistogram: could not find pile-up file '"
//				+ pileUpEstimationFile + "'";
//
//	cout << "Using pile-up estimation file " << pileUpEstimationFile << endl;
//	boost::scoped_ptr<TFile> file(TFile::Open(pileUpEstimationFile.c_str()));
//	boost::shared_ptr<TH1D> pileUp((TH1D*) file->Get("pileup")->Clone());
//	file->Close();
//
//	return pileUp;
//}

void EventWeightProvider::generate_weights() {

	/*
	 * TODO:
	 * Summer11 PU_S3 and PU_S4 in-time PU only: PoissonOneXDist
	 * if Summer11 OOT average PU, use PoissonIntDist
	 * If Spring11 or Summer11 3D reweighting, use probdistFlat10 (compare to true)
	 * Fall11, use Fall2011
	 */

	//Needs a flag for 2011 MC tried energyInTeV?
	if (Globals::energyInTeV == 8) {
		pileUpWeights = generateWeights(Summer2012);
	} else if (Globals::energyInTeV == 7 && Globals::NTupleVersion < 11) {
		pileUpWeights = generateWeights(Fall2011);
	} else if (Globals::energyInTeV == 7 && Globals::NTupleVersion == 11) {
		pileUpWeights = generateWeights(Summer11Leg);
	} else if (Globals::energyInTeV == 13) {
		pileUpWeights = generateWeights(Spring2015_50ns);
	}

//	cout << "Pile up weights" << endl;
//
//	for (unsigned int index = 0; index < pileUpWeights.size(); ++index){
//		cout << index << ": " << pileUpWeights.at(index) << endl;
//	}
}

boost::array<double, NWEIGHTSSIZE> EventWeightProvider::generateWeights(
		const boost::array<double, NWEIGHTSSIZE> inputMC) {
	boost::array<double, NWEIGHTSSIZE> weights;

	double s = 0.0;

	for (unsigned int npu = 0; npu < inputMC.size(); ++npu) {
		if (npu >= (unsigned int) estimatedPileUp->GetNbinsX())
			break;
		DATAdistribution[npu] = estimatedPileUp->GetBinContent(estimatedPileUp->GetXaxis()->FindBin(npu));
		if (inputMC[npu] > 0)
			weights[npu] = DATAdistribution[npu] / inputMC[npu];
		else
			weights[npu] = 0;
		s += DATAdistribution[npu];
	}
	/**
	 * normalize weights such that the total sum of weights over thw whole sample is 1.0,
	 * i.e., sum_i  result[i] * npu_probs[i] should be 1.0 (!)
	 */
	for (unsigned int npu = 0; npu < pileUpWeights.size(); ++npu) {
		weights[npu] /= s;
	}

	return weights;
}

unsigned long EventWeightProvider::getNumberOfEventsWithTooHighPileUp() const {
	return numberOfEventsWithTooHighPileUp;
}

} // namespace BAT


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/min.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( oneminus_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return minus(One<A0>(), a0);
    }
  };
} } }

#include <boost/simd/arch/common/scalar/function/oneminus_s.hpp>

#endif