/* statistic_tests.hpp header file
 *
 * Copyright Jens Maurer 2000
 * 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)
 *
 * $Id$
 *
 */

#ifndef STATISTIC_TESTS_HPP
#define STATISTIC_TESTS_HPP

#include <stdexcept>
#include <iterator>
#include <vector>
#include <boost/limits.hpp>
#include <algorithm>
#include <cmath>

#include <boost/config.hpp>
#include <boost/bind.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/variate_generator.hpp>

#include "integrate.hpp"

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
namespace std
{
  inline double pow(double a, double b) { return ::pow(a,b); }
  inline double ceil(double x) { return ::ceil(x); }
} // namespace std
#endif

template<class T>
inline T fac(int k)
{
  T result = 1;
  for(T i = 2; i <= k; ++i)
    result *= i;
  return result;
}

template<class T>
T binomial(int n, int k)
{
  if(k < n/2)
    k = n-k;
  T result = 1;
  for(int i = k+1; i<= n; ++i)
    result *= i;
  return result / fac<T>(n-k);
}

template<class T>
T stirling2(int n, int m)
{
  T sum = 0;
  for(int k = 0; k <= m; ++k)
    sum += binomial<T>(m, k) * std::pow(double(k), n) *
      ( (m-k)%2 == 0 ? 1 : -1);
  return sum / fac<T>(m);
}

/*
 * Experiments which create an empirical distribution in classes,
 * suitable for the chi-square test.
 */
// std::floor(gen() * classes)

class experiment_base
{
public:
  experiment_base(int cls) : _classes(cls) { }
  unsigned int classes() const { return _classes; }
protected:
  unsigned int _classes;
};

class equidistribution_experiment : public experiment_base
{
public:
  explicit equidistribution_experiment(unsigned int classes)
    : experiment_base(classes) { }

  template<class NumberGenerator, class Counter>
  void run(NumberGenerator & f, Counter & count, int n) const
  {
    assert((f.min)() == 0 &&
           static_cast<unsigned int>((f.max)()) == classes()-1);
    for(int i = 0; i < n; ++i)
      count(f());
  }
  double probability(int /*i*/) const { return 1.0/classes(); }
};

// two-dimensional equidistribution experiment
class equidistribution_2d_experiment : public equidistribution_experiment
{
public:
  explicit equidistribution_2d_experiment(unsigned int classes)
    : equidistribution_experiment(classes) { }

  template<class NumberGenerator, class Counter>
  void run(NumberGenerator & f, Counter & count, int n) const
  {
    unsigned int range = (f.max)()+1;
    assert((f.min)() == 0 && range*range == classes());
    for(int i = 0; i < n; ++i) {
      int y1 = f();
      int y2 = f();
      count(y1 + range * y2);
    }
  }
};

// distribution experiment: assume a probability density and
// count events so that an equidistribution results.
class distribution_experiment : public equidistribution_experiment
{
public:
  template<class Distribution>
  distribution_experiment(Distribution dist , unsigned int classes)
    : equidistribution_experiment(classes), limit(classes)
  {
    for(unsigned int i = 0; i < classes-1; ++i)
      limit[i] = quantile(dist, (i+1)*0.05);
    limit[classes-1] = std::numeric_limits<double>::infinity();
    if(limit[classes-1] < (std::numeric_limits<double>::max)())
      limit[classes-1] = (std::numeric_limits<double>::max)();
#if 0
    std::cout << __PRETTY_FUNCTION__ << ": ";
    for(unsigned int i = 0; i < classes; ++i)
      std::cout << limit[i] << " ";
    std::cout << std::endl;
#endif
  }

  template<class NumberGenerator, class Counter>
  void run(NumberGenerator & f, Counter & count, int n) const
  {
    for(int i = 0; i < n; ++i) {
      limits_type::const_iterator it =
        std::lower_bound(limit.begin(), limit.end(), f());
      count(it-limit.begin());
    }
  }
private:
  typedef std::vector<double> limits_type;
  limits_type limit;
};

template<bool up, bool is_float>
struct runs_direction_helper
{
  template<class T>
  static T init(T)
  {
    return (std::numeric_limits<T>::max)();
  }
};

template<>
struct runs_direction_helper<true, true>
{
  template<class T>
  static T init(T)
  {
    return -(std::numeric_limits<T>::max)();
  }
};

template<>
struct runs_direction_helper<true, false>
{
  template<class T>
  static T init(T)
  {
    return (std::numeric_limits<T>::min)();
  }
};

// runs-up/runs-down experiment
template<bool up>
class runs_experiment : public experiment_base
{
public:
  explicit runs_experiment(unsigned int classes) : experiment_base(classes) { }

  template<class NumberGenerator, class Counter>
  void run(NumberGenerator & f, Counter & count, int n) const
  {
    typedef typename NumberGenerator::result_type result_type;
    result_type init =
      runs_direction_helper<
        up,
        !std::numeric_limits<result_type>::is_integer
      >::init(result_type());
    result_type previous = init;
    unsigned int length = 0;
    for(int i = 0; i < n; ++i) {
      result_type val = f();
      if(up ? previous <= val : previous >= val) {
        previous = val;
        ++length;
      } else {
        count((std::min)(length, classes())-1);
        length = 0;
        previous = init;
        // don't use this value, so that runs are independent
      }
    }
  }
  double probability(unsigned int r) const
  {
    if(r == classes()-1)
      return 1.0/fac<double>(classes());
    else
      return static_cast<double>(r+1)/fac<double>(r+2);
  }
};

// gap length experiment
class gap_experiment : public experiment_base
{
public:
  template<class Dist>
  gap_experiment(unsigned int classes, const Dist & dist, double alpha, double beta)
    : experiment_base(classes), alpha(alpha), beta(beta), low(quantile(dist, alpha)), high(quantile(dist, beta)) {}

  template<class NumberGenerator, class Counter>
  void run(NumberGenerator & f, Counter & count, int n) const
  {
    typedef typename NumberGenerator::result_type result_type;
    unsigned int length = 0;
    for(int i = 0; i < n; ) {
      result_type value = f();
      if(value < low || value > high)
        ++length;
      else {
        count((std::min)(length, classes()-1));
        length = 0;
        ++i;
      }
    }
  }
  double probability(unsigned int r) const
  {
    double p = beta-alpha;
    if(r == classes()-1)
      return std::pow(1-p, static_cast<double>(r));
    else
      return p * std::pow(1-p, static_cast<double>(r));
  }
private:
  double alpha, beta;
  double low, high;
};

// poker experiment
class poker_experiment : public experiment_base
{
public:
  poker_experiment(unsigned int d, unsigned int k)
    : experiment_base(k), range(d)
  {
    assert(range > 1);
  }

  template<class UniformRandomNumberGenerator, class Counter>
  void run(UniformRandomNumberGenerator & f, Counter & count, int n) const
  {
    typedef typename UniformRandomNumberGenerator::result_type result_type;
    assert(std::numeric_limits<result_type>::is_integer);
    assert((f.min)() == 0);
    assert((f.max)() == static_cast<result_type>(range-1));
    std::vector<result_type> v(classes());
    for(int i = 0; i < n; ++i) {
      for(unsigned int j = 0; j < classes(); ++j)
        v[j] = f();
      std::sort(v.begin(), v.end());
      result_type prev = v[0];
      int r = 1;     // count different values in v
      for(unsigned int i = 1; i < classes(); ++i) {
        if(prev != v[i]) {
          prev = v[i];
          ++r;
        }
      }
      count(r-1);
    }
  }

  double probability(unsigned int r) const
  {
    ++r;       // transform to 1 <= r <= 5
    double result = range;
    for(unsigned int i = 1; i < r; ++i)
      result *= range-i;
    return result / std::pow(range, static_cast<double>(classes())) *
      stirling2<double>(classes(), r);
  }
private:
  unsigned int range;
};

// coupon collector experiment
class coupon_collector_experiment : public experiment_base
{
public:
  coupon_collector_experiment(unsigned int d, unsigned int cls)
    : experiment_base(cls), d(d)
  {
    assert(d > 1);
  }

  template<class UniformRandomNumberGenerator, class Counter>
  void run(UniformRandomNumberGenerator & f, Counter & count, int n) const
  {
    typedef typename UniformRandomNumberGenerator::result_type result_type;
    assert(std::numeric_limits<result_type>::is_integer);
    assert((f.min)() == 0);
    assert((f.max)() == static_cast<result_type>(d-1));
    std::vector<bool> occurs(d);
    for(int i = 0; i < n; ++i) {
      occurs.assign(d, false);
      unsigned int r = 0;            // length of current sequence
      int q = 0;                     // number of non-duplicates in current set
      for(;;) {
        result_type val = f();
        ++r;
        if(!occurs[val]) {       // new set element
          occurs[val] = true;
          ++q;
          if(q == d)
            break;     // one complete set
        }
      }
      count((std::min)(r-d, classes()-1));
    }
  }
  double probability(unsigned int r) const
  {
    if(r == classes()-1)
      return 1-fac<double>(d)/
        std::pow(static_cast<double>(d), static_cast<double>(d+classes()-2)) *
        stirling2<double>(d+classes()-2, d);
    else
      return fac<double>(d)/
        std::pow(static_cast<double>(d), static_cast<double>(d+r)) *
        stirling2<double>(d+r-1, d-1);
  }
private:
  int d;
};

// permutation test
class permutation_experiment : public equidistribution_experiment
{
public:
  permutation_experiment(unsigned int t)
    : equidistribution_experiment(fac<int>(t)), t(t)
  {
    assert(t > 1);
  }

  template<class UniformRandomNumberGenerator, class Counter>
  void run(UniformRandomNumberGenerator & f, Counter & count, int n) const
  {
    typedef typename UniformRandomNumberGenerator::result_type result_type;
    std::vector<result_type> v(t);
    for(int i = 0; i < n; ++i) {
      for(int j = 0; j < t; ++j) {
        v[j] = f();
      }
      int x = 0;
      for(int r = t-1; r > 0; r--) {
        typename std::vector<result_type>::iterator it =
          std::max_element(v.begin(), v.begin()+r+1);
        x = (r+1)*x + (it-v.begin());
        std::iter_swap(it, v.begin()+r);
      }
      count(x);
    }
  }
private:
  int t;
};

