//==========================================================================
//  This file is part of HMMlib.
//
//  HMMlib is free software: you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as
//  published by the Free Software Foundation, either version 3 of
//  the License, or (at your option) any later version.
//
//  HMMlib is distributed in the hope that it will be useful, but
//  WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
//  Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with HMMlib. If not, see
//  <http://www.gnu.org/licenses/>.
//
//  Copyright (C) 2010  Bioinformatics Research Centre, Aarhus University.
//  Author: Andreas Sand (asand@birc.au.dk)
//==========================================================================

#include "HMMlib/hmm_table.hpp"
#include "HMMlib/hmm_vector.hpp"
#include "HMMlib/hmm.hpp"
using namespace hmmlib;

#include <boost/shared_ptr.hpp>
#include <boost/python/module.hpp>
#include <boost/python/list.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/def.hpp>
#include <boost/python/class.hpp>
#include <boost/python/operators.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/overloads.hpp>
using namespace boost::python;
using namespace boost;

static void copy_matrix_to_lists(list &dst, const HMMMatrix<double> &src) {
  for(int r = 0; r < src.get_no_rows(); ++r) {
    list dst_r;
    for(int c = 0; c < src.get_no_columns(); ++c) {
      dst_r.append(src(r,c));
    }
    dst.append(dst_r);
  }
}

static void copy_lists_to_matrix(HMMMatrix<double> &dst, list &src) {
  for(int r = 0; r < len(src); ++r) {
    for(int c = 0; c < len(src[0]); ++c) {
      dst(r,c) = extract<double>(src[r][c]);
    }
  }
}

static void copy_vector_to_list(list &dst, const HMMVector<double> &src) {
  for(int i = 0; i < src.get_size(); ++i) {
    dst.append(src(i));
  }
}

static void copy_list_to_sequence(sequence &dst, const list &src) {
  for(int i = 0; i < len(src); ++i) {
    dst.push_back(extract<unsigned int>(src[i]));
  }
}

static void copy_sequence_to_list(list &dst, const sequence &src) {
  for(size_t i = 0; i < src.size(); ++i) {
    dst.append(src[i]);
  }
}

static void copy_list_to_vector(HMMVector<double> &dst, const list &src) {
  for(int i = 0; i < len(src); ++i) {
    dst(i) = extract<double>(src[i]);
  }
}

static double get_item_table(object self, object idxTuple) {
  HMMTable<double> &t = extract<HMMTable<double>&>(self);
  int i = extract<int>(idxTuple[0]);
  int j = extract<int>(idxTuple[1]);

  // negative indices
  if(i < 0)
    i = t.get_no_rows() - i;
  if(j < 0)
    j = t.get_no_columns() - j;

  return t(i,j);
}

static void set_item_table(object self, object idxTuple, object value) {
  HMMTable<double> &t = extract<HMMTable<double>&>(self);
  double s = extract<double>(value);
  int i = extract<int>(idxTuple[0]);
  int j = extract<int>(idxTuple[1]);

  // negative indices
  if(i < 0)
    i = t.get_no_rows() - i;
  if(j < 0)
    j = t.get_no_columns() - j;

  t(i,j) = s;
}

static double get_item_vector(object self, int i) {
  HMMVector<double> &v = extract<HMMVector<double>&>(self);

  // negative indices
  if(i < 0)
    i = v.get_size() - i;

  return v(i);
}

static void set_item_vector(object self, int i, object value) {
  HMMVector<double> &v = extract<HMMVector<double>&>(self);
  double s = extract<double>(value);

  // negative indices
  if(i < 0)
    i = v.get_size() - i;

  v(i) = s;
}

static unsigned int get_item_sequence(object self, int i) {
  sequence &s = extract<sequence&>(self);

  // negative indices
  if(i < 0)
    i = s.size() - i;

  return s[i];
}

static void set_item_sequence(object self, int i, object value) {
  sequence &s = extract<sequence&>(self);
  unsigned int v = extract<double>(value);

  // negative indices
  if(i < 0)
    i = s.size() - i;

  s[i] = v;
}

// static double likelihood_simple(object self, object obsseq_object) {
//   HMM<double> &hmm = extract<HMM<double>&>(self);
//   sequence obsseq = extract<sequence>(obsseq_object);
//   int length = obsseq.size();

//   HMMMatrix<double> f(length, hmm.get_no_states());
//   HMMVector<double> scales(length);

//   hmm.forward(obsseq, scales, f);
//   return hmm.likelihood(scales);
// }

static double likelihood_lists_from_obsseq(object self, object seq_list_object) {
  HMM<double> &hmm = extract<HMM<double>&>(self);
  list seq_list = extract<list>(seq_list_object);
  int length = len(seq_list);

  sequence obsseq;
  copy_list_to_sequence(obsseq, seq_list);

  HMMMatrix<double> f(length, hmm.get_no_states());
  HMMVector<double> scales(length);

  hmm.forward(obsseq, scales, f);
  return hmm.likelihood(scales);
}

static double likelihood_lists_from_scales(object self, object scales_list_object) {
  HMM<double> &hmm = extract<HMM<double>&>(self);
  list scales_list = extract<list>(scales_list_object);
  int length = len(scales_list);

  HMMVector<double> scales(length);
  for(int i = 0; i < length; ++i)
    scales(i) = extract<double>(scales_list[i]);

  return hmm.likelihood(scales);
}

// static boost::python::tuple viterbi_simple(object self, object obsseq_object) {
//   HMM<double> &hmm = extract<HMM<double>&>(self);
//   sequence obsseq = extract<sequence>(obsseq_object);

//   sequence hiddenseq(obsseq.size());
//   double loglik = hmm.viterbi(obsseq,hiddenseq);

//   return boost::python::make_tuple<sequence, double>(hiddenseq, loglik);
// }

static boost::python::tuple viterbi_lists(object self, object obsseq_list_object) {
  HMM<double> &hmm = extract<HMM<double>&>(self);
  list obsseq_list = extract<list>(obsseq_list_object);
  int length = len(obsseq_list);

  sequence obsseq;
  copy_list_to_sequence(obsseq, obsseq_list);

  sequence hiddenseq(length);
  double loglik = hmm.viterbi(obsseq,hiddenseq);

  list hiddenseq_list;
  copy_sequence_to_list(hiddenseq_list, hiddenseq);

  return boost::python::make_tuple<list, double>(hiddenseq_list, loglik);
}

// static boost::python::tuple forward_simple(object self, object obsseq_object) {
//   HMM<double> &hmm = extract<HMM<double>&>(self);
//   sequence obsseq = extract<sequence>(obsseq_object);
//   int length = obsseq.size();

//   HMMVector<double> scales(length);
//   HMMMatrix<double> f(length, hmm.get_no_states());

//   hmm.forward(obsseq, scales, f);

//   return boost::python::make_tuple<HMMVector<double>, HMMMatrix<double> >(scales, f);
// }

static boost::python::tuple forward_lists(object self, object obsseq_list_object) {
  HMM<double> &hmm = extract<HMM<double>&>(self);
  list obsseq_list = extract<list>(obsseq_list_object);
  int length = len(obsseq_list);
  int no_states = hmm.get_no_states();

  sequence obsseq;
  copy_list_to_sequence(obsseq, obsseq_list);

  HMMVector<double> scales(length);
  HMMMatrix<double> f(length, no_states);

  hmm.forward(obsseq, scales, f);

  list f_list;
  copy_matrix_to_lists(f_list, f);
  list scales_list;
  copy_vector_to_list(scales_list, scales);

  return boost::python::make_tuple<list, list >(scales_list, f_list);
}

static list backward_lists(object self, object obsseq_list_object, object scales_list_object) {
  HMM<double> &hmm = extract<HMM<double>&>(self);
  list obsseq_list = extract<list>(obsseq_list_object);
  list scales_list = extract<list>(scales_list_object);
  int length = len(obsseq_list);
  int no_states = hmm.get_no_states();

  sequence obsseq;
  copy_list_to_sequence(obsseq, obsseq_list);
  HMMVector<double> scales(length);
  copy_list_to_vector(scales, scales_list);

  HMMMatrix<double> B(length, no_states);

  hmm.backward(obsseq, scales, B);

  list B_list;
  copy_matrix_to_lists(B_list, B);

  return B_list;
}

static boost::python::tuple forward_and_backward(object self, object obsseq_list_object) {
  HMM<double> &hmm = extract<HMM<double>&>(self);
  list obsseq_list = extract<list>(obsseq_list_object);
  int length = len(obsseq_list);
  int no_states = hmm.get_no_states();

  sequence obsseq;
  copy_list_to_sequence(obsseq, obsseq_list);

  HMMVector<double> scales(length);
  HMMMatrix<double> F(length, no_states);
  HMMMatrix<double> B(length, no_states);
  hmm.forward(obsseq, scales, F);
  hmm.backward(obsseq, scales, B);

  list F_list;
  copy_matrix_to_lists(F_list, F);
  list B_list;
  copy_matrix_to_lists(B_list, B);
  list scales_list;
  copy_vector_to_list(scales_list, scales);

  return boost::python::make_tuple<list,list,list>(F_list, B_list, scales_list);
}

static list posterior_decoding_lists_from_obsseq(object self, object obsseq_list_object) {
  HMM<double> &hmm = extract<HMM<double>&>(self);
  list obsseq_list = extract<list>(obsseq_list_object);
  int length = len(obsseq_list);
  int no_states = hmm.get_no_states();

  sequence obsseq;
  copy_list_to_sequence(obsseq, obsseq_list);

  HMMVector<double> scales(length);
  HMMMatrix<double> F(length, no_states);
  HMMMatrix<double> B(length, no_states);
  HMMMatrix<double> post(length, no_states);
  hmm.forward(obsseq, scales, F);
  hmm.backward(obsseq, scales, B);
  hmm.posterior_decoding(obsseq, F, B, scales, post);

  list post_list;
  copy_matrix_to_lists(post_list, post);

  return post_list;
}

static list posterior_decoding_lists(object self, object obsseq_list_object, object F_list_object, object B_list_object, object scales_list_object) {
  HMM<double> &hmm = extract<HMM<double>&>(self);
  list obsseq_list = extract<list>(obsseq_list_object);
  list F_list = extract<list>(F_list_object);
  list B_list = extract<list>(B_list_object);
  list scales_list = extract<list>(scales_list_object);
  int length = len(obsseq_list);
  int no_states = hmm.get_no_states();

  sequence obsseq;
  copy_list_to_sequence(obsseq, obsseq_list);

  HMMMatrix<double> F(length, no_states);
  copy_lists_to_matrix(F, F_list);

  HMMMatrix<double> B(length, no_states);
  copy_lists_to_matrix(B, B_list);

  HMMVector<double> scales(length);
  copy_list_to_vector(scales, scales_list);

  HMMMatrix<double> post(length, no_states);

  hmm.posterior_decoding(obsseq, F, B, scales, post);

  list post_list;
  copy_matrix_to_lists(post_list, post);

  return post_list;
}

static boost::python::tuple baum_welch_lists_from_obsseq(object self, object obsseq_list_object) {
  HMM<double> &hmm = extract<HMM<double>&>(self);
  list obsseq_list = extract<list>(obsseq_list_object);
  int length = len(obsseq_list);
  int no_states = hmm.get_no_states();
  int alphabet_size = hmm.get_alphabet_size();

  sequence obsseq;
  copy_list_to_sequence(obsseq, obsseq_list);

  HMMVector<double> scales(length);
  HMMMatrix<double> F(length, no_states);
  HMMMatrix<double> B(length, no_states);

  hmm.forward(obsseq, scales, F);
  hmm.backward(obsseq, scales, B);

  HMMVector<double> pi_counts(no_states);
  HMMMatrix<double> T_counts(no_states, no_states);
  HMMMatrix<double> E_counts(alphabet_size, no_states);

  hmm.baum_welch(obsseq, F, B, scales, pi_counts, T_counts, E_counts);

  list pi_counts_list;
  copy_vector_to_list(pi_counts_list, pi_counts);

  list T_counts_list;
  copy_matrix_to_lists(T_counts_list, T_counts);

  list E_counts_list;
  copy_matrix_to_lists(E_counts_list, E_counts);

  return boost::python::make_tuple<list, list, list>(pi_counts_list, T_counts_list, E_counts_list);
}

static boost::python::tuple baum_welch_lists(object self, object obsseq_list_object, object F_list_object, object B_list_object, object scales_list_object) {
  HMM<double> &hmm = extract<HMM<double>&>(self);
  list obsseq_list = extract<list>(obsseq_list_object);
  list F_list = extract<list>(F_list_object);
  list B_list = extract<list>(B_list_object);
  list scales_list = extract<list>(scales_list_object);
  int length = len(obsseq_list);
  int no_states = hmm.get_no_states();
  int alphabet_size = hmm.get_alphabet_size();

  sequence obsseq;
  copy_list_to_sequence(obsseq, obsseq_list);

  HMMMatrix<double> F(length, no_states);
  copy_lists_to_matrix(F, F_list);

  HMMMatrix<double> B(length, no_states);
  copy_lists_to_matrix(B, B_list);

  HMMVector<double> scales(length);
  copy_list_to_vector(scales, scales_list);

  HMMVector<double> pi_counts(no_states);
  HMMMatrix<double> T_counts(no_states, no_states);
  HMMMatrix<double> E_counts(alphabet_size, no_states);

  hmm.baum_welch(obsseq, F, B, scales, pi_counts, T_counts, E_counts);

  list pi_counts_list;
  copy_vector_to_list(pi_counts_list, pi_counts);

  list T_counts_list;
  copy_matrix_to_lists(T_counts_list, T_counts);

  list E_counts_list;
  copy_matrix_to_lists(E_counts_list, E_counts);

  return boost::python::make_tuple<list, list, list>(pi_counts_list, T_counts_list, E_counts_list);
}

struct HMMTableCallback : HMMTable<double>, wrapper<HMMTable<double> > {

  HMMTableCallback(const int no_rows, const int no_columns, double val = 0.0)
    :HMMTable<double>(no_rows, no_columns, val)
  { }

  ~HMMTableCallback() {
    this->get_override("~HMMTableCallback")();
  }

  HMMTableCallback &operator=(double val) {
    return this->get_override("operator=")(val);
  }
};

BOOST_PYTHON_MODULE(pyhmmlib) {
  class_<HMMTableCallback, shared_ptr<HMMTableCallback>,noncopyable>("HMMTable", no_init)
    .def(init<int, int>())
    .def(init<int, int, double>())
    .def("column_sum", &HMMTable<double>::column_sum)
    .def("row_sum", &HMMTable<double>::row_sum)
    .def("reset", &HMMTable<double>::reset)
    .def("get_no_rows", &HMMTable<double>::get_no_rows)
    .def("get_no_columns", &HMMTable<double>::get_no_columns)
    .def(self += double())
    .def(self -= double())
    .def(self *= double())
    .def(self /= double())
    .def("__setitem__", set_item_table)
    .def("__getitem__", get_item_table)
    ;

  class_<HMMMatrix<double>, shared_ptr<HMMMatrix<double> >, bases<HMMTable<double> >, noncopyable>("HMMMatrix", no_init)
    .def(init<int, int>())
    .def(init<int, int, double>())
    .def("transpose", &HMMMatrix<double>::transpose)
    ;

  class_<HMMVector<double>, shared_ptr<HMMVector<double> >, bases<HMMTable<double> >, noncopyable>("HMMVector", no_init)
    .def(init<int>())
    .def(init<int, double>())
    .def("__getitem__", get_item_vector)
    .def("__setitem__", set_item_vector)
    .def("sum", &HMMVector<double>::sum)
    .def("get_size", &HMMVector<double>::get_size)
    ;

  class_<sequence>("Sequence", no_init)
    .def(init<int>())
    .def("__setitem__", set_item_sequence)
    .def("__getitem__", get_item_sequence)
    .def("__len__", &sequence::size)
    .def("len", &sequence::size)
    ;

  class_<HMM<double>, noncopyable>("HMM", no_init)
    .def(init<shared_ptr<HMMVector<double> >,
	 shared_ptr<HMMMatrix<double> >,
	 shared_ptr< HMMMatrix<double> > >())
    .def("get_alphabet_size", &HMM<double>::get_alphabet_size)
    .def("get_no_states", &HMM<double>::get_no_states)
    .def("forward", &HMM<double>::forward)
    .def("forward_lists", &forward_lists)
    .def("backward", &HMM<double>::backward)
    .def("backward_lists", &backward_lists)
    .def("forward_and_backward", &forward_and_backward)
    .def("viterbi", &HMM<double>::viterbi)
    .def("viterbi_lists", &viterbi_lists)
    .def("likelihood", &HMM<double>::likelihood)
    .def("likelihood_lists_from_obsseq", &likelihood_lists_from_obsseq)
    .def("likelihood_lists_from_scales", &likelihood_lists_from_scales)
    .def("baum_welch", &HMM<double>::baum_welch)
    .def("baum_welch_lists", &baum_welch_lists)
    .def("baum_welch_lists_from_obsseq", &baum_welch_lists_from_obsseq)
    .def("posterior_decoding", &HMM<double>::posterior_decoding)
    .def("posterior_decoding_lists", &posterior_decoding_lists)
    .def("posterior_decoding_lists_from_obsseq", &posterior_decoding_lists_from_obsseq)
    ;

}


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#ifndef MFMG_DEALII_TRILINOS_MATRIX_OPERATOR_HPP
#define MFMG_DEALII_TRILINOS_MATRIX_OPERATOR_HPP

#include <mfmg/common/operator.hpp>

#include <deal.II/lac/trilinos_sparse_matrix.h>

namespace mfmg
{
template <typename VectorType>
class DealIITrilinosMatrixOperator final : public Operator<VectorType>
{
public:
  using vector_type = VectorType;

  DealIITrilinosMatrixOperator(
      std::shared_ptr<dealii::TrilinosWrappers::SparseMatrix> sparse_matrix);

  virtual ~DealIITrilinosMatrixOperator() override = default;

  void apply(vector_type const &x, vector_type &y,
             OperatorMode mode = OperatorMode::NO_TRANS) const override;

  std::shared_ptr<Operator<VectorType>> transpose() const override;

  std::shared_ptr<Operator<VectorType>>
  multiply(std::shared_ptr<Operator<VectorType> const> b) const override;

  std::shared_ptr<Operator<VectorType>> multiply_transpose(
      std::shared_ptr<Operator<VectorType> const> b) const override;

  std::shared_ptr<vector_type> build_domain_vector() const override;

  std::shared_ptr<vector_type> build_range_vector() const override;

  size_t grid_complexity() const override;

  size_t operator_complexity() const override;

  std::shared_ptr<dealii::TrilinosWrappers::SparseMatrix const>
  get_matrix() const;

private:
  std::shared_ptr<dealii::TrilinosWrappers::SparseMatrix> _sparse_matrix;
};
} // namespace mfmg

#endif


// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// 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.

/**
 * \file mpc_solver_base.hpp
 * \brief
 * \details
 *
 * \author Autonomous Space Robotics Lab (ASRL)
 */
#pragma once

#include <algorithm>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>

#include <Eigen/Cholesky>
#include <Eigen/Dense>

#include <vtr_logging/logging.hpp>
#include <vtr_path_tracker/robust_mpc/mpc/mpc_types.hpp>
#include <vtr_path_tracker/robust_mpc/optimization/mpc_nominal_model.hpp>

namespace vtr {
namespace path_tracker {

/** \brief Base class defining the interface to the path tracker framework */
class MpcSolverBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /// \todo: add documentation
  /** \brief */
  typedef struct {
    float weight_head, weight_head_final, weight_lat, weight_lat_final;
    float weight_u, weight_v, weight_du, weight_dv;
    float barrier_norm;
    bool flg_en_mpcConstraints, flg_en_robustMpcConstraints;
    bool flg_compute_dw_externally = true;

    float w_max;  ///< max allowed turn rate
  } opt_params_t;

  /** \brief */
  typedef struct {
    Eigen::MatrixXf weight_Q, weight_Ru, weight_Rv;
  } opt_weight_mtx_t;

  /** \brief */
  typedef struct {
    bool flg_uncertain_pose_fails_constraints;
    bool flg_nominal_pose_fails_constraints;
    bool flg_nominal_pose_grossly_fails_constraints;
    bool flg_x_opt_and_pred_dont_match;
    bool flg_hessian_not_pos_def;
    bool flg_des_vel_one_point_turn;
    float delta_x_pred_opt;
    int num_failed_opt_results;
  } opt_flgs_t;

  Eigen::MatrixXf test_out_;
  opt_params_t opt_params;

  float u_km1, v_km1;

  Eigen::MatrixXf x_target, v_desired, x_desired;
  std::vector<float> norm_ds;
  std::vector<float> slope_step, slope_step1, slope_step2;
  std::vector<float> norm_grad_L_ds_opt_vec;

 protected:
  int size_x, size_u, size_v;
  int lookahead;

  opt_weight_mtx_t opt_weight_mtxs;
  Eigen::MatrixXf opt_variable, u_opt, v_opt;

  int curr_iteration;

  int solver_mode;
  float step_size;
  float step_max;
  float step_min;

 public:
  /** Make sure to initialize all flags to false in solver_base_implementation
   * **/
  opt_flgs_t result_flgs;

  void set_sizes(int size_x_in, int size_u_in, int size_v_in);
  void set_weights(opt_params_t opt_params_in);
  void set_lookahead(int lookahead_in);
  void reset_solver();
  virtual void reset_solver_specific() = 0;

  /**  \brief Copy out u_opt from previous cmd */
  void copy_opt_km1(const int entries_to_skip, const int entries_to_copy);
  void compute_weight_matrices();
  virtual MpcNominalModel::model_state_t *select_x_pred(int index) = 0;
  virtual MpcNominalModel::model_trajectory_t *select_x_pred() = 0;
  virtual void set_desired_speed(int &index, const double &speed) = 0;
  virtual void get_desired_ctrl(int &index) = 0;
  virtual void set_desired_speed_ctrl(int &index, const double &speed,
                                      const double &ctrl) = 0;
  virtual void post_process_x_pred(int &index) = 0;

  virtual void compute_solver_update(const local_path_t &local_path,
                                     int iteration) = 0;
  virtual void get_u(Eigen::MatrixXf &u_mat) = 0;
  virtual void get_v(Eigen::MatrixXf &v_mat) = 0;
  virtual void get_w(Eigen::MatrixXf &w_mat) = 0;
  void reset_cmd_km1();
  virtual void set_cmd_km1(const float &u_km1, const float &v_km1) = 0;

  MpcNominalModel::model_trajectory_t x_pred, x_opt;
};

}  // namespace path_tracker
}  // namespace vtr


#include "ehrenfest.hpp"
#include "electronic.hpp"
#include "constants.hpp"
#include "util.hpp"
#include <armadillo>
#include <complex>
#include <cmath>

ConfigBlockReader Ehrenfest::setup_reader()
{
    using types = ConfigBlockReader::types;
    ConfigBlockReader reader{"ehrenfest"};
    reader.add_entry("amplitude_file", "cs.dat");
    reader.add_entry("dtc", types::DOUBLE);

    {
      std::vector<std::complex<double>> cs {};
      reader.add_entry("amplitudes", cs);
    }

    return reader;
}

void Ehrenfest::get_reader_data(ConfigBlockReader& reader) {
  {
    double in_dtc;
    reader.get_data("dtc", in_dtc);  // in fs
    dtc = in_dtc * (1e-15 / AU2SI_TIME);  // fs -> a.u.
  }
  reader.get_data("amplitude_file", amplitude_file);

  /* added in BOMD::add_qm_keys() */
  reader.get_data("min_state", min_state);

  {
    size_t excited_states;
    /* added in BOMD::add_qm_keys() */
    reader.get_data("excited_states", excited_states);

    nstates = excited_states + 1 - min_state;
  }

  if (nstates < 2){
    throw std::logic_error("Doesn't make sense to propagate the electronic wavefunction on only 1 surface!");
  }

  if (!(min_state <= active_state && active_state <= min_state + nstates)){
    throw std::range_error("Active state not in the range of propagating states!");
  }

  active_state -= min_state;

  energy.set_size(nstates);

  U.set_size(nstates, nstates);
  T.set_size(nstates, nstates);
  V.set_size(nstates, nstates);
  phases.set_size(nstates);
  phases.ones();

  qm_grds.set_size(3, NQM(), nstates);

  {
    std::vector<std::complex<double>> cs_vec {};
    reader.get_data("amplitudes", cs_vec);
    if (cs_vec.size() > 0){
      if (cs_vec.size() != (arma::uword) nstates){
        throw std::runtime_error("Number of Amplitudes do not match number of hopping surfaces!");
      }
      double norm = 0;
      for (const auto& c: cs_vec){
        norm += std::norm(c);
      }
      std::cerr << "[Ehrenfest] amplitude norm = " << norm << std::endl;
      if (std::abs(1.0 -norm) > 1e-6){
        throw std::runtime_error("Amplitudes are not normed; check your input!");
      }
      arma::cx_vec cs_arma(nstates);
      for (int i = 0; i < nstates; i++){
        cs_arma(i) = cs_vec[i];
      }
      c = Electronic(cs_arma);
    }
    else{
      c.reset(nstates, 1, active_state);
    }
  }
}

// This is our primary hook into the Gromacs (or other) MD loop
double Ehrenfest::update_gradient(void){
  qm->update();
  mm_grds.set_size(3, NMM(), nstates);

  // get gradients, energies, and overlap
  {
    PropMap props{};
    auto states = util::range(min_state, min_state + nstates);
    props.emplace(QMProperty::qmgradient_multi, states, &qm_grds);
    props.emplace(QMProperty::mmgradient_multi, states, &mm_grds);
    props.emplace(QMProperty::energies, states, &energy);
    props.emplace(QMProperty::wfoverlap, &U);
    qm->get_properties(props);
  }

  // write populations/amplitudes first so we include the initial conditions
  {
    std::ofstream output(amplitude_file, std::ios_base::app);
    auto a = c();
    a.transform( [](std::complex<double> ci) { return std::norm(ci); } );
    arma::real(a).st().print(output);
    output.close();
  }
  saveh5(c.get(), "amps");
  saveh5(U, "overlapraw");
  Electronic::phase_match(U, phases);
  saveh5(U, "overlap");
  T = util::logmat_unitary(U) / dtc;
  V = arma::diagmat(energy);

  // compute max time step for electronic propagation (Jain 2016 eqs. 20, 21)
  {
    double dtq_ =
      std::min(dtc/20, // make sure 20dtq < dtc
               std::min(dtc,
                        std::min(0.02 / T.max(),
                                 0.02 / arma::max( V.diag() - arma::mean(V.diag()) )
                                 )
                        ));

    dtq = dtc / std::round(dtc / dtq_);
  }

  const size_t n_steps = (size_t) dtc / dtq;
  const std::complex<double> I(0,1);

  // Propagate electronic coefficients
  for (size_t nt = 0; nt < n_steps; nt++){
    c.advance_exact(V - I*T, dtq);
  }

  double eh_energy = 0;
  mm_grd.zeros();
  qm_grd.zeros();
  for (int i = 0; i < nstates; i++){
    double ai = std::norm(c(i));
    eh_energy += ai * energy(i);
    qm_grd += ai * qm_grds.slice(i);
    mm_grd += ai * mm_grds.slice(i);
  }

  //coupling terms
  {
    arma::mat d(3, NQM()+NMM());
    // Upper triangle of dij
    for (arma::uword j=1; j < (arma::uword) nstates; j++){
      for (arma::uword i=0; i < j; i++){
        PropMap props{};
        props.emplace(QMProperty::nacvector, {min_state + i, min_state + j}, &d);
        d *= phases(i) * phases(j);
        qm->get_properties(props);
        const arma::mat & dqm = d.submat(arma::span::all, arma::span(0, NQM()-1));
        double cdv = 2 * (std::conj(c(i))*c(j)).real() * (energy(j) - energy(i));
        //std::cout << "[Ehrenfest] " << call_idx() << ": " <<
        //i << "," << j << ": " << cdv << std::endl;
        qm_grd += cdv * dqm;
        if (NMM() > 0){
          const arma::mat & dmm = d.submat(arma::span::all, arma::span(NQM(), NMM()-1));
          mm_grd += cdv * dmm;
        }
      }
    }
  }
  return eh_energy;
}


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// Inner.hpp
// This file is part of the Garamon for e3ga.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

// \file BasisTransformations.hpp
// \author V. Nozick, S. Breuils
// \brief this files generates and load the elements of the transformation matrices into array of Eigen matrices

#ifndef E3GA_BASISTRANSFORMATIONS_HPP__
#define E3GA_BASISTRANSFORMATIONS_HPP__
#pragma once

#include <Eigen/Sparse>
#include <vector>
#include <array>
#include <algorithm>
#include <iterator>
#include <sstream>

/*!
 * @namespace e3ga
 */
namespace e3ga{

	/// Decode the string encodedPerGradeMatrixComponents to a vector of T. This vector will contain the components of the transformation matrices
	template<typename T>
	const std::vector<T> decodeStringToVecOfT(std::string encodedPerGradeMatrixComponents) {
	    std::vector<T> resultDecodedGrade;
	    std::istringstream streamStringOverFloat(encodedPerGradeMatrixComponents);
	    std::copy(std::istream_iterator<float>(streamStringOverFloat),
		std::istream_iterator<float>(),
		std::back_inserter(resultDecodedGrade));
	    return resultDecodedGrade;
	}

	/// initialize all the direct transformation matrices using array of eigen sparse matrices

	/// initialize all the inverse transformation matrices using array of eigen sparse matrices

}/// End of Namespace

#endif // E3GA_BASISTRANSFORMATIONS_HPP__


#include <boost/foreach.hpp>
#include <rosbag/view.h>
#include <tf/tfMessage.h>
#include <geometry_msgs/TransformStamped.h>
#include <iostream>
#include <exception>
#include "common.h"

#define foreach BOOST_FOREACH

using std::cout;
using std::endl;
using std::exception;

void tfToCV(const tf::Transform &src, cv::Mat &position, cv::Mat &orientation)
{
	position = cv::Mat (3,1,CV_64F);
	tf::Vector3 p = src.getOrigin();
	position.at<double>(0) = p.x(),
		position.at<double>(1) = p.y(),
		position.at<double>(2) = p.z();

	orientation = cv::Mat (4,1,CV_64F);
	tf::Quaternion otn = src.getRotation();
	orientation.at<double>(0) = otn.x(),
		orientation.at<double>(1) = otn.y(),
		orientation.at<double>(2) = otn.z(),
		orientation.at<double>(3) = otn.w();
}

void recomputeNewCameraParameter (
	// Original
	double fx1, double fy1, double cx1, double cy1,
	// New
	double &fx2, double &fy2, double &cx2, double &cy2,
	int width1, int height1,
	int width2, int height2
)
{
	double ratio = (double)width1 / (double)width2;
	fx2 = fx1 / ratio;
	fy2 = fy1 / ratio;
	cx2 = cx1 / ratio;
	cy2 = cy1 / ratio;
}

void tf2positiondirection (const tf::Transform &pose, float positiondirection[6])
{
	// position
	positiondirection[0] = pose.getOrigin().x();
	positiondirection[1] = pose.getOrigin().y();
	positiondirection[2] = pose.getOrigin().z();
	float fdirx = pose.getRotation().x(),
		fdiry = pose.getRotation().y(),
		fdirz = pose.getRotation().z(),
		fdirnorm;
	fdirnorm = sqrtf(fdirx*fdirx + fdiry*fdiry + fdirz*fdirz);
	fdirx /= fdirnorm;
	fdiry /= fdirnorm;
	fdirz /= fdirnorm;
	positiondirection[3] = fdirx;
	positiondirection[4] = fdiry;
	positiondirection[5] = fdirz;
}

tf::Transform FramePose (ORB_SLAM2::Frame *cframe)
{
	cv::Mat Rwc = cframe->mTcw.rowRange(0,3).colRange(0,3).t();
	cv::Mat twc = -Rwc * cframe->mTcw.rowRange(0,3).col(3);
	tf::Matrix3x3 M(Rwc.at<float>(0,0),Rwc.at<float>(0,1),Rwc.at<float>(0,2),
					Rwc.at<float>(1,0),Rwc.at<float>(1,1),Rwc.at<float>(1,2),
					Rwc.at<float>(2,0),Rwc.at<float>(2,1),Rwc.at<float>(2,2));
	tf::Vector3 V(twc.at<float>(0), twc.at<float>(1), twc.at<float>(2));

	return tf::Transform(M, V);
}


//
// Copyright (c) 2017 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/boostorg/beast
//

#ifndef BOOST_BEAST_DETAIL_VARINT_HPP
#define BOOST_BEAST_DETAIL_VARINT_HPP

#include <boost/static_assert.hpp>
#include <cstdlib>
#include <iterator>
#include <type_traits>

namespace boost {
namespace beast {
namespace detail {

// https://developers.google.com/protocol-buffers/docs/encoding#varints

inline std::size_t varint_size(std::size_t value) {
  std::size_t n = 1;
  while (value > 127) {
    ++n;
    value /= 128;
  }
  return n;
}

template <class FwdIt> std::size_t varint_read(FwdIt &first) {
  using value_type = typename std::iterator_traits<FwdIt>::value_type;
  BOOST_STATIC_ASSERT(std::is_integral<value_type>::value &&
                      sizeof(value_type) == 1);
  std::size_t value = 0;
  std::size_t factor = 1;
  while ((*first & 0x80) != 0) {
    value += (*first++ & 0x7f) * factor;
    factor *= 128;
  }
  value += *first++ * factor;
  return value;
}

template <class FwdIt> void varint_write(FwdIt &first, std::size_t value) {
  using value_type = typename std::iterator_traits<FwdIt>::value_type;
  BOOST_STATIC_ASSERT(std::is_integral<value_type>::value &&
                      sizeof(value_type) == 1);
  while (value > 127) {
    *first++ = static_cast<value_type>(0x80 | value);
    value /= 128;
  }
  *first++ = static_cast<value_type>(value);
}

} // namespace detail
} // namespace beast
} // namespace boost

#endif


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

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/combine.hpp>
#include <boost/simd/function/shuffle.hpp>

namespace boost { namespace simd { namespace detail
{
  //------------------------------------------------------------------------------------------------
  // This meta-permutation implements the scan pattern required for scan-trees.
  template<int Step> struct scan_perm
  {
    template<typename I, typename> struct apply
    {
      static const int relative_index = I::value / Step;
      static const int index = relative_index % 2 ? (relative_index*Step - 1) : -1;
      using type = std::integral_constant<int,index>;
    };
  };
  //------------------------------------------------------------------------------------------------
  // This meta-function implements the scan pattern mask to handle scan-tree neutral element
  template<int Step> struct scan_mask
  {
    template<typename T, int I> struct apply
    {
      static const int relative_index = I / Step;
      using type = std::integral_constant<T,relative_index % 2 ? T(0) : T(~0)>;
    };

    template<typename V, typename... N>
    static BOOST_FORCEINLINE V mask(V const& z, brigand::list<N...> const&)
    {
      using pi_t = boost::dispatch::as_integer_t<V>;
      return z & pi_t( apply<typename pi_t::value_type, N::value>::type::value... );
    }
    template<typename V> static BOOST_FORCEINLINE V mask(V const& z)
    {
      return mask(z, typename V::traits::element_range{});
    }
  };

  template<int Step, int Cardinal> struct scan
  {
    using next_t = scan<Step*2,Cardinal>;
    template<typename Op, typename V, typename U>
    BOOST_FORCEINLINE U operator()(Op const& op, V const& a0, U const& z) const BOOST_NOEXCEPT
    {
      // we perform a unary shuffle + mask in order to ensure that most ISA can
      // generate the scan-tree pattern without using an emulated shuffle
      // TODO: Make this dependent on has_native_shuffle<ABI>
      auto shf = boost::simd::shuffle< scan_perm<Step> >(a0) | scan_mask<Step>::mask(z);
      next_t next;
      return next( op, op(a0, shf), z );
    }
  };

  template<int End> struct scan<End,End>
  {
    template<typename Op, typename V, typename U>
    BOOST_FORCEINLINE U  operator()(Op const& op, V const& a0, U const& z) const BOOST_NOEXCEPT
    {
      auto shf = boost::simd::shuffle< scan_perm<End> >(a0) | scan_mask<End>::mask(z);
      return op(a0, shf);
    }
  };
} } }

namespace boost { namespace simd { namespace ext
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  BOOST_DISPATCH_OVERLOAD_FALLBACK( ( typename F, typename BinOp, typename NeutralElement
                                    , typename Arg, typename Ext
                                    )
                                  , bd::cumulative_<F,BinOp,NeutralElement>
                                  , bs::simd_
                                  , bs::pack_<bd::unspecified_<Arg>, Ext>
                                  )
  {
    using scalar_t  = decltype( bd::functor<F>()( std::declval<typename Arg::value_type>() ) );
    using result_t  = typename Arg::template rebind<scalar_t>;
    using function_t = bd::functor<BinOp>;

    // ---------------------------------------------------------------------------------------------
    // Aggregate case
    template<typename... N> static BOOST_FORCEINLINE
    result_t do_( function_t const& op, result_t const&, Arg const& a0
                , aggregate_storage const&, brigand::list<N...> const&
                )
    {
      bd::functor<F>      f;

      auto lo = f(a0.storage()[0]);
      return combine( lo, op( lo.back(), f(a0.storage()[1])) );
    }

    // ---------------------------------------------------------------------------------------------
    // singleton case
    template<typename K, typename N> static BOOST_FORCEINLINE
    result_t do_( function_t const&, result_t const&
                , Arg const& a0, K const&, brigand::list<N> const&
                )
    {
      return a0;
    }

    // ---------------------------------------------------------------------------------------------
    // Native case
    template<typename N0, typename N1, typename... N> static BOOST_FORCEINLINE
    result_t do_( function_t const& op, result_t const& z, Arg const& a0
                , native_storage const&, brigand::list<N0,N1,N...> const&
                )
    {
      return detail::scan<1,Arg::static_size/2>{}(op,a0,z);
    }

    // ---------------------------------------------------------------------------------------------
    // Scalar case
    template<typename N0, typename N1, typename... N> static BOOST_FORCEINLINE
    result_t do_( function_t const& op, result_t const&, Arg const& a0
                , scalar_storage const&, brigand::list<N0,N1,N...> const&
                )
    {
      result_t that = a0;
      that[1] = op(bs::extract<1>(a0), bs::extract<0>(that));

      (void)std::initializer_list<bool> { ( ( that[N::value] = op ( bs::extract<N::value>(a0)
                                                                  , bs::extract<N::value-1>(that)
                                                                  )
                                            ),true
                                          )...
                                        };

      return that;
    }

    BOOST_FORCEINLINE result_t operator()(Arg const& a0) const
    {
      return do_( function_t{}
                , bd::functor<NeutralElement>{}(as_<result_t>{})
                , a0, typename Arg::storage_kind{}, typename Arg::traits::static_range{});
    }
  };
} } }

#endif


#include <boost/assert.hpp>
#include <cmath>
#include <limits>
#include <utility>

#include "mh_moves/mh_moves.hpp"
#include "mh_moves/mh_move_weights.hpp"
#include "sampling/simple.hpp"
#include "mh_moves/utility.hpp"
#include "tools/debug.hpp"

namespace biggles {

namespace mh_moves
{

float get_log_splitting_prob(shared_const_track_ptr& track1, shared_const_track_ptr& track2) {
    shared_const_track_ptr tr_lo;
    shared_const_track_ptr tr_hi;
    if (track1->first_time_stamp() < track2->first_time_stamp()) {
        tr_lo = track1;
        tr_hi = track2;
    } else {
        tr_lo = track2;
        tr_hi = track1;
    }
    float log_splitting_prob = 0.f;
    if (tr_hi->observations().first_time_stamp() > tr_lo->observations().last_time_stamp()) // is there a gap?
    {
        time_stamp last_obs_first_track = tr_lo->observations().last_time_stamp();
        time_stamp split_ts = tr_hi->first_time_stamp();
        log_splitting_prob = -logf(split_ts - last_obs_first_track + 1);
    }
    return log_splitting_prob;
}

/// \brief gets the back merge partner for the front piece
bool get_merge_partner(const shared_const_track_ptr &piece1, const track_collection &tracks,
    shared_const_track_ptr &partner, float &log_prob)
{
    back_weight_merging back_weight(piece1);
    track_collection::const_iterator it =
        sampling::weighted_choice(tracks.begin(), tracks.end(), back_weight, log_prob);
    if (it == tracks.end())
        return false;
    partner = *it;
    return true;
}

bool merge(const partition_ptr_t& start_partition_ptr, partition_ptr_t& end_partition_ptr, float& proposal_density_ratio)
{
    //typedef std::pair<shared_const_track_ptr, shared_const_track_ptr> track_pair;

    capability_recorder_ptr cap_rec_ptr = start_partition_ptr->get_capability_recorder();
    BOOST_ASSERT(cap_rec_ptr.get() not_eq 0); // there actually is something assigned

    const track_collection& tracks(start_partition_ptr->tracks());
    if (tracks.empty())
        return false;

    float log_track_sample = 0.f;
    front_weight_merging fw(start_partition_ptr->first_time_stamp(), start_partition_ptr->last_time_stamp());
    track_collection::const_iterator it =
        sampling::weighted_choice(tracks.begin(), tracks.end(), fw, log_track_sample);
    if (it == tracks.end())
        return false;
    shared_const_track_ptr piece1(*it);

    shared_const_track_ptr piece2;
    float log_partner_prob = 0.f;
    bool partner_found = get_merge_partner(piece1, tracks, piece2, log_partner_prob);

    if (not partner_found)
        return false;

    // create a new track by merging old ones
    // we also re-sample a new dynamic drag as 1
    shared_track_ptr new_track(new track(*piece1, *piece2, 1.f));
    BOOST_ASSERT(new_track->size() == piece1->size() + piece2->size());
    BOOST_ASSERT(new_track->size() >= 4);
    BOOST_ASSERT(new_track->duration() >= 3);

    // create copies of the tracks. The clutter is unaffected
    shared_track_collection_ptr new_tracks(new track_collection(tracks));

    cap_rec_ptr->add_erase_track(piece1);
    cap_rec_ptr->add_erase_track(piece2);
    cap_rec_ptr->add_insert_track(new_track);
    cap_rec_ptr->set_editing_finished();

    // remove old tracks
    new_tracks->remove(piece1);
    new_tracks->remove(piece2);

    // insert new one
    new_tracks->insert(new_track);

    // update partition
    end_partition_ptr = partition_ptr_t(
        new partition(start_partition_ptr->pool(), new_tracks, start_partition_ptr->clutter_ptr(),
                        start_partition_ptr->expansion()));

    // calculate forward log prob.
    float forward_log_prob = log_track_sample + log_partner_prob;

    // calculate backward log prob. = prob of selecting new track and splitting at right place
    /*
    float backward_log_prob = 1.f;
    BOOST_VERIFY(sample_track_with_minimum_size(*new_tracks, 4, backward_log_prob) != new_tracks->end());
    BOOST_ASSERT(backward_log_prob <= 0.f);
    */
    float backward_log_prob = get_track_log_prob(split_weight_t(), *new_tracks, new_track);
    float log_ts_prob = 0.f;
    time_stamp ts_to_split = std::max(piece2->first_time_stamp(),
                                piece1->first_time_stamp());
    log_ts_prob = get_time_stamp_to_split_track_prob(new_track, ts_to_split);
    //if (not std::isfinite(log_ts_prob)) OK(log_ts_prob);
    //sample_time_stamp_to_split_track(new_track, foo, log_ts_prob); // this is non-sense
    backward_log_prob += log_ts_prob;
    backward_log_prob += get_log_splitting_prob(piece1, piece2);

    proposal_density_ratio = backward_log_prob - forward_log_prob;

    return true;
}

}

}


#ifndef AIKIDO_CONSTRAINT_DART_TSR_HPP_
#define AIKIDO_CONSTRAINT_DART_TSR_HPP_

#include <Eigen/Dense>
#include <dart/math/MathTypes.hpp>

#include "aikido/constraint/Differentiable.hpp"
#include "aikido/constraint/Projectable.hpp"
#include "aikido/constraint/Sampleable.hpp"
#include "aikido/constraint/Testable.hpp"
#include "aikido/statespace/SE3.hpp"

namespace aikido {
namespace constraint {
namespace dart {

AIKIDO_DECLARE_POINTERS(TSR)

/// TSRs describe end-effector constraint sets as subsets of SE(3).
/// A TSR consists of three parts:
///     T0_w: transform from the origin to the TSR frame w
///     B_w: 6 × 2 matrix of bounds in the coordinates of w.
///     Tw_e: end-effector offset transform in the coordinates of w
/// See:
/// Berenson, Dmitry, Siddhartha S. Srinivasa, and James Kuffner.
/// "Task space regions: A framework for pose-constrained manipulation
/// planning." IJRR 2001:
/// http://repository.cmu.edu/cgi/viewcontent.cgi?article=2024&context=robotics
class TSR
  : public Sampleable
  , public Differentiable
  , public Testable
  , public Projectable
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  using Differentiable::getValueAndJacobian;

  /// Constructor.
  /// \param _rng Random number generator used by SampleGenerators for this TSR.
  /// \param _T0_w transform from the origin to the TSR frame w
  /// \param _Bw 6 × 2 matrix of bounds in the coordinates of w.
  ///        Top three rows bound translation, and bottom three rows
  ///        bound rotation following Roll-Pitch-Yaw convention.
  ///        _Bw(i, 0) should be less than or equal to _Bw(i, 1).
  /// \param _Tw_e end-effector offset transform in the coordinates of w
  /// \param _testableTolerance tolerance used in isSatisfiable as testable
  TSR(std::unique_ptr<common::RNG> _rng,
      const Eigen::Isometry3d& _T0_w = Eigen::Isometry3d::Identity(),
      const Eigen::Matrix<double, 6, 2>& _Bw
      = Eigen::Matrix<double, 6, 2>::Zero(),
      const Eigen::Isometry3d& _Tw_e = Eigen::Isometry3d::Identity(),
      double _testableTolerance = 1e-6);

  /// Constructor with default random seed generator.
  /// \param _T0_w transform from the origin to the TSR frame w
  /// \param _Bw 6 × 2 matrix of bounds in the coordinates of w.
  ///        Top three rows bound translation, and bottom three rows
  ///        bound rotation following Roll-Pitch-Yaw convention.
  ///        _Bw(i, 0) should be less than or equal to _Bw(i, 1).
  /// \param _Tw_e end-effector offset transform in the coordinates of w
  /// \param _testableTolerance tolerance used in isSatisfiable as testable
  TSR(const Eigen::Isometry3d& _T0_w = Eigen::Isometry3d::Identity(),
      const Eigen::Matrix<double, 6, 2>& _Bw
      = Eigen::Matrix<double, 6, 2>::Zero(),
      const Eigen::Isometry3d& _Tw_e = Eigen::Isometry3d::Identity(),
      double _testableTolerance = 1e-6);

  TSR(const TSR& other);
  TSR(TSR&& other);

  TSR& operator=(const TSR& other);
  TSR& operator=(TSR&& other);

  virtual ~TSR() = default;

  // Documentation inherited.
  statespace::ConstStateSpacePtr getStateSpace() const override;

  /// Returns the SE3 which this TSR operates in.
  std::shared_ptr<statespace::SE3> getSE3() const;

  // Documentation inherited.
  std::unique_ptr<SampleGenerator> createSampleGenerator() const override;

  // Documentation inherited.
  bool isSatisfied(
      const statespace::StateSpace::State* _s,
      TestableOutcome* outcome = nullptr) const override;

  /// Return an instance of DefaultTestableOutcome, since this class doesn't
  /// have a more specialized TestableOutcome derivative assigned to it.
  std::unique_ptr<TestableOutcome> createOutcome() const override;

  /// Throws an invalid_argument exception if this TSR is invalid.
  /// For a TSR to be valid, mBw(i, 0) <= mBw(i, 1).
  void validate() const;

  /// Set the random number generator used by SampleGenerators for this TSR.
  void setRNG(std::unique_ptr<common::RNG> rng);

  // Documentation inherited.
  std::size_t getConstraintDimension() const override;

  // Documentation inherited.
  void getValue(const statespace::StateSpace::State* _s, Eigen::VectorXd& _out)
      const override;

  /// Jacobian of TSR with respect to the se(3) tangent vector of _s.
  /// The jacobian is w.r.t. the origin frame.
  /// se(3) tangent vector follows dart convention:
  ///   top 3 rows is the angle-axis representation of _s's rotation.
  ///   bottom 3 rows represent the translation.
  /// \param _s State to be evaluated at.
  /// \param[out] _out Jacobian, 6 x 6 matrix.
  void getJacobian(
      const statespace::StateSpace::State* _s,
      Eigen::MatrixXd& _out) const override;

  // Documentation inherited.
  std::vector<ConstraintType> getConstraintTypes() const override;

  // Documentation inherited.
  bool project(
      const statespace::StateSpace::State* _s,
      statespace::StateSpace::State* _out) const override;

  /// Get the testable tolerance used in isSatisfiable.
  double getTestableTolerance();

  /// Set the testable tolerance used in isSatisfiable.
  /// \param _testableTolerance Testable tolerance to set.
  void setTestableTolerance(double _testableTolerance);

  /// Transformation from origin frame into the TSR frame "w".
  /// "w" is usually centered at the origin of an object held by the hand
  ///  or at a location on an object that is useful for grasping.
  Eigen::Isometry3d mT0_w;

  /// Bounds on "wiggling" in `x, y, z, roll, pitch, yaw`.
  Eigen::Matrix<double, 6, 2> mBw;

  /// Transformation from "w" frame into end frame.
  /// This often represent an offset from "w" to the origin of the end-effector.
  Eigen::Isometry3d mTw_e;

private:
  /// Tolerance used in isSatisfied as a testable
  double mTestableTolerance;
  std::unique_ptr<common::RNG> mRng;
  std::shared_ptr<statespace::SE3> mStateSpace;
};

} // namespace dart
} // namespace constraint
} // namespace aikido

#endif // AIKIDO_CONSTRAINT_DART_TSR_HPP_


#pragma once
#include <EigenLinearSolvers/EigenConjugateGradient[CRS].h>

#include <sofa/helper/ScopedAdvancedTimer.h>

namespace EigenLinearSolvers
{

template <class TBlockType>
void EigenConjugateGradient<sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>, sofa::linearalgebra::
FullVector<typename sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>::Real>>::init()
{
    updatePreconditioner();
}

template <class TBlockType>
void EigenConjugateGradient<sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>, sofa::linearalgebra::FullVector<
typename sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>::Real>>::reinit()
{
    updatePreconditioner();
}

template <class TBlockType>
void EigenConjugateGradient<sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>, sofa::linearalgebra::FullVector<typename sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>::Real> >
    ::solve(Matrix& A, Vector& x, Vector& b)
{
    SOFA_UNUSED(A);
    sofa::helper::ScopedAdvancedTimer solveTimer("solve");

    EigenVectorXdMap xMap(x.ptr(), x.size());
    EigenVectorXdMap bMap(b.ptr(), b.size());

    std::visit([&bMap, &xMap](auto&& solver)
    {
        xMap = solver.solveWithGuess(bMap, xMap);
    }, m_solver);
}

template <class TBlockType>
void EigenConjugateGradient<sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>, sofa::linearalgebra::FullVector<typename sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>::Real> >
    ::invert(Matrix& A)
{
    {
        sofa::helper::ScopedAdvancedTimer copyTimer("copyMatrixData");
        Mfiltered.copyNonZeros(A);
        Mfiltered.compress();
    }

    if (!m_map)
    {
        m_map = std::make_unique<EigenSparseMatrixMap>(Mfiltered.rows(), Mfiltered.cols(), Mfiltered.getColsValue().size(),
            (typename EigenSparseMatrixMap::StorageIndex*)Mfiltered.rowBegin.data(), (typename EigenSparseMatrixMap::StorageIndex*)Mfiltered.colsIndex.data(), Mfiltered.colsValue.data());
    }

    sofa::helper::ScopedAdvancedTimer factorizationTimer("factorization");

    std::visit([this](auto&& solver)
    {
        solver.setTolerance(this->d_tolerance.getValue());
        solver.setMaxIterations(this->d_maxIter.getValue());
        solver.compute(*m_map);
    }, m_solver);
}

template <class TBlockType>
void EigenConjugateGradient<sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>, sofa::linearalgebra::FullVector<
typename sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>::Real>>::updatePreconditioner()
{
    if (m_usedPreconditioner != d_preconditioner.getValue().getSelectedId())
    {
        switch(d_preconditioner.getValue().getSelectedId())
        {
        case 0:  m_solver.template emplace<std::variant_alternative_t<0, decltype(m_solver)> >(); break;
        case 1:  m_solver.template emplace<std::variant_alternative_t<1, decltype(m_solver)> >(); break;
        case 2:  m_solver.template emplace<std::variant_alternative_t<2, decltype(m_solver)> >(); break;
        default: m_solver.template emplace<std::variant_alternative_t<1, decltype(m_solver)> >(); break;
        }
        m_usedPreconditioner = d_preconditioner.getValue().getSelectedId();
        if (m_usedPreconditioner >= std::variant_size_v<decltype(m_solver)>)
            m_usedPreconditioner = 1;

        MfilteredrowBegin.clear();
        MfilteredcolsIndex.clear();
        m_map.reset();
    }
}

template <class TBlockType>
EigenConjugateGradient<sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>, sofa::linearalgebra::FullVector<
typename sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>::Real>>::EigenConjugateGradient(): Inherit1()
    , d_maxIter(initData(&d_maxIter, 25u, "iterations","Maximum number of iterations of the Conjugate Gradient solution"))
    , d_tolerance(initData(&d_tolerance, (Real)1e-5,"tolerance","Desired accuracy of the Conjugate Gradient solution evaluating: |r|²/|b|² (ratio of current residual norm over initial residual norm)"))
    , d_preconditioner(initData(&d_preconditioner, sofa::helper::OptionsGroup(3, "identity", "diagonal", "incompleteLUT"), "preconditioner", "The preconditioner used by this solver"))
{
    sofa::helper::getWriteOnlyAccessor(d_preconditioner)->setSelectedItem(1);
}
}


/**
 * \ file UniversalFixedDelayLineFilter.cpp
 */

#include <ATK/Delay/UniversalFixedDelayLineFilter.h>
#include <ATK/Delay/SIMD/UniversalFixedDelayLineFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

#include <ATK/Tools/SIMD/BroadcastFilter.h>
#include <ATK/Tools/SIMD/ReduceFilter.h>

#include <ATK/Mock/SimpleSinusGeneratorFilter.h>
#include <ATK/Mock/TriangleCheckerFilter.h>

#include <ATK/Tools/SumFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE(BOOST_PP_CAT(SIMDPP_ARCH_NAMESPACE, UniversalFixedDelayLineFilter_sinus_line100_delay50_test ))
{
  std::array<float, PROCESSSIZE> data;
  for(ptrdiff_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<float, PROCESSSIZE> outdata;

  ATK::BroadcastFilter<float, simdpp::float32<4>> broadcaster;
  broadcaster.set_input_sampling_rate(48000);
  broadcaster.set_input_port(0, &generator, 0);

  ATK::UniversalFixedDelayLineFilter<simdpp::float32<4>> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &broadcaster, 0);
  filter.set_delay(50);

  ATK::ReduceFilter<simdpp::float32<4>, float> reducer;
  reducer.set_input_sampling_rate(48000);
  reducer.set_input_port(0, &filter, 0);

  ATK::OutPointerFilter<float> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &reducer, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(ptrdiff_t i = 0; i < 50; ++i)
  {
    BOOST_REQUIRE_EQUAL(0, outdata[i]);
  }

  for(ptrdiff_t i = 50; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i - 50] * 4, outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE(BOOST_PP_CAT(SIMDPP_ARCH_NAMESPACE, UniversalFixedDelayLineFilter_sinus_line25_delay24_blend_1_feedforward_1_feedback_0_test))
{
  std::array<float, PROCESSSIZE> data;
  for(ptrdiff_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  ATK::BroadcastFilter<float, simdpp::float32<4>> broadcaster;
  broadcaster.set_input_sampling_rate(48000);
  broadcaster.set_input_port(0, &generator, 0);

  std::array<float, PROCESSSIZE> outdata;

  ATK::UniversalFixedDelayLineFilter<simdpp::float32<4>> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &broadcaster, 0);
  filter.set_delay(24);
  filter.set_blend(1);
  filter.set_feedback(0);
  filter.set_feedforward(1);

  ATK::ReduceFilter<simdpp::float32<4>, float> reducer;
  reducer.set_input_sampling_rate(48000);
  reducer.set_input_port(0, &filter, 0);

  ATK::OutPointerFilter<float> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &reducer, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(ptrdiff_t i = 24; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 0.0001f);
  }
}

BOOST_AUTO_TEST_CASE(BOOST_PP_CAT(SIMDPP_ARCH_NAMESPACE, UniversalFixedDelayLineFilter_sinus_line25_delay24_blend_0_feedforward_0_feedback_1_test))
{
  std::array<float, PROCESSSIZE> data;
  for(ptrdiff_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  ATK::BroadcastFilter<float, simdpp::float32<4>> broadcaster;
  broadcaster.set_input_sampling_rate(48000);
  broadcaster.set_input_port(0, &generator, 0);

  std::array<float, PROCESSSIZE> outdata;

  ATK::UniversalFixedDelayLineFilter<simdpp::float32<4>> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &broadcaster, 0);
  filter.set_delay(24);
  filter.set_blend(0);
  filter.set_feedback(.9999999);
  filter.set_feedforward(0);

  ATK::ReduceFilter<simdpp::float32<4>, float> reducer;
  reducer.set_input_sampling_rate(48000);
  reducer.set_input_port(0, &filter, 0);

  ATK::OutPointerFilter<float> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &reducer, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(ptrdiff_t i = 24; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 0.0001f);
  }
}

#ifdef SIMDPP_EMIT_DISPATCHER

BOOST_AUTO_TEST_CASE(UniversalFixedDelayLineFilter_sinus_line100_delay50_test_dispatcher)
{
  std::array<float, PROCESSSIZE> data;
  for(ptrdiff_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<float, PROCESSSIZE> outdata;

  auto broadcaster = ATK::createBroadcastFilter<float, 4>();
  broadcaster->set_input_sampling_rate(48000);
  broadcaster->set_input_port(0, &generator, 0);

  auto filter = ATK::createUniversalFixedDelayLineFilter<float, 4>(100);
  filter->set_input_sampling_rate(48000);
  filter->set_input_port(0, broadcaster.get(), 0);
  dynamic_cast<ATK::DelayInterface*>(filter.get())->set_delay(50);

  auto reducer = ATK::createReduceFilter<float, 4>();
  reducer->set_input_sampling_rate(48000);
  reducer->set_input_port(0, filter.get(), 0);

  ATK::OutPointerFilter<float> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, reducer.get(), 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(ptrdiff_t i = 0; i < 50; ++i)
  {
    BOOST_REQUIRE_EQUAL(0, outdata[i]);
  }

  for(ptrdiff_t i = 50; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i - 50] * 4, outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE(UniversalFixedDelayLineFilter_sinus_line25_delay24_blend_1_feedforward_1_feedback_0_test_dispatcher)
{
  std::array<float, PROCESSSIZE> data;
  for(ptrdiff_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  auto broadcaster = ATK::createBroadcastFilter<float, 4>();
  broadcaster->set_input_sampling_rate(48000);
  broadcaster->set_input_port(0, &generator, 0);

  std::array<float, PROCESSSIZE> outdata;

  auto filter = ATK::createUniversalFixedDelayLineFilter<float, 4>(25);
  filter->set_input_sampling_rate(48000);
  filter->set_input_port(0, broadcaster.get(), 0);
  dynamic_cast<ATK::DelayInterface*>(filter.get())->set_delay(24);
  dynamic_cast<ATK::UniversalDelayInterface<float>*>(filter.get())->set_blend(1);
  dynamic_cast<ATK::UniversalDelayInterface<float>*>(filter.get())->set_feedback(0);
  dynamic_cast<ATK::UniversalDelayInterface<float>*>(filter.get())->set_feedforward(1);

  auto reducer = ATK::createReduceFilter<float, 4>();
  reducer->set_input_sampling_rate(48000);
  reducer->set_input_port(0, filter.get(), 0);

  ATK::OutPointerFilter<float> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, reducer.get(), 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(ptrdiff_t i = 24; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 0.0001f);
  }
}

BOOST_AUTO_TEST_CASE(UniversalFixedDelayLineFilter_sinus_line25_delay24_blend_0_feedforward_0_feedback_1_test_dispatcher)
{
  std::array<float, PROCESSSIZE> data;
  for(ptrdiff_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  auto broadcaster = ATK::createBroadcastFilter<float, 4>();
  broadcaster->set_input_sampling_rate(48000);
  broadcaster->set_input_port(0, &generator, 0);

  std::array<float, PROCESSSIZE> outdata;

  auto filter = ATK::createUniversalFixedDelayLineFilter<float, 4>(25);
  filter->set_input_sampling_rate(48000);
  filter->set_input_port(0, broadcaster.get(), 0);
  dynamic_cast<ATK::DelayInterface*>(filter.get())->set_delay(24);
  dynamic_cast<ATK::UniversalDelayInterface<float>*>(filter.get())->set_blend(0);
  dynamic_cast<ATK::UniversalDelayInterface<float>*>(filter.get())->set_feedback(.999999);
  dynamic_cast<ATK::UniversalDelayInterface<float>*>(filter.get())->set_feedforward(0);

  auto reducer = ATK::createReduceFilter<float, 4>();
  reducer->set_input_sampling_rate(48000);
  reducer->set_input_port(0, filter.get(), 0);

  ATK::OutPointerFilter<float> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, reducer.get(), 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(ptrdiff_t i = 24; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 0.0001f);
  }
}

#endif


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.

// Legacy code to test some BroadcastVectorInner vertices.
// This code is no longer used.

#define BOOST_TEST_MODULE ChannelMul

#include "../lib/popops/ExprOpUtil.hpp"
#include <boost/multi_array.hpp>
#include <functional>
#include <iostream>
#include <limits>
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_test/Util.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <poputil/VertexTemplates.hpp>

// Tolerances used in tests
#define FLOAT_REL_TOL 0.01
#define HALF_REL_TOL 0.1
#define FLOAT_ABS_TOL 1e-6
#define HALF_ABS_TOL 1e-3

using namespace poplar;
using namespace poplar::program;
using namespace poputil;
using namespace poplibs_test::util;
using namespace poplibs_support;

const OptionFlags options;

struct TestCase {
  poplar::Type type;
  std::size_t scaleLen;
  std::size_t actsLen;
};

struct TestCaseData {
  std::vector<double> scale;
  std::vector<double> actsIn;

  std::unique_ptr<char[]> rawScale;
  std::unique_ptr<char[]> rawActsIn;
  std::unique_ptr<char[]> rawActsOut;
};

static bool channelMulTests(const std::vector<TestCase> &cases) {

  const std::size_t overwriteLen = 32;

  auto device = createTestDevice(TEST_TARGET, 1, 1);
  const auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);

  // One compute set, with a vertex for each test case.
  auto cs = graph.addComputeSet("cs");

  Sequence uploadProg;
  Sequence downloadProg;
  std::vector<std::pair<std::string, char *>> tmap;
  std::vector<TestCaseData> tcData(cases.size());

  for (std::size_t i = 0; i < cases.size(); ++i) {
    const auto &tc = cases[i];
    if (tc.actsLen % tc.scaleLen != 0) {
      return false;
    }

    std::cout << "Test case [" << i << "]: "
              << " scaleLen: " << tc.scaleLen << " actsLen: " << tc.actsLen
              << " type: " << tc.type.toString() << "\n";

    std::string suffix = "_" + std::to_string(i);

    auto scale = graph.addVariable(tc.type, {tc.scaleLen}, "scale" + suffix);
    auto actsIn = graph.addVariable(tc.type, {tc.actsLen}, "actsIn" + suffix);
    auto actsOut = graph.addVariable(tc.type, {tc.actsLen + overwriteLen},
                                     "actsOut" + suffix);
    graph.setTileMapping(scale, 0);
    graph.setTileMapping(actsIn, 0);
    graph.setTileMapping(actsOut, 0);

    std::string templateVertexName =
        templateVertex("popops::BroadcastVectorInnerSupervisor",
                       popops::expr::BinaryOpType::MULTIPLY, tc.type);

    auto v =
        graph.addVertex(cs, templateVertexName,
                        {{"data", actsIn}, {"out", actsOut}, {"B", scale}});

    auto actsBlockCount = tc.actsLen / tc.scaleLen;
    auto actsBlockCountPacked =
        ((actsBlockCount / 6) << 3) | (actsBlockCount % 6);
    uint16_t actsBlockCountPacked16 = actsBlockCountPacked;

    if (actsBlockCountPacked16 != actsBlockCountPacked)
      return false;

    graph.setInitialValue(v["dataBlockCountPacked"], actsBlockCountPacked16);

    graph.setTileMapping(v, 0);

    tcData[i].rawScale = allocateHostMemoryForTensor(
        scale, "scale" + suffix, graph, uploadProg, downloadProg, tmap);
    tcData[i].rawActsIn = allocateHostMemoryForTensor(
        actsIn, "acts" + suffix, graph, uploadProg, downloadProg, tmap);
    tcData[i].rawActsOut = allocateHostMemoryForTensor(
        actsOut, "out" + suffix, graph, uploadProg, downloadProg, tmap);

    tcData[i].scale.resize(tc.scaleLen);
    tcData[i].actsIn.resize(tc.actsLen);

    std::mt19937 randomEngine;
    writeRandomValues(target, scale.elementType(), tcData[i].scale.data(),
                      tcData[i].scale.data() + tcData[i].scale.size(), -2., 2.,
                      randomEngine);
    writeRandomValues(target, actsIn.elementType(), tcData[i].actsIn.data(),
                      tcData[i].actsIn.data() + tcData[i].actsIn.size(), -2.,
                      2., randomEngine);

    copy(target, tcData[i].scale.data(), tcData[i].scale.size(),
         scale.elementType(), tcData[i].rawScale.get());
    copy(target, tcData[i].actsIn.data(), tcData[i].actsIn.size(),
         actsIn.elementType(), tcData[i].rawActsIn.get());
    // Set actsOut to 1, to verify there are no overwrites.
    std::vector<double> ones(actsOut.numElements(), 1.0);
    copy(target, ones.data(), ones.size(), actsOut.elementType(),
         tcData[i].rawActsOut.get());
  }

  std::cout << "Executing engine\n";

  auto prog = Execute(cs);
  Engine engine(graph, Sequence(uploadProg, prog, downloadProg), options);

  attachStreams(engine, tmap);

  device.bind([&](const Device &d) {
    engine.load(d);
    engine.run(0);
  });

  std::cout << "Checking results\n";

  // Check the results for each test case.

  for (std::size_t i = 0; i < tcData.size(); ++i) {
    const auto &tc = cases[i];

    std::cout << "Checking case [" << i << "]\n";

    // Convert back to double.
    std::vector<double> actsOut(tcData[i].actsIn.size() + overwriteLen, 0.0);

    copy(target, tc.type, tcData[i].rawActsOut.get(), actsOut.data(),
         actsOut.size());

    // Check the answer.
    std::vector<double> actsOutRef(actsOut.size(), 1.0);
    for (std::size_t k = 0; k < tc.actsLen; ++k)
      actsOutRef[k] = tcData[i].actsIn[k] * tcData[i].scale[k % tc.scaleLen];

    const double absoluteTolerance =
        tc.type == FLOAT ? FLOAT_ABS_TOL : HALF_ABS_TOL;
    const double relativeTolerance =
        tc.type == FLOAT ? FLOAT_REL_TOL : HALF_REL_TOL;

    auto matchesModel =
        checkIsClose("out", actsOut.data(), {actsOut.size()}, actsOutRef.data(),
                     actsOut.size(), relativeTolerance, absoluteTolerance);
    if (!matchesModel)
      return false;
  }

  return true;
}

const bool isNotSim = !isSimulator(TEST_TARGET);

BOOST_AUTO_TEST_CASE(ChannelMulTiny) {
  std::vector<TestCase> cases = {
      {HALF, 1, 8},  {HALF, 4, 16},  {HALF, 8, 32},  {HALF, 5, 15},
      {FLOAT, 1, 8}, {FLOAT, 4, 16}, {FLOAT, 8, 32}, {FLOAT, 5, 15},
  };
  BOOST_TEST(channelMulTests(cases));
}

BOOST_AUTO_TEST_CASE(ChannelMulSmall,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {HALF, 1, 480},   {HALF, 4, 480},  {HALF, 8, 480},  {HALF, 12, 480},
      {HALF, 16, 480},  {HALF, 1, 15},   {HALF, 4, 12},   {HALF, 8, 40},
      {HALF, 5, 15},    {HALF, 8, 168},

      {FLOAT, 1, 480},  {FLOAT, 4, 480}, {FLOAT, 8, 480}, {FLOAT, 12, 480},
      {FLOAT, 16, 480}, {FLOAT, 1, 15},  {FLOAT, 4, 12},  {FLOAT, 8, 40},
      {FLOAT, 5, 15},   {FLOAT, 8, 168},
  };
  BOOST_TEST(channelMulTests(cases));
}

BOOST_AUTO_TEST_CASE(ChannelMulLarge1_half,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {HALF, 1, 8000},
  };
  BOOST_TEST(channelMulTests(cases));
}

BOOST_AUTO_TEST_CASE(ChannelMulLarge8_half,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {HALF, 8, 8000},
  };
  BOOST_TEST(channelMulTests(cases));
}

BOOST_AUTO_TEST_CASE(ChannelMulLarge1_float,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {FLOAT, 1, 8000},
  };
  BOOST_TEST(channelMulTests(cases));
}

BOOST_AUTO_TEST_CASE(ChannelMulLarge8_float,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {FLOAT, 8, 8000},
  };
  BOOST_TEST(channelMulTests(cases));
}

// Above an addend length over 2044, we switch to scalar code. Check that works.
BOOST_AUTO_TEST_CASE(ChannelMul_MaxChannels_MultipleOfFour_half,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  for (std::size_t scaleLen = 2040; scaleLen <= 2052; scaleLen += 4) {
    std::vector<TestCase> cases = {
        {HALF, scaleLen, scaleLen * 4},
    };
    BOOST_TEST(channelMulTests(cases));
  }
}