#ifndef STAN_MATH_TORSTEN_EVENT_HISTORY_HPP
#define STAN_MATH_TORSTEN_EVENT_HISTORY_HPP

#include <iomanip>
#include <stan/math/torsten/return_type.hpp>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <stan/math/torsten/PKModel/functions.hpp>
#include <stan/math/torsten/pk_nsys.hpp>
#include <Eigen/Dense>
#include <iostream>
#include <algorithm>
#include <vector>

namespace torsten {

  /**
   * The EventHistory class defines objects that contain a vector of Events,
   * along with a series of functions that operate on them.
   */
  template<typename T0, typename T1, typename T2, typename T3, typename T4_container, typename T5, typename T6>
  struct EventHistory {
    using T4 = typename stan::math::value_type<T4_container>::type;
    using T_scalar = typename torsten::return_t<T0, T1, T2, T3, T4, T5, T6>::type;
    using T_time = typename torsten::return_t<T0, T1, T3, T6, T2>::type;
    using T_rate = typename torsten::return_t<T2, T5>::type;
    using T_amt = typename torsten::return_t<T1, T5>::type;
    using Param = std::pair<double, std::array<int, 3> >;
    using rate_t = std::pair<double, std::vector<T2> >;

    const std::vector<T0>& time_;
    const std::vector<T1>& amt_;
    const std::vector<T2>& rate_;
    const std::vector<T3>& ii_;
    const std::vector<int>& evid_;
    const std::vector<int>& cmt_;
    const std::vector<int>& addl_;
    const std::vector<int>& ss_;

    std::vector<Param> param_index;
    const std::vector<T4_container>& theta_;
    const std::vector<std::vector<T5> >& biovar_;
    const std::vector<std::vector<T6> >& tlag_;

    // internally generated events
    const size_t num_event_times;
    std::vector<T_time> gen_time;
    std::vector<T1> gen_amt;
    std::vector<T2> gen_rate;
    std::vector<T3> gen_ii;
    std::vector<int> gen_cmt;
    std::vector<int> gen_addl;
    std::vector<int> gen_ss;

    using IDVec = std::array<int, 7>;
    // 0: original(0)/generated(1)
    // 1: index in original/generated arrays
    // 2: evid
    // 3: is new?(0/1)
    // 4: theta id
    // 5: biovar id
    // 6: tlag id
    std::vector<IDVec> index;

    // rate at distinct time
    std::vector<rate_t> rates;
    std::vector<int> rate_index;

    inline bool keep(const IDVec& id)  const { return id[0] == 0; }
    inline bool isnew(const IDVec& id) const { return id[3] == 1; }
    inline int evid (const IDVec& id) const { return id[2] ; }

    inline bool keep(int i)  const { return keep(index[i]); }
    inline bool isnew(int i) const { return isnew(index[i]); }
    inline int evid (int i) const { return evid(index[i]); }

    /*
     * for a population with data in ragged array form, we
     * form the events history using the population data and
     * the location of the individual in the ragged arrays.
     * In this constructor we assume @c p_ii.size() > 1 and
     * @c p_ss.size() > 1.
     */
    EventHistory(int ncmt, int ibegin, int isize,
                 const std::vector<T0>& p_time, const std::vector<T1>& p_amt,
                 const std::vector<T2>& p_rate, const std::vector<T3>& p_ii,
                 const std::vector<int>& p_evid, const std::vector<int>& p_cmt,
                 const std::vector<int>& p_addl, const std::vector<int>& p_ss,
                 int ibegin_theta, int isize_theta,
                 const std::vector<T4_container>& theta,
                 int ibegin_biovar, int isize_biovar,
                 const std::vector<std::vector<T5> >& biovar,
                 int ibegin_tlag, int isize_tlag,
                 const std::vector<std::vector<T6> >& tlag) :
      time_(p_time),
      amt_(p_amt),
      rate_(p_rate),
      ii_(p_ii),
      evid_(p_evid),
      cmt_(p_cmt),
      addl_(p_addl),
      ss_(p_ss),
      param_index(isize),
      theta_(theta),
      biovar_(biovar),
      tlag_(tlag),
      num_event_times(isize),
      index(isize, {0, 0, 0, 0, ibegin_theta, ibegin_biovar, ibegin_tlag})
    {
      const int iend = ibegin + isize;
      using stan::math::check_greater_or_equal;
      static const char* caller = "EventHistory::EventHistory";
      check_greater_or_equal(caller, "isize", isize , 1);
      check_greater_or_equal(caller, "time size", p_time.size() , size_t(iend));
      check_greater_or_equal(caller, "amt size", p_amt.size()   , size_t(iend));
      check_greater_or_equal(caller, "rate size", p_rate.size() , size_t(iend));
      check_greater_or_equal(caller, "ii size", p_ii.size()     , size_t(iend));
      check_greater_or_equal(caller, "evid size", p_evid.size() , size_t(iend));
      check_greater_or_equal(caller, "cmt size", p_cmt.size()   , size_t(iend));
      check_greater_or_equal(caller, "addl size", p_addl.size() , size_t(iend));
      check_greater_or_equal(caller, "ss size", p_ss.size()     , size_t(iend));
      for (size_t i = 0; i < isize; ++i) {
        index[i][1] = ibegin + i;
        index[i][2] = evid_[ibegin + i];
        index[i][4] = isize_theta   > 1 ? ibegin_theta  + i : ibegin_theta;
        index[i][5] = isize_biovar  > 1 ? ibegin_biovar + i : ibegin_biovar;
        index[i][6] = isize_tlag    > 1 ? ibegin_tlag   + i : ibegin_tlag;
      }
      insert_addl_dose();
      sort_state_time();

      for (int i = 0; i < isize; ++i) {
        int j = isize_theta   > 1 ? ibegin_theta  + i : ibegin_theta;
        int k = isize_biovar  > 1 ? ibegin_biovar + i : ibegin_biovar;
        int l = isize_tlag    > 1 ? ibegin_tlag   + i : ibegin_tlag;
        param_index[i] = std::make_pair<double, std::array<int, 3> >(stan::math::value_of(time_[ibegin + i]), {j, k, l });
      }
      param_sort();

      attach_event_parameters();
      insert_lag_dose();
      generate_rates(ncmt);
      attach_event_parameters();
    }

    EventHistory(int ncmt,
                 const std::vector<T0>& p_time, const std::vector<T1>& p_amt,
                 const std::vector<T2>& p_rate, const std::vector<T3>& p_ii,
                 const std::vector<int>& p_evid, const std::vector<int>& p_cmt,
                 const std::vector<int>& p_addl, const std::vector<int>& p_ss,
                 const std::vector<T4_container>& theta,
                 const std::vector<std::vector<T5> >& biovar,
                 const std::vector<std::vector<T6> >& tlag)
    : EventHistory(ncmt,
                   0, p_time.size(), p_time, p_amt, p_rate, p_ii, p_evid, p_cmt, p_addl, p_ss,
                   0, theta.size(), theta,
                   0, biovar.size(), biovar,
                   0, tlag.size(), tlag)
    {}

    void attach_event_parameters() {
      int nEvent = num_state_times();
      assert(nEvent > 0);
      int len_Parameters = param_index.size();  // numbers of events for which parameters are determined
      assert(len_Parameters > 0);

      if (!param_check()) param_sort();
      param_index.resize(nEvent);

      int iEvent = 0;
      for (int i = 0; i < len_Parameters - 1; i++) {
        while (isnew(iEvent)) iEvent++;  // skip new events
        assert(std::get<0>(param_index[i]) == time(iEvent));  // compare time of "old' events to time of parameters.
        iEvent++;
      }

      if (len_Parameters == 1)  {
        for (int i = 0; i < nEvent; i++) {
          param_index[i] = std::make_pair<double, std::array<int, 3> >(stan::math::value_of(time(i)) , std::array<int,3>(std::get<1>(param_index[0])));
          index[i][3] = 0;
        }
      } else {  // parameters are event dependent.
        std::vector<double> times(nEvent, 0);
        for (int i = 0; i < nEvent; i++) times[i] = param_index[i].first;
        iEvent = 0;

        Param newParameter;
        int j = 0;
        std::vector<Param>::const_iterator lower = param_index.begin();
        std::vector<Param>::const_iterator it_param_end = param_index.begin() + len_Parameters;
        for (int iEvent = 0; iEvent < nEvent; ++iEvent) {
          if (isnew(iEvent)) {
            // Find the index corresponding to the time of the new event in the
            // times vector.
            const double t = stan::math::value_of(time(iEvent));
            lower = std::lower_bound(lower, it_param_end, t,
                                     [](const Param& t1, const double& t2) {return t1.first < t2;});
            newParameter = lower == (it_param_end) ? param_index[len_Parameters-1] : *lower;
            newParameter.first = t;
            param_index[len_Parameters + j] = newParameter;
            index[iEvent][3] = 0;
            j++;
          }
        }
      }
      param_sort();
    }

    bool is_reset(int i) const {
      return evid(i) == 3 || evid(i) == 4;
    }

    bool is_reset_event(const IDVec& id) {
      return evid(id) == 3 || evid(id) == 4;
    }

    bool is_dosing(int i) const {
      return evid(i) == 1 || evid(i) == 4;
    }

    bool is_log_sum_exp_dosing(int i) const {
      return evid(i) == 11;
    }

    /*
     * if an event is steady-state dosing event.
     */
    bool is_ss_dosing(int i) const {
      return (is_dosing(i) && (ss(i) == 1 || ss(i) == 2)) || ss(i) == 3;
    }

    static bool is_dosing(const std::vector<int>& event_id, int i) {
      return event_id[i] == 1 || event_id[i] == 4;
    }

    bool is_bolus_dosing(int i) const {
      const double eps = 1.0E-12;
      return is_dosing(i) && rate(i) < eps;
    }