// birthday spacing experiment test
class birthday_spacing_experiment : public experiment_base
{
public:
  birthday_spacing_experiment(unsigned int d, int n, int m)
    : experiment_base(d), n(n), m(m)
  {
  }

  template<class UniformRandomNumberGenerator, class Counter>
  void run(UniformRandomNumberGenerator & f, Counter & count, int n_total) const
  {
    typedef typename UniformRandomNumberGenerator::result_type result_type;
    assert(std::numeric_limits<result_type>::is_integer);
    assert((f.min)() == 0);
    assert((f.max)() == static_cast<result_type>(m-1));

    for(int j = 0; j < n_total; j++) {
      std::vector<result_type> v(n);
      std::generate_n(v.begin(), n, f);
      std::sort(v.begin(), v.end());
      std::vector<result_type> spacing(n);
      for(int i = 0; i < n-1; i++)
        spacing[i] = v[i+1]-v[i];
      spacing[n-1] = v[0] + m - v[n-1];
      std::sort(spacing.begin(), spacing.end());
      unsigned int k = 0;
      for(int i = 0; i < n-1; ++i) {
        if(spacing[i] == spacing[i+1])
          ++k;
      }
      count((std::min)(k, classes()-1));
    }
  }

  double probability(unsigned int r) const
  {
    assert(classes() == 4);
    assert(m == (1<<25));
    assert(n == 512);
    static const double prob[] = { 0.368801577, 0.369035243, 0.183471182,
                                   0.078691997 };
    return prob[r];
  }
private:
  int n, m;
};
/*
 * Misc. helper functions.
 */

template<class Float>
struct distribution_function
{
  typedef Float result_type;
  typedef Float argument_type;
  typedef Float first_argument_type;
  typedef Float second_argument_type;
};

// computes P(K_n <= t) or P(t1 <= K_n <= t2).  See Knuth, 3.3.1
class kolmogorov_smirnov_probability : public distribution_function<double>
{
public:
  kolmogorov_smirnov_probability(int n)
    : approx(n > 50), n(n), sqrt_n(std::sqrt(double(n)))
  {
    if(!approx)
      n_n = std::pow(static_cast<double>(n), n);
  }

  double cdf(double t) const
  {
    if(approx) {
      return 1-std::exp(-2*t*t)*(1-2.0/3.0*t/sqrt_n);
    } else {
      t *= sqrt_n;
      double sum = 0;
      for(int k = static_cast<int>(std::ceil(t)); k <= n; k++)
        sum += binomial<double>(n, k) * std::pow(k-t, k) *
          std::pow(t+n-k, n-k-1);
      return 1 - t/n_n * sum;
    }
  }
  //double operator()(double t1, double t2) const
  //{ return operator()(t2) - operator()(t1); }

private:
  bool approx;
  int n;
  double sqrt_n;
  double n_n;
};

inline double cdf(const kolmogorov_smirnov_probability& dist, double val)
{
  return dist.cdf(val);
}

inline double quantile(const kolmogorov_smirnov_probability& dist, double val)
{
    return invert_monotone_inc(boost::bind(&cdf, dist, _1), val, 0.0, 1000.0);
}

/*
 * Experiments for generators with continuous distribution functions
 */
class kolmogorov_experiment
{
public:
  kolmogorov_experiment(int n) : n(n), ksp(n) { }
  template<class NumberGenerator, class Distribution>
  double run(NumberGenerator & gen, Distribution distrib) const
  {
    const int m = n;
    typedef std::vector<double> saved_temp;
    saved_temp a(m,1.0), b(m,0);
    std::vector<int> c(m,0);
    for(int i = 0; i < n; ++i) {
      double val = static_cast<double>(gen());
      double y = cdf(distrib, val);
      int k = static_cast<int>(std::floor(m*y));
      if(k >= m)
        --k;    // should not happen
      a[k] = (std::min)(a[k], y);
      b[k] = (std::max)(b[k], y);
      ++c[k];
    }
    double kplus = 0, kminus = 0;
    int j = 0;
    for(int k = 0; k < m; ++k) {
      if(c[k] > 0) {
        kminus = (std::max)(kminus, a[k]-j/static_cast<double>(n));
        j += c[k];
        kplus = (std::max)(kplus, j/static_cast<double>(n) - b[k]);
      }
    }
    kplus *= std::sqrt(double(n));
    kminus *= std::sqrt(double(n));
    // std::cout << "k+ " << kplus << "   k- " << kminus << std::endl;
    return kplus;
  }
  double probability(double x) const
  {
    return cdf(ksp, x);
  }
private:
  int n;
  kolmogorov_smirnov_probability ksp;
};

struct power_distribution
{
  power_distribution(double t) : t(t) {}
  double t;
};

double cdf(const power_distribution& dist, double val)
{
  return std::pow(val, dist.t);
}

// maximum-of-t test (KS-based)
template<class UniformRandomNumberGenerator>
class maximum_experiment
{
public:
  typedef UniformRandomNumberGenerator base_type;
  maximum_experiment(base_type & f, int n, int t) : f(f), ke(n), t(t)
  { }

  double operator()() const
  {
    generator gen(f, t);
    return ke.run(gen, power_distribution(t));
  }

private:
  struct generator {
    generator(base_type & f, int t) : f(f, boost::uniform_01<>()), t(t) { }
    double operator()()
    {
      double mx = f();
      for(int i = 1; i < t; ++i)
        mx = (std::max)(mx, f());
      return mx;
    }
  private:
    boost::variate_generator<base_type&, boost::uniform_01<> > f;
    int t;
  };
  base_type & f;
  kolmogorov_experiment ke;
  int t;
};

// compute a chi-square value for the distribution approximation error
template<class ForwardIterator, class UnaryFunction>
typename UnaryFunction::result_type
chi_square_value(ForwardIterator first, ForwardIterator last,
                 UnaryFunction probability)
{
  typedef std::iterator_traits<ForwardIterator> iter_traits;
  typedef typename iter_traits::value_type counter_type;
  typedef typename UnaryFunction::result_type result_type;
  unsigned int classes = std::distance(first, last);
  result_type sum = 0;
  counter_type n = 0;
  for(unsigned int i = 0; i < classes; ++first, ++i) {
    counter_type count = *first;
    n += count;
    sum += (count/probability(i)) * count;  // avoid overflow
  }
#if 0
  for(unsigned int i = 0; i < classes; ++i) {
    // std::cout << (n*probability(i)) << " ";
    if(n * probability(i) < 5)
      std::cerr << "Not enough test runs for slot " << i
                << " p=" << probability(i) << ", n=" << n
                << std::endl;
  }
#endif
  // std::cout << std::endl;
  // throw std::invalid_argument("not enough test runs");

  return sum/n - n;
}
template<class RandomAccessContainer>
class generic_counter
{
public:
  explicit generic_counter(unsigned int classes) : container(classes, 0) { }
  void operator()(int i)
  {
    assert(i >= 0);
    assert(static_cast<unsigned int>(i) < container.size());
    ++container[i];
  }
  typename RandomAccessContainer::const_iterator begin() const
  { return container.begin(); }
  typename RandomAccessContainer::const_iterator end() const
  { return container.end(); }

private:
  RandomAccessContainer container;
};

// chi_square test
template<class Experiment, class Generator>
double run_experiment(const Experiment & experiment, Generator & gen, int n)
{
  generic_counter<std::vector<int> > v(experiment.classes());
  experiment.run(gen, v, n);
  return chi_square_value(v.begin(), v.end(),
                          boost::bind(&Experiment::probability,
                                       experiment, boost::placeholders::_1));
}

// chi_square test
template<class Experiment, class Generator>
double run_experiment(const Experiment & experiment, const Generator & gen, int n)
{
  generic_counter<std::vector<int> > v(experiment.classes());
  experiment.run(gen, v, n);
  return chi_square_value(v.begin(), v.end(),
                          boost::bind(&Experiment::probability,
                                       experiment, boost::placeholders::_1));
}

// number generator with experiment results (for nesting)
template<class Experiment, class Generator>
class experiment_generator_t
{
public:
  experiment_generator_t(const Experiment & exper, Generator & gen, int n)
    : experiment(exper), generator(gen), n(n) { }
  double operator()() const { return run_experiment(experiment, generator, n); }
private:
  const Experiment & experiment;
  Generator & generator;
  int n;
};

template<class Experiment, class Generator>
experiment_generator_t<Experiment, Generator>
experiment_generator(const Experiment & e, Generator & gen, int n)
{
  return experiment_generator_t<Experiment, Generator>(e, gen, n);
}

template<class Experiment, class Generator, class Distribution>
class ks_experiment_generator_t
{
public:
  ks_experiment_generator_t(const Experiment & exper, Generator & gen,
                            const Distribution & distrib)
    : experiment(exper), generator(gen), distribution(distrib) { }
  double operator()() const { return experiment.run(generator, distribution); }
private:
  const Experiment & experiment;
  Generator & generator;
  Distribution distribution;
};

template<class Experiment, class Generator, class Distribution>
ks_experiment_generator_t<Experiment, Generator, Distribution>
ks_experiment_generator(const Experiment & e, Generator & gen,
                        const Distribution & distrib)
{
  return ks_experiment_generator_t<Experiment, Generator, Distribution>
    (e, gen, distrib);
}

#endif /* STATISTIC_TESTS_HPP */


#pragma once
#ifndef OPENGM_SUBGRADIENT_SSVM_LEARNER_HXX
#define OPENGM_SUBGRADIENT_SSVM_LEARNER_HXX

#include <iomanip>
#include <vector>
#include <opengm/inference/inference.hxx>
#include <opengm/graphicalmodel/weights.hxx>
#include <opengm/utilities/random.hxx>
#include <opengm/learning/gradient-accumulator.hxx>
#include <opengm/learning/weight_averaging.hxx>

#ifdef WITH_OPENMP
#include <omp.h>
#endif

#include <boost/circular_buffer.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

namespace opengm {
    namespace learning {

    template<class T>
    double gen_normal_3(T &generator)
    {
      return generator();
    }

    // Version that fills a vector
    template<class T>
    void gen_normal_3(T &generator,
                  std::vector<double> &res)
    {
      for(size_t i=0; i<res.size(); ++i)
        res[i]=generator();
    }

