#ifndef PENNFAC_HPP
#define PENNFAC_HPP

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

#define lapack_complex_float std::complex<float>
#define lapack_complex_double std::complex<double>
#include "lapacke.h"

#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 "nn/MLP.hpp"

#ifndef SAASRG_SAMPLE
#define SAASRG_SAMPLE
typedef struct _sample {
  std::vector<double> s;
  std::vector<double> pure_a;
  std::vector<double> a;
  std::vector<double> next_s;
  double r;
  bool goal_reached;

} sample;
#endif

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

  OfflineCaclaAg(unsigned int _nb_motors, unsigned int _nb_sensors)
    : arch::AACAgent<NN, arch::AgentGPUProgOptions>(_nb_motors, _nb_sensors), nb_sensors(_nb_sensors), empty_action(), last_state(_nb_sensors, 0.f) {

  }

  virtual ~OfflineCaclaAg() {
    delete vnn;
    delete ann;

    delete ann_testing;
    if(batch_norm_actor != 0)
      delete ann_testing_blob;
    if(batch_norm_critic != 0)
      delete vnn_testing;

    delete hidden_unit_v;
    delete hidden_unit_a;

    delete correlation_matrix;

    if(oun == nullptr)
      delete oun;
  }

  const std::vector<double>& _run(double reward, const std::vector<double>& sensors,
                                  bool learning, bool goal_reached, bool) override {

    // protect batch norm from testing data and poor data
    vector<double>* next_action = ann_testing->computeOut(sensors);
    if (last_action.get() != nullptr && learning)
      trajectory.push_back( {last_state, *last_pure_action, *last_action, sensors, reward, goal_reached});

    last_pure_action.reset(new 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) {
        vector<double>* randomized_action = bib::Proba<double>::multidimentionnalTruncatedGaussianZeroMean(this->nb_motors, noise, -1.f, 1.f);
        caffe::caffe_cpu_gemv(CblasTrans, this->nb_motors, this->nb_motors, (double)1.f, correlation_matrix->data(), randomized_action->data(), (double)1.f, next_action->data());
        delete 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());
    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");
    batch_norm_actor        = pt->get<uint>("agent.batch_norm_actor");
    batch_norm_critic       = pt->get<uint>("agent.batch_norm_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");
    correlation_importance = pt->get<double>("agent.correlation_importance");
    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){
      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");
    } catch(boost::exception const& ) {
    }

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

    if(lambda >=0. && batch_norm_critic != 0){
      LOG_DEBUG("to be done!");
      exit(1);
    }

#ifdef CAFFE_CPU_ONLY
    LOG_INFO("CPU mode");
    (void) command_args;
#else
    if(command_args->count("gpu") == 0 || command_args->count("cpu") > 0){
      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

    ann = new NN(nb_sensors, *hidden_unit_a, this->nb_motors, alpha_a, 1, hidden_layer_type, actor_output_layer_type, batch_norm_actor, true, momentum);

    vnn = new NN(nb_sensors, nb_sensors, *hidden_unit_v, alpha_v, 1, -1, hidden_layer_type, batch_norm_critic, false, momentum);

    ann_testing = new NN(*ann, false, ::caffe::Phase::TEST);

    std::vector<double> correlation_matrix_hf = {
        1.374865,  0.237821,  0.014352,  0.292353, -0.314855, -0.435762,
        0.,  0.786414, -0.260977, -0.172376, -0.175636, -0.066117,
        0., 0.,  0.713604, 0.099008,  0.049184, -0.030153,
        0., 0.,  0.,  0.780265, -0.007977, -0.261878,
        0., 0.,  0., 0.,  0.592841,  0.148877,
        0., 0., 0., 0.,  0.,  0.664371
    };
    correlation_matrix = new std::vector<double>(this->nb_motors*this->nb_motors);
    ASSERT(correlation_matrix_hf.size() == correlation_matrix->size(), "pb size");
    std::copy(correlation_matrix_hf.begin(), correlation_matrix_hf.end(), correlation_matrix->begin());
    LAPACKE_dpotrf(LAPACK_ROW_MAJOR, 'U', this->nb_motors, correlation_matrix->data(), this->nb_motors);
    bib::Logger::PRINT_ELEMENTS(*correlation_matrix);

    if(batch_norm_actor != 0)
      ann_testing_blob = new NN(*ann, false, ::caffe::Phase::TEST);
    if(batch_norm_critic != 0)
      vnn_testing = new NN(*vnn, false, ::caffe::Phase::TEST);

    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_testing->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 = [&]() {
        decltype(vnn_testing->computeOutVFBlob(all_next_states, empty_action)) all_nextV;
        if(batch_norm_critic != 0)
        {
          double* weights = new double[vnn->number_of_parameters(false)];
          vnn->copyWeightsTo(weights, false);
          vnn_testing->copyWeightsFrom(weights, false);
          delete[] weights;
          all_nextV = vnn_testing->computeOutVFBlob(all_next_states, empty_action);
        } else
          all_nextV = vnn->computeOutVFBlob(all_next_states, empty_action);
        auto all_V = vnn->computeOutVFBlob(all_states, empty_action);

#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());
        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) {
          if (index_ep >= 0 && trajectory_end_points[index_ep] - 1 == li){
              prev_delta = 0.;
              index_ep--;
          }
          pdiff[li] = pvtarget[li] + prev_delta;
          prev_delta = this->gamma * lambda * pdiff[li];
          --li;
        }
        ASSERT(pdiff[trajectory.size() -1] == pvtarget[trajectory.size() -1], "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
        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
    trajectory_end_points.push_back(trajectory.size());
    if (episode % update_each_episode != 0)
      return;

    if(trajectory.size() > 0){
      vnn->increase_batchsize(trajectory.size());
      if(batch_norm_critic != 0)
        vnn_testing->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);

      decltype(vnn->computeOutVFBlob(all_next_states, empty_action)) all_nextV, all_mine;
      if(batch_norm_critic != 0)
      {
        double* weights = new double[vnn->number_of_parameters(false)];
        vnn->copyWeightsTo(weights, false);
        vnn_testing->copyWeightsFrom(weights, false);
        delete[] weights;
        all_nextV = vnn_testing->computeOutVFBlob(all_next_states, empty_action);
        all_mine = vnn_testing->computeOutVFBlob(all_states, empty_action);
      } else {
        all_nextV = vnn->computeOutVFBlob(all_next_states, empty_action);
        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());
      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) {
          if (index_ep >= 0 && trajectory_end_points[index_ep] - 1 == li){
              prev_delta = 0.;
              index_ep--;
          }
          diff[li] = pdeltas[li] + prev_delta;
          prev_delta = this->gamma * lambda * diff[li];
          --li;
        }
        ASSERT(diff[trajectory.size() -1] == pdeltas[trajectory.size() -1], "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.) {
          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;
      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.) {
            std::copy(disable_back_ac.begin(), disable_back_ac.end(), pdisable_back_treg + li * this->nb_motors);
        }
        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) {
        for(uint sia = 0; sia < stoch_iter_actor; sia++){
          ann->increase_batchsize(trajectory.size());
          //learn BN
          auto ac_out = ann->computeOutBlob(all_states);
          if(batch_norm_actor != 0) {
            //re-compute ac_out with BN as testing
            double* weights = new double[ann->number_of_parameters(false)];
            ann->copyWeightsTo(weights, false);
            ann_testing_blob->copyWeightsFrom(weights, false);
            delete[] weights;
            delete ac_out;
            ann_testing_blob->increase_batchsize(trajectory.size());
            ac_out = ann_testing_blob->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, 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, 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, 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) trajectory.size()*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);
          }

          //correlation cost
          caffe::Blob<double> target_corr(trajectory.size(), this->nb_motors, 1, 1);

          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_cpu_gemm(CblasNoTrans, CblasTrans, trajectory.size(), this->nb_motors, this->nb_motors, (double)1.f, ac_out->cpu_data(), correlation_matrix->data(), (double)0., target_corr.mutable_cpu_data());
          caffe::caffe_sub(size_cost_cacla, target_corr.cpu_data(), ac_out->cpu_data(), target_corr.mutable_cpu_diff());
          caffe::caffe_scal(size_cost_cacla, correlation_importance, target_corr.mutable_cpu_diff());

          caffe::caffe_add(size_cost_cacla, target_corr.cpu_diff(), diff_cac.cpu_data(), diff_cac.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_cpu_gemm(CblasNoTrans, CblasTrans, trajectory.size(), this->nb_motors, this->nb_motors, (double)1.f, ac_out->cpu_data(), correlation_matrix->data(), (double)0., target_corr.mutable_cpu_data());
            caffe::caffe_sub(size_cost_cacla, target_corr.cpu_data(), ac_out->cpu_data(), target_corr.mutable_cpu_diff());
            caffe::caffe_scal(size_cost_cacla, correlation_importance, target_corr.mutable_cpu_diff());

            caffe::caffe_add(size_cost_cacla, target_corr.cpu_diff(), diff_cac.cpu_data(), diff_cac.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_gemm(CblasNoTrans, CblasTrans, trajectory.size(), this->nb_motors, this->nb_motors, (double)1.f, ac_out->gpu_data(), correlation_matrix->data(), (double)0., target_corr.mutable_gpu_data());
            caffe::caffe_gpu_sub(size_cost_cacla, target_corr.gpu_data(), ac_out->gpu_data(), target_corr.mutable_gpu_diff());
            caffe::caffe_gpu_scal(size_cost_cacla, correlation_importance, target_corr.mutable_gpu_diff());

            caffe::caffe_gpu_add(size_cost_cacla, target_corr.gpu_diff(), diff_cac.gpu_data(), diff_cac.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;
        }
      } else if(batch_norm_actor != 0){
        //learn BN even if every action were bad
        ann->increase_batchsize(trajectory.size());
        delete ann->computeOutBlob(all_states);
      }

      conserved_beta = beta;
      mean_beta /= (double) number_effective_actor_update;

      delete all_nextV;
      delete all_mine;

      if(batch_norm_actor != 0)
        ann_testing->increase_batchsize(1);
    }

    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 {
      ann->save(path+".actor");
      vnn->save(path+".critic");
    }
  }

  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");
    vnn->load(path+".critic");
  }

  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;
  }

 protected:
  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;
  }

 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 batch_norm_actor, batch_norm_critic, 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;
  double alpha_v, alpha_a;

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

  NN* ann;
  NN* vnn;
  NN* ann_testing;
  NN* ann_testing_blob;
  NN* vnn_testing;
  std::vector<double>* correlation_matrix;

  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;
  double correlation_importance;

  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