    /*
     * use current event #i as template to @c push_back to
     * another event.
     */
    void insert_event(int i) {
      index.push_back({1, int(gen_time.size()), index[i][2], 1, index[i][4], index[i][5], index[i][6]});
      gen_time. push_back(time (i));
      gen_amt.  push_back(amt  (i));
      gen_rate. push_back(rate (i));
      gen_ii.   push_back(ii   (i));
      gen_cmt.  push_back(cmt  (i));
      gen_addl. push_back(addl (i));
      gen_ss.   push_back(ss   (i));
    }

    /**
     * Add events to EventHistory object, corresponding to additional dosing,
     * administered at specified inter-dose interval. This information is stored
     * in the addl and ii members of the EventHistory object.
     *
     * Events is sorted at the end of the procedure.
     */
    void insert_addl_dose() {
      for (int i = 0; i < num_state_times(); i++) {
        if (is_dosing(i) && ((addl(i) > 0) && (ii(i) > 0))) {
          for (int j = 1; j <= addl(i); j++) {
            insert_event(i);
            gen_time.back() += j * ii(i);
            gen_ii.back() = 0;
            gen_addl.back() = 0;
            gen_ss.back() = 0;
          }
        }
      }
    }

    /*
     * sort PMX events and nonevents times
     */
    void sort_state_time() { std::stable_sort(index.begin(), index.end(),
                                              [this](const IDVec &a, const IDVec &b) {
                                                using stan::math::value_of;
                                                double ta = keep(a) ? value_of(time_[a[1]]) : value_of(gen_time[a[1]]);
                                                double tb = keep(b) ? value_of(time_[b[1]]) : value_of(gen_time[b[1]]);
                                                return ta < tb;
                                              });
    }

    /*
     * return if an event is a "reset" event(evid=3 or 4)
     */
    void param_sort() {
      std::sort(param_index.begin(), param_index.end(),
                [](const Param& a, const Param& b)
                { return a.first < b.first; });
    }

    bool param_check() {
      // check that elements are in chronological order.
      int i = param_index.size() - 1;
      bool ordered = true;

      while (i > 0 && ordered) {
        ordered = (param_index[i].first >= param_index[i-1].first);
        i--;
      }
      return ordered;
    }

    void generate_rates(int nCmt) {
      using std::vector;
      using stan::math::value_of;

      const int n = num_state_times();
      for (size_t i = 0; i < n; ++i) {
        if ((is_dosing(i)) && (rate(i) > 0 && amt(i) > 0)) {
          insert_event(i);
          index.back()[2] = 2;    // reset evid
          gen_time. back() += amt(i)/rate(i);
          gen_amt.  back() = 0;
          gen_rate. back() = 0;
          gen_ii.   back() = 0;
          gen_addl. back() = 0;
          gen_ss.   back() = 0;
        }
      }
      sort_state_time();

      rate_t newRate{0.0, std::vector<T2>(nCmt, 0.0)};
      // unique_times is sorted
      std::vector<int> ut(unique_times());
      for (auto i : ut) {
        newRate.first = value_of(time(i));
        rates.push_back(newRate);
      }
      // check sorted?
      std::sort(rates.begin(), rates.end(), [](rate_t const &a, rate_t const &b) {
          return a.first < b.first;
        });

      for (size_t i = 0; i < num_state_times(); ++i) {
        if ((is_dosing(i)) && (rate(i) > 0 && amt(i) > 0)) {
          double t0 = value_of(time(i));
          double t1 = t0 + value_of(amt(i)/rate(i));
          for (auto&& r : rates) {
            if (r.first > t0 && r.first <= t1) {
              r.second[cmt(i) - 1] += rate(i);
            }
          }
        }
      }

      /*
       * rate index points to the rates for each state time,
       * since there is one rates vector per time, not per event.
       */
      rate_index.resize(index.size());
      int iRate = 0;
      for (size_t i = 0; i < index.size(); ++i) {
        if (rates[iRate].first != time(i)) iRate++;
        rate_index[i] = iRate;
      }
    }

    // Access functions
    inline T_time time (const IDVec& id) const { return keep(id) ? time_[id[1]] : gen_time[id[1]] ; }
    inline const T1& amt      (const IDVec& id) const { return keep(id) ? amt_[id[1]] : gen_amt[id[1]] ; }
    inline const T2& rate     (const IDVec& id) const { return keep(id) ? rate_[id[1]] : gen_rate[id[1]] ; }
    inline const T3& ii       (const IDVec& id) const { return keep(id) ? ii_[id[1]] : gen_ii[id[1]] ; }
    inline int cmt     (const IDVec& id) const { return keep(id) ? cmt_[id[1]] : gen_cmt[id[1]] ; }
    inline int addl    (const IDVec& id) const { return keep(id) ? addl_[id[1]] : gen_addl[id[1]] ; }
    inline int ss      (const IDVec& id) const { return keep(id) ? ss_[id[1]] : gen_ss[id[1]] ; }

    inline T_time time (int i) const { return time(index[i]); }
    inline const T1& amt      (int i) const { return amt (index[i]); }
    inline const T2& rate     (int i) const { return rate(index[i]); }
    inline const T3& ii       (int i) const { return ii  (index[i]); }
    inline int cmt     (int i) const { return cmt (index[i]); }
    inline int addl    (int i) const { return addl(index[i]); }
    inline int ss      (int i) const { return ss  (index[i]); }

    inline size_t num_state_times() const { return index.size(); }

    inline std::vector<T_rate> fractioned_rates(int i) const {
      const int n = rates[0].second.size();
      const std::vector<T2>& r = rates[rate_index[i]].second;
      std::vector<T_rate> res(r.size());
      for (size_t j = 0; j < r.size(); ++j) {
        res[j] = r[j] * bioavailability(i, j);
      }
      return res;
    }

    inline T_amt fractioned_amt(int i) const {
      return bioavailability(i, cmt(i) - 1) * amt(i);
    }

    std::vector<int> unique_times() {
      std::vector<int> t(index.size());
      std::iota(t.begin(), t.end(), 0);
      auto last = std::unique(t.begin(), t.end(),
                              [this](const int& i, const int& j) {return time(i) == time(j);});
      t.erase(last, t.end());
      return t;
    }

    /**
     * Implement absorption lag times by modifying the times of the dosing events.
     * Two cases: parameters are either constant or vary with each event.
     * Function sorts events at the end of the procedure.
     *
     * @tparam T_parameters type of scalar model parameters
     * @return - modified events that account for absorption lag times
     */
    void insert_lag_dose() {
      // reverse loop so we don't process same lagged events twice
      int nEvent = num_state_times();
      int iEvent = nEvent - 1;
      while (iEvent >= 0) {
        if (is_dosing(iEvent)) {
          if (GetValueTlag(iEvent, cmt(iEvent) - 1) != 0) {
            insert_event(iEvent);
            gen_time.back() += GetValueTlag(iEvent, cmt(iEvent) - 1);

            // Events[iEvent].evid = 2;  // Check
            index[iEvent][2] = 2;
            // The above statement changes events so that CleanEvents does
            // not return an object identical to the original. - CHECK
          }
        }
        iEvent--;
      }
      sort_state_time();
    }

    inline const T4_container& model_param(int i) const {
      return theta_[param_index[i].second[0]];
    }

    inline const T4& GetValue(int iEvent, int iParameter) const {
      return theta_[std::get<1>(param_index[iEvent])[0]][iParameter];
    }

    inline const T5& bioavailability(int iEvent, int iParameter) const {
      return biovar_[std::get<1>(param_index[iEvent])[1]][iParameter];
    }

    inline const T6& GetValueTlag(int iEvent, int iParameter) const {
      return tlag_[std::get<1>(param_index[iEvent])[2]][iParameter];
    }

    /*
     * Overloading the << Operator
     */
    friend std::ostream& operator<<(std::ostream& os, const EventHistory& ev) {
      const int w = 6;
      os << "\n";
      os << std::setw(w) << "time" <<
        std::setw(w) << "amt" <<
        std::setw(w) << "rate" <<
        std::setw(w) << "ii" <<
        std::setw(w) << "evid" <<
        std::setw(w) << "cmt" <<
        std::setw(w) << "addl" <<
        std::setw(w) << "ss" <<
        std::setw(w) << "keep" <<
        std::setw(w) << "isnew" << "\n";
      for (size_t i = 0; i < ev.size(); ++i) {
        os <<
          std::setw(w)   << ev.time(i) << " " <<
          std::setw(w-1) << ev.amt(i) << " " <<
          std::setw(w-1) << ev.rate(i) << " " <<
          std::setw(w-1) << ev.ii(i) << " " <<
          std::setw(w-1) << ev.evid(i) << " " <<
          std::setw(w-1) << ev.cmt(i) << " " <<
          std::setw(w-1) << ev.addl(i) << " " <<
          std::setw(w-1) << ev.ss(i) << " " <<
          std::setw(w-1) << ev.keep(i) << " " <<
          std::setw(w-1) << ev.isnew(i) << "\n";
      }
      return os;
    }
  };

}    // torsten namespace
#endif


#ifndef HPENFACAG_HPP_INCLUDED
#define HPENFACAG_HPP_INCLUDED

#include <vector>
#include <string>
#include <boost/serialization/list.hpp>
#include <boost/serialization/set.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/deque.hpp>
#include <caffe/util/math_functions.hpp>

#include "arch/AACAgent.hpp"
#include "bib/Seed.hpp"
#include "bib/Utils.hpp"
#include "bib/OrnsteinUhlenbeckNoise.hpp"
#include "bib/MetropolisHasting.hpp"
#include "bib/XMLEngine.hpp"
#include "bib/IniParser.hpp"
#include "bib/OnlineNormalizer.hpp"
#include "nn/MLP.hpp"

#ifdef PARALLEL_INTERACTION
#include <mpi.h>
#include <boost/mpi.hpp>
#endif

#ifndef SAASRG_SAMPLE
#define SAASRG_SAMPLE
typedef struct _sample {
  std::vector<double> s;
  std::vector<double> goal_achieved;
  std::vector<double> goal_achieved_unnormed;
  std::vector<double> pure_a;
  std::vector<double> a;
  std::vector<double> next_s;
  std::vector<double> next_goal_achieved_unnormed;
  double r;
  bool goal_reached;
  double prob;
  bool artificial;
  bool interest;

  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive& ar, const unsigned int) {
    ar& BOOST_SERIALIZATION_NVP(s);
    ar& BOOST_SERIALIZATION_NVP(goal_achieved);
    ar& BOOST_SERIALIZATION_NVP(goal_achieved_unnormed);
    ar& BOOST_SERIALIZATION_NVP(pure_a);
    ar& BOOST_SERIALIZATION_NVP(a);
    ar& BOOST_SERIALIZATION_NVP(next_s);
    ar& BOOST_SERIALIZATION_NVP(next_goal_achieved_unnormed);
    ar& BOOST_SERIALIZATION_NVP(r);
    ar& BOOST_SERIALIZATION_NVP(goal_reached);
    ar& BOOST_SERIALIZATION_NVP(prob);
    ar& BOOST_SERIALIZATION_NVP(artificial);
    ar& BOOST_SERIALIZATION_NVP(interest);
  }
} sample;
#endif

template<typename NN = MLP>
class OfflineCaclaAg : public arch::AACAgent<NN, arch::AgentGPUProgOptions> {
 public:
  typedef NN PolicyImpl;

  OfflineCaclaAg(unsigned int _nb_motors, unsigned int _nb_sensors, uint _goal_size, uint _goal_start)
      : arch::AACAgent<NN, arch::AgentGPUProgOptions>(_nb_motors, _nb_sensors-_goal_size), nb_sensors(_nb_sensors-_goal_size), empty_action(),
      last_state(_nb_sensors-_goal_size, 0.f), last_goal_achieved(_goal_size, 0.f), goal_size(_goal_size), goal_start(_goal_start-_goal_size),
      normalizer(nb_sensors) {

        LOG_DEBUG("goal size " << goal_size);
        LOG_DEBUG("goal start " << goal_start);
  }

  virtual ~OfflineCaclaAg() {
    if(vnn != nullptr)
      delete vnn;
    delete ann;
    delete ann_noblob;

    delete hidden_unit_v;
    delete hidden_unit_a;

    if(oun == nullptr)
      delete oun;
  }

  const std::vector<double>& _run(double reward, const std::vector<double>& sensors,
                                  const std::vector<double>& goal_achieved, bool learning, bool as, bool) override {
    std::vector<double> normed_sensors(nb_sensors);
    normalizer.transform_with_double_clip(normed_sensors, sensors, false);

    // protect batch norm from testing data and poor data
    vector<double>* next_action = ann_noblob->computeOut(normed_sensors);
    if (last_action.get() != nullptr && learning) {
      double prob = bib::Proba<double>::truncatedGaussianDensity(*last_action, *last_pure_action, noise);
      bool gr = reward >= -0.0000001;
      trajectory.push_back( {last_state, last_goal_achieved, last_goal_achieved, *last_pure_action, *last_action, sensors, goal_achieved, reward, gr, prob, false, true});
      if (gr)
        trajectory_end_points.push_back(trajectory.size());

//      auto sa = trajectory.back();
//      double nr = dense_reward(sa.goal_achieved_unnormed, sa.s,
//                                                    sa.next_goal_achieved_unnormed, sa.next_s,
//                                                    sa.s, sa.next_s);
//      LOG_DEBUG(reward << " " << nr);
    }

    last_pure_action.reset(new std::vector<double>(*next_action));
    if(learning) {
      if(gaussian_policy == 1) {
        vector<double>* randomized_action = bib::Proba<double>::multidimentionnalTruncatedGaussian(*next_action, noise);
        delete next_action;
        next_action = randomized_action;
      } else if(gaussian_policy == 2) {
        oun->step(*next_action);
      } else if(gaussian_policy == 3 && bib::Utils::rand01() < noise2) {
        vector<double>* randomized_action = bib::Proba<double>::multidimentionnalTruncatedGaussian(*next_action, noise);
        delete next_action;
        next_action = randomized_action;
      } else if(gaussian_policy == 4) {
        vector<double>* randomized_action = bib::Proba<double>::multidimentionnalTruncatedGaussian(*next_action, noise * pow(noise2, noise3 - ((double) step)));
        delete next_action;
        next_action = randomized_action;
      } else if(gaussian_policy == 5) {
        if (bib::Utils::rand01() < noise2){
          for (uint i = 0; i < next_action->size(); i++)
            next_action->at(i) = bib::Utils::randin(-1.f, 1.f);
        } else {
          vector<double>* randomized_action = bib::Proba<double>::multidimentionnalTruncatedGaussian(*next_action, noise);
          delete next_action;
          next_action = randomized_action;
        }
      } else if(bib::Utils::rand01() < noise) { //e-greedy
        for (uint i = 0; i < next_action->size(); i++)
          next_action->at(i) = bib::Utils::randin(-1.f, 1.f);
      }
    }
    last_action.reset(next_action);

    std::copy(sensors.begin(), sensors.end(), last_state.begin());
    std::copy(goal_achieved.begin(), goal_achieved.end(), last_goal_achieved.begin());
    step++;

    return *next_action;
  }

  void _unique_invoke(boost::property_tree::ptree* pt, boost::program_options::variables_map* command_args) override {
//     bib::Seed::setFixedSeedUTest();
    hidden_unit_v           = bib::to_array<uint>(pt->get<std::string>("agent.hidden_unit_v"));
    hidden_unit_a           = bib::to_array<uint>(pt->get<std::string>("agent.hidden_unit_a"));
    noise                   = pt->get<double>("agent.noise");
    gaussian_policy         = pt->get<uint>("agent.gaussian_policy");
    number_fitted_iteration = pt->get<uint>("agent.number_fitted_iteration");
    stoch_iter_actor        = pt->get<uint>("agent.stoch_iter_actor");
    stoch_iter_critic       = pt->get<uint>("agent.stoch_iter_critic");
    actor_output_layer_type = pt->get<uint>("agent.actor_output_layer_type");
    hidden_layer_type       = pt->get<uint>("agent.hidden_layer_type");
    alpha_a                 = pt->get<double>("agent.alpha_a");
    alpha_v                 = pt->get<double>("agent.alpha_v");
    lambda                  = pt->get<double>("agent.lambda");
    momentum                = pt->get<uint>("agent.momentum");
    beta_target             = pt->get<double>("agent.beta_target");
    ignore_poss_ac          = pt->get<bool>("agent.ignore_poss_ac");
    conserve_beta           = pt->get<bool>("agent.conserve_beta");
    disable_trust_region = pt->get<bool>("agent.disable_trust_region");
    disable_cac                 = pt->get<bool>("agent.disable_cac");
    hindsight_nb_destination = pt->get<uint>("agent.hindsight_nb_destination");
    gae                     = false;
    update_each_episode = 1;

    if(gaussian_policy == 2) {
      double oun_theta = pt->get<double>("agent.noise2");
      double oun_dt = pt->get<double>("agent.noise3");
      oun = new bib::OrnsteinUhlenbeckNoise<double>(this->nb_motors, noise, oun_theta, oun_dt);
    } else if (gaussian_policy == 3 || gaussian_policy == 5){
      noise2 = pt->get<double>("agent.noise2");
    } else if (gaussian_policy == 4){
      noise2 = pt->get<double>("agent.noise2");
      noise3 = pt->get<double>("agent.noise3");
    }

    try {
      update_each_episode     = pt->get<uint>("agent.update_each_episode");
#ifdef PARALLEL_INTERACTION
      if (update_each_episode % (world.size() - 1) != 0) {
        LOG_ERROR("update_each_episode must be a multiple of (number of worker - 1)");
        exit(1);
      }
      update_each_episode = update_each_episode / (world.size() - 1);
#endif
    } catch(boost::exception const& ) {
    }

    if(lambda >= 0.)
      gae = pt->get<bool>("agent.gae");

#ifdef CAFFE_CPU_ONLY
    LOG_INFO("CPU mode");
    (void) command_args;
#else

    if(command_args->count("gpu") == 0 || command_args->count("cpu") > 0
#ifdef PARALLEL_INTERACTION
      || world.rank() != 0
#endif
     )
    {
      caffe::Caffe::set_mode(caffe::Caffe::Brew::CPU);
      LOG_INFO("CPU mode");
    } else {
      caffe::Caffe::set_mode(caffe::Caffe::Brew::GPU);
      caffe::Caffe::SetDevice((*command_args)["gpu"].as<uint>());
      LOG_INFO("GPU mode");
    }
#endif

    LOG_INFO("dimensionality of NN " << nb_sensors << " (in) " << this->nb_motors << " (out).");
    ann = new NN(nb_sensors, *hidden_unit_a, this->nb_motors, alpha_a, 1, hidden_layer_type, actor_output_layer_type, 0, true, momentum);
    ann_noblob = new NN(*ann, false, ::caffe::Phase::TEST);

#ifdef PARALLEL_INTERACTION
    std::vector<double> weights(ann->number_of_parameters(false), 0.f);
    if (world.rank() == 0)
      ann->copyWeightsTo(weights.data(), false);

    broadcast(world, weights, 0);
    if (world.rank() != 0)
      ann->copyWeightsFrom(weights.data(), false);
#endif

#ifdef PARALLEL_INTERACTION
    if (world.rank() == 0)
      vnn = new NN(nb_sensors, nb_sensors, *hidden_unit_v, alpha_v, 1, -1, hidden_layer_type, 0, false, momentum);
#else
    vnn = new NN(nb_sensors, nb_sensors, *hidden_unit_v, alpha_v, 1, -1, hidden_layer_type, 0, false, momentum);
#endif
    bestever_score = std::numeric_limits<double>::lowest();
  }

  void _start_episode(const std::vector<double>& sensors, bool) override {
    std::copy(sensors.begin(), sensors.end(), last_state.begin());

    last_action = nullptr;
    last_pure_action = nullptr;

    step = 0;
    if(gaussian_policy == 2)
      oun->reset();

    double* weights = new double[ann->number_of_parameters(false)];
    ann->copyWeightsTo(weights, false);
    ann_noblob->copyWeightsFrom(weights, false);
    delete[] weights;
  }

  void update_critic(const caffe::Blob<double>& all_states, const caffe::Blob<double>& all_next_states,
    const caffe::Blob<double>& r_gamma_coef) {

    if (trajectory.size() > 0) {
      caffe::Blob<double> v_target(trajectory.size(), 1, 1, 1);

      //remove trace of old policy
      auto iter = [&]() {
        auto all_nextV = vnn->computeOutVFBlob(all_next_states, empty_action);
        auto all_V = vnn->computeOutVFBlob(all_states, empty_action);
        //all_V must be computed after all_nextV to use learn_blob_no_full_forward

//#ifdef CAFFE_CPU_ONLY
        caffe::caffe_mul(trajectory.size(), r_gamma_coef.cpu_diff(), all_nextV->cpu_data(), v_target.mutable_cpu_data());
        caffe::caffe_add(trajectory.size(), r_gamma_coef.cpu_data(), v_target.cpu_data(), v_target.mutable_cpu_data());

        double *pv_target = v_target.mutable_cpu_data();
        double min_ = - (1.f/(1.f-this->gamma));
        for(int i=0;i<trajectory.size();i++){
            if(pv_target[i] > 0.0)
                pv_target[i] = 0.f;
            else if (pv_target[i] < min_)
                pv_target[i] = min_;
        }

        caffe::caffe_sub(trajectory.size(), v_target.cpu_data(), all_V->cpu_data(), v_target.mutable_cpu_data());
//#else
//      switch (caffe::Caffe::mode()) {
//      case caffe::Caffe::CPU:
//        caffe::caffe_mul(trajectory.size(), r_gamma_coef.cpu_diff(), all_nextV->cpu_data(), v_target.mutable_cpu_data());
//        caffe::caffe_add(trajectory.size(), r_gamma_coef.cpu_data(), v_target.cpu_data(), v_target.mutable_cpu_data());
//        caffe::caffe_sub(trajectory.size(), v_target.cpu_data(), all_V->cpu_data(), v_target.mutable_cpu_data());
//        break;
//      case caffe::Caffe::GPU:
//        caffe::caffe_gpu_mul(trajectory.size(), r_gamma_coef.gpu_diff(), all_nextV->gpu_data(), v_target.mutable_gpu_data());
//        caffe::caffe_gpu_add(trajectory.size(), r_gamma_coef.gpu_data(), v_target.gpu_data(), v_target.mutable_gpu_data());
//        caffe::caffe_gpu_sub(trajectory.size(), v_target.gpu_data(), all_V->gpu_data(), v_target.mutable_gpu_data());
//        break;
//      }
//#endif

//     Simple computation for lambda return
//    move v_target from GPU to CPU
        double* pdiff = v_target.mutable_cpu_diff();
        const double* pvtarget = v_target.cpu_data();
        int li=trajectory.size() - 1;
        double prev_delta = 0.;
        int index_ep = trajectory_end_points.size() - 1;
        for (auto it = trajectory.rbegin(); it != trajectory.rend(); it++) {
          if (index_ep >= 0 && trajectory_end_points[index_ep] - 1 == li){
              prev_delta = 0.;
              index_ep--;
          }
//          if(pvtarget[li] + all_V->cpu_data()[li] > 0.0001f || pvtarget[li] + all_V->cpu_data()[li] < -50.0001f)
//            LOG_DEBUG("MIGHT BE PROBLEMATIC " << (pvtarget[li]+all_V->cpu_data()[li]) << " " << r_gamma_coef.cpu_diff()[li] << " " << all_nextV->cpu_data()[li] << " " << r_gamma_coef.cpu_data()[li]);

          if (it->artificial) {
            pdiff[li] = pvtarget[li] * std::min(it->prob, pbar) + prev_delta * std::min(it->prob, cbar);
            prev_delta = this->gamma * lambda * pdiff[li];
          } else {
            pdiff[li] = pvtarget[li] + prev_delta;
            prev_delta = this->gamma * lambda * pdiff[li];
          }
          --li;
        }
//        ASSERT(pdiff[trajectory.size() -1] == pvtarget[trajectory.size() -1] * std::min(trajectory[trajectory.size() - 1].prob, pbar), "pb lambda");

//         move diff to GPU
#ifdef CAFFE_CPU_ONLY
        caffe::caffe_add(trajectory.size(), v_target.cpu_diff(), all_V->cpu_data(), v_target.mutable_cpu_data());
#else
      switch (caffe::Caffe::mode()) {
      case caffe::Caffe::CPU:
        caffe::caffe_add(trajectory.size(), v_target.cpu_diff(), all_V->cpu_data(), v_target.mutable_cpu_data());
        break;
      case caffe::Caffe::GPU:
        caffe::caffe_gpu_add(trajectory.size(), v_target.gpu_diff(), all_V->gpu_data(), v_target.mutable_gpu_data());
        break;
      }
#endif
        if (stoch_iter_critic == 1)
          vnn->learn_blob_no_full_forward(all_states, empty_action, v_target);
        else
          vnn->learn_blob(all_states, empty_action, v_target, stoch_iter_critic);

        delete all_V;
        delete all_nextV;
      };

      for(uint i=0; i<number_fitted_iteration; i++)
        iter();
    }
  }

  void end_episode(bool learning) override {
//     LOG_FILE("policy_exploration", ann->hash());
    if(!learning){
      return;
    }

    //learning phase
    if (trajectory_end_points.size() == 0 || trajectory_end_points.back() != trajectory.size())
      trajectory_end_points.push_back(trajectory.size());
    if (episode % update_each_episode != 0)
      return;

//
// Remove junk data
//
    std::deque<double> varsums(trajectory_end_points.size(), 0.f);
    for (int traj = trajectory_end_points.size() - 1 ; traj >= 0 ; traj--) {
      int beg = traj == 0 ? 0 : trajectory_end_points[traj-1];
      int end = trajectory_end_points[traj];
      if (end - beg > 1) {
        for (int goal_dim=0; goal_dim < goal_size; goal_dim++) {
          std::function<double(const sample&)> get = [goal_dim](const sample&  s) {
            return s.goal_achieved_unnormed[goal_dim];
          };
          varsums[traj] += bib::Utils::variance<>(trajectory.cbegin() + beg, trajectory.cbegin() + end, get);
        }
      }

      //goal_achieved hasn't change at all during the trajectory
      if (varsums[traj] <= 1e-8) {
        // tag already achieved task where actor won't be update
        if (trajectory[beg].r >= -0.0001) {
          for (auto it = trajectory.begin() + beg; it != trajectory.begin() + end; it++)
              it->interest=false;
		}
      }
    }

#ifdef PARALLEL_INTERACTION
    if (world.rank() == 0) {
      trajectory_end_points.clear();
      varsums.clear();

      std::vector<std::deque<sample>> all_traj;
      std::vector<std::deque<int>> all_traj_ep;
      std::vector<std::deque<double>> all_varsums;
      gather(world, trajectory, all_traj, 0);
      gather(world, trajectory_end_points, all_traj_ep, 0);
      gather(world, varsums, all_varsums, 0);

      ASSERT(all_traj.size() == all_traj_ep.size(), "pb");
      for (auto it : all_varsums)
        for (auto it2 : it)
            varsums.push_back(it2);
      for (int i=0; i < all_traj.size() ; i++) {
        for (auto d2 : all_traj_ep[i])
          trajectory_end_points.push_back(trajectory.size() + d2);

        trajectory.insert(trajectory.end(), all_traj[i].begin(), all_traj[i].end());
      }
    } else {
      gather(world, trajectory, 0);
      gather(world, trajectory_end_points, 0);
      gather(world, varsums, 0);
    }

    if (world.rank() == 0) {
#endif

//     LOG_DEBUG("#############");
//     for (int i=0;i<trajectory.size(); i++) {
//       bib::Logger::PRINT_ELEMENTS(trajectory[i].goal_achieved);
//       LOG_DEBUG(trajectory[i].r << " " <<i);
//     }
//     bib::Logger::PRINT_ELEMENTS(trajectory_end_points);
//     LOG_DEBUG("#############");
//     LOG_DEBUG("#############");

//
//    update norm on batch
//
    for (int i=0;i<trajectory.size(); i++) {
      normalizer.update_batch_clip_before(trajectory[i].s, goal_size);//ignore fixed goal in update
      normalizer.update_batch_clip_before(trajectory[i].goal_achieved);
    }

#ifdef PARALLEL_INTERACTION
    }

//     synchronize normalizer
    bib::OnlineNormalizer on(this->nb_sensors);
    if (world.rank() == 0)
      on.copyFrom(normalizer);

    broadcast(world, on, 0);

    if (world.rank() != 0)
      normalizer.copyFrom(on);

    if (world.rank() == 0) {
#endif

    if (trajectory.size() == 0) {
      LOG_INFO("no data left");
      nb_sample_update = 0;
      ASSERT(trajectory_end_points.size() == 0, "");
      return;
    }

//     LOG_DEBUG("#############");
//     for (int i=0;i<trajectory.size(); i++) {
//       bib::Logger::PRINT_ELEMENTS(trajectory[i].goal_achieved);
//       LOG_DEBUG(trajectory[i].r << " " <<i);
// //       if(trajectory[i].r >= 0 ){
// //         bib::Logger::PRINT_ELEMENTS(trajectory[i].goal_achieved, ("HERE "+std::to_string(i)+" ").c_str());
// //       }
//     }
// //     bib::Logger::PRINT_ELEMENTS(trajectory_end_points);
// //     exit(1);

//
//  perform norm on batch
//
     for (int i=0;i<trajectory.size(); i++) {
       std::vector<double> normed_sensors(nb_sensors);
       std::vector<double> normed_goal_size(goal_size);
       std::vector<double> normed_next_s(nb_sensors);
       normalizer.transform_with_double_clip(normed_sensors, trajectory[i].s, false);
       normalizer.transform_with_double_clip(normed_goal_size, trajectory[i].goal_achieved, false);
       normalizer.transform_with_double_clip(normed_next_s, trajectory[i].next_s, false);

       std::copy(normed_sensors.begin(), normed_sensors.end(), trajectory[i].s.begin());
       std::copy(normed_goal_size.begin(), normed_goal_size.end(), trajectory[i].goal_achieved.begin());
       std::copy(normed_next_s.begin(), normed_next_s.end(), trajectory[i].next_s.begin());
     }

//     LOG_DEBUG("#############");
//     for (int i=0;i<trajectory.size(); i++) {
//       bib::Logger::PRINT_ELEMENTS(trajectory[i].s);
//       bib::Logger::PRINT_ELEMENTS(trajectory[i].goal_achieved);
//       LOG_DEBUG(trajectory[i].r << " " <<i);
//     }
//     exit(1);

//
// data augmentation part
//
   int saved_trajsize=trajectory.size();
   int saved_trajend_point=trajectory_end_points.size();

   for(int i=0;i < saved_trajend_point; i++) {
//      don't generate trajectory where goal achieved hasn't changed
     if (varsums[i] <= 1e-8)
       continue;

      int min_index=0;
      if(i>0)
        min_index=trajectory_end_points[i-1];

	  if(trajectory_end_points[i]-1 == min_index)
		continue;

      for(int j=0;j<hindsight_nb_destination;j++) {
          uint destination = bib::Seed::unifRandInt(min_index, trajectory_end_points[i]-1);

          for(int k=min_index;k<=destination;k++) {
            sample sa = trajectory[k];
            trajectory.push_back(sa);
            trajectory.back().artificial = true;
            std::copy(trajectory[destination].goal_achieved.begin(),
                  trajectory[destination].goal_achieved.end(),
                  trajectory.back().s.begin() + goal_start);
            std::copy(trajectory[destination].goal_achieved.begin(),
                  trajectory[destination].goal_achieved.end(),
                  trajectory.back().next_s.begin() + goal_start);

 //         sparse reward
            if ( sparse_reward(sa.goal_achieved_unnormed, trajectory[destination].goal_achieved_unnormed)) {
              trajectory.back().r = 0.f;
              trajectory.back().goal_reached = true;
              trajectory_end_points.push_back(trajectory.size());
            }
 //         --

//          dense reward
//          trajectory.back().r = dense_reward(sa.goal_achieved_unnormed, trajectory[destination].goal_achieved_unnormed,
//                                                  sa.next_goal_achieved_unnormed, trajectory[destination].next_goal_achieved_unnormed,
//                                                  sa.s, sa.next_s);
//          if ( trajectory.back().r >= -0.0000001 ) {
//              trajectory_end_points.push_back(trajectory.size());
//              trajectory.back().goal_reached = true;
//          }
//        --

            //should remove junk data after data
          }
     	  if (trajectory_end_points.back() != trajectory.size())
          	trajectory_end_points.push_back(trajectory.size());
      }
   }
//
// tag artificial junk data
//
    for (int traj = trajectory_end_points.size() - 1 ; traj >= saved_trajend_point ; traj--) {
      int beg = traj == 0 ? 0 : trajectory_end_points[traj-1];
      int end = trajectory_end_points[traj];
	  double varsum = 0.f;
      if (end - beg > 1) {
        for (int goal_dim=0; goal_dim < goal_size; goal_dim++) {
          std::function<double(const sample&)> get = [goal_dim](const sample&  s) {
            return s.goal_achieved_unnormed[goal_dim];
          };
          varsum += bib::Utils::variance<>(trajectory.cbegin() + beg, trajectory.cbegin() + end, get);
        }
      }

      //goal_achieved hasn't change at all during the trajectory
      if (varsum <= 1e-8 && trajectory[beg].r >= -0.0001) {
        for (auto it = trajectory.begin() + beg; it != trajectory.begin() + end; it++){
            it->interest=false;
		}
      }
    }

//
//  compute importance sampling ratio on artificial data
//
    int artificial_data_size = trajectory.size() - saved_trajsize;
    if (artificial_data_size > 0) {
      caffe::Blob<double> all_states(artificial_data_size, nb_sensors, 1, 1);
      double* pall_states = all_states.mutable_cpu_data();

      int li=0;
      for (int i = saved_trajsize; i < trajectory.size(); i++) {
        std::copy(trajectory[i].s.begin(), trajectory[i].s.end(), pall_states + li * nb_sensors);
        li++;
      }

      ann->increase_batchsize(artificial_data_size);
      auto ac_out = ann->computeOutBlob(all_states);
      li=0;
      for (int i = saved_trajsize; i < trajectory.size(); i++) {
        trajectory[i].prob = bib::Proba<double>::truncatedGaussianDensity(trajectory[i].a, ac_out->cpu_data(), noise, li * this->nb_motors) / trajectory[i].prob;
        li++;
      }
      delete ac_out;
    }

//     LOG_DEBUG("#############");
//     for (int i=0;i<trajectory.size(); i++){
//       bib::Logger::PRINT_ELEMENTS(trajectory[i].s, trajectory[i].artificial ? "arti " : "real ");
//       bib::Logger::PRINT_ELEMENTS(trajectory[i].goal_achieved);
//       LOG_DEBUG(trajectory[i].r << " " <<i);
//     }
//     LOG_DEBUG("#############");
//     LOG_DEBUG("#############");
//     LOG_DEBUG("#############");
//     exit(1);

    if(trajectory.size() > 0)
      vnn->increase_batchsize(trajectory.size());

    caffe::Blob<double> all_states(trajectory.size(), nb_sensors, 1, 1);
    caffe::Blob<double> all_next_states(trajectory.size(), nb_sensors, 1, 1);
    //store reward in data and gamma coef in diff
    caffe::Blob<double> r_gamma_coef(trajectory.size(), 1, 1, 1);

    double* pall_states = all_states.mutable_cpu_data();
    double* pall_states_next = all_next_states.mutable_cpu_data();
    double* pr_all = r_gamma_coef.mutable_cpu_data();
    double* pgamma_coef = r_gamma_coef.mutable_cpu_diff();

    int li=0;
    for (auto it : trajectory) {
      std::copy(it.s.begin(), it.s.end(), pall_states + li * nb_sensors);
      std::copy(it.next_s.begin(), it.next_s.end(), pall_states_next + li * nb_sensors);
      pr_all[li]=it.r;
      pgamma_coef[li]= it.goal_reached ? 0.000f : this->gamma;
      li++;
    }

    update_critic(all_states, all_next_states, r_gamma_coef);

    if (trajectory.size() > 0) {
      const std::vector<double> disable_back_ac(this->nb_motors, 0.00f);
      caffe::Blob<double> deltas(trajectory.size(), 1, 1, 1);

      auto all_nextV = vnn->computeOutVFBlob(all_next_states, empty_action);
      auto all_mine = vnn->computeOutVFBlob(all_states, empty_action);

//#ifdef CAFFE_CPU_ONLY
      caffe::caffe_mul(trajectory.size(), r_gamma_coef.cpu_diff(), all_nextV->cpu_data(), deltas.mutable_cpu_data());
      caffe::caffe_add(trajectory.size(), r_gamma_coef.cpu_data(), deltas.cpu_data(), deltas.mutable_cpu_data());

      double *pv_target = deltas.mutable_cpu_data();
     double min_ = - (1.f/(1.f-this->gamma));
     for(int i=0;i<trajectory.size();i++){
         if(pv_target[i] > 0.0)
             pv_target[i] = 0.f;
         else if (pv_target[i] < min_)
             pv_target[i] = min_;
     }

      caffe::caffe_sub(trajectory.size(), deltas.cpu_data(), all_mine->cpu_data(), deltas.mutable_cpu_data());
//#else
//      switch (caffe::Caffe::mode()) {
//      case caffe::Caffe::CPU:
//        caffe::caffe_mul(trajectory.size(), r_gamma_coef.cpu_diff(), all_nextV->cpu_data(), deltas.mutable_cpu_data());
//        caffe::caffe_add(trajectory.size(), r_gamma_coef.cpu_data(), deltas.cpu_data(), deltas.mutable_cpu_data());
//        caffe::caffe_sub(trajectory.size(), deltas.cpu_data(), all_mine->cpu_data(), deltas.mutable_cpu_data());
//        break;
//      case caffe::Caffe::GPU:
//        caffe::caffe_gpu_mul(trajectory.size(), r_gamma_coef.gpu_diff(), all_nextV->gpu_data(), deltas.mutable_gpu_data());
//        caffe::caffe_gpu_add(trajectory.size(), r_gamma_coef.gpu_data(), deltas.gpu_data(), deltas.mutable_gpu_data());
//        caffe::caffe_gpu_sub(trajectory.size(), deltas.gpu_data(), all_mine->gpu_data(), deltas.mutable_gpu_data());
//        break;
//      }
//#endif

      if(gae){
        //        Simple computation for lambda return
        //        move deltas from GPU to CPU
        double * diff = deltas.mutable_cpu_diff();
        const double* pdeltas = deltas.cpu_data();
        int li=trajectory.size() - 1;
        double prev_delta = 0.;
        int index_ep = trajectory_end_points.size() - 1;
        for (auto it = trajectory.rbegin(); it != trajectory.rend(); it++) {
          if (index_ep >= 0 && trajectory_end_points[index_ep] - 1 == li){
              prev_delta = 0.;
              index_ep--;
          }

          if(it->artificial) {
            diff[li] = pdeltas[li] * std::min(it->prob, pbar) + prev_delta * std::min(it->prob, cbar);
            prev_delta = this->gamma * lambda * diff[li];
          } else {
            diff[li] = pdeltas[li] + prev_delta;
            prev_delta = this->gamma * lambda * diff[li];
          }
          --li;
        }
//        ASSERT(diff[trajectory.size() -1] == pdeltas[trajectory.size() -1] * std::min(trajectory[trajectory.size() - 1].prob, pbar), "pb lambda");

        caffe::caffe_copy(trajectory.size(), deltas.cpu_diff(), deltas.mutable_cpu_data());
      }

      uint n=0;
      posdelta_mean=0.f;
      //store target in data, and disable in diff
      caffe::Blob<double> target_cac(trajectory.size(), this->nb_motors, 1, 1);
      caffe::Blob<double> target_treg(trajectory.size(), this->nb_motors, 1, 1);
      caffe::caffe_set(target_cac.count(), static_cast<double>(1.f), target_cac.mutable_cpu_diff());
      caffe::caffe_set(target_treg.count(), static_cast<double>(1.f), target_treg.mutable_cpu_diff());
      caffe::Blob<double> deltas_blob(trajectory.size(), this->nb_motors, 1, 1);
      caffe::caffe_set(deltas_blob.count(), static_cast<double>(1.f), deltas_blob.mutable_cpu_data());

      double* pdisable_back_cac = target_cac.mutable_cpu_diff();
      double* pdisable_back_treg = target_treg.mutable_cpu_diff();
      double* pdeltas_blob = deltas_blob.mutable_cpu_data();
      double* ptarget_cac = target_cac.mutable_cpu_data();
      double* ptarget_treg = target_treg.mutable_cpu_data();
      const double* pdeltas = deltas.cpu_data();
      li=0;
      //cacla cost
      for(auto it = trajectory.begin(); it != trajectory.end() ; ++it) {
        std::copy(it->a.begin(), it->a.end(), ptarget_cac + li * this->nb_motors);
        if(pdeltas[li] > 0. && it->interest) {
          posdelta_mean += pdeltas[li];
          n++;
        } else {
          std::copy(disable_back_ac.begin(), disable_back_ac.end(), pdisable_back_cac + li * this->nb_motors);
        }
        if(!disable_cac)
            std::fill(pdeltas_blob + li * this->nb_motors, pdeltas_blob + (li+1) * this->nb_motors, pdeltas[li]);
        li++;
      }
      //penalty cost
      li=0;
      int number_non_artificial_sample = 0;
      for(auto it = trajectory.begin(); it != trajectory.end() ; ++it) {
        std::copy(it->pure_a.begin(), it->pure_a.end(), ptarget_treg + li * this->nb_motors);
        if(ignore_poss_ac && pdeltas[li] > 0. || it->artificial) {
            std::copy(disable_back_ac.begin(), disable_back_ac.end(), pdisable_back_treg + li * this->nb_motors);
        }
        if (! it->artificial)
          number_non_artificial_sample++;
        li++;
      }

      ratio_valid_advantage = ((float)n) / ((float) trajectory.size());
      posdelta_mean = posdelta_mean / ((float) trajectory.size());
      int size_cost_cacla=trajectory.size()*this->nb_motors;

      double beta=0.0001f;
      mean_beta=0.f;
      if(conserve_beta)
        beta=conserved_beta;
      mean_beta += beta;

      if(n > 0) {
        ann->increase_batchsize(trajectory.size());
        for(uint sia = 0; sia < stoch_iter_actor; sia++){
          //learn BN
          auto ac_out = ann->computeOutBlob(all_states);
          ann->ZeroGradParameters();

          number_effective_actor_update = sia;
          if(disable_trust_region)
              beta=0.f;
          else if (sia > 0) {
            //compute deter distance(pi, pi_old)
            caffe::Blob<double> diff_treg(trajectory.size(), this->nb_motors, 1, 1);
            double l2distance = 0.f;
#ifdef CAFFE_CPU_ONLY
            caffe::caffe_sub(size_cost_cacla, target_treg.cpu_data(), ac_out->cpu_data(), diff_treg.mutable_cpu_data());
            caffe::caffe_mul(size_cost_cacla, target_treg.cpu_diff(), diff_treg.cpu_data(), diff_treg.mutable_cpu_data());
            caffe::caffe_mul(size_cost_cacla, diff_treg.cpu_data(), diff_treg.cpu_data(), diff_treg.mutable_cpu_data());
            l2distance = caffe::caffe_cpu_asum(size_cost_cacla, diff_treg.cpu_data());
#else
          switch (caffe::Caffe::mode()) {
          case caffe::Caffe::CPU:
            caffe::caffe_sub(size_cost_cacla, target_treg.cpu_data(), ac_out->cpu_data(), diff_treg.mutable_cpu_data());
            caffe::caffe_mul(size_cost_cacla, target_treg.cpu_diff(), diff_treg.cpu_data(), diff_treg.mutable_cpu_data());
            caffe::caffe_mul(size_cost_cacla, diff_treg.cpu_data(), diff_treg.cpu_data(), diff_treg.mutable_cpu_data());
            l2distance = caffe::caffe_cpu_asum(size_cost_cacla, diff_treg.cpu_data());
            break;
          case caffe::Caffe::GPU:
            caffe::caffe_gpu_sub(size_cost_cacla, target_treg.gpu_data(), ac_out->gpu_data(), diff_treg.mutable_gpu_data());
            caffe::caffe_gpu_mul(size_cost_cacla, target_treg.gpu_diff(), diff_treg.gpu_data(), diff_treg.mutable_gpu_data());
            caffe::caffe_gpu_mul(size_cost_cacla, diff_treg.gpu_data(), diff_treg.gpu_data(), diff_treg.mutable_gpu_data());
            caffe::caffe_gpu_asum(size_cost_cacla, diff_treg.gpu_data(), &l2distance);
            break;
          }
#endif
            l2distance = std::sqrt(l2distance/((double) number_non_artificial_sample*this->nb_motors));

            if (l2distance < beta_target/1.5)
                beta = beta/2.;
            else if (l2distance > beta_target*1.5)
                beta = beta*2.;

            beta=std::max(std::min((double)20.f, beta), (double) 0.01f);
            mean_beta += beta;
            conserved_l2dist = l2distance;
            //LOG_DEBUG(std::setprecision(7) << l2distance << " " << beta << " " << beta_target << " " << sia);
          }

          const auto actor_actions_blob = ann->getNN()->blob_by_name(MLP::actions_blob_name);

          caffe::Blob<double> diff_cac(trajectory.size(), this->nb_motors, 1, 1);
          caffe::Blob<double> diff_treg(trajectory.size(), this->nb_motors, 1, 1);
          double * ac_diff = nullptr;
#ifdef CAFFE_CPU_ONLY
          ac_diff = actor_actions_blob->mutable_cpu_diff();
          caffe::caffe_sub(size_cost_cacla, target_cac.cpu_data(), ac_out->cpu_data(), diff_cac.mutable_cpu_data());
          caffe::caffe_mul(size_cost_cacla, diff_cac.cpu_data(), deltas_blob.cpu_data(), diff_cac.mutable_cpu_data());
          caffe::caffe_mul(size_cost_cacla, target_cac.cpu_diff(), diff_cac.cpu_data(), diff_cac.mutable_cpu_data());

          caffe::caffe_sub(size_cost_cacla, target_treg.cpu_data(), ac_out->cpu_data(), diff_treg.mutable_cpu_data());
          caffe::caffe_scal(size_cost_cacla, beta, diff_treg.mutable_cpu_data());
          caffe::caffe_mul(size_cost_cacla, target_treg.cpu_diff(), diff_treg.cpu_data(), diff_treg.mutable_cpu_data());

          caffe::caffe_add(size_cost_cacla, diff_cac.cpu_data(), diff_treg.cpu_data(), ac_diff);
          caffe::caffe_scal(size_cost_cacla, (double) -1.f, ac_diff);
#else
          switch (caffe::Caffe::mode()) {
          case caffe::Caffe::CPU:
            ac_diff = actor_actions_blob->mutable_cpu_diff();
            caffe::caffe_sub(size_cost_cacla, target_cac.cpu_data(), ac_out->cpu_data(), diff_cac.mutable_cpu_data());
            caffe::caffe_mul(size_cost_cacla, diff_cac.cpu_data(), deltas_blob.cpu_data(), diff_cac.mutable_cpu_data());
            caffe::caffe_mul(size_cost_cacla, target_cac.cpu_diff(), diff_cac.cpu_data(), diff_cac.mutable_cpu_data());

            caffe::caffe_sub(size_cost_cacla, target_treg.cpu_data(), ac_out->cpu_data(), diff_treg.mutable_cpu_data());
            caffe::caffe_scal(size_cost_cacla, beta, diff_treg.mutable_cpu_data());
            caffe::caffe_mul(size_cost_cacla, target_treg.cpu_diff(), diff_treg.cpu_data(), diff_treg.mutable_cpu_data());

            caffe::caffe_add(size_cost_cacla, diff_cac.cpu_data(), diff_treg.cpu_data(), ac_diff);
            caffe::caffe_scal(size_cost_cacla, (double) -1.f, ac_diff);
            break;
          case caffe::Caffe::GPU:
            ac_diff = actor_actions_blob->mutable_gpu_diff();
            caffe::caffe_gpu_sub(size_cost_cacla, target_cac.gpu_data(), ac_out->gpu_data(), diff_cac.mutable_gpu_data());
            caffe::caffe_gpu_mul(size_cost_cacla, diff_cac.gpu_data(), deltas_blob.gpu_data(), diff_cac.mutable_gpu_data());
            caffe::caffe_gpu_mul(size_cost_cacla, target_cac.gpu_diff(), diff_cac.gpu_data(), diff_cac.mutable_gpu_data());

            caffe::caffe_gpu_sub(size_cost_cacla, target_treg.gpu_data(), ac_out->gpu_data(), diff_treg.mutable_gpu_data());
            caffe::caffe_gpu_scal(size_cost_cacla, beta, diff_treg.mutable_gpu_data());
            caffe::caffe_gpu_mul(size_cost_cacla, target_treg.gpu_diff(), diff_treg.gpu_data(), diff_treg.mutable_gpu_data());

            caffe::caffe_gpu_add(size_cost_cacla, diff_cac.gpu_data(), diff_treg.gpu_data(), ac_diff);
            caffe::caffe_gpu_scal(size_cost_cacla, (double) -1.f, ac_diff);
            break;
          }
#endif

          ann->actor_backward();
          ann->updateFisher(n);
          ann->regularize();
          ann->getSolver()->ApplyUpdate();
          ann->getSolver()->set_iter(ann->getSolver()->iter() + 1);
          delete ac_out;
        }
      }

      conserved_beta = beta;
      if (number_effective_actor_update != 0)
        mean_beta /= (double) number_effective_actor_update;

      delete all_nextV;
      delete all_mine;
    }

#ifdef PARALLEL_INTERACTION
    }
    std::vector<double> weights(ann->number_of_parameters(false), 0.f);
    if (world.rank() == 0)
        ann->copyWeightsTo(weights.data(), false);

    broadcast(world, weights, 0);
    if (world.rank() != 0)
        ann->copyWeightsFrom(weights.data(), false);
#endif
    nb_sample_update = trajectory.size();
    trajectory.clear();
    trajectory_end_points.clear();
  }

  void end_instance(bool learning) override {
    if(learning)
      episode++;
  }

  void save(const std::string& path, bool savebest, bool learning) override {
    if(savebest) {
      if(!learning && this->sum_weighted_reward >= bestever_score) {
        bestever_score = this->sum_weighted_reward;
        ann->save(path+".actor");
      }
    } else {
#ifdef PARALLEL_INTERACTION
      if(world.rank() == 0 ) {
#endif
        ann->save(path+".actor");
        vnn->save(path+".critic");
        bib::XMLEngine::save<>(normalizer, "normalizer", path+".normalizer.data");
#ifdef PARALLEL_INTERACTION
      }
#endif
    }
  }

  void save_run() override {
    ann->save("continue.actor");
    vnn->save("continue.critic");
    struct algo_state st = {episode};
    bib::XMLEngine::save(st, "algo_state", "continue.algo_state.data");
  }

  void load(const std::string& path) override {
    ann->load(path+".actor");
#ifndef PARALLEL_INTERACTION
    vnn->load(path+".critic");
#else
    if (world.rank() == 0)
      vnn->load(path+".critic");
#endif
    bib::XMLEngine::load<>(normalizer, "normalizer", path+".normalizer.data");
  }

  void load_previous_run() override {
    ann->load("continue.actor");
    vnn->load("continue.critic");
    auto p3 = bib::XMLEngine::load<struct algo_state>("algo_state", "continue.algo_state.data");
    episode = p3->episode;
    delete p3;
  }

  double criticEval(const std::vector<double>&, const std::vector<double>&) override {
    LOG_INFO("not implemented");
    return 0;
  }

  arch::Policy<NN>* getCopyCurrentPolicy() override {
//         return new arch::Policy<MLP>(new MLP(*ann) , gaussian_policy ? arch::policy_type::GAUSSIAN : arch::policy_type::GREEDY, noise, decision_each);
    return nullptr;
  }

  uint getGoalSize(){
    return goal_size;
  }

  bool sparse_reward(const std::vector<double>&  a, const std::vector<double>&  b) {
    double sum = 0.f;
    for (int i=0;i<a.size();i++){
      double diff = a[i] - b[i];
      sum += diff*diff;
    }
    sum = std::sqrt(sum);
    return sum < 0.05f;
  };

  double dense_reward(const std::vector<double>&  goal_achieved, const std::vector<double>&  desired_goal,
                               const std::vector<double>&  next_goal_achieved, const std::vector<double>&  next_desired_goal,
                               const std::vector<double>&  observation, const std::vector<double>&  next_observation) {

     if (sparse_reward(goal_achieved, desired_goal))
        return 0.f;

     std::vector<double> mid_goal(3, 0.f);
     mid_goal[0] = next_goal_achieved[0] - (next_desired_goal[0] - next_goal_achieved[0] > 0.f ? 1.f : -1.f)*0.04f;
     mid_goal[1] = next_goal_achieved[1] - (next_desired_goal[1] - next_goal_achieved[1] > 0.f ? 1.f : -1.f)*0.04f;
     mid_goal[2] = next_goal_achieved[2] + 0.07f;

     double dist_obj_hand;
     {
       std::vector<double> diff(3, 0.f);
       std::transform(mid_goal.begin(), mid_goal.end(), observation.begin() + 3, diff.begin(), std::minus<double>());
       dist_obj_hand = bib::Utils::euclidien_dist_ref(diff, 0.f)*3.f;
     }
     {
       std::vector<double> diff(3, 0.f);
       std::transform(mid_goal.begin(), mid_goal.end(), next_observation.begin() + 3, diff.begin(), std::minus<double>());
       dist_obj_hand -= bib::Utils::euclidien_dist_ref(diff, 0.f)*3.f;
     }

     double dist_goal;
     {
       std::vector<double> diff(3, 0.f);
       std::transform(goal_achieved.begin(), goal_achieved.end(), desired_goal.begin(), diff.begin(), std::minus<double>());
       dist_goal = bib::Utils::euclidien_dist_ref(diff, 0.f)*3.f;
     }
     {
       std::vector<double> diff(3, 0.f);
       std::transform(next_goal_achieved.begin(), next_goal_achieved.end(), next_desired_goal.begin(), diff.begin(), std::minus<double>());
       dist_goal -= bib::Utils::euclidien_dist_ref(diff, 0.f)*3.f;
     }
     if(dist_goal < 0.00005 and dist_goal >= 0.000000001)
        dist_goal = 0.f;

     double r = dist_obj_hand + 100*dist_goal;
     if (r > 0.5)
        r = 0.5;
     else if (r < -0.5)
        r = -0.5;

     return -1. + 0.5 + r;
   }

#ifdef PARALLEL_INTERACTION
  int getMPIrank() {
    return world.rank();
  }
#endif

 protected:
#ifndef PARALLEL_INTERACTION
  void _display(std::ostream& out) const override {
    out << std::setw(12) << std::fixed << std::setprecision(10) << this->sum_weighted_reward/this->gamma << " " << this->sum_reward <<
        " " << std::setw(8) << std::fixed << std::setprecision(5) << vnn->error() << " " << noise << " " << nb_sample_update <<
          " " << std::setprecision(3) << ratio_valid_advantage << " " << vnn->weight_l1_norm() << " " << ann->weight_l1_norm(true);
  }

//clear all; close all; wndw = 10; X=load('0.learning.data'); X=filter(ones(wndw,1)/wndw, 1, X); startx=0; starty=800; width=350; height=350; figure('position',[startx,starty,width,height]); plot(X(:,3), "linewidth", 2); xlabel('learning episode', "fontsize", 16); ylabel('sum rewards', "fontsize", 16); startx+=width; figure('position',[startx,starty,width,height]); plot(X(:,9), "linewidth", 2); xlabel('learning episode', "fontsize", 16); ylabel('beta', "fontsize", 16); startx+=width; figure('position',[startx,starty,width,height]) ; plot(X(:,8), "linewidth", 2); xlabel('learning episode', "fontsize", 16); ylabel('valid adv', "fontsize", 16); ylim([0, 1]); startx+=width; figure('position',[startx,starty,width,height]) ; plot(X(:,11), "linewidth", 2); hold on; plot(X(:,12), "linewidth", 2, "color", "red"); legend("critic", "actor"); xlabel('learning episode', "fontsize", 16); ylabel('||\theta||_1', "fontsize", 16); startx+=width; figure('position',[startx,starty,width,height]) ; plot(X(:,10), "linewidth", 2); xlabel('learning episode', "fontsize", 16); ylabel('||\mu_{old}-\mu||_2', "fontsize", 16); startx+=width; figure('position',[startx,starty,width,height]) ; plot(X(:,14), "linewidth", 2); xlabel('learning episode', "fontsize", 16); ylabel('effective actor. upd.', "fontsize", 16);

  void _dump(std::ostream& out) const override {
    out << std::setw(25) << std::fixed << std::setprecision(22) << this->sum_weighted_reward/this->gamma << " " <<
    this->sum_reward << " " << std::setw(8) << std::fixed << std::setprecision(5) << vnn->error() << " " <<
    nb_sample_update << " " << std::setprecision(3) << ratio_valid_advantage << " " << std::setprecision(10) <<
    mean_beta << " " << conserved_l2dist << " " << std::setprecision(3) << vnn->weight_l1_norm() << " " <<
    ann->weight_l1_norm(true) << " " << std::setprecision(6)  << posdelta_mean << " " << number_effective_actor_update;
  }
#else
  void _dump(std::ostream& out) const override {
    out << std::setw(25) << std::fixed << std::setprecision(22) << this->sum_weighted_reward/this->gamma << " " <<
    this->sum_reward << " " << std::setw(8) << std::fixed << std::setprecision(5) << (world.rank() == 0 ? vnn->error() : 0) << " " <<
    nb_sample_update << " " << std::setprecision(3) << ratio_valid_advantage << " " << std::setprecision(10) <<
    mean_beta << " " << conserved_l2dist << " " << std::setprecision(3) << (world.rank() == 0 ?  vnn->weight_l1_norm() : 0) << " " <<
    ann->weight_l1_norm(true) << " " << std::setprecision(6)  << posdelta_mean << " " << number_effective_actor_update;
  }

  void _display(std::ostream& out) const override {
    out << std::setw(12) << std::fixed << std::setprecision(10) << this->sum_weighted_reward/this->gamma << " " << this->sum_reward <<
        " " << std::setw(8) << std::fixed << std::setprecision(5) << (world.rank() == 0 ? vnn->error() : 0) << " " << noise << " " << nb_sample_update <<
          " " << std::setprecision(3) << ratio_valid_advantage << " " << (world.rank() == 0 ?  vnn->weight_l1_norm() : 0) << " " << ann->weight_l1_norm(true);
  }
#endif

 private:
  uint nb_sensors;
  uint episode = 1;
  uint step = 0;

  double noise, noise2, noise3;
  uint gaussian_policy;
  bool gae, ignore_poss_ac, conserve_beta, disable_trust_region, disable_cac;
  uint number_fitted_iteration, stoch_iter_actor, stoch_iter_critic;
  uint actor_output_layer_type, hidden_layer_type, momentum;
  double lambda, beta_target;
  double conserved_beta= 0.0001f;
  double mean_beta= 0.f;
  double conserved_l2dist= 0.f;
  int number_effective_actor_update = 0;

  std::shared_ptr<std::vector<double>> last_action;
  std::shared_ptr<std::vector<double>> last_pure_action;
  std::vector<double> last_state;
  std::vector<double> last_goal_achieved;
  double alpha_v, alpha_a;

  std::deque<sample> trajectory;
  std::deque<int> trajectory_end_points;

  NN* ann;
  NN* ann_noblob;
  NN* vnn = nullptr;

  std::vector<uint>* hidden_unit_v;
  std::vector<uint>* hidden_unit_a;
  caffe::Blob<double> empty_action; //dummy action cause c++ cannot accept null reference
  double bestever_score;
  int update_each_episode;
  bib::OrnsteinUhlenbeckNoise<double>* oun = nullptr;
  float ratio_valid_advantage=0;
  int nb_sample_update = 0;
  double posdelta_mean = 0;

  //hindsight
  uint goal_size;
  uint goal_start;
  uint hindsight_nb_destination;

  //v trace
  double pbar = 1;
  double cbar = 1;

  bib::OnlineNormalizer normalizer;

#ifdef PARALLEL_INTERACTION
  boost::mpi::communicator world;
#endif

  struct algo_state {
    uint episode;

    friend class boost::serialization::access;
    template <typename Archive>
    void serialize(Archive& ar, const unsigned int) {
      ar& BOOST_SERIALIZATION_NVP(episode);
    }
  };
};

#endif


//=============================================================================================================
/**
* @file     minimumnorm.cpp
* @author   Christoph Dinh <chdinh@nmr.mgh.harvard.edu>;
*           Matti Hamalainen <msh@nmr.mgh.harvard.edu>
* @version  1.0
* @date     February, 2013
*
* @section  LICENSE
*
* Copyright (C) 2012, Christoph Dinh and Matti Hamalainen. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification, are permitted provided that
* the following conditions are met:
*     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
*       following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
*       the following disclaimer in the documentation and/or other materials provided with the distribution.
*     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
*       to endorse or promote products derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
* @brief    Implementation of the MinimumNorm Class.
*
*/

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "minimumnorm.h"

#include <mne/mne_sourceestimate.h>
#include <fiff/fiff_evoked.h>

//*************************************************************************************************************
//=============================================================================================================
// STL INCLUDES
//=============================================================================================================

#include <iostream>

//*************************************************************************************************************
//=============================================================================================================
// Eigen INCLUDES
//=============================================================================================================

#include <Eigen/Core>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace Eigen;
using namespace MNELIB;
using namespace INVERSELIB;

//*************************************************************************************************************
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

MinimumNorm::MinimumNorm(const MNEInverseOperator &p_inverseOperator, float lambda, const QString method)
: m_inverseOperator(p_inverseOperator)
, inverseSetup(false)
{
    this->setRegularization(lambda);
    this->setMethod(method);
}

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

MinimumNorm::MinimumNorm(const MNEInverseOperator &p_inverseOperator, float lambda, bool dSPM, bool sLORETA)
: m_inverseOperator(p_inverseOperator)
, inverseSetup(false)
{
    this->setRegularization(lambda);
    this->setMethod(dSPM, sLORETA);
}

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

MNESourceEstimate MinimumNorm::calculateInverse(const FiffEvoked &p_fiffEvoked, bool pick_normal)
{
    //
    //   Set up the inverse according to the parameters
    //
    qint32 nave = p_fiffEvoked.nave;

    if(!m_inverseOperator.check_ch_names(p_fiffEvoked.info))
    {
        qWarning("Channel name check failed.");
        return MNESourceEstimate();
    }

    doInverseSetup(nave,pick_normal);

    //
    //   Pick the correct channels from the data
    //
    FiffEvoked t_fiffEvoked = p_fiffEvoked.pick_channels(inv.noise_cov->names);

    printf("Picked %d channels from the data\n",t_fiffEvoked.info.nchan);

    //Results
    float tmin = ((float)t_fiffEvoked.first) / t_fiffEvoked.info.sfreq;
    float tstep = 1/t_fiffEvoked.info.sfreq;

    return calculateInverse(t_fiffEvoked.data, tmin, tstep);

//    //
//    //   Set up the inverse according to the parameters
//    //
//    qint32 nave = p_fiffEvoked.nave;

//    if(!m_inverseOperator.check_ch_names(p_fiffEvoked.info))
//    {
//        qWarning("Channel name check failed.");
//        return SourceEstimate();
//    }

//    //ToDo his could be heavily accelerated for real time calculation -> ToDo calculate inverse RT
//    MNEInverseOperator inv = m_inverseOperator.prepare_inverse_operator(nave, m_fLambda, m_bdSPM, m_bsLORETA);
//    //
//    //   Pick the correct channels from the data
//    //
//    FiffEvoked t_fiffEvoked = p_fiffEvoked.pick_channels(inv.noise_cov->names);

//    printf("Picked %d channels from the data\n",t_fiffEvoked.info.nchan);
//    printf("Computing inverse...");

//    MatrixXd K;
//    SparseMatrix<double> noise_norm;
//    QList<VectorXi> vertno;
//    Label label;
//    inv.assemble_kernel(label, m_sMethod, pick_normal, K, noise_norm, vertno);

//    MatrixXd sol = K * t_fiffEvoked.data; //apply imaging kernel

//    if (inv.source_ori == FIFFV_MNE_FREE_ORI)
//    {
//        printf("combining the current components...");
//        MatrixXd sol1(sol.rows()/3,sol.cols());
//        for(qint32 i = 0; i < sol.cols(); ++i)
//        {
//            VectorXd* tmp = MNEMath::combine_xyz(sol.block(0,i,sol.rows(),1));
//            sol1.block(0,i,sol.rows()/3,1) = tmp->cwiseSqrt();
//            delete tmp;
//        }
//        sol.resize(sol1.rows(),sol1.cols());
//        sol = sol1;
//    }

//    if (m_bdSPM)
//    {
//        printf("(dSPM)...");
//        sol = inv.noisenorm*sol;
//    }
//    else if (m_bsLORETA)
//    {
//        printf("(sLORETA)...");
//        sol = inv.noisenorm*sol;
//    }
//    printf("[done]\n");

//    //Results
//    float tmin = ((float)t_fiffEvoked.first) / t_fiffEvoked.info.sfreq;
//    float tstep = 1/t_fiffEvoked.info.sfreq;

//    QList<VectorXi> t_qListVertices;
//    for(qint32 h = 0; h < inv.src.size(); ++h)
//        t_qListVertices.push_back(inv.src[h].vertno);

//    return SourceEstimate(sol, t_qListVertices, tmin, tstep);
}

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

MNESourceEstimate MinimumNorm::calculateInverse(const MatrixXd &data, float tmin, float tstep) const
{
    if(!inverseSetup)
    {
        qWarning("Inverse not setup -> call doInverseSetup first!");
        return MNESourceEstimate();
    }

    MatrixXd sol = K * data; //apply imaging kernel

    if (inv.source_ori == FIFFV_MNE_FREE_ORI)
    {
        printf("combining the current components...");
        MatrixXd sol1(sol.rows()/3,sol.cols());
        for(qint32 i = 0; i < sol.cols(); ++i)
        {
            VectorXd* tmp = MNEMath::combine_xyz(sol.block(0,i,sol.rows(),1));
            sol1.block(0,i,sol.rows()/3,1) = tmp->cwiseSqrt();
            delete tmp;
        }
        sol.resize(sol1.rows(),sol1.cols());
        sol = sol1;
    }

    if (m_bdSPM)
    {
        printf("(dSPM)...");
        sol = inv.noisenorm*sol;
    }
    else if (m_bsLORETA)
    {
        printf("(sLORETA)...");
        sol = inv.noisenorm*sol;
    }
    printf("[done]\n");

    //Results
    VectorXi p_vecVertices(inv.src[0].vertno.size() + inv.src[1].vertno.size());
    p_vecVertices << inv.src[0].vertno, inv.src[1].vertno;

//    VectorXi p_vecVertices();
//    for(qint32 h = 0; h < inv.src.size(); ++h)
//        t_qListVertices.push_back(inv.src[h].vertno);

    return MNESourceEstimate(sol, p_vecVertices, tmin, tstep);

}

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

void MinimumNorm::doInverseSetup(qint32 nave, bool pick_normal)
{
    //
    //   Set up the inverse according to the parameters
    //
    inv = m_inverseOperator.prepare_inverse_operator(nave, m_fLambda, m_bdSPM, m_bsLORETA);

    printf("Computing inverse...");
    inv.assemble_kernel(label, m_sMethod, pick_normal, K, noise_norm, vertno);

    std::cout << "K " << K.rows() << " x " << K.cols() << std::endl;

    inverseSetup = true;
}

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

const char* MinimumNorm::getName() const
{
    return "Minimum Norm Estimate";
}

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

const MNESourceSpace& MinimumNorm::getSourceSpace() const
{
    return m_inverseOperator.src;
}

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

void MinimumNorm::setMethod(QString method)
{
    if(method.compare("MNE") == 0)
        setMethod(false, false);
    else if(method.compare("dSPM") == 0)
        setMethod(true, false);
    else if(method.compare("sLORETA") == 0)
        setMethod(false, true);
    else
    {
        qWarning("Method not recognized!");
        method = "dSPM";
        setMethod(true, false);
    }

    printf("\tSet minimum norm method to %s.\n", method.toUtf8().constData());
}

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

void MinimumNorm::setMethod(bool dSPM, bool sLORETA)
{
    if(dSPM && sLORETA)
    {
        qWarning("Cant activate dSPM and sLORETA at the same time! - Activating dSPM");
        m_bdSPM = true;
        m_bsLORETA = false;
    }
    else
    {
        m_bdSPM = dSPM;
        m_bsLORETA = sLORETA;
        if(dSPM)
            m_sMethod = QString("dSPM");
        else if(sLORETA)
            m_sMethod = QString("sLORETA");
        else
            m_sMethod = QString("MNE");

    }
}

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

void MinimumNorm::setRegularization(float lambda)
{
    m_fLambda = lambda;
}