    template<class DATASET>
    class Rws
    {
    public:
        typedef DATASET DatasetType;
        typedef typename DATASET::GMType   GMType;
        typedef typename DATASET::GMWITHLOSS GMWITHLOSS;
        typedef typename DATASET::LossType LossType;
        typedef typename GMType::ValueType ValueType;
        typedef typename GMType::IndexType IndexType;
        typedef typename GMType::LabelType LabelType;
        typedef opengm::learning::Weights<double> WeightsType;
        typedef typename std::vector<LabelType>::const_iterator LabelIterator;
        typedef FeatureAccumulator<GMType, LabelIterator> FeatureAcc;

        typedef std::vector<LabelType> ConfType;
        typedef boost::circular_buffer<ConfType> ConfBuffer;
        typedef std::vector<ConfBuffer> ConfBufferVec;

        class Parameter{
        public:

            Parameter(){
                eps_ = 0.00001;
                maxIterations_ = 10000;
                stopLoss_ = 0.0;
                learningRate_ = 1.0;
                C_ = 1.0;
                averaging_ = -1;
                p_ = 10;
                sigma_ = 1.0;
            }

            double eps_;
            size_t maxIterations_;
            double stopLoss_;
            double learningRate_;
            double C_;
            int averaging_;
            size_t p_;
            double sigma_;
        };

        Rws(DATASET&, const Parameter& );

        template<class INF>
        void learn(const typename INF::Parameter& para);
        //template<class INF, class VISITOR>
        //void learn(typename INF::Parameter para, VITITOR vis);

        const opengm::learning::Weights<double>& getWeights(){return weights_;}
        Parameter& getLerningParameters(){return para_;}

        double getLoss(const GMType & gm ,const GMWITHLOSS  & gmWithLoss, std::vector<LabelType> & labels){

            double loss = 0 ;
            std::vector<LabelType> subConf(20,0);

            for(size_t fi=gm.numberOfFactors(); fi<gmWithLoss.numberOfFactors(); ++fi){
                for(size_t v=0; v<gmWithLoss[fi].numberOfVariables(); ++v){
                    subConf[v] = labels[ gmWithLoss[fi].variableIndex(v)];
                }
                loss +=  gmWithLoss[fi](subConf.begin());
            }
            return loss;
        }

    private:

        double updateWeights();

        DATASET& dataset_;
        WeightsType  weights_;
        Parameter para_;
        size_t iteration_;
        FeatureAcc featureAcc_;
        WeightRegularizer<ValueType> wReg_;
        WeightAveraging<double> weightAveraging_;
    };

    template<class DATASET>
    Rws<DATASET>::Rws(DATASET& ds, const Parameter& p )
    :   dataset_(ds),
        para_(p),
        iteration_(0),
        featureAcc_(ds.getNumberOfWeights()),
        wReg_(2, 1.0/p.C_),
        weightAveraging_(ds.getWeights(),p.averaging_)
    {
        featureAcc_.resetWeights();
        weights_ = opengm::learning::Weights<double>(ds.getNumberOfWeights());
    }

    template<class DATASET>
    template<class INF>
    void Rws<DATASET>::learn(const typename INF::Parameter& para){

        const size_t nModels = dataset_.getNumberOfModels();
        const size_t nWegihts = dataset_.getNumberOfWeights();

        //for(size_t wi=0; wi<nWegihts; ++wi){
        //    dataset_.getWeights().setWeight(wi, 0.0);
        //}

        RandomUniform<size_t> randModel(0, nModels);
        boost::math::normal_distribution<ValueType> nDist(0.0, para_.sigma_);
        std::vector< std::vector<ValueType> > noiseVecs(para_.p_, std::vector<ValueType>(nWegihts));
        std::vector<ValueType> lossVec(para_.p_);

        std::vector<ValueType> gradient(nWegihts);

        boost::variate_generator<boost::mt19937, boost::normal_distribution<> >
        generator(boost::mt19937(time(0)),boost::normal_distribution<>(0.0, para_.sigma_));

        std::cout<<"online mode "<<nWegihts<<"\n";

        std::cout <<"start loss"<< std::setw(6) << std::setfill(' ') << iteration_ << ':'
                          << std::setw(8) << dataset_. template getTotalLossParallel<INF>(para) <<"  \n\n\n\n";

        for(iteration_=0 ; iteration_<para_.maxIterations_; ++iteration_){

            // get random model
            const size_t gmi = randModel();

            // save the current weights
            WeightsType currentWeights  = dataset_.getWeights();

            featureAcc_.resetWeights();

            // lock the model
            dataset_.lockModel(gmi);

            for(size_t p=0; p<para_.p_; ++p){

                // fill noise
                gen_normal_3(generator, noiseVecs[p]);

                // add noise to the weights
                for(size_t wi=0; wi<nWegihts; ++wi){
                    const ValueType cw = currentWeights[wi];
                    const ValueType nw = cw + noiseVecs[p][wi];
                    dataset_.getWeights().setWeight(wi, nw);
                }

                const GMType & gm = dataset_.getModel(gmi);
                // do inference
                std::vector<LabelType> arg;
                opengm::infer<INF>(gm, para, arg);
                lossVec[p] = dataset_.getLoss(arg, gmi);

                //featureAcc_.accumulateModelFeatures(gm, dataset_.getGT(gmi).begin(), arg.begin());
                // update weights
                //const double wChange =updateWeights();
            }

            //for(size_t wi=0; wi<nWegihts; ++wi){
            //    gradient[wi] = featureAcc_.getWeight(wi);
            //}
            std::fill(gradient.begin(), gradient.end(),0.0);
            for(size_t p=0; p<para_.p_; ++p){
                for(size_t wi=0; wi<nWegihts; ++wi){
                    gradient[wi] += (1.0/para_.p_)*(noiseVecs[p][wi])*lossVec[p];
                }
            }

            const ValueType actualLearningRate = para_.learningRate_/(1.0 + iteration_);
            //const ValueType actualLearningRate = para_.learningRate_;///(1.0 + iteration_);
            // do update
            for(size_t wi=0; wi<nWegihts; ++wi){
                const ValueType oldWeight = currentWeights[wi];
                const ValueType newWeights = (oldWeight - actualLearningRate*gradient[wi])*para_.C_;
                //std::cout<<"wi "<<newWeights<<"\n";
                dataset_.getWeights().setWeight(wi, newWeights);
            }
            std::cout<<"\n";
            dataset_.unlockModel(gmi);

            if(iteration_%10==0){
            //if(iteration_%nModels*2 == 0 ){
                std::cout << '\n'
                          << std::setw(6) << std::setfill(' ') << iteration_ << ':'
                          << std::setw(8) << dataset_. template getTotalLossParallel<INF>(para) <<"  "<< std::flush;

            }

        }

        weights_ = dataset_.getWeights();
    }

    template<class DATASET>
    double Rws<DATASET>::updateWeights(){

        const size_t nWegihts = dataset_.getNumberOfWeights();

        WeightsType p(nWegihts);
        WeightsType newWeights(nWegihts);

        for(size_t wi=0; wi<nWegihts; ++wi){
            p[wi] =  dataset_.getWeights().getWeight(wi);
            p[wi] += para_.C_ * featureAcc_.getWeight(wi);
        }

        double wChange = 0.0;

        for(size_t wi=0; wi<nWegihts; ++wi){
            const double wOld = dataset_.getWeights().getWeight(wi);
            const double wNew = wOld - (para_.learningRate_/double(iteration_+1))*p[wi];
            newWeights[wi] = wNew;
        }

        weightAveraging_(newWeights);

        weights_ = dataset_.getWeights();
        return wChange;
    }
}
}
#endif


/* jacobian.cpp -- Computation of Jacobian matrix

    Copyright (C) 2012-2014 University of Reading
    Copyright (C) 2015-2020 European Centre for Medium-Range Weather Forecasts

    Author: Robin Hogan <r.j.hogan@ecmwf.int>

    This file is part of the Adept library.

*/

#ifdef _OPENMP
#include <omp.h>
#endif

#include <adept_arrays.h>

namespace adept {

  namespace internal {
    static const int MULTIPASS_SIZE = ADEPT_REAL_PACKET_SIZE == 1 ? ADEPT_MULTIPASS_SIZE : ADEPT_REAL_PACKET_SIZE;
  }

  using namespace internal;

  template <typename T>
  T _check_long_double() {
    // The user may have requested Real to be of type "long double" by
    // specifying ADEPT_REAL_TYPE_SIZE=16. If the present system can
    // only support double then sizeof(long double) will be 8, but
    // Adept will not be emitting the best code for this, so it is
    // probably better to fail forcing the user to specify
    // ADEPT_REAL_TYPE_SIZE=8.
    ADEPT_STATIC_ASSERT(ADEPT_REAL_TYPE_SIZE != 16 || ADEPT_REAL_TYPE_SIZE == sizeof(Real),
			COMPILER_DOES_NOT_SUPPORT_16_BYTE_LONG_DOUBLE);
    return 1;
  }

#if ADEPT_REAL_PACKET_SIZE > 1
  void
  Stack::jacobian_forward_kernel(Real* __restrict gradient_multipass_b) const
  {

    // Loop forward through the derivative statements
    for (uIndex ist = 1; ist < n_statements_; ist++) {
      const Statement& statement = statement_[ist];
      // We copy the LHS to "a" in case it appears on the RHS in any
      // of the following statements
      Packet<Real> a; // Zeroed automatically
      // Loop through operations
      for (uIndex iop = statement_[ist-1].end_plus_one;
	   iop < statement.end_plus_one; iop++) {
	Packet<Real> g(gradient_multipass_b+index_[iop]*MULTIPASS_SIZE);
	Packet<Real> m(multiplier_[iop]);
	a += m * g;
      }
      // Copy the results
      a.put(gradient_multipass_b+statement.index*MULTIPASS_SIZE);
    } // End of loop over statements
  }
#else
  void
  Stack::jacobian_forward_kernel(Real* __restrict gradient_multipass_b) const
  {

    // Loop forward through the derivative statements
    for (uIndex ist = 1; ist < n_statements_; ist++) {
      const Statement& statement = statement_[ist];
      // We copy the LHS to "a" in case it appears on the RHS in any
      // of the following statements
      Block<MULTIPASS_SIZE,Real> a; // Zeroed automatically
      // Loop through operations
      for (uIndex iop = statement_[ist-1].end_plus_one;
	   iop < statement.end_plus_one; iop++) {
	for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	  a[i] += multiplier_[iop]*gradient_multipass_b[index_[iop]*MULTIPASS_SIZE+i];
	}
      }
      // Copy the results
      for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	gradient_multipass_b[statement.index*MULTIPASS_SIZE+i] = a[i];
      }
    } // End of loop over statements
  }
#endif

  void
  Stack::jacobian_forward_kernel_extra(Real* __restrict gradient_multipass_b,
				       uIndex n_extra) const
  {

    // Loop forward through the derivative statements
    for (uIndex ist = 1; ist < n_statements_; ist++) {
      const Statement& statement = statement_[ist];
      // We copy the LHS to "a" in case it appears on the RHS in any
      // of the following statements
      Block<MULTIPASS_SIZE,Real> a; // Zeroed automatically
      // Loop through operations
      for (uIndex iop = statement_[ist-1].end_plus_one;
	   iop < statement.end_plus_one; iop++) {
	for (uIndex i = 0; i < n_extra; i++) {
	  a[i] += multiplier_[iop]*gradient_multipass_b[index_[iop]*MULTIPASS_SIZE+i];
	}
      }
      // Copy the results
      for (uIndex i = 0; i < n_extra; i++) {
	gradient_multipass_b[statement.index*MULTIPASS_SIZE+i] = a[i];
      }
    } // End of loop over statements
  }

  // Compute the Jacobian matrix, parallelized using OpenMP. Normally
  // the user would call the jacobian or jacobian_forward functions,
  // and the OpenMP version would only be called if OpenMP is
  // available and the Jacobian matrix is large enough for
  // parallelization to be worthwhile.  Note that jacobian_out must be
  // allocated to be at least of size m*n, where m is the number of
  // dependent variables and n is the number of independents. The
  // independents and dependents must have already been identified
  // with the functions "independent" and "dependent", otherwise this
  // function will fail with FAILURE_XXDEPENDENT_NOT_IDENTIFIED. The
  // offsets in memory of the two dimensions are provided by
  // dep_offset and indep_offset. This is implemented using a forward
  // pass, appropriate for m>=n.
  void
  Stack::jacobian_forward_openmp(Real* jacobian_out,
				 Index dep_offset, Index indep_offset) const
  {

    // Number of blocks to cycle through, including a possible last
    // block containing fewer than MULTIPASS_SIZE variables
    int n_block = (n_independent() + MULTIPASS_SIZE - 1)
      / MULTIPASS_SIZE;
    uIndex n_extra = n_independent() % MULTIPASS_SIZE;

#pragma omp parallel
    {
      //      std::vector<Block<MULTIPASS_SIZE,Real> >
      //	gradient_multipass_b(max_gradient_);
      uIndex gradient_multipass_size = max_gradient_*MULTIPASS_SIZE;
      Real* __restrict gradient_multipass_b
	= alloc_aligned<Real>(gradient_multipass_size);

#pragma omp for schedule(static)
      for (int iblock = 0; iblock < n_block; iblock++) {
	// Set the index to the dependent variables for this block
	uIndex i_independent =  MULTIPASS_SIZE * iblock;

	uIndex block_size = MULTIPASS_SIZE;
	// If this is the last iteration and the number of extra
	// elements is non-zero, then set the block size to the number
	// of extra elements. If the number of extra elements is zero,
	// then the number of independent variables is exactly divisible
	// by MULTIPASS_SIZE, so the last iteration will be the
	// same as all the rest.
	if (iblock == n_block-1 && n_extra > 0) {
	  block_size = n_extra;
	}

	// Set the initial gradients all to zero
	for (uIndex i = 0; i < gradient_multipass_size; i++) {
	  gradient_multipass_b[i] = 0.0;
	}
	// Each seed vector has one non-zero entry of 1.0
	for (uIndex i = 0; i < block_size; i++) {
	  gradient_multipass_b[independent_index_[i_independent+i]*MULTIPASS_SIZE+i] = 1.0;
	}

	jacobian_forward_kernel(gradient_multipass_b);

	// Copy the gradients corresponding to the dependent variables
	// into the Jacobian matrix
	if (indep_offset == 1) {
	  for (uIndex idep = 0; idep < n_dependent(); idep++) {
	    for (uIndex i = 0; i < block_size; i++) {
	      jacobian_out[idep*dep_offset+i_independent+i]
		= gradient_multipass_b[dependent_index_[idep]*MULTIPASS_SIZE+i];
	    }
	  }
	}
	else {
	  for (uIndex idep = 0; idep < n_dependent(); idep++) {
	    for (uIndex i = 0; i < block_size; i++) {
	      jacobian_out[(i_independent+i)*indep_offset+idep*dep_offset]
		= gradient_multipass_b[dependent_index_[idep]*MULTIPASS_SIZE+i];
	    }
	  }
	}
      } // End of loop over blocks
      free_aligned(gradient_multipass_b);
    } // End of parallel section
  } // End of jacobian function

  // Compute the Jacobian matrix; note that jacobian_out must be
  // allocated to be of size m*n, where m is the number of dependent
  // variables and n is the number of independents. The independents
  // and dependents must have already been identified with the
  // functions "independent" and "dependent", otherwise this function
  // will fail with FAILURE_XXDEPENDENT_NOT_IDENTIFIED. This is
  // implemented using a forward pass, appropriate for m>=n.
  void
  Stack::jacobian_forward(Real* jacobian_out,
			  Index dep_offset, Index indep_offset) const
  {
    if (independent_index_.empty() || dependent_index_.empty()) {
      throw(dependents_or_independents_not_identified());
    }

    // If either of the offsets are zero, set them to the size of the
    // other dimension, which assumes that the full Jacobian matrix is
    // contiguous in memory.
    if (dep_offset <= 0) {
      dep_offset = n_independent();
    }
    if (indep_offset <= 0) {
      indep_offset = n_dependent();
    }

#ifdef _OPENMP
    if (have_openmp_
	&& !openmp_manually_disabled_
	&& n_independent() > MULTIPASS_SIZE
	&& omp_get_max_threads() > 1) {
      // Call the parallel version
      jacobian_forward_openmp(jacobian_out, dep_offset, indep_offset);
      return;
    }
#endif

    // For optimization reasons, we process a block of
    // MULTIPASS_SIZE columns of the Jacobian at once; calculate
    // how many blocks are needed and how many extras will remain
    uIndex n_block = n_independent() / MULTIPASS_SIZE;
    uIndex n_extra = n_independent() % MULTIPASS_SIZE;

    ///gradient_multipass_.resize(max_gradient_);
    uIndex gradient_multipass_size = max_gradient_*MULTIPASS_SIZE;
    Real* __restrict gradient_multipass_b
      = alloc_aligned<Real>(gradient_multipass_size);

    // Loop over blocks of MULTIPASS_SIZE columns
    for (uIndex iblock = 0; iblock < n_block; iblock++) {
      // Set the index to the dependent variables for this block
      uIndex i_independent =  MULTIPASS_SIZE * iblock;

      // Set the initial gradients all to zero
      ///zero_gradient_multipass();
      for (uIndex i = 0; i < gradient_multipass_size; i++) {
	gradient_multipass_b[i] = 0.0;
      }

      // Each seed vector has one non-zero entry of 1.0
      for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	gradient_multipass_b[independent_index_[i_independent+i]*MULTIPASS_SIZE+i] = 1.0;
      }

      jacobian_forward_kernel(gradient_multipass_b);

      // Copy the gradients corresponding to the dependent variables
      // into the Jacobian matrix
      if (indep_offset == 1) {
	for (uIndex idep = 0; idep < n_dependent(); idep++) {
	  for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	    jacobian_out[idep*dep_offset+i_independent+i]
	      = gradient_multipass_b[dependent_index_[idep]*MULTIPASS_SIZE+i];
	  }
	}
      }
      else {
	for (uIndex idep = 0; idep < n_dependent(); idep++) {
	  for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	    jacobian_out[(i_independent+i)*indep_offset+idep*dep_offset]
	      = gradient_multipass_b[dependent_index_[idep]*MULTIPASS_SIZE+i];
	  }
	}
      }
      i_independent += MULTIPASS_SIZE;
    } // End of loop over blocks

    // Now do the same but for the remaining few columns in the matrix
    if (n_extra > 0) {
      uIndex i_independent =  MULTIPASS_SIZE * n_block;
      ///zero_gradient_multipass();
      for (uIndex i = 0; i < gradient_multipass_size; i++) {
	gradient_multipass_b[i] = 0.0;
      }

      for (uIndex i = 0; i < n_extra; i++) {
	gradient_multipass_b[independent_index_[i_independent+i]*MULTIPASS_SIZE+i] = 1.0;
      }

      jacobian_forward_kernel_extra(gradient_multipass_b, n_extra);

      if (indep_offset == 1) {
	for (uIndex idep = 0; idep < n_dependent(); idep++) {
	  for (uIndex i = 0; i < n_extra; i++) {
	    jacobian_out[idep*dep_offset+i_independent+i]
	      = gradient_multipass_b[dependent_index_[idep]*MULTIPASS_SIZE+i];
	  }
	}
      }
      else {
	for (uIndex idep = 0; idep < n_dependent(); idep++) {
	  for (uIndex i = 0; i < n_extra; i++) {
	    jacobian_out[(i_independent+i)*indep_offset+idep*dep_offset]
	      = gradient_multipass_b[dependent_index_[idep]*MULTIPASS_SIZE+i];
	  }
	}
      }
    }

    free_aligned(gradient_multipass_b);
  }

  // Compute the Jacobian matrix, parallelized using OpenMP.  Normally
  // the user would call the jacobian or jacobian_reverse functions,
  // and the OpenMP version would only be called if OpenMP is
  // available and the Jacobian matrix is large enough for
  // parallelization to be worthwhile.  Note that jacobian_out must be
  // allocated to be at least of size m*n, where m is the number of
  // dependent variables and n is the number of independents. The
  // independents and dependents must have already been identified
  // with the functions "independent" and "dependent", otherwise this
  // function will fail with FAILURE_XXDEPENDENT_NOT_IDENTIFIED. The
  // offsets in memory of the two dimensions are provided by
  // dep_offset and indep_offset.  This is implemented using a reverse
  // pass, appropriate for m<n.
  void
  Stack::jacobian_reverse_openmp(Real* jacobian_out,
				 Index dep_offset, Index indep_offset) const
  {

    // Number of blocks to cycle through, including a possible last
    // block containing fewer than MULTIPASS_SIZE variables
    int n_block = (n_dependent() + MULTIPASS_SIZE - 1)
      / MULTIPASS_SIZE;
    uIndex n_extra = n_dependent() % MULTIPASS_SIZE;

    // Inside the OpenMP loop, the "this" pointer may be NULL if the
    // adept::Stack pointer is declared as thread-local and if the
    // OpenMP memory model uses thread-local storage for private
    // data. If this is the case then local pointers to or copies of
    // the following members of the adept::Stack object may need to be
    // made: dependent_index_ n_statements_ statement_ multiplier_
    // index_ independent_index_ n_dependent() n_independent().
    // Limited testing implies this is OK though.

#pragma omp parallel
    {
      std::vector<Block<MULTIPASS_SIZE,Real> >
	gradient_multipass_b(max_gradient_);

#pragma omp for schedule(static)
      for (int iblock = 0; iblock < n_block; iblock++) {
	// Set the index to the dependent variables for this block
	uIndex i_dependent =  MULTIPASS_SIZE * iblock;

	uIndex block_size = MULTIPASS_SIZE;
	// If this is the last iteration and the number of extra
	// elements is non-zero, then set the block size to the number
	// of extra elements. If the number of extra elements is zero,
	// then the number of independent variables is exactly divisible
	// by MULTIPASS_SIZE, so the last iteration will be the
	// same as all the rest.
	if (iblock == n_block-1 && n_extra > 0) {
	  block_size = n_extra;
	}

	// Set the initial gradients all to zero
	for (std::size_t i = 0; i < gradient_multipass_b.size(); i++) {
	  gradient_multipass_b[i].zero();
	}
	// Each seed vector has one non-zero entry of 1.0
	for (uIndex i = 0; i < block_size; i++) {
	  gradient_multipass_b[dependent_index_[i_dependent+i]][i] = 1.0;
	}

	// Loop backward through the derivative statements
	for (uIndex ist = n_statements_-1; ist > 0; ist--) {
	  const Statement& statement = statement_[ist];
	  // We copy the RHS to "a" in case it appears on the LHS in any
	  // of the following statements
	  Real a[MULTIPASS_SIZE];
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	  // For large blocks, we only process the ones where a[i] is
	  // non-zero
	  uIndex i_non_zero[MULTIPASS_SIZE];
#endif
	  uIndex n_non_zero = 0;
	  for (uIndex i = 0; i < block_size; i++) {
	    a[i] = gradient_multipass_b[statement.index][i];
	    gradient_multipass_b[statement.index][i] = 0.0;
	    if (a[i] != 0.0) {
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	      i_non_zero[n_non_zero++] = i;
#else
	      n_non_zero = 1;
#endif
	    }
	  }

	  // Only do anything for this statement if any of the a values
	  // are non-zero
	  if (n_non_zero) {
	    // Loop through the operations
	    for (uIndex iop = statement_[ist-1].end_plus_one;
		 iop < statement.end_plus_one; iop++) {
	      // Try to minimize pointer dereferencing by making local
	      // copies
	      Real multiplier = multiplier_[iop];
	      Real* __restrict gradient_multipass
		= &(gradient_multipass_b[index_[iop]][0]);
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	      // For large blocks, loop over only the indices
	      // corresponding to non-zero a
	      for (uIndex i = 0; i < n_non_zero; i++) {
		gradient_multipass[i_non_zero[i]] += multiplier*a[i_non_zero[i]];
	      }
#else
	      // For small blocks, do all indices
	      for (uIndex i = 0; i < block_size; i++) {
	      //	      for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
		gradient_multipass[i] += multiplier*a[i];
	      }
#endif
	    }
	  }
	} // End of loop over statement
	// Copy the gradients corresponding to the independent
	// variables into the Jacobian matrix
	if (dep_offset == 1) {
	  for (uIndex iindep = 0; iindep < n_independent(); iindep++) {
	    for (uIndex i = 0; i < block_size; i++) {
	      jacobian_out[iindep*indep_offset+i_dependent+i]
		= gradient_multipass_b[independent_index_[iindep]][i];
	    }
	  }
	}
	else {
	  for (uIndex iindep = 0; iindep < n_independent(); iindep++) {
	    for (uIndex i = 0; i < block_size; i++) {
	      jacobian_out[iindep*indep_offset+(i_dependent+i)*dep_offset]
		= gradient_multipass_b[independent_index_[iindep]][i];
	    }
	  }
	}
      } // End of loop over blocks
    } // end #pragma omp parallel
  } // end jacobian_reverse_openmp

  // Compute the Jacobian matrix; note that jacobian_out must be
  // allocated to be of size m*n, where m is the number of dependent
  // variables and n is the number of independents. The independents
  // and dependents must have already been identified with the
  // functions "independent" and "dependent", otherwise this function
  // will fail with FAILURE_XXDEPENDENT_NOT_IDENTIFIED.  This is
  // implemented using a reverse pass, appropriate for m<n.
  void
  Stack::jacobian_reverse(Real* jacobian_out,
			  Index dep_offset, Index indep_offset) const
  {
    if (independent_index_.empty() || dependent_index_.empty()) {
      throw(dependents_or_independents_not_identified());
    }

    // If either of the offsets are zero, set them to the size of the
    // other dimension, which assumes that the full Jacobian matrix is
    // contiguous in memory.
    if (dep_offset <= 0) {
      dep_offset = n_independent();
    }
    if (indep_offset <= 0) {
      indep_offset = n_dependent();
    }

#ifdef _OPENMP
    if (have_openmp_
	&& !openmp_manually_disabled_
	&& n_dependent() > MULTIPASS_SIZE
	&& omp_get_max_threads() > 1) {
      // Call the parallel version
      jacobian_reverse_openmp(jacobian_out,
			      dep_offset, indep_offset);
      return;
    }
#endif

    //    gradient_multipass_.resize(max_gradient_);
    std::vector<Block<MULTIPASS_SIZE,Real> >
      gradient_multipass_b(max_gradient_);

    // For optimization reasons, we process a block of
    // MULTIPASS_SIZE rows of the Jacobian at once; calculate
    // how many blocks are needed and how many extras will remain
    uIndex n_block = n_dependent() / MULTIPASS_SIZE;
    uIndex n_extra = n_dependent() % MULTIPASS_SIZE;
    uIndex i_dependent = 0; // uIndex of first row in the block we are
			    // currently computing
    // Loop over the of MULTIPASS_SIZE rows
    for (uIndex iblock = 0; iblock < n_block; iblock++) {
      // Set the initial gradients all to zero
      //      zero_gradient_multipass();
      for (std::size_t i = 0; i < gradient_multipass_b.size(); i++) {
	gradient_multipass_b[i].zero();
      }

      // Each seed vector has one non-zero entry of 1.0
      for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	gradient_multipass_b[dependent_index_[i_dependent+i]][i] = 1.0;
      }
      // Loop backward through the derivative statements
      for (uIndex ist = n_statements_-1; ist > 0; ist--) {
	const Statement& statement = statement_[ist];
	// We copy the RHS to "a" in case it appears on the LHS in any
	// of the following statements
	Real a[MULTIPASS_SIZE];
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	// For large blocks, we only process the ones where a[i] is
	// non-zero
	uIndex i_non_zero[MULTIPASS_SIZE];
#endif
	uIndex n_non_zero = 0;
	for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	  a[i] = gradient_multipass_b[statement.index][i];
	  gradient_multipass_b[statement.index][i] = 0.0;
	  if (a[i] != 0.0) {
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	    i_non_zero[n_non_zero++] = i;
#else
	    n_non_zero = 1;
#endif
	  }
	}
	// Only do anything for this statement if any of the a values
	// are non-zero
	if (n_non_zero) {
	  // Loop through the operations
	  for (uIndex iop = statement_[ist-1].end_plus_one;
	       iop < statement.end_plus_one; iop++) {
	    // Try to minimize pointer dereferencing by making local
	    // copies
	    Real multiplier = multiplier_[iop];
	    Real* __restrict gradient_multipass
	      = &(gradient_multipass_b[index_[iop]][0]);
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	    // For large blocks, loop over only the indices
	    // corresponding to non-zero a
	    for (uIndex i = 0; i < n_non_zero; i++) {
	      gradient_multipass[i_non_zero[i]] += multiplier*a[i_non_zero[i]];
	    }
#else
	    // For small blocks, do all indices
	    for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	      gradient_multipass[i] += multiplier*a[i];
	    }
#endif
	  }
	}
      } // End of loop over statement
      // Copy the gradients corresponding to the independent variables
      // into the Jacobian matrix
      if (dep_offset == 1) {
	for (uIndex iindep = 0; iindep < n_independent(); iindep++) {
	  for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	    jacobian_out[iindep*indep_offset+i_dependent+i]
	      = gradient_multipass_b[independent_index_[iindep]][i];
	  }
	}
      }
      else {
	for (uIndex iindep = 0; iindep < n_independent(); iindep++) {
	  for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	    jacobian_out[iindep*indep_offset+(i_dependent+i)*dep_offset]
	      = gradient_multipass_b[independent_index_[iindep]][i];
	  }
	}
      }
      i_dependent += MULTIPASS_SIZE;
    } // End of loop over blocks

    // Now do the same but for the remaining few rows in the matrix
    if (n_extra > 0) {
      for (std::size_t i = 0; i < gradient_multipass_b.size(); i++) {
	gradient_multipass_b[i].zero();
      }
      //      zero_gradient_multipass();
      for (uIndex i = 0; i < n_extra; i++) {
	gradient_multipass_b[dependent_index_[i_dependent+i]][i] = 1.0;
      }
      for (uIndex ist = n_statements_-1; ist > 0; ist--) {
	const Statement& statement = statement_[ist];
	Real a[MULTIPASS_SIZE];
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	uIndex i_non_zero[MULTIPASS_SIZE];
#endif
	uIndex n_non_zero = 0;
	for (uIndex i = 0; i < n_extra; i++) {
	  a[i] = gradient_multipass_b[statement.index][i];
	  gradient_multipass_b[statement.index][i] = 0.0;
	  if (a[i] != 0.0) {
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	    i_non_zero[n_non_zero++] = i;
#else
	    n_non_zero = 1;
#endif
	  }
	}
	if (n_non_zero) {
	  for (uIndex iop = statement_[ist-1].end_plus_one;
	       iop < statement.end_plus_one; iop++) {
	    Real multiplier = multiplier_[iop];
	    Real* __restrict gradient_multipass
	      = &(gradient_multipass_b[index_[iop]][0]);
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	    for (uIndex i = 0; i < n_non_zero; i++) {
	      gradient_multipass[i_non_zero[i]] += multiplier*a[i_non_zero[i]];
	    }
#else
	    for (uIndex i = 0; i < n_extra; i++) {
	      gradient_multipass[i] += multiplier*a[i];
	    }
#endif
	  }
	}
      }
      if (dep_offset == 1) {
	for (uIndex iindep = 0; iindep < n_independent(); iindep++) {
	  for (uIndex i = 0; i < n_extra; i++) {
	    jacobian_out[iindep*indep_offset+i_dependent+i]
	      = gradient_multipass_b[independent_index_[iindep]][i];
	  }
	}
      }
      else {
	for (uIndex iindep = 0; iindep < n_independent(); iindep++) {
	  for (uIndex i = 0; i < n_extra; i++) {
	    jacobian_out[iindep*indep_offset+(i_dependent+i)*dep_offset]
	      = gradient_multipass_b[independent_index_[iindep]][i];
	  }
	}
      }
    }
  }

  // Return the Jacobian matrix in the matrix "jac", using the forward
  // or reverse method depending which would be faster
  void Stack::jacobian(Array<2,Real,false> jac) const {
    if (jac.dimension(0) != n_dependent()
	|| jac.dimension(1) != n_independent()) {
      throw size_mismatch("Jacobian matrix has wrong size");
    }
    if (n_independent() <= n_dependent()) {
      jacobian_forward(jac.data(), jac.offset(0), jac.offset(1));
    }
    else {
      jacobian_reverse(jac.data(), jac.offset(0), jac.offset(1));
    }
  }

  // Return the Jacobian matrix in the matrix "jac", explicitly
  // specifying whether to use the forward or reverse method
  void Stack::jacobian_forward(Array<2,Real,false> jac) const {
    if (jac.dimension(0) != n_dependent()
	|| jac.dimension(1) != n_independent()) {
      throw size_mismatch("Jacobian matrix has wrong size");
    }
    jacobian_forward(jac.data(), jac.offset(0), jac.offset(1));
  }

  void Stack::jacobian_reverse(Array<2,Real,false> jac) const {
    if (jac.dimension(0) != n_dependent()
	|| jac.dimension(1) != n_independent()) {
      throw size_mismatch("Jacobian matrix has wrong size");
    }
    jacobian_reverse(jac.data(), jac.offset(0), jac.offset(1));
  }

  // Return the Jacobian matrix using the forward or reverse method
  // depending which would be faster
  Array<2,Real,false> Stack::jacobian() const {
    Array<2,Real,false> jac(n_dependent(), n_independent());
    if (n_independent() <= n_dependent()) {
      jacobian_forward(jac.data(), jac.offset(0), jac.offset(1));
    }
    else {
      jacobian_reverse(jac.data(), jac.offset(0), jac.offset(1));
    }
    return jac;
  }

  // Return the Jacobian matrix, explicitly specifying whether to use
  // the forward or reverse method
  Array<2,Real,false> Stack::jacobian_forward() const {
    Array<2,Real,false> jac(n_dependent(), n_independent());
    jacobian_forward(jac.data(), jac.offset(0), jac.offset(1));
    return jac;
  }

  Array<2,Real,false> Stack::jacobian_reverse() const {
    Array<2,Real,false> jac(n_dependent(), n_independent());
    jacobian_reverse(jac.data(), jac.offset(0), jac.offset(1));
    return jac;
  }

} // End namespace adept


#include "fluid_solver.h"
#include <deal.II/physics/elasticity/kinematics.h>
#include <deal.II/physics/elasticity/standard_tensors.h>

namespace Fluid
{
  template <int dim>
  BlockVector<double> FluidSolver<dim>::get_current_solution() const
  {
    return present_solution;
  }

  template <int dim>
  FluidSolver<dim>::FluidSolver(Triangulation<dim> &tria,
                                const Parameters::AllParameters &parameters,
                                std::shared_ptr<Function<dim>> bc)
    : triangulation(tria),
      fe(FE_Q<dim>(parameters.fluid_velocity_degree),
         dim,
         FE_Q<dim>(parameters.fluid_pressure_degree),
         1),
      scalar_fe(parameters.fluid_velocity_degree),
      dof_handler(triangulation),
      scalar_dof_handler(triangulation),
      volume_quad_formula(parameters.fluid_velocity_degree + 1),
      face_quad_formula(parameters.fluid_velocity_degree + 1),
      time(parameters.end_time,
           parameters.time_step,
           parameters.output_interval,
           parameters.refinement_interval,
           parameters.save_interval),
      timer(std::cout, TimerOutput::never, TimerOutput::wall_times),
      parameters(parameters),
      boundary_values(bc)
  {
  }

  template <int dim>
  void FluidSolver<dim>::setup_dofs()
  {
    // The first step is to associate DoFs with a given mesh.
    dof_handler.distribute_dofs(fe);
    scalar_dof_handler.distribute_dofs(scalar_fe);

    // We renumber the components to have all velocity DoFs come before
    // the pressure DoFs to be able to split the solution vector in two blocks
    // which are separately accessed in the block preconditioner.
    DoFRenumbering::Cuthill_McKee(dof_handler);
    std::vector<unsigned int> block_component(dim + 1, 0);
    block_component[dim] = 1;
    DoFRenumbering::component_wise(dof_handler, block_component);

    dofs_per_block.resize(2);
    DoFTools::count_dofs_per_block(
      dof_handler, dofs_per_block, block_component);
    unsigned int dof_u = dofs_per_block[0];
    unsigned int dof_p = dofs_per_block[1];

    std::cout << "   Number of active fluid cells: "
              << triangulation.n_active_cells() << std::endl
              << "   Number of degrees of freedom: " << dof_handler.n_dofs()
              << " (" << dof_u << '+' << dof_p << ')' << std::endl;
  }

  template <int dim>
  void FluidSolver<dim>::make_constraints()
  {
    nonzero_constraints.clear();
    zero_constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler, nonzero_constraints);
    DoFTools::make_hanging_node_constraints(dof_handler, zero_constraints);
    for (auto itr = parameters.fluid_dirichlet_bcs.begin();
         itr != parameters.fluid_dirichlet_bcs.end();
         ++itr)
      {
        // First get the id, flag and value from the input file
        unsigned int id = itr->first;
        unsigned int flag = itr->second.first;
        std::vector<double> value = itr->second.second;

        // To make VectorTools::interpolate_boundary_values happy,
        // a vector of bool and a vector of double which are of size
        // dim + 1 are required.
        std::vector<bool> mask(dim + 1, false);
        std::vector<double> augmented_value(dim + 1, 0.0);
        // 1-x, 2-y, 3-xy, 4-z, 5-xz, 6-yz, 7-xyz
        switch (flag)
          {
          case 1:
            mask[0] = true;
            augmented_value[0] = value[0];
            break;
          case 2:
            mask[1] = true;
            augmented_value[1] = value[0];
            break;
          case 3:
            mask[0] = true;
            mask[1] = true;
            augmented_value[0] = value[0];
            augmented_value[1] = value[1];
            break;
          case 4:
            mask[2] = true;
            augmented_value[2] = value[0];
            break;
          case 5:
            mask[0] = true;
            mask[2] = true;
            augmented_value[0] = value[0];
            augmented_value[2] = value[1];
            break;
          case 6:
            mask[1] = true;
            mask[2] = true;
            augmented_value[1] = value[0];
            augmented_value[2] = value[1];
            break;
          case 7:
            mask[0] = true;
            mask[1] = true;
            mask[2] = true;
            augmented_value[0] = value[0];
            augmented_value[1] = value[1];
            augmented_value[2] = value[2];
            break;
          default:
            AssertThrow(false, ExcMessage("Unrecogonized component flag!"));
            break;
          }
        if (parameters.use_hard_coded_values == 1)
          {
            VectorTools::interpolate_boundary_values(
              MappingQGeneric<dim>(parameters.fluid_velocity_degree),
              dof_handler,
              id,
              *boundary_values,
              nonzero_constraints,
              ComponentMask(mask));
          }
        else
          {
            VectorTools::interpolate_boundary_values(
              MappingQGeneric<dim>(parameters.fluid_velocity_degree),
              dof_handler,
              id,
              Functions::ConstantFunction<dim>(augmented_value),
              nonzero_constraints,
              ComponentMask(mask));
          }
        VectorTools::interpolate_boundary_values(
          MappingQGeneric<dim>(parameters.fluid_velocity_degree),
          dof_handler,
          id,
          Functions::ZeroFunction<dim>(dim + 1),
          zero_constraints,
          ComponentMask(mask));
      }
    nonzero_constraints.close();
    zero_constraints.close();
  }

  template <int dim>
  void FluidSolver<dim>::setup_cell_property()
  {
    cell_property.initialize(
      triangulation.begin_active(), triangulation.end(), 1);
    for (auto cell = triangulation.begin_active(); cell != triangulation.end();
         ++cell)
      {
        const std::vector<std::shared_ptr<CellProperty>> p =
          cell_property.get_data(cell);
        p[0]->indicator = 0;
        p[0]->fsi_acceleration = 0;
        p[0]->fsi_stress = 0;
      }
  }

  template <int dim>
  void FluidSolver<dim>::initialize_system()
  {
    system_matrix.clear();
    mass_matrix.clear();
    mass_schur.clear();

    BlockDynamicSparsityPattern dsp(dofs_per_block, dofs_per_block);
    DoFTools::make_sparsity_pattern(dof_handler, dsp, nonzero_constraints);
    sparsity_pattern.copy_from(dsp);

    system_matrix.reinit(sparsity_pattern);
    mass_matrix.reinit(sparsity_pattern);

    present_solution.reinit(dofs_per_block);
    solution_increment.reinit(dofs_per_block);
    system_rhs.reinit(dofs_per_block);

    // Compute the sparsity pattern for mass schur in advance.
    // It should be the same as \f$BB^T\f$.
    DynamicSparsityPattern schur_pattern(dofs_per_block[1], dofs_per_block[1]);
    schur_pattern.compute_mmult_pattern(sparsity_pattern.block(1, 0),
                                        sparsity_pattern.block(0, 1));
    mass_schur_pattern.copy_from(schur_pattern);
    mass_schur.reinit(mass_schur_pattern);

    // Cell property
    setup_cell_property();

    stress = std::vector<std::vector<Vector<double>>>(
      dim,
      std::vector<Vector<double>>(dim,
                                  Vector<double>(scalar_dof_handler.n_dofs())));
  }

  template <int dim>
  void FluidSolver<dim>::refine_mesh(const unsigned int min_grid_level,
                                     const unsigned int max_grid_level)
  {
    TimerOutput::Scope timer_section(timer, "Refine mesh");

    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());
    FEValuesExtractors::Vector velocity(0);
    using type = std::map<types::boundary_id, const Function<dim, double> *>;
    KellyErrorEstimator<dim>::estimate(
      dof_handler,
      QGauss<dim - 1>(parameters.fluid_velocity_degree),
      type(),
      present_solution,
      estimated_error_per_cell,
      fe.component_mask(velocity));
    GridRefinement::refine_and_coarsen_fixed_fraction(
      triangulation, estimated_error_per_cell, 0.6, 0.4);
    if (triangulation.n_levels() > max_grid_level)
      {
        for (auto cell = triangulation.begin_active(max_grid_level);
             cell != triangulation.end();
             ++cell)
          {
            cell->clear_refine_flag();
          }
      }

    for (auto cell = triangulation.begin_active(min_grid_level);
         cell != triangulation.end_active(min_grid_level);
         ++cell)
      {
        cell->clear_coarsen_flag();
      }

    BlockVector<double> buffer(present_solution);
    SolutionTransfer<dim, BlockVector<double>> solution_transfer(dof_handler);

    triangulation.prepare_coarsening_and_refinement();
    solution_transfer.prepare_for_coarsening_and_refinement(buffer);

    triangulation.execute_coarsening_and_refinement();

    setup_dofs();
    make_constraints();
    initialize_system();

    solution_transfer.interpolate(buffer, present_solution);
    nonzero_constraints.distribute(present_solution);
  }

  template <int dim>
  void FluidSolver<dim>::output_results(const unsigned int output_index) const
  {
    TimerOutput::Scope timer_section(timer, "Output results");

    std::cout << "Writing results..." << std::endl;
    std::vector<std::string> solution_names(dim, "velocity");
    solution_names.push_back("pressure");

    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      data_component_interpretation(
        dim, DataComponentInterpretation::component_is_part_of_vector);
    data_component_interpretation.push_back(
      DataComponentInterpretation::component_is_scalar);
    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(present_solution,
                             solution_names,
                             DataOut<dim>::type_dof_data,
                             data_component_interpretation);

    // Indicator
    Vector<float> ind(triangulation.n_active_cells());
    int i = 0;
    for (auto cell = triangulation.begin_active(); cell != triangulation.end();
         ++cell)
      {
        auto p = cell_property.get_data(cell);
        ind[i++] = p[0]->indicator;
      }
    data_out.add_data_vector(ind, "Indicator");

    // stress
    data_out.add_data_vector(scalar_dof_handler, stress[0][0], "Sxx");
    data_out.add_data_vector(scalar_dof_handler, stress[0][1], "Sxy");
    data_out.add_data_vector(scalar_dof_handler, stress[1][1], "Syy");
    if (dim == 3)
      {
        data_out.add_data_vector(scalar_dof_handler, stress[0][2], "Sxz");
        data_out.add_data_vector(scalar_dof_handler, stress[1][2], "Syz");
        data_out.add_data_vector(scalar_dof_handler, stress[2][2], "Szz");
      }

    data_out.build_patches(parameters.fluid_pressure_degree);

    std::string basename = "fluid";
    std::string filename =
      basename + "-" + Utilities::int_to_string(output_index, 6) + ".vtu";

    std::ofstream output(filename);
    data_out.write_vtu(output);

    static std::vector<std::pair<double, std::string>> times_and_names;
    times_and_names.push_back({time.current(), filename});
    std::ofstream pvd_output(basename + ".pvd");
    DataOutBase::write_pvd_record(pvd_output, times_and_names);
  }

  template <int dim>
  void FluidSolver<dim>::update_stress()
  {
    for (unsigned int i = 0; i < dim; ++i)
      {
        for (unsigned int j = 0; j < dim; ++j)
          {
            stress[i][j] = 0.0;
          }
      }
    std::vector<int> surrounding_cells(scalar_dof_handler.n_dofs(), 0);
    // The stress tensors are stored as 2D vectors of shape dim*dim
    // at cell and quadrature point level.
    std::vector<std::vector<Vector<double>>> cell_stress(
      dim,
      std::vector<Vector<double>>(dim,
                                  Vector<double>(scalar_fe.dofs_per_cell)));
    std::vector<std::vector<Vector<double>>> quad_stress(
      dim,
      std::vector<Vector<double>>(dim,
                                  Vector<double>(volume_quad_formula.size())));

    // The projection matrix from quadrature points to the dofs.
    FullMatrix<double> qpt_to_dof(scalar_fe.dofs_per_cell,
                                  volume_quad_formula.size());
    FETools::compute_projection_from_quadrature_points_matrix(
      scalar_fe, volume_quad_formula, volume_quad_formula, qpt_to_dof);

    FEValues<dim> fe_values(fe,
                            volume_quad_formula,
                            update_values | update_gradients |
                              update_quadrature_points | update_JxW_values);
    const unsigned int n_q_points = volume_quad_formula.size();
    const FEValuesExtractors::Vector velocities(0);
    const FEValuesExtractors::Scalar pressure(dim);
    std::vector<SymmetricTensor<2, dim>> sym_grad_v(n_q_points);
    std::vector<double> p(n_q_points);

    auto cell = dof_handler.begin_active();
    auto scalar_cell = scalar_dof_handler.begin_active();
    std::vector<types::global_dof_index> dof_indices(scalar_fe.dofs_per_cell);
    for (; cell != dof_handler.end(); ++cell, ++scalar_cell)
      {
        scalar_cell->get_dof_indices(dof_indices);
        fe_values.reinit(cell);

        // Fluid symmetric velocity gradient
        fe_values[velocities].get_function_symmetric_gradients(present_solution,
                                                               sym_grad_v);
        // Fluid pressure
        fe_values[pressure].get_function_values(present_solution, p);

        // Loop over all quadrature points to set FSI forces.
        for (unsigned int q = 0; q < volume_quad_formula.size(); ++q)
          {
            SymmetricTensor<2, dim> sigma =
              -p[q] * Physics::Elasticity::StandardTensors<dim>::I +
              2 * parameters.viscosity * sym_grad_v[q];
            for (unsigned int i = 0; i < dim; ++i)
              {
                for (unsigned int j = 0; j < dim; ++j)
                  {
                    quad_stress[i][j][q] = sigma[i][j];
                  }
              }
          }

        for (unsigned int i = 0; i < dim; ++i)
          {
            for (unsigned int j = 0; j < dim; ++j)
              {
                qpt_to_dof.vmult(cell_stress[i][j], quad_stress[i][j]);
                for (unsigned int k = 0; k < scalar_fe.dofs_per_cell; ++k)
                  {
                    stress[i][j][dof_indices[k]] += cell_stress[i][j][k];
                    if (i == 0 && j == 0)
                      surrounding_cells[dof_indices[k]]++;
                  }
              }
          }
      }

    for (unsigned int i = 0; i < dim; ++i)
      {
        for (unsigned int j = 0; j < dim; ++j)
          {
            for (unsigned int k = 0; k < scalar_dof_handler.n_dofs(); ++k)
              {
                stress[i][j][k] /= surrounding_cells[k];
              }
          }
      }
  }

  template class FluidSolver<2>;
  template class FluidSolver<3>;
} // namespace Fluid


/*
Copyright 2017. All rights reserved.
Computer Vision Group, Visual Computing Institute
RWTH Aachen University, Germany

This file is part of the rwth_mot framework.
Authors: Aljosa Osep (osep -at- vision.rwth-aachen.de)

rwth_mot framework is free software; you can redistribute it and/or modify it under the
terms of the GNU General Public License as published by the Free Software
Foundation; either version 3 of the License, or any later version.

rwth_mot framework is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with
rwth_mot framework; if not, write to the Free Software Foundation, Inc., 51 Franklin
Street, Fifth Floor, Boston, MA 02110-1301, USA
*/

#include <tracking/kalman_filter.h>

// eigen
#include <Eigen/LU>

// std
#include <iostream>
#include <fstream>

namespace GOT {
    namespace tracking {

        // -------------------------------------------------------------------------------
        // +++ Implementation: Kalman Filter Base +++
        // -------------------------------------------------------------------------------

        KalmanFilter::KalmanFilter(const Parameters &params) : state_vector_dim_(0), params_(params)  {

        }

        void KalmanFilter::ComputePrediction(const Eigen::VectorXd &u, Eigen::VectorXd &x_prio,
                                             Eigen::MatrixXd &P_prio) const {
            x_prio = A_*x_ + u; // Linear

            // Project covariance ahead
            P_prio = A_*P_*A_.transpose() + G_ ;
        }

        void KalmanFilter::Prediction() {
            assert(this->state_vector_dim_==A_.rows());
            assert(this->state_vector_dim_==A_.cols());
            assert(this->state_vector_dim_==G_.rows());
            assert(this->state_vector_dim_==G_.cols());

            Eigen::VectorXd u_zero, x_prio;
            Eigen::MatrixXd P_prio;
            u_zero.setZero(x_.size());
            this->ComputePrediction(u_zero, x_prio, P_prio);

            if (x_prio.size() != this->params_.state_vector_dim_) {
                return;
            }

            x_ = x_prio;
            P_ = P_prio;

            // Optionally, store whole history. Eg. for visualization.
            if (params_.store_history_) {
                this->state_predictions_.push_back(x_);
                this->cov_predictions_.push_back(P_);
            }
        }

        void KalmanFilter::Prediction(const Eigen::VectorXd &u) {
            assert(this->state_vector_dim_==A_.rows());
            assert(this->state_vector_dim_==A_.cols());
            assert(this->state_vector_dim_==G_.rows());
            assert(this->state_vector_dim_==G_.cols());

            Eigen::VectorXd x_prio;
            Eigen::MatrixXd P_prio;
            this->ComputePrediction(u, x_prio, P_prio);

            if (x_prio.size() != this->params_.state_vector_dim_) {
                return;
            }

            x_ = x_prio;
            P_ = P_prio;

            // Optionally, store whole history. Eg. for visualization.
            if (params_.store_history_) {
                this->state_predictions_.push_back(x_);
                this->cov_predictions_.push_back(P_);
            }
        }

        /// H is the observation model matrix (residual = z_t - H*x)
        void KalmanFilter::Correction(const Eigen::VectorXd z_t, const Eigen::MatrixXd &observation_cov_t, const Eigen::MatrixXd &H) {
            Eigen::MatrixXd H_transposed = H.transpose();

            // Compute Kalman gain
            Eigen::MatrixXd HPH_plus_obs_cov= (H*P_*H_transposed)+observation_cov_t;
            Eigen::MatrixXd K = P_*H_transposed*HPH_plus_obs_cov.inverse();

            // Update the estimate
            Eigen::VectorXd measurement_residual = z_t - H*x_;

            x_ = x_ + K*measurement_residual; //(z_t - H_*x_);

            // Update the covariance
            Eigen::MatrixXd  KH = K*H;
            P_ = (Eigen::MatrixXd::Identity(KH.rows(), KH.cols()) - KH)*P_;

            // Optionally, store whole history. Eg. for visualization.
            if (params_.store_history_) {
                this->state_corrections_.push_back(x_);
                this->cov_corrections_.push_back(P_);
                this->measurements_.push_back(z_t);
                this->cov_measurements_.push_back(observation_cov_t);
                this->kalman_gain_.push_back(K);
            }
        }

        /// Format: /path/to/dir/name_%s.txt
        /// Note: state dim. is not stored. At the client side, it is assumed to be simply 'known'.
        void KalmanFilter::SaveHistoryToFile(const char *filename) const {
            if (!this->params_.store_history_) {
                std::cout << "KalmanFilter::Error: Attempting to save Kalman history, but the params.save_history flag is off! Abort!" << std::endl;
                return;
            }

            /// States
            char buff[500];
            snprintf(buff, 500, filename, "state_predictions");
            std::ofstream state_pred_stream(buff);

            snprintf(buff, 500, filename, "state_corrections");
            std::ofstream state_corr_stream(buff);

            snprintf(buff, 500, filename, "state_measurements");
            std::ofstream state_meas_stream(buff);

            for (const auto &vec : state_predictions_)
                state_pred_stream << vec.transpose() << std::endl;

            for (const auto &vec : state_corrections_)
                state_corr_stream << vec.transpose() << std::endl;

            for (const auto &vec : measurements_)
                state_meas_stream << vec.transpose() << std::endl;

            //! Covariances
            snprintf(buff, 500, filename, "covariance_predictions");
            std::ofstream cov_pred_stream(buff);

            snprintf(buff, 500, filename, "covariance_corrections");
            std::ofstream cov_corr_stream(buff);

            snprintf(buff, 500, filename, "covariance_measurements");
            std::ofstream cov_meas_stream(buff);

            for (const auto &mat : cov_predictions_)
                cov_pred_stream << mat << std::endl;

            for (const auto &mat : cov_corrections_)
                cov_corr_stream << mat << std::endl;

            for (const auto &mat : cov_measurements_)
                cov_meas_stream << mat << std::endl;

            // Close file streams
            state_pred_stream.close();
            state_corr_stream.close();
            state_meas_stream.close();

            cov_pred_stream.close();
            cov_corr_stream.close();
            cov_meas_stream.close();
        }

        // Setters / Getters
        const Eigen::VectorXd& KalmanFilter::x() const {
            return x_;
        }

        void KalmanFilter::set_x(const Eigen::VectorXd &state) {
            this->x_ = state;
        }

        const Eigen::MatrixXd& KalmanFilter::P() const {
            return this->P_;
        }

        void KalmanFilter::set_G(const Eigen::MatrixXd& G) {
            this->G_ = G;
        }

        const Eigen::MatrixXd& KalmanFilter::G() const {
            return this->G_;
        }

        const std::vector<Eigen::VectorXd>& KalmanFilter::state_predictions() const {
            return this->state_predictions_;
        }

        const std::vector<Eigen::VectorXd>& KalmanFilter::state_corrections() const {
            return this->state_corrections_;
        }

        const std::vector<Eigen::MatrixXd>& KalmanFilter::cov_predictions() const {
            return this->cov_predictions_;
        }

        const std::vector<Eigen::MatrixXd>& KalmanFilter::cov_corrections() const {
            return this->cov_corrections_;
        }

        const std::vector<Eigen::VectorXd>& KalmanFilter::measurements() const {
            return this->measurements_;
        }

        const std::vector<Eigen::MatrixXd>& KalmanFilter::cov_measurements() const {
            return this->cov_measurements_;
        }

        const KalmanFilter::Parameters KalmanFilter::parameters() const {
            return params_;
        }

        void KalmanFilter::set_A(const Eigen::MatrixXd &A) {
            this->A_ = A;
        }

        const Eigen::MatrixXd &KalmanFilter::A() const {
            return this->A_;
        }

        const std::vector<Eigen::MatrixXd> &KalmanFilter::kalman_gains() const {
            return this->kalman_gain_;
        }

        // -------------------------------------------------------------------------------
        // +++ Implementation: Little example: simple Const-Velocity model +++
        // -------------------------------------------------------------------------------

        ConstantVelocityKalmanFilter::ConstantVelocityKalmanFilter(const Parameters &params) : params_(params), KalmanFilter(
                static_cast<KalmanFilter::Parameters>(params)) {
        }

        void ConstantVelocityKalmanFilter::Init(const Eigen::VectorXd &x_0, const Eigen::MatrixXd &P_0) {
            // Set params
            this->state_vector_dim_ = params_.state_vector_dim_;
            const double dt = params_.dt; // Assume delta_t parameter is given

            assert(this->state_vector_dim_==x_0.size());
            assert(this->state_vector_dim_==P_0.rows());
            assert(this->state_vector_dim_==P_0.cols());

            // Set initial state
            this->x_ = x_0;
            this->P_ = P_0;

            // Set up transition matrix A (simply adds change in velocity to pos.)
            A_.setIdentity(state_vector_dim_, state_vector_dim_);
            A_(0,2) = dt;
            A_(1,3) = dt;

            G_ = Eigen::Matrix4d::Identity() *0.1*0.1; // Default process noise.
        }
    }
}


#ifndef STAN_OPTIMIZATION_LBFGS_UPDATE_HPP
#define STAN_OPTIMIZATION_LBFGS_UPDATE_HPP

#include <stan/math/prim/fun/Eigen.hpp>
#include <boost/circular_buffer.hpp>
#include <tuple>
#include <vector>

namespace stan {
namespace optimization {
/**
 * Implement a limited memory version of the BFGS update.  This
 * class maintains a circular buffer of inverse Hessian updates
 * which can be applied to compute the search direction.
 **/
template <typename Scalar = double, int DimAtCompile = Eigen::Dynamic>
class LBFGSUpdate {
 public:
  typedef Eigen::Matrix<Scalar, DimAtCompile, 1> VectorT;
  typedef Eigen::Matrix<Scalar, DimAtCompile, DimAtCompile> HessianT;
  // NOLINTNEXTLINE(build/include_what_you_use)
  typedef std::tuple<Scalar, VectorT, VectorT> UpdateT;

  explicit LBFGSUpdate(size_t L = 5) : _buf(L) {}

  /**
   * Set the number of inverse Hessian updates to keep.
   *
   * @param L New size of buffer.
   **/
  void set_history_size(size_t L) { _buf.rset_capacity(L); }

  /**
   * Add a new set of update vectors to the history.
   *
   * @param yk Difference between the current and previous gradient vector.
   * @param sk Difference between the current and previous state vector.
   * @param reset Whether to reset the approximation, forgetting about
   * previous values.
   * @return In the case of a reset, returns the optimal scaling of the
   * initial Hessian
   * approximation which is useful for predicting step-sizes.
   **/
  inline Scalar update(const VectorT &yk, const VectorT &sk,
                       bool reset = false) {
    Scalar skyk = yk.dot(sk);

    Scalar B0fact;
    if (reset) {
      B0fact = yk.squaredNorm() / skyk;
      _buf.clear();
    } else {
      B0fact = 1.0;
    }

    // New updates are pushed to the "back" of the circular buffer
    Scalar invskyk = 1.0 / skyk;
    _gammak = skyk / yk.squaredNorm();
    _buf.push_back();
    _buf.back() = std::tie(invskyk, yk, sk);

    return B0fact;
  }

  /**
   * Compute the search direction based on the current (inverse) Hessian
   * approximation and given gradient.
   *
   * @param[out] pk The negative product of the inverse Hessian and gradient
   * direction gk.
   * @param[in] gk Gradient direction.
   **/
  inline void search_direction(VectorT &pk, const VectorT &gk) const {
    std::vector<Scalar> alphas(_buf.size());
    typename boost::circular_buffer<UpdateT>::const_reverse_iterator buf_rit;
    typename boost::circular_buffer<UpdateT>::const_iterator buf_it;
    typename std::vector<Scalar>::const_iterator alpha_it;
    typename std::vector<Scalar>::reverse_iterator alpha_rit;

    pk.noalias() = -gk;
    for (buf_rit = _buf.rbegin(), alpha_rit = alphas.rbegin();
         buf_rit != _buf.rend(); buf_rit++, alpha_rit++) {
      Scalar alpha;
      const Scalar &rhoi(std::get<0>(*buf_rit));
      const VectorT &yi(std::get<1>(*buf_rit));
      const VectorT &si(std::get<2>(*buf_rit));

      alpha = rhoi * si.dot(pk);
      pk -= alpha * yi;
      *alpha_rit = alpha;
    }
    pk *= _gammak;
    for (buf_it = _buf.begin(), alpha_it = alphas.begin(); buf_it != _buf.end();
         buf_it++, alpha_it++) {
      Scalar beta;
      const Scalar &rhoi(std::get<0>(*buf_it));
      const VectorT &yi(std::get<1>(*buf_it));
      const VectorT &si(std::get<2>(*buf_it));

      beta = rhoi * yi.dot(pk);
      pk += (*alpha_it - beta) * si;
    }
  }

 protected:
  boost::circular_buffer<UpdateT> _buf;
  Scalar _gammak;
};
}  // namespace optimization
}  // namespace stan

#endif