#ifndef BOXES_DOMAIN_HPP
#define BOXES_DOMAIN_HPP

/**************************************************************************
 * A wrapper for a disjunctive domain of intervals from "Boxes: A
 * Symbolic Abstract Domain of Boxes" by A. Gurfinkel and S. Chaki
 * published in SAS'10.
 **************************************************************************/

#include <crab/config.h>
#include <crab/common/debug.hpp>
#include <crab/common/stats.hpp>
#include <crab/common/types.hpp>
#include <crab/domains/operators_api.hpp>

#ifndef HAVE_LDD
/*
 * Dummy implementation if ldd not found
 */
#define LDD_NOT_FOUND "No LDD. Run cmake with -DUSE_LDD=ON"
namespace crab {
   namespace domains {
      template<typename Number, typename VariableName, size_t LddSize = 100>
      class boxes_domain:
         public ikos::writeable,
         public numerical_domain< Number, VariableName>,
         public bitwise_operators< Number, VariableName >,
         public division_operators< Number, VariableName >,
         public array_operators< Number, VariableName>,
         public pointer_operators< Number, VariableName>,
	 public boolean_operators< Number, VariableName> {

       public:
        using typename numerical_domain< Number, VariableName>::linear_expression_t;
        using typename numerical_domain< Number, VariableName>::linear_constraint_t;
        using typename numerical_domain< Number, VariableName>::linear_constraint_system_t;
	typedef disjunctive_linear_constraint_system<Number, VariableName>
	disjunctive_linear_constraint_system_t;
        using typename numerical_domain< Number, VariableName>::variable_t;
        using typename numerical_domain< Number, VariableName>::number_t;
        using typename numerical_domain< Number, VariableName>::varname_t;
        typedef boxes_domain <Number, VariableName, LddSize> boxes_domain_t;
        typedef interval <Number> interval_t;

        boxes_domain(): ikos::writeable() { }

        static boxes_domain_t top() { CRAB_ERROR (LDD_NOT_FOUND); }

        static boxes_domain_t bottom() { CRAB_ERROR (LDD_NOT_FOUND); }

        boxes_domain (const boxes_domain_t& other):
            ikos::writeable() { }

        bool is_bottom() { CRAB_ERROR (LDD_NOT_FOUND); }

        bool is_top() { CRAB_ERROR (LDD_NOT_FOUND); }

        bool operator<=(boxes_domain_t other) { CRAB_ERROR (LDD_NOT_FOUND); }

        void operator|=(boxes_domain_t other)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        boxes_domain_t operator|(boxes_domain_t other)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        boxes_domain_t operator&(boxes_domain_t other)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        boxes_domain_t operator||(boxes_domain_t other)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        template<typename Thresholds>
        boxes_domain_t widening_thresholds (boxes_domain_t other,
                                            const Thresholds &ts)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        boxes_domain_t operator&& (boxes_domain_t other)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void operator-=(VariableName var)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        interval_t operator[](VariableName v)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void set(VariableName v, interval_t ival)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void operator += (linear_constraint_system_t csts)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void assign (VariableName x, linear_expression_t e)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void apply (operation_t op, VariableName x, VariableName y, Number z)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void apply(operation_t op, VariableName x, VariableName y, VariableName z)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void apply(operation_t op, VariableName x, Number k)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void apply(conv_operation_t op, VariableName x, VariableName y, unsigned width)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void apply(conv_operation_t op, VariableName x, Number k, unsigned width)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void apply(bitwise_operation_t op, VariableName x, VariableName y, VariableName z)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void apply(bitwise_operation_t op, VariableName x, VariableName y, Number k)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void apply(div_operation_t op, VariableName x, VariableName y, VariableName z)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void apply(div_operation_t op, VariableName x, VariableName y, Number k)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        linear_constraint_system_t to_linear_constraint_system ()
        { CRAB_ERROR (LDD_NOT_FOUND); }

        disjunctive_linear_constraint_system_t
	to_disjunctive_linear_constraint_system ()
        { CRAB_ERROR (LDD_NOT_FOUND); }

        void write(crab_os& o)
        { CRAB_ERROR (LDD_NOT_FOUND); }

        static std::string getDomainName () {
          return "Dummy Boxes";
        }
      };

   } // namespace domains
}// namespace crab

#else
/*
 *  Real implementation starts here
 */

#include <crab/domains/ldd/ldd.hpp>
#include <crab/domains/ldd/ldd_print.hpp>
#include <crab/domains/domain_traits.hpp>
#include <boost/bimap.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/optional.hpp>

namespace crab {

   namespace domains {

      using namespace crab::domains::ldd;

      /*
       * The wrapper has two global datastructures:
       * 1) a ldd manager and
       * 2) a map from VariableName to ldd dimension.
       *
       * FIXME: since the ldd manager is shared we need to fix a
       * single size for all ldds. Since ldds are sparse we can fix a
       * size big enough for our programs.
       *
       * FIXME: Ldd_TermReplace seems to leak memory sometimes.
       */
      template<typename Number, typename VariableName, size_t LddSize>
      class boxes_domain_:
         public ikos::writeable,
         public numerical_domain< Number, VariableName>,
         public bitwise_operators< Number, VariableName >,
         public division_operators< Number, VariableName >,
         public array_operators< Number, VariableName>,
         public pointer_operators< Number, VariableName>,
	 public boolean_operators< Number, VariableName>{

        typedef interval_domain <Number, VariableName> interval_domain_t;
       public:
        using typename numerical_domain< Number, VariableName>::linear_expression_t;
        using typename numerical_domain< Number, VariableName>::linear_constraint_t;
        using typename numerical_domain< Number, VariableName>::linear_constraint_system_t;
	typedef disjunctive_linear_constraint_system<Number, VariableName>
	disjunctive_linear_constraint_system_t;
        using typename numerical_domain< Number, VariableName>::variable_t;
        using typename numerical_domain< Number, VariableName>::number_t;
        using typename numerical_domain< Number, VariableName>::varname_t;

        typedef boxes_domain_ <Number, VariableName, LddSize> boxes_domain_t;
        typedef interval <Number> interval_t;

       private:
        // --- map from crab variable index to ldd term index
        typedef boost::bimap< VariableName , int > var_map_t;
        typedef typename var_map_t::value_type binding_t;

        LddNodePtr m_ldd;
        static LddManager* m_ldd_man;
        static var_map_t m_var_map;

        static LddManager* get_ldd_man () {
          if (!m_ldd_man) {
            DdManager* cudd = Cudd_Init (0, 0, CUDD_UNIQUE_SLOTS, 127, 0);
            theory_t* theory = ldd::create_box_theory<Number> (LddSize);
            CRAB_LOG ("boxes",
		      crab::outs() << "Created a ldd of size " << LddSize <<"\n";);
            m_ldd_man = Ldd_Init (cudd, theory);
            //Cudd_AutodynEnable (cudd, CUDD_REORDER_GROUP_SIFT);
	    Ldd_SanityCheck (get_ldd_man ());
          }
          return m_ldd_man;
        }

	inline theory_t* get_theory () {
	  return Ldd_GetTheory (get_ldd_man());
	}

        static int num_of_vars () {
          return m_var_map.left.size ();
        }

        int get_var_dim (VariableName v) const {
          auto it = m_var_map.left.find (v);
          if (it != m_var_map.left.end ()) {
            return it->second;
          } else {
	    // XXX: reserved dim 0 for SPECIAL variable
            unsigned int id = m_var_map.size () + 1;
            if (id >= LddSize) {
              CRAB_ERROR ("The Ldd size of ", LddSize, " needs to be larger");
            }
            m_var_map.insert (binding_t (v, id));
            return id;
          }
        }

        inline constant_t mk_cst (ikos::z_number k) {
          mpq_class kk ((mpz_class) k);
          return (constant_t) tvpi_create_cst (kk.get_mpq_t ());
        }

        inline constant_t mk_cst (ikos::q_number k) {
          mpq_class kk ((mpq_class) k);
          return (constant_t) tvpi_create_cst (kk.get_mpq_t ());
        }

        // convex approximation
        void convex_approx () {
          m_ldd = lddPtr(get_ldd_man(),
			 Ldd_TermMinmaxApprox(get_ldd_man(), &*m_ldd));
        }

        LddNodePtr convex_approx (LddNodePtr ldd) {
          return lddPtr(get_ldd_man(),
			Ldd_TermMinmaxApprox(get_ldd_man(), &*ldd));
        }

        // pre: ldd is not either true or false
        void project (LddNodePtr& ldd, VariableName v) const {

          std::vector<int> qvars;
	  // num_of_vars is shared by all ldd's
	  qvars.reserve (num_of_vars()-1);
          for (auto p: m_var_map.left)
            if (!(p.first == v))
	      qvars.push_back (get_var_dim (p.first));

  	  ldd =  lddPtr (get_ldd_man(),
			 Ldd_MvExistAbstract(get_ldd_man(), &*ldd,
					     &qvars[0], qvars.size()));
	}

        LddNodePtr join (LddNodePtr v1, LddNodePtr v2) {
          return lddPtr (get_ldd_man(), Ldd_Or (get_ldd_man(), &*v1, &*v2));
        }

        /** return term for variable v, neg for negation of variable */
        linterm_t term_from_var(VariableName v, bool neg = false)
        {
          int dim = get_var_dim (v);
          int sgn = neg ? -1 : 1;
          linterm_t term =
              Ldd_GetTheory (get_ldd_man())->
	    create_linterm_sparse_si (&dim, &sgn, 1);
          return term;
        }

        /** return term from SPECIAL variable $0 */
        linterm_t term_from_special_var(bool neg = false)
        {
          int dim = 0; // reserved for SPECIAL variable
	  int sgn = neg ? -1 : 1;
          return  Ldd_GetTheory (get_ldd_man())->
	    create_linterm_sparse_si (&dim, &sgn, 1);
        }

	void copy_term (VariableName v, boost::optional<VariableName> x)
	{
          if (is_top () || is_bottom ()) return ;

	  this->operator-=(v); // remove v before assigning new term

          linterm_t lhs = term_from_var (v);
          linterm_t rhs;

	  if (x)
	    rhs = term_from_var (*x);
	  else
	    rhs = term_from_special_var ();

          m_ldd = lddPtr(get_ldd_man(),
                         Ldd_TermCopy(get_ldd_man(), &(*m_ldd), lhs, rhs));
          Ldd_GetTheory (get_ldd_man())->destroy_term(lhs);
          Ldd_GetTheory (get_ldd_man())->destroy_term(rhs);

	  if (!x) {
	    // XXX: if we copy the SPECIAL var to v we forget the SPECIAL
	    //      var after we copy.
	    crab::CrabStats::count (getDomainName() + ".count.forget");
	    crab::ScopedCrabStats __st__(getDomainName() + ".forget");
	    int dim = 0; // SPECIAL variable is dim 0
	    m_ldd =  lddPtr (get_ldd_man(),
			     Ldd_ExistsAbstract (get_ldd_man(), &*m_ldd, dim));
	  }

	}

        /**
	 **  All the expressiveness about numerical operations with boxes
	 **  is limited to:
	 **     v := a * x + [k.lb(),k.ub()],
	 **     where a is a constant, k an interval and and x variable
	 **  Each numerical operation is reduced to this form.
	 */
        void apply_ldd (VariableName v, VariableName x, Number a, interval_t k) {
          if (is_top () || is_bottom ()) return ;

          linterm_t t = term_from_var (v);
          linterm_t r = term_from_var (x);

          constant_t c = mk_cst (a);

	  constant_t kmin = NULL;
	  constant_t kmax = NULL;
	  if (k.lb().is_finite ())
	    kmin = mk_cst (*(k.lb().number()));
	  if (k.ub().is_finite ())
	    kmax = mk_cst (*(k.ub().number()));

          m_ldd = lddPtr(get_ldd_man(),
                         Ldd_TermReplace(get_ldd_man(), &(*m_ldd),
					 t, r, c, kmin, kmax));

          Ldd_GetTheory (get_ldd_man())->destroy_term(t);
          Ldd_GetTheory (get_ldd_man())->destroy_term(r);
          Ldd_GetTheory (get_ldd_man())->destroy_cst(c);
	  if (kmin)
	    Ldd_GetTheory (get_ldd_man())->destroy_cst(kmin);
	  if (kmax)
	    Ldd_GetTheory (get_ldd_man())->destroy_cst(kmax);
        }

        void num_from_ldd_cst (constant_t cst, ikos::z_number& res) {
          mpq_class v;
          // XXX We know that the theory is tvpi, use its method direclty.
          tvpi_cst_set_mpq (v.get_mpq_t (), (tvpi_cst_t) cst);
          res = ikos::z_number (static_cast<mpz_class> (v));
        }

        void num_from_ldd_cst (constant_t cst, ikos::q_number& res) {
          mpq_class v;
          // XXX We know that the theory is tvpi, use its method direclty.
          tvpi_cst_set_mpq (v.get_mpq_t (), (tvpi_cst_t) cst);
	  res = ikos::q_number(v);
        }

        linear_expression_t expr_from_ldd_term (linterm_t term) {
          linear_expression_t e (0);
          for(size_t i = 0;i < (size_t) get_theory()->term_size (term); i++) {
	    Number k (0); // any value
            num_from_ldd_cst (get_theory()->term_get_coeff (term,i), k);
            VariableName v =  getVarName (get_theory()->term_get_var(term,i));
            e = e + (k * linear_expression_t (v));
          }
          return e;
        }

	linear_constraint<ikos::z_number,VariableName>
	cst_from_ldd_strict_cons (linear_expression<ikos::z_number,VariableName> l,
				  linear_expression<ikos::z_number,VariableName> r) {
	  // l < r <-> l+1 <= r
	  linear_expression<ikos::z_number,VariableName> e = l +1;
	  e = e - r;
	  return linear_constraint<ikos::z_number,VariableName>
	    (e, linear_constraint_t::INEQUALITY);
	}

	linear_constraint<ikos::q_number,VariableName>
	cst_from_ldd_strict_cons (linear_expression<ikos::q_number,VariableName> l,
				  linear_expression<ikos::q_number,VariableName> r) {
	  CRAB_WARN ("Crab does not support strict inequalities");
	  return linear_constraint<ikos::q_number,VariableName>::get_true();
	}

        linear_constraint_t cst_from_ldd_cons(lincons_t lincons) {

          linear_expression_t lhs =
	    expr_from_ldd_term(get_theory()->get_term(lincons));
	  Number rhs (0); // any value
          num_from_ldd_cst(get_theory()->get_constant(lincons), rhs);

          if (get_theory()->is_strict (lincons)) {
	    return cst_from_ldd_strict_cons (lhs, rhs);
          } else  {
            // lhs <= rhs
            linear_expression_t e = lhs - rhs;
            return linear_constraint_t(e, linear_constraint_t::INEQUALITY);
          }
        }

        // Pre: n is convex and cannot be bottom
        void to_lin_cst_sys_recur(LddNode* n,
				  linear_constraint_system_t& csts) {

          LddNode *N = Ldd_Regular (n);
          if (N == Ldd_GetTrue (get_ldd_man ())) return;

          auto cst = cst_from_ldd_cons (Ldd_GetCons(get_ldd_man (), N));

          if (Ldd_Regular (Ldd_T(N)) == Ldd_GetTrue (get_ldd_man ()) &&
              Ldd_Regular (Ldd_E(N)) == Ldd_GetTrue (get_ldd_man ()))
            csts += cst;
          else if (Ldd_Regular (Ldd_T(N)) == Ldd_GetTrue (get_ldd_man ()))
            csts += cst.negate ();
          else
            csts += cst;

          to_lin_cst_sys_recur (Ldd_T (N), csts);
          to_lin_cst_sys_recur (Ldd_E (N), csts);

        }

        // Create a new ldd representing the constraint e
        // where e can be one of
        //    c*x <= k | c*x == k | c*x != k
        //    and c is 1 or -1 and k is an integer constant
        typedef typename linear_constraint_t::kind_t kind_t;

        LddNodePtr gen_unit_constraint (Number coef, linterm_t term, kind_t kind,
					Number k) {

          assert (coef == 1 || coef == -1);

          if (kind == kind_t::EQUALITY) {  // x == k <-> x<=k and !(x<k)
            // x<=k
            constant_t c1 = mk_cst (k);
	    // XXX: make copy of term so that we can free memory using
	    // destroy_lincons without having double free
	    linterm_t copy_term = get_theory()->dup_term (term);
            lincons_t cons1 = get_theory()->create_cons (copy_term, 0 /*non-strict*/, c1);
            LddNodePtr n1 = lddPtr (get_ldd_man(),
				    get_theory()->to_ldd (get_ldd_man(), cons1));
            // !(x<k)
            constant_t c2 = mk_cst (k);
            lincons_t cons2 = get_theory()->create_cons (term, 1 /*strict*/, c2);
            LddNodePtr n2 = lddPtr(get_ldd_man(),
				   Ldd_Not (get_theory()->to_ldd (get_ldd_man(), cons2)));
            get_theory()->destroy_lincons (cons1);
            get_theory()->destroy_lincons (cons2);
	    return lddPtr (get_ldd_man (),
			   Ldd_And (get_ldd_man(), &*n1, &*n2));
          } else if (kind == kind_t::INEQUALITY) {
            // case 1:  x <= k
            // case 2: -x <= k
            constant_t c = mk_cst (k);
            lincons_t cons = get_theory()->create_cons (term, 0 /*non-strict*/, c);
            LddNodePtr n = lddPtr (get_ldd_man(),
				   get_theory()->to_ldd (get_ldd_man(), cons));
            get_theory()->destroy_lincons (cons);
	    return n;
          } else { // assert (kind == kind_t::DISEQUALITY)
            // case 1:  x != k  <->  x < k OR  x > k <->  x < k OR -x < -k
            // case 2: -x != k  <-> -x < k OR -x > k <-> -x < k OR  x < -k
            constant_t c1 = mk_cst (k);
	    // XXX: make copy of term so that we can free memory using
	    // destroy_lincons without having double free
	    linterm_t copy_term = get_theory()->dup_term (term);
            lincons_t cons1 = get_theory()->create_cons(copy_term, 1 /*strict*/, c1);
            LddNodePtr n1 = lddPtr (get_ldd_man(),
				    get_theory()->to_ldd (get_ldd_man(), cons1));
            constant_t c2 = mk_cst (-k);
            lincons_t cons2 = get_theory()->create_cons (term, 1 /*strict*/, c2);
            LddNodePtr n2 = lddPtr (get_ldd_man(),
				    get_theory()->to_ldd (get_ldd_man(), cons2));
            LddNodePtr n3 = lddPtr (get_ldd_man (),
				    Ldd_Or (get_ldd_man(), &*n1, &*n2));
            get_theory()->destroy_lincons (cons1);
            get_theory()->destroy_lincons (cons2);
	    return n3;
          }
        }

	LddNodePtr gen_unit_constraint(Number coef, variable_t var, kind_t kind,
				       Number k) {
	  linterm_t term = term_from_var (var.name (), (coef == 1 ? false : true));
	  return gen_unit_constraint(coef, term, kind, k);
	}

	LddNodePtr gen_unit_constraint(Number coef, kind_t kind, Number k) {
	  linterm_t term = term_from_special_var ((coef == 1 ? false : true));
	  return gen_unit_constraint(coef, term, kind, k);
	}

        void add_unit_constraint(Number coef, variable_t x, kind_t kind, Number k) {
	  LddNodePtr n = gen_unit_constraint (coef, x, kind, k);
	  m_ldd = lddPtr (get_ldd_man(), Ldd_And (get_ldd_man(), &*m_ldd, &*n));
        }

	// Given a constraint a1*x1 + ... + an*xn <= k and pivot xi,
        // it computes the interval:
	//  k - intv (a1*x1 + ... + ai-1*xi-1 + ai+1*xi+1 + ... an*xn)
	interval_t compute_residual(linear_constraint_t cst, variable_t pivot) {
	  interval_t residual(cst.constant());
	  for (typename linear_constraint_t::iterator it = cst.begin();
	       it != cst.end(); ++it) {
	    variable_t v = it->second;
	    if (v.index() != pivot.index()) {
	      residual =
		residual - (interval_t (it->first) * this->operator[](v.name()));
	    }
	  }
	  return residual;
	}

	void intvcst_from_lin_const (linear_constraint_t cst,
				     linear_constraint_system_t &intvcsts) {
	  for (typename linear_constraint_t::iterator it = cst.begin();
	       it != cst.end(); ++it) {
	    number_t c = it->first;
	    variable_t pivot = it->second;
	    interval_t rhs = compute_residual(cst, pivot) / interval_t(c);

	    if (!(rhs.lb().is_finite () && rhs.ub().is_finite ())) continue;

	    linear_expression_t term = (c*pivot);
	    if (auto k = rhs.singleton()) {
	      linear_expression_t e = term - *k;
	      intvcsts += linear_constraint_t(e,cst.kind());
	    } else {
	      number_t min = *(rhs.lb().number());
	      number_t max = *(rhs.ub().number());
	      switch (cst.kind()) {
	      case kind_t::EQUALITY: {
		intvcsts += linear_constraint_t (term >= min);
		intvcsts += linear_constraint_t (term <= max);
		break;
	      }
	      case kind_t::INEQUALITY: {
		intvcsts += linear_constraint_t(term <= max);
		break;
	      }
	      default:
		assert (cst.kind() == kind_t::DISEQUATION);
		// TODO: pivot != [min,max] <---> pivot < min or pivot > max
	      }
	    }
	  }
	}

        boxes_domain_ (LddNodePtr ldd): m_ldd (ldd) {
	  // TODO: one of the template parameters of boxes should be
	  // an user option to choose when the size of the ldd should
	  // be reduced.
          const int CST_FACTOR = 10; /* JN: some magic constant factor */
          int threshold = num_of_vars () * CST_FACTOR;
	  unsigned num_paths = Ldd_PathSize (NULL, &*m_ldd);
	  if (num_paths > threshold) {
	    convex_approx ();
	    CRAB_WARN ("ldd size was too large: ", num_paths, ". Made ldd convex.");
	  }
        }

	void to_disjunctive_linear_constraint_system_aux
	(LddManager *ldd, LddNode *n,
	 disjunctive_linear_constraint_system_t &e,
	 std::vector<int> &list) {
	    /**
	     * the latest negative constraint to be printed.
	     * is NULL if there isn't one
	     */
	    lincons_t negc = nullptr;

	    DdNode *N = Cudd_Regular (n);

	    if (cuddIsConstant (N)) {
	      /* n == N here implies that n is one */
	      if (n == N) {

		linear_constraint_system_t csts;

		/* for each level */
		for (int i = 0; i < ldd->cudd->size; i++) {
		  /* let p be the index of level i */
		  int p = ldd->cudd->invperm [i];
		  /* let v be the value of p */
		  int v = list [p];

		  /* skip don't care */
		  if (v == 2) continue;

		  if (v == 0 && ldd->ddVars [p] != nullptr)
		    {
		      lincons_t c;
		      c = ldd->theory->negate_cons (ldd->ddVars [p]);

		      if (negc != nullptr) {
			/* consider negative constraint if it is not implied
			   by c
			*/
			if (!ldd->theory->is_stronger_cons (c, negc)) {
			  csts += cst_from_ldd_cons (negc);
			}
			ldd->theory->destroy_lincons (negc);
		      }

		      /* store the current constraint to be considered later */
		      negc = c;
		      continue;
		    }

		  /* if there is a negative constraint waiting to be
		     considered, conjoin it now
		  */
		  if (negc != nullptr) {
		    csts += cst_from_ldd_cons (negc);
		    ldd->theory->destroy_lincons (negc);
		    negc = nullptr;
		  }

		  /* if v is not a don't care but p does not correspond to a
		   * constraint, consider it as a Boolean variable */
		  if (v != 2 && ldd->ddVars [p] == nullptr)  {
		    // XXX: I don't know what to do here
		    //fprintf (stderr, "%sb%d", (v == 0 ? "!" : " "), p);
		  }
		  /* v is true */
		  else if (v == 1) {
		    csts += cst_from_ldd_cons (ldd->ddVars [p]);
		  }
		} // end for

		/* if there is a constraint waiting to be considered, do it
		   now */
		if (negc != nullptr) {
		  csts += cst_from_ldd_cons (negc);
		  ldd->theory->destroy_lincons (negc);
		  negc = nullptr;
		}
		e += csts;
	      }
	    }
	    else {
	      DdNode *Nv = Cudd_NotCond (cuddT(N), N != n);
	      DdNode *Nnv = Cudd_NotCond (cuddE(N), N != n);
	      int index = N->index;
	      list[index] = 0;
	      to_disjunctive_linear_constraint_system_aux(ldd, Nnv, e, list);
	      list[index] = 1;
	      to_disjunctive_linear_constraint_system_aux(ldd, Nv, e, list);
	      list[index] = 2;
	    }
	    return;
       }

        // return x >= 1 or $0 >=1 if !x
	inline LddNode* gen_true_var (boost::optional<VariableName> x)
	{ // x >=1 <--> -x <= -1
	  if (x) {
	    auto r = gen_unit_constraint (Number(-1), variable_t(*x),
					  linear_constraint_t::INEQUALITY,
					  Number(-1));
	    return &*r;
	  }
	  else {
	    auto r = gen_unit_constraint (Number(-1),
					  linear_constraint_t::INEQUALITY,
					  Number(-1));
	    return &*r;
	  }
	}

	// return x <= 0 or $0 <= 0 if !x
	inline LddNode* gen_false_var (boost::optional<VariableName> x)
	{
	  if (x) {
	    auto r = gen_unit_constraint (Number(1), variable_t(*x),
					  linear_constraint_t::INEQUALITY,
					  Number(0));
	    return &*r;
	  } else {
	    auto r = gen_unit_constraint (Number(1),
					  linear_constraint_t::INEQUALITY,
					  Number(0));
	    return &*r;
	  }
	}

	// return Ite(y>=1 Op z>=1, x>=1, x <= 0)
	LddNode* gen_binary_bool(bool_operation_t op,
				 boost::optional<VariableName> x,
				 VariableName y, VariableName z)
	{
	  switch (op) {
	  case OP_BAND:
	    return Ldd_Ite(get_ldd_man (),
			   Ldd_And(get_ldd_man(), gen_true_var(y), gen_true_var(z)),
			   gen_true_var(x), gen_false_var(x));
	    break;
	  case OP_BOR:
	    return Ldd_Ite(get_ldd_man (),
			   Ldd_Or(get_ldd_man(), gen_true_var(y), gen_true_var(z)),
			   gen_true_var(x), gen_false_var(x));
	    break;
	  case OP_BXOR:
	    return Ldd_Ite(get_ldd_man (),
			   Ldd_Xor(get_ldd_man(), gen_true_var(y), gen_true_var(z)),
			   gen_true_var(x), gen_false_var(x));
	    break;
	  default: CRAB_ERROR ("Unknown boolean operator");
	  }
	}

        LddNodePtr getLdd () {
          return m_ldd;
        }

        VariableName getVarName (int v) const {
          auto it = m_var_map.right.find (v);
          if (it != m_var_map.right.end ())
             return it->second;
          else {
             CRAB_ERROR ("Index ", v, " cannot be mapped back to a variable name");
          }
        }

       public:

        boxes_domain_()
	  : ikos::writeable(),
	  m_ldd (lddPtr (get_ldd_man(), Ldd_GetTrue (get_ldd_man()))) {}

        ~boxes_domain_ () {
          // DdManager *cudd = nullptr;
          // theory_t *theory = nullptr;
          // if (m_ldd_man)  {
	  //   cudd = Ldd_GetCudd (m_ldd_man);
	  //   theory = Ldd_GetTheory (m_ldd_man);
	  //   Ldd_Quit (m_ldd_man);
	  // }
          // if (theory) tvpi_destroy_theory(theory);
          // if (cudd) Cudd_Quit(cudd);
        }

        static boxes_domain_t top() {
	  return boxes_domain_t (lddPtr (get_ldd_man(),
					 Ldd_GetTrue (get_ldd_man())));
        }

        static boxes_domain_t bottom() {
	  return boxes_domain_t (lddPtr (get_ldd_man(),
					 Ldd_GetFalse (get_ldd_man())));
        }

        boxes_domain_ (const boxes_domain_t& other):
	  ikos::writeable(), m_ldd (other.m_ldd) {
          crab::CrabStats::count (getDomainName () + ".count.copy");
          crab::ScopedCrabStats __st__(getDomainName() + ".copy");
        }

        boxes_domain_t& operator=(const boxes_domain_t& other) {
          crab::CrabStats::count (getDomainName() + ".count.copy");
          crab::ScopedCrabStats __st__(getDomainName() + ".copy");
          if (this != &other)
            m_ldd = other.m_ldd;
          return *this;
        }

        bool is_bottom() const {
          return &*m_ldd == Ldd_GetFalse (get_ldd_man());
        }

        bool is_top() const {
          return &*m_ldd == Ldd_GetTrue (get_ldd_man());
        }

        bool operator<=(boxes_domain_t other) {
          crab::CrabStats::count (getDomainName() + ".count.leq");
          crab::ScopedCrabStats __st__(getDomainName() + ".leq");

          bool res = Ldd_TermLeq (get_ldd_man(), &(*m_ldd), &(*other.m_ldd));

          // CRAB_LOG ("boxes",
          //           crab::outs() << "Check if " <<  *this << " <= " <<  other
          //                     <<  " ---> " <<  res <<"\n";);
          return res;
        }

        void operator|=(boxes_domain_t other) {
          *this = *this | other;
        }

        boxes_domain_t operator|(boxes_domain_t other) {
          crab::CrabStats::count (getDomainName() + ".count.join");
          crab::ScopedCrabStats __st__(getDomainName() + ".join");

          return boxes_domain_t (join (m_ldd, other.m_ldd));
        }

        boxes_domain_t operator&(boxes_domain_t other) {
          crab::CrabStats::count (getDomainName() + ".count.meet");
          crab::ScopedCrabStats __st__(getDomainName() + ".meet");

          return boxes_domain_t (lddPtr (get_ldd_man(),
                                         Ldd_And (get_ldd_man(),
						  &*m_ldd, &*other.m_ldd)));
        }

        boxes_domain_t operator||(boxes_domain_t other) {
          crab::CrabStats::count (getDomainName() + ".count.widening");
          crab::ScopedCrabStats __st__(getDomainName() + ".widening");

          // It is not necessarily true that the new value is bigger
          // than the old value so we apply
          // widen(old, new) = widen (old, (join (old,new)))
          LddNodePtr v = join (m_ldd, other.m_ldd);
          LddNodePtr w = lddPtr (get_ldd_man (),
                                 Ldd_BoxWiden2 (get_ldd_man (), &*m_ldd, &*v));
          #if 1
          /** ensure that 'w' is only the fronteer of the compution.
              Not sure whether this is still a widening though
          */
          w = lddPtr (get_ldd_man (),
                      Ldd_And (get_ldd_man (), &*w, Ldd_Not (&*m_ldd)));
          /** ensure the output is at least as big as newV */
          w = lddPtr (get_ldd_man (), Ldd_Or (get_ldd_man (), &*w, &*other.m_ldd));
          #endif
          boxes_domain_t res (w);

          CRAB_LOG ("boxes",
                    crab::outs() << "Widening " <<  *this << " and " <<  other
                                 <<  "=" << res <<"\n";);
          return res;
        }

        template<typename Thresholds>
        boxes_domain_t widening_thresholds (boxes_domain_t other,
                                            const Thresholds & /*ts*/) {
          //CRAB_WARN (" boxes widening operator with thresholds not implemented");
          return (*this || other);
        }

        boxes_domain_t operator&& (boxes_domain_t other) {
          crab::CrabStats::count (getDomainName() + ".count.narrowing");
          crab::ScopedCrabStats __st__(getDomainName() + ".narrowing");

          boxes_domain_t res (*this & other);
          //CRAB_WARN (" boxes narrowing operator replaced with meet");
          CRAB_LOG ("boxes",
                    crab::outs() << "Narrowing " << *this << " and " << other << "="
		                 << res <<"\n";);
          return res;
        }

        void operator-=(VariableName var) {
          crab::CrabStats::count (getDomainName() + ".count.forget");
          crab::ScopedCrabStats __st__(getDomainName() + ".forget");

          if (is_bottom ()) return;

          int id = get_var_dim (var);
          m_ldd =  lddPtr (get_ldd_man(),
                           Ldd_ExistsAbstract (get_ldd_man(), &*m_ldd, id));
        }

        // remove all variables [begin,...end)
        template<typename Iterator>
        void forget (Iterator begin, Iterator end) {
          if (is_bottom ()) return;

	  std::vector<int> qvars;
	  qvars.reserve (std::distance(begin, end));
          for (auto v: boost::make_iterator_range (begin, end))
	    qvars.push_back (get_var_dim (v));

  	  m_ldd =  lddPtr (get_ldd_man(),
			   Ldd_MvExistAbstract (get_ldd_man(), &*m_ldd,
						&qvars[0], qvars.size()));
        }

        // dual of forget: remove all variables except [begin,...end)
        template<typename Iterator>
        void project (Iterator begin, Iterator end) {
          crab::CrabStats::count (getDomainName() + ".count.project");
          crab::ScopedCrabStats __st__(getDomainName() + ".project");

          if (is_bottom ()) return;

          std::set<VariableName> s1,s2,s3;
          for (auto p: m_var_map.left) s1.insert (p.first);
          s2.insert (begin, end);
          boost::set_difference (s1,s2,std::inserter (s3, s3.end ()));
          forget (s3.begin (), s3.end ());
        }

        void operator+=(linear_constraint_t cst)
        {
          crab::CrabStats::count (getDomainName() + ".count.add_constraints");
          crab::ScopedCrabStats __st__(getDomainName() + ".add_constraints");

          if (is_bottom () || cst.is_tautology ())
            return;

          if (cst.is_contradiction ())  {
            m_ldd = lddPtr (get_ldd_man(), Ldd_GetFalse (get_ldd_man()));
            return;
          }

          linear_expression_t exp = cst.expression();
          unsigned int size = exp.size ();
          if (size == 0) return; // this should not happen
          else if (size == 1) {
            auto it = exp.begin();
            Number cx = it->first;
            if (cx == 1 || cx == -1) {
              Number k = -exp.constant();
              variable_t x = it->second;
              add_unit_constraint (cx, x, cst.kind (), k);
            } else {
	      // XXX: this is not possible when program is originated by clang/llvm
              CRAB_WARN ("non-unit coefficients not implemented in boxes");
	    }
          }
          else if (size >= 2) {
	    linear_constraint_system_t intvcsts;
	    intvcst_from_lin_const (cst, intvcsts);
	    crab::outs () << cst << " converted to interval constraints : "
		          << intvcsts << "\n";
	    this->operator+=(intvcsts);
          }

          CRAB_LOG("boxes",
		   crab::outs() << "Assume(" << cst << ") --> " <<  *this <<"\n";);
        }

        void operator+=(linear_constraint_system_t csts) {
          if (is_bottom ()) return;
          for (auto cst : csts) operator += (cst);
        }

        void set (VariableName v, interval_t ival) {
          crab::CrabStats::count (getDomainName() + ".count.assign");
          crab::ScopedCrabStats __st__(getDomainName() + ".assign");

          if (is_bottom ()) return ;

          constant_t kmin = NULL, kmax = NULL;
          if (boost::optional <Number> l = ival.lb ().number ())
            kmin = mk_cst (*l);
          if (boost::optional <Number> u = ival.ub ().number ())
            kmax = mk_cst (*u);

          linterm_t t = term_from_var(v);
          m_ldd = lddPtr(get_ldd_man(),
                         Ldd_TermReplace(get_ldd_man(), &(*m_ldd),
					 t, NULL, NULL, kmin, kmax));

          if (kmin) get_theory()->destroy_cst(kmin);
          if (kmax) get_theory()->destroy_cst(kmax);
          get_theory()->destroy_term(t);
        }

        interval_t operator[](VariableName v) {

          crab::CrabStats::count (getDomainName() + ".count.to_intervals");
          crab::ScopedCrabStats __st__(getDomainName() + ".to_intervals");

          if (is_bottom ())
            return interval_t::bottom ();
          if (is_top ())
            return interval_t::top ();

	  LddNodePtr ldd (m_ldd);
	  project (ldd, v);

          // convert to interval domain
          linear_constraint_system_t csts;
	  ldd = convex_approx (ldd);
          to_lin_cst_sys_recur (&*ldd, csts);
          interval_domain_t intv;
          for (auto cst: csts) {intv += cst;}

          // finally project onto v in the interval domain
          return intv[v];
        }

	// x := e
        void assign (VariableName x, linear_expression_t e) {
          crab::CrabStats::count (getDomainName() + ".count.assign");
          crab::ScopedCrabStats __st__(getDomainName() + ".assign");

          if (is_bottom ())
            return;

          if (e.is_constant ()) {
            constant_t c = mk_cst(e.constant ());
            linterm_t t = term_from_var (x);
            m_ldd = lddPtr(get_ldd_man(),
                           Ldd_TermReplace(get_ldd_man(), &(*m_ldd),
					   t, NULL, NULL, c, c));
            get_theory()->destroy_cst (c);
            get_theory()->destroy_term(t);
          } else if (boost::optional<variable_t> v = e.get_variable()){
            VariableName y = (*v).name();
            if (!(x==y)) copy_term(x,y); //apply_ldd (x, y, 1, 0);
          } else {
	    // XXX: this is not possible when program originated by clang/llvm
	    CRAB_WARN("x:= linexp not implemented in boxes");
	    this->operator-=(x);
          }

          CRAB_LOG("boxes",
		   crab::outs() << "--- " << x << ":=" << e << "\n"
		                << *this <<"\n";);
        }

	// x := y op k
        void apply (operation_t op, VariableName x, VariableName y, Number k) {
          crab::CrabStats::count (getDomainName() + ".count.apply");
          crab::ScopedCrabStats __st__(getDomainName() + ".apply");

          if (is_bottom ())
            return;

          switch(op){
            case OP_ADDITION:
	      apply_ldd (x, y, 1, k);
	      break;
            case OP_SUBTRACTION:
	      apply_ldd (x, y, 1, -k);
	      break;
	    case OP_MULTIPLICATION:
	      apply_ldd (x, y, k, number_t(0));
	      break;
	    case OP_DIVISION: {
	      // XXX: loss of precision
	      interval_t yi = operator[](y);
	      interval_t zi (k);
	      interval_t xi = yi / zi;
	      set (x, xi);
	      break;
	    }
	    default: CRAB_ERROR ("Boxes unreachable");
          }

          CRAB_LOG("boxes",
                   crab::outs() << "--- apply " << x << " := " << y << " " <<  op
                             << " " << k << " --- " <<  *this <<"\n";);
        }

	// x := x op k
        void apply(operation_t op, VariableName x, Number k) {
          crab::CrabStats::count (getDomainName() + ".count.apply");
          crab::ScopedCrabStats __st__(getDomainName() + ".apply");

          if (is_bottom ())
            return;

          switch(op){
            case OP_ADDITION:
              apply_ldd (x, x, 1, k);
              break;
            case OP_SUBTRACTION:
              apply_ldd (x, x, 1, -k);
              break;
            case OP_MULTIPLICATION:
              apply_ldd (x, x, k, number_t(0));
              break;
  	    case OP_DIVISION: {
	      // XXX: loss of precision
	      interval_t yi = operator[](x);
	      interval_t zi (k);
	      interval_t xi = yi / zi;
	      set(x, xi);
	      break;
	    }
          }
          CRAB_LOG("boxes",
                   crab::outs() << "--- apply " << x << " := " << x << " "
		                << op << " " << k <<  "---" <<  *this <<"\n";);
        }

	// x := y op z
        void apply(operation_t op, VariableName x, VariableName y, VariableName z) {
          crab::CrabStats::count (getDomainName() + ".count.apply");
          crab::ScopedCrabStats __st__(getDomainName() + ".apply");

          if (is_bottom ())
            return;

	  // --- if z is a singleton we do not lose precision
	  interval_t zi = operator[](z);
	  if (auto k = zi.singleton ()) {
	    apply (op, x, y, *k);
	    return;
	  }

	  // --- if y or z is top then we give up
	  interval_t yi = operator[](y);
	  if (yi.is_top () || zi.is_top()) {
	    this->operator-=(x);
	    return;
	  }

	  // --- if y is a singleton we do not lose precision
	  if (auto k = yi.singleton()) {
	    if (op == OP_ADDITION || op == OP_MULTIPLICATION) {
	      apply (op, x, z, *k);
	      return;
	    } else if (op == OP_SUBTRACTION) {
	      // x = yi - z <--> x = -z + [yi.lb,yi.ub]
	      apply_ldd (x, z, -1, *k);
	      return;
	    }
	  }

	  // -- We need to abstract either y or z and lose some
	  // precision. We abstract the one with the smaller interval.
	  if (op == OP_ADDITION) {
	    if (yi <= zi) { // abstract y
	      // x = yi + z <--> x = z + yi
	      apply_ldd (x, z, 1, yi);
	    } else {        // abstract z
	      // x = y + zi
	      apply_ldd (x, y, 1, zi);
	    }
	    return;
	  }

	  // -- We need to abstract either y or z and lose some
	  // precision. We abstract the one with the smaller interval.
	  if (op == OP_SUBTRACTION) {
	    if (yi <= zi) { // abstract y
	      // x = yi - z <-->  x = -z + yi
	      apply_ldd (x, z, -1, yi);
	    } else {        // abstract z
	      // x = y - zi
	      apply_ldd (x, y, 1, zi * number_t(-1));
	    }
	    return;
	  }

	  // --- we go to intervals and lose precision
	  switch (op) {
	    case OP_MULTIPLICATION: {
	      interval_t xi = yi * zi;
	      set (x,xi);
	      break;
	    }
	    case OP_DIVISION: {
	      interval_t xi = yi / zi;
	      set (x,xi);
	      break;
	    }
	    default: CRAB_ERROR("unreachable");
	  }
	}

        // bitwise_operators_api
        void apply(conv_operation_t op,
		   VariableName x, VariableName y, unsigned width) {
          // since reasoning about infinite precision we simply assign and
          // ignore the width.
          assign(x, linear_expression_t(y));
        }

        void apply(conv_operation_t op, VariableName x, Number k, unsigned width) {
          // since reasoning about infinite precision we simply assign
          // and ignore the width.
          assign(x, k);
        }

        void apply(bitwise_operation_t op,
		   VariableName x, VariableName y, VariableName z) {
          crab::CrabStats::count (getDomainName() + ".count.apply");
          crab::ScopedCrabStats __st__(getDomainName() + ".apply");

          if (is_bottom ())
            return;

          // Convert to intervals and perform the operation
          interval_t yi = operator[](y);
          interval_t zi = operator[](z);
          interval_t xi = interval_t::bottom();
          switch (op) {
            case OP_AND:  xi = yi.And(zi); break;
            case OP_OR:   xi = yi.Or(zi) ; break;
            case OP_XOR:  xi = yi.Xor(zi); break;
            case OP_SHL:  xi = yi.Shl(zi); break;
            case OP_LSHR: xi = yi.LShr(zi); break;
            case OP_ASHR: xi = yi.AShr(zi); break;
          }
          set(x, xi);
        }

        void apply(bitwise_operation_t op,
		   VariableName x, VariableName y, Number k) {
          crab::CrabStats::count (getDomainName() + ".count.apply");
          crab::ScopedCrabStats __st__(getDomainName() + ".apply");

          if (is_bottom ())
            return;

          // Convert to intervals and perform the operation
          interval_t yi = operator[](y);
          interval_t zi(k);
          interval_t xi = interval_t::bottom();
          switch (op) {
            case OP_AND:  xi = yi.And(zi); break;
            case OP_OR:   xi = yi.Or(zi) ; break;
            case OP_XOR:  xi = yi.Xor(zi); break;
            case OP_SHL:  xi = yi.Shl(zi); break;
            case OP_LSHR: xi = yi.LShr(zi); break;
            case OP_ASHR: xi = yi.AShr(zi); break;
          }
          set(x, xi);
        }

        // division_operators_api
        void apply(div_operation_t op,
		   VariableName x, VariableName y, VariableName z) {
          crab::CrabStats::count (getDomainName() + ".count.apply");
          crab::ScopedCrabStats __st__(getDomainName() + ".apply");

          if (is_bottom ())
            return;

          if (op == OP_SDIV){
            apply(OP_DIVISION, x, y, z);
          }
          else{
            // Convert to intervals and perform the operation
            interval_t yi = operator[](y);
            interval_t zi = operator[](z);
            interval_t xi = interval_t::bottom();
            switch (op) {
              case OP_UDIV: xi = yi.UDiv(zi); break;
              case OP_SREM: xi = yi.SRem(zi); break;
              case OP_UREM: xi = yi.URem(zi); break;
              default: CRAB_ERROR("Boxes: unreachable");
            }
            set(x, xi);
          }
        }

        void apply(div_operation_t op, VariableName x, VariableName y, Number k) {
          crab::CrabStats::count (getDomainName() + ".count.apply");
          crab::ScopedCrabStats __st__(getDomainName() + ".apply");

          if (is_bottom ())
            return;

          if (op == OP_SDIV){
            apply(OP_DIVISION, x, y, k);
          }
          else{
            // Convert to intervals and perform the operation
            interval_t yi = operator[](y);
            interval_t zi(k);
            interval_t xi = interval_t::bottom();
            switch (op) {
              case OP_UDIV: xi = yi.UDiv(zi); break;
              case OP_SREM: xi = yi.SRem(zi); break;
              case OP_UREM: xi = yi.URem(zi); break;
              default: CRAB_ERROR("Boxes: unreachable");
            }
            set(x, xi);
          }
        }

	////////
	//// boolean_operators_api
	////////

	void assign_bool_cst (VariableName lhs, linear_constraint_t cst) override
	{
          crab::CrabStats::count (getDomainName() + ".count.assign_bool_cst");
          crab::ScopedCrabStats __st__(getDomainName() + ".assign_bool_cst");

	  if (is_bottom ()) return;

	  // XXX: lhs should not appear in cst so we can remove lhs
	  // without losing precision
	  this->operator-=(lhs);

	  if (cst.is_tautology ()) {
	    // m_ldd &= lhs >= 1
	    m_ldd = lddPtr (get_ldd_man(),
			    Ldd_And (get_ldd_man(), &*m_ldd, gen_true_var(lhs)));
	  } else if (cst.is_contradiction ()) {
	    // m_ldd &= lhs <= 0
	    m_ldd = lddPtr (get_ldd_man(),
			    Ldd_And (get_ldd_man(), &*m_ldd, gen_false_var(lhs)));
	  } else {
	    linear_expression_t exp = cst.expression();
	    unsigned int size = exp.size ();
	    if (size == 0) return; // this should not happen
	    else if (size == 1) {
	      auto it = exp.begin();
	      Number cx = it->first;
	      if (cx == 1 || cx == -1) {
		Number k = -exp.constant();
		variable_t vx = it->second;
		// m_ldd &= ite (cst, lhs >=1, lhs<=0);
		m_ldd = lddPtr (get_ldd_man (),
				Ldd_And (get_ldd_man(), &*m_ldd,
				   Ldd_Ite(get_ldd_man (),
					   &*(gen_unit_constraint (cx, vx, cst.kind (),k)),
					   gen_true_var(lhs), gen_false_var(lhs))));

	      } else {
		CRAB_WARN ("non-unit coefficients not implemented in boxes");
	      }
	    } else if (size >= 2) {
	      CRAB_WARN ("non-unary constraints not implemented in boxes");
	    }
	  }

	  CRAB_LOG("boxes",
		   crab::outs () << lhs << ":=" << cst << "=\n" << *this << "\n");
	}

	void assign_bool_var (VariableName x, VariableName y) override {
          crab::CrabStats::count (getDomainName() + ".count.assign_bool_var");
          crab::ScopedCrabStats __st__(getDomainName() + ".assign_bool_var");

	  copy_term (x, y);

	  CRAB_LOG("boxes",
		   crab::outs()  << x << ":=" << y << "=\n" << *this << "\n");
	}

	void apply_binary_bool(bool_operation_t op, VariableName x,
			       VariableName y, VariableName z) override {
          crab::CrabStats::count (getDomainName() + ".count.apply_bin_bool");
          crab::ScopedCrabStats __st__(getDomainName() + ".apply_bin_bool");

	  // XXX: if *lhs is null then it represents the SPECIAL
	  // variable $0.
	  boost::optional<VariableName> lhs;

	  if (!(x == y) && !(x == z)) {
	    lhs = boost::optional<VariableName> (x);
	    // XXX: x does not appear on the rhs so we can remove it
	    // without losing precision.
	    this->operator-=(x);
	  }

	  m_ldd = lddPtr (get_ldd_man (),
			  Ldd_And(get_ldd_man(), &*m_ldd,
				  gen_binary_bool(op, lhs, y, z)));

	  if ((x == y) || (x == z)) {
	    // XXX: if we are here we added ite(y op z, $0 >=1, $0 <= 0);
	    // so we still need to assign $0 to x:
	    copy_term(x, boost::optional<VariableName>());
	  }

	  CRAB_LOG("boxes",
		   crab::outs () << x << ":=" << y << " " << op << " " << z << "=\n"
     		                 << *this << "\n");

	}

	void assume_bool (VariableName x, bool is_negated) override {
          crab::CrabStats::count (getDomainName() + ".count.assume_bool");
          crab::ScopedCrabStats __st__(getDomainName() + ".assume_bool");

	  m_ldd = lddPtr (get_ldd_man(),
			  Ldd_And (get_ldd_man(), &*m_ldd,
				   ((is_negated) ?
				    gen_false_var (x): gen_true_var (x))));

	  CRAB_LOG("boxes",
		 if (!is_negated)
		   crab::outs () << "assume(" << x << ")=" << *this << "\n";
		 else
		   crab::outs () << "assume(not(" << x << "))=" << *this << "\n";);
	}

        linear_constraint_system_t to_linear_constraint_system () {
          linear_constraint_system_t csts;

          if(is_bottom ()) {
            csts += linear_constraint_t::get_false();
          }
          else if(is_top ()) {
            csts += linear_constraint_t::get_true();
          } else {
	    // --- produce convex approximation
	    LddNodePtr v = convex_approx (m_ldd);
	    // --- extract linear inequalities from the convex ldd
	    to_lin_cst_sys_recur(&*v, csts);
	  }
          return csts;
        }

	disjunctive_linear_constraint_system_t
	to_disjunctive_linear_constraint_system () {
	  LddManager *ldd_man = getLddManager (m_ldd);

	  std::vector<int> list;
	  list.reserve (ldd_man->cudd->size);
	  for (int i=0; i < ldd_man->cudd->size; i++) list.push_back(2);

	  disjunctive_linear_constraint_system_t r;
	  to_disjunctive_linear_constraint_system_aux (ldd_man, m_ldd.get (),
						       r, list);
	  return r;
	}

	void write (crab_os& o) {
          if (is_top ()) {
            o << "{}";
	  } else if (is_bottom ())  {
            o << "_|_";
	  } else  {
	    auto r = to_disjunctive_linear_constraint_system ();
	    o << r;
          }
        }

        static std::string getDomainName ()
	{ return "Boxes"; }
      };

     template<typename N, typename V, size_t S>
     LddManager* boxes_domain_<N,V,S>::m_ldd_man = nullptr;

     template<typename N, typename V, size_t S>
     typename boxes_domain_<N,V,S>::var_map_t boxes_domain_<N,V,S>::m_var_map;

     template<typename Number, typename VariableName, size_t LddSize>
     class domain_traits <boxes_domain_<Number,VariableName, LddSize> > {
      public:
       typedef boxes_domain_<Number, VariableName, LddSize> boxes_domain_t;

       template<class CFG>
       static void do_initialization (CFG cfg) { }

       static void normalize (boxes_domain_t& inv) { }

       template <typename Iter>
       static void forget(boxes_domain_t& inv, Iter it, Iter end) {
         inv.forget (it, end);
       }

       template <typename Iter>
       static void project(boxes_domain_t& inv, Iter it, Iter end) {
         inv.project (it, end);
       }

       static void expand(boxes_domain_t& inv, VariableName x, VariableName new_x) {
         CRAB_WARN ("expand operation not implemented in boxes");
       }

     };

    #if 0
     // Without copy-on-write
    template<typename Number, typename VariableName, size_t LddSize=3000>
    using boxes_domain = boxes_domain_<Number,VariableName,LddSize>;
    #else
    // Quick wrapper which uses shared references with copy-on-write.
    template<class Number, class VariableName, size_t LddSize=3000>
    class boxes_domain: public writeable,
               public numerical_domain<Number, VariableName >,
               public bitwise_operators<Number,VariableName >,
               public division_operators<Number, VariableName >,
               public array_operators<Number, VariableName >,
	       public pointer_operators<Number, VariableName >,
               public boolean_operators<Number, VariableName > {
      public:
      using typename numerical_domain< Number, VariableName >::linear_expression_t;
      using typename numerical_domain< Number, VariableName >::linear_constraint_t;
      using typename numerical_domain< Number, VariableName >::linear_constraint_system_t;
      typedef disjunctive_linear_constraint_system<Number, VariableName>
      disjunctive_linear_constraint_system_t;
      using typename numerical_domain< Number, VariableName >::variable_t;
      using typename numerical_domain< Number, VariableName >::number_t;
      using typename numerical_domain< Number, VariableName >::varname_t;
      typedef typename linear_constraint_t::kind_t constraint_kind_t;
      typedef interval<Number>  interval_t;
      typedef boxes_domain <Number, VariableName, LddSize> boxes_domain_t;

    private:

      typedef boxes_domain_ <Number, VariableName, LddSize> boxes_impl_t;
      typedef std::shared_ptr<boxes_impl_t> boxes_ref_t;

      boxes_ref_t _ref;

      boxes_domain(boxes_ref_t ref) : _ref(ref) { }

      boxes_domain_t create(boxes_impl_t&& t)
      { return std::make_shared<boxes_impl_t>(std::move(t)); }

      void detach(void)
      {
        if(!_ref || !_ref.unique())
          _ref = std::make_shared<boxes_impl_t>(*_ref);
      }

      boxes_impl_t& ref(void) { return *_ref; }

      const boxes_impl_t& ref(void) const { return *_ref; }

    public:

      boxes_domain(bool is_bottom = false): _ref(nullptr)
      {
	if (is_bottom)
	  _ref = std::make_shared<boxes_impl_t>(boxes_impl_t::bottom());
	else
	  _ref = std::make_shared<boxes_impl_t>(boxes_impl_t::top());
      }

      static boxes_domain_t top() { return boxes_domain(false); }

      static boxes_domain_t bottom() { return boxes_domain(true); }

      boxes_domain(const boxes_domain_t& o)
        : _ref(o._ref) { }

      boxes_domain_t& operator=(const boxes_domain_t& o)
      {
        _ref = o._ref;
        return *this;
      }

      bool is_bottom() { return ref().is_bottom(); }
      bool is_top() { return ref().is_top(); }
      bool operator<=(boxes_domain_t& o) { return ref() <= o.ref(); }
      void operator|=(boxes_domain_t o) { detach(); ref() |= o.ref(); }
      boxes_domain_t operator|(boxes_domain_t o) { return create(ref() | o.ref()); }
      boxes_domain_t operator||(boxes_domain_t o) { return create(ref() || o.ref()); }
      boxes_domain_t operator&(boxes_domain_t o) { return create(ref() & o.ref()); }
      boxes_domain_t operator&&(boxes_domain_t o) { return create(ref() && o.ref()); }

      template<typename Thresholds>
      boxes_domain_t widening_thresholds (boxes_domain_t o, const Thresholds &ts) {
        return create(ref().template widening_thresholds<Thresholds>(o.ref(), ts));
      }

      void operator+=(linear_constraint_system_t csts) { detach(); ref() += csts; }
      void operator-=(VariableName v) { detach(); ref() -= v; }
      interval_t operator[](VariableName x) { return ref()[x]; }
      void set(VariableName x, interval_t intv) { detach(); ref().set(x, intv); }

      template<typename Iterator>
      void forget (Iterator vIt, Iterator vEt)
      { detach(); ref().forget(vIt, vEt); }
      void assign(VariableName x, linear_expression_t e)
      { detach(); ref().assign(x, e); }
      void apply(operation_t op, VariableName x, VariableName y, Number k)
      { detach(); ref().apply(op, x, y, k); }
      void apply(conv_operation_t op, VariableName x, VariableName y, unsigned width)
      { detach(); ref().apply(op, x, y, width); }
      void apply(conv_operation_t op, VariableName x, Number k, unsigned width)
      { detach(); ref().apply(op, x, k, width); }
      void apply(bitwise_operation_t op, VariableName x, VariableName y, Number k)
      { detach(); ref().apply(op, x, y, k); }
      void apply(bitwise_operation_t op, VariableName x, VariableName y, VariableName z)
      { detach(); ref().apply(op, x, y, z); }
      void apply(operation_t op, VariableName x, VariableName y, VariableName z)
      { detach(); ref().apply(op, x, y, z); }
      void apply(div_operation_t op, VariableName x, VariableName y, VariableName z)
      { detach(); ref().apply(op, x, y, z); }
      void apply(div_operation_t op, VariableName x, VariableName y, Number k)
      { detach(); ref().apply(op, x, y, k); }

      void assign_bool_cst (VariableName x, linear_constraint_t cst)
      { detach(); ref().assign_bool_cst(x,cst);}
      void assign_bool_var (VariableName x, VariableName y)
      { detach(); ref().assign_bool_var(x,y);}
      void apply_binary_bool(bool_operation_t op,VariableName x, VariableName y, VariableName z)
      { detach(); ref().apply_binary_bool(op,x,y,z);}
      void assume_bool (VariableName x, bool is_negated)
      { detach(); ref().assume_bool(x,is_negated);}

      template<typename Iterator>
      void project (Iterator vIt, Iterator vEt) { detach(); ref().project(vIt, vEt); }

      void write(crab_os& o) { ref().write(o); }

      linear_constraint_system_t to_linear_constraint_system ()
      { return ref().to_linear_constraint_system(); }

      disjunctive_linear_constraint_system_t
      to_disjunctive_linear_constraint_system ()
      { return ref().to_disjunctive_linear_constraint_system(); }

      static std::string getDomainName () { return boxes_impl_t::getDomainName(); }

    };

    template<typename Number, typename VariableName, size_t LddSize>
    class domain_traits <boxes_domain<Number,VariableName, LddSize> > {
    public:
      typedef boxes_domain<Number, VariableName, LddSize> boxes_domain_t;

      template<class CFG>
      static void do_initialization (CFG cfg) {}

      static void normalize (boxes_domain_t& inv) {}

      template <typename Iter>
      static void forget (boxes_domain_t& inv, Iter it, Iter end)
      { inv.forget (it, end); }

      template <typename Iter>
      static void project (boxes_domain_t& inv, Iter it, Iter end)
      { inv.project (it, end); }

      static void expand (boxes_domain_t& inv, VariableName x, VariableName new_x)
      { CRAB_WARN ("expand operation not implemented in boxes"); }
    };
    #endif

   } // namespace domains
}// namespace crab
#endif /* HAVE_LDD */
#endif


///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//                 polar v1.0 - August the 15th 2019                         //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//  Copyright 2019 Matteo Tommasini                                          //
//                                                                           //
//  Licensed under the Apache License, Version 2.0 (the "License");          //
//  you may not use this file except in compliance with the License.         //
//  You may obtain a copy of the License at                                  //
//                                                                           //
//      http://www.apache.org/licenses/LICENSE-2.0                           //
//                                                                           //
//  Unless required by applicable law or agreed to in writing, software      //
//  distributed under the License is distributed on an "AS IS" BASIS,        //
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. //
//  See the License for the specific language governing permissions and      //
//  limitations under the License.                                           //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

#ifndef VIBRASOLVER_H
#define VIBRASOLVER_H

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include "gaussiandataset.hpp"

class VibraSolver
{
   private:

   Eigen::MatrixXd Lx;
   Eigen::VectorXd wavenumbers,
                   IR_intensities,
                   VCD_intensities;

   // this is computed in atomic units
   std::vector<Eigen::Vector3d> dmu_dq,
                                 dm_dqdot;

   // this comes from VCD class, where it is computed in cgs units
   Eigen::VectorXd  m10_robust;
   Eigen::VectorXd mu01_robust;

   bool has_IR  = false,
        has_VCD = false;

   // utility members
   void Diagonalize(const Eigen::MatrixXd&, Eigen::VectorXd&, Eigen::MatrixXd&);
   void SolveSecularEquation(const GaussianDataset&, Eigen::MatrixXd&, Eigen::VectorXd&);
   void WriteMatrixInOctaveFormat(const Eigen::MatrixXd&,
                                  const std::string&,
                                  std::ofstream&) const;

   public:

   VibraSolver() = default;
   explicit VibraSolver(const GaussianDataset&);

   // getters
   int GetNumberOfAtoms() const;
   Eigen::MatrixXd GetCartesianNuclearDisplacements() const;
   Eigen::VectorXd GetWavenumbers() const;
   Eigen::VectorXd GetIRIntensities() const;
   Eigen::VectorXd GetVCDIntensities() const;
   std::vector<Eigen::Vector3d> GetElectricDipoleDerivativesVsNormalCoordinates() const;
   std::vector<Eigen::Vector3d> GetMagneticDipoleDerivativesVsNormalCoordinates() const;
   Eigen::VectorXd Get_m10_Robust() const;
   Eigen::VectorXd Get_mu01_Robust() const;

   // output
   void PrintReport() const;

};

#endif // VIBRASOLVER_H


/**
 * @file EuclideanParamBlock.hpp
 * @brief Header file for the EuclideanParamBlock class.
 * @author Jianzhu Huai
 */

#ifndef INCLUDE_OKVIS_CERES_EUCLIDEANPARAMBLOCK_HPP_
#define INCLUDE_OKVIS_CERES_EUCLIDEANPARAMBLOCK_HPP_

#include <okvis/ceres/ParameterBlockDynamic.hpp>
#include <Eigen/Core>
#include <okvis/Time.hpp>

namespace okvis {
/// \brief ceres Namespace for ceres-related functionality implemented in okvis.
namespace ceres {

/// \brief Wraps the parameter block for camera projection intrinsic estimate
class EuclideanParamBlock
    : public ParameterBlockDynamic {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef Eigen::Matrix<double, Eigen::Dynamic, 1> estimate_t;
  /// \brief Set the time.
  /// @param[in] timestamp The timestamp of this state.
  void setTimestamp(const okvis::Time& timestamp) { timestamp_ = timestamp; }

  /// \brief Default constructor (assumes not fixed).
  EuclideanParamBlock() : ParameterBlockDynamic(0, 0) {
    ParameterBlock::setFixed(false);
  }

  /// \brief Trivial destructor.
  virtual ~EuclideanParamBlock() {
  }

  /// \brief Constructor with estimate and time.
  /// @param[in] ProjectionIntrinsicParams
  /// @param[in] id The (unique) ID of this block.
  /// @param[in] timestamp The timestamp of this state.
  /// @param[in] dim The dim and minDim of this state.
  EuclideanParamBlock(const Eigen::VectorXd& params, uint64_t id,
      const okvis::Time& timestamp, const int dim) :
      ParameterBlockDynamic(dim, dim) {
    setEstimate(params);
    setId(id);
    setTimestamp(timestamp);
    setFixed(false);
  }

  // setters
  /// @brief Set estimate of this parameter block.
  /// @param[in] ProjectionIntrinsicParams The estimate to set this to.
  void setEstimate(
      const Eigen::VectorXd& params) {
    for (int i = 0; i < dim_; ++i)
      parameters_[i] = params[i];
  }

  /// getters
  /// @brief Get estimate.
  Eigen::VectorXd estimate() const {
    Eigen::VectorXd params(dim_);
    for (int i = 0; i < dim_; ++i)
      params[i] = parameters_[i];
    return params;
  }

  /// \brief Get the time.
  /// \return The timestamp of this state.
  okvis::Time timestamp() const { return timestamp_; }

  // minimal internal parameterization
  // x0_plus_Delta=Delta_Chi[+]x0
  /// \brief Generalization of the addition operation,
  ///        x_plus_delta = Plus(x, delta)
  ///        with the condition that Plus(x, 0) = x.
  /// @param[in] x0 Variable.
  /// @param[in] Delta_Chi Perturbation.
  /// @param[out] x0_plus_Delta Perturbed x.
  virtual void plus(const double* x0, const double* Delta_Chi,
                    double* x0_plus_Delta) const {
    Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, 1> > x0_(x0, dim_);
    Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, 1> > Delta_Chi_(
        Delta_Chi, minDim_);
    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1> > x0_plus_Delta_(
        x0_plus_Delta, dim_);
    x0_plus_Delta_ = x0_ + Delta_Chi_;
  }

  /// \brief The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
  /// @param[in] x0 Variable.
  /// @param[out] jacobian The Jacobian.
  virtual void plusJacobian(const double* /*unused: x*/,
                            double* jacobian) const {
    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
                             Eigen::RowMajor> >
        identity(jacobian, dim_, minDim_);
    identity.setIdentity();
  }

  // Delta_Chi=x0_plus_Delta[-]x0
  /// \brief Computes the minimal difference between a variable x and a
  /// perturbed variable x_plus_delta
  /// @param[in] x0 Variable.
  /// @param[in] x0_plus_Delta Perturbed variable.
  /// @param[out] Delta_Chi Minimal difference.
  /// \return True on success.
  virtual void minus(const double* x0, const double* x0_plus_Delta,
                     double* Delta_Chi) const {
    Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, 1> > x0_(x0, dim_);
    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1> > Delta_Chi_(Delta_Chi, minDim_);
    Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, 1> > x0_plus_Delta_(
        x0_plus_Delta, dim_);
    Delta_Chi_ = x0_plus_Delta_ - x0_;
  }

  /// \brief Computes the Jacobian from minimal space to naively
  /// overparameterised space as used by ceres.
  //  /// @param[in] x0 Variable.
  /// @param[out] jacobian the Jacobian (dimension minDim x dim).
  /// \return True on success.
  virtual void liftJacobian(const double* /*unused: x*/,
                            double* jacobian) const {
    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
                             Eigen::RowMajor> >
        identity(jacobian, minDim_, dim_);
    identity.setIdentity();
  }

  /// @brief Return parameter block type as string
  virtual std::string typeInfo() const { return "EuclideanParamBlock"; }

 private:
  okvis::Time timestamp_;  ///< Time of this state.
};
}  // namespace ceres
}  // namespace okvis

#endif /* INCLUDE_OKVIS_CERES_EUCLIDEANPARAMBLOCK_HPP_ */