#include "conex/cone_program.h"
#include "conex/equality_constraint.h"
#include "conex/linear_constraint.h"
#include "conex/quadratic_cone_constraint.h"
#include <Eigen/Dense>

namespace conex {

using Eigen::MatrixXd;
using Eigen::VectorXd;

// Creates KKT matrix of the form:
//  G << 1, 1,  1,
//       1, 1, -1,
//       1, -1, 0;
// which we fail to factor.
void EqualityConstraintFailingLDLT() {
  int num_vars = 2;
  int num_equalities = 1;
  int num_inequalities = 1;

  MatrixXd A = MatrixXd::Random(num_inequalities, num_vars);
  A << 1, 1;
  MatrixXd C(num_inequalities, 1);
  C.setConstant(1);
  LinearConstraint linear_inequality{A, C};

  MatrixXd eq = MatrixXd::Random(num_equalities, num_vars);
  MatrixXd eq_affine(num_equalities, 1);
  eq_affine.setZero();
  eq << 1, -1;

  Program prog(num_vars);
  prog.AddConstraint(EqualityConstraints{eq, eq_affine}, {0, 1});
  prog.AddConstraint(linear_inequality);

  VectorXd linear_cost(num_vars);
  linear_cost = A.transpose() * C;

  VectorXd solution(num_vars);
  Solve(linear_cost, prog, conex::SolverConfiguration(), solution.data());

  DUMP(eq * solution - eq_affine);
  DUMP(solution);
}

// The centering parameter moves around.
void DoBadInitialization(bool fail) {
  int num_vars = 5;
  int num_inequalities = num_vars + 10;
  int num_equalities = 1;

  MatrixXd A = MatrixXd::Random(num_inequalities, num_vars);
  MatrixXd C(num_inequalities, 1);

  VectorXd optimal_slack(num_inequalities);
  VectorXd optimal_dual(num_inequalities);
  VectorXd optimal_y(num_vars);

  optimal_slack.setConstant(1);
  optimal_dual.setConstant(1);
  int m = num_inequalities * .5;
  optimal_slack.topRows(m).setConstant(1e-3);
  optimal_dual.bottomRows(num_inequalities - m).setConstant(1e-3);

  optimal_y = Eigen::MatrixXd::Random(num_vars, 1);

  C = optimal_slack + A * optimal_y;

  LinearConstraint linear_inequality{A, C};

  Program prog(num_vars);
  MatrixXd eq = MatrixXd::Zero(num_equalities, num_vars);
  Eigen::MatrixXd Bi(1, 3);
  Bi << 1, 2, 3;
  for (int i = 0; i < num_equalities; i++) {
    std::vector<int> vars{0, i + 1, num_vars - 1};
    for (size_t j = 0; j < vars.size(); j++) {
      eq(i, vars.at(j)) = Bi(0, j);
    }
    // prog.AddConstraint(EqualityConstraints{Bi, eq.row(i) * optimal_y    },
    // vars);
  }

  MatrixXd eq_affine(num_equalities, 1);
  eq_affine = eq * optimal_y;

  prog.AddConstraint(EqualityConstraints{eq, eq_affine});
  prog.AddConstraint(linear_inequality);

  VectorXd linear_cost(num_vars);
  linear_cost = A.transpose() * optimal_dual;

  VectorXd solution(num_vars);
  auto config = conex::SolverConfiguration();
  config.final_centering_steps = 10;

  // TODO(FrankPermenter): Understand occasional poor convergence when lowered.
  config.initial_centering_steps_coldstart = 0;
  if (!fail) {
    config.initial_centering_steps_coldstart = 10;
  }
  config.max_iterations = 40;
  config.divergence_upper_bound = .5;
  Solve(linear_cost, prog, config, solution.data());
}

class MPCFailingLDLT {
 public:
  template <typename T>
  using vector = std::vector<T>;
  static constexpr int T = 3;
  static constexpr int nu = 1;
  static constexpr int nx = 2;

  auto InputVars(int i) {
    int offset = T * nx + i * nu;
    // x, u x u
    vector<int> y;
    for (int i = 0; i < nu; i++) {
      y.push_back(i + offset);
    }
    return y;
  }

  auto StateVars(int i) {
    assert(i >= 1);
    int offset = (i - 1) * nx;
    vector<int> y;
    for (int i = 0; i < nx; i++) {
      y.push_back(i + offset);
    }
    return y;
  }

  auto StageVars(int i, bool next_state) {
    vector<int> y;
    if (i > 0) {
      for (const auto& c : StateVars(i)) {
        y.push_back(c);
      }
    }

    for (const auto& c : InputVars(i)) {
      y.push_back(c);
    }

    for (const auto& c : StateVars(i + 1)) {
      y.push_back(c);
    }
    return y;
  }

  auto DynamicsConstraint(const MatrixXd& Ai, const MatrixXd& Bi, int i) {
    if (i > 0) {
      MatrixXd A(nx, 2 * nx + nu);
      A << Ai, Bi, -MatrixXd::Identity(nx, nx);
      return A;
    } else {
      MatrixXd A(nx, nx + nu);
      A << Bi, -MatrixXd::Identity(nx, nx);
      return A;
    }
  }

  void Run(bool fail) {
    int num_vars = T * (nu + nx + 2);
    int epigraph_start = T * (nu + nx);

    MatrixXd Ai = MatrixXd::Random(nx, nx);
    MatrixXd Bi = MatrixXd::Random(nx, nu);
    MatrixXd f = MatrixXd::Random(nx, 1);
    MatrixXd Hxu = MatrixXd::Random(2, nu);
    MatrixXd gxu = MatrixXd::Random(2, 1);

    conex::Program prog(num_vars);
    for (int i = 0; i < T; i++) {
      MatrixXd M(nx, nu + 2 * nx);
      prog.AddConstraint(
          conex::EqualityConstraints{DynamicsConstraint(Ai, Bi, i), f},
          StageVars(i, true));

      if (fail) {
        if (i > 0 && i < T - 1) {
          prog.AddConstraint(conex::LinearConstraint{Hxu, gxu},
                             StageVars(i, false));
        }
      }

      AddQuadraticCostEpigraph(&prog, MatrixXd::Identity(nu, nu), InputVars(i),
                               epigraph_start++);
      AddQuadraticCostEpigraph(&prog, MatrixXd::Identity(nx, nx),
                               StateVars(i + 1), epigraph_start++);
    }

    Eigen::VectorXd var(num_vars);
    VectorXd linear_cost(num_vars);
    linear_cost.setConstant(-1);
    auto config = conex::SolverConfiguration();
    config.inv_sqrt_mu_max = 1e4;
    config.final_centering_steps = 10;
    config.max_iterations = 50;
    conex::Solve(linear_cost, prog, config, var.data());
    DUMP(var);
  }
};

void LPFailSlater(int number_of_implicit_equations) {
  double distance_to_infeasible = 0;
  SolverConfiguration config;
  config.prepare_dual_variables = true;
  config.inv_sqrt_mu_max = 100;
  config.final_centering_tolerance = 1;
  config.infeasibility_threshold = 2000000;
  config.final_centering_steps = 5;

  int m = 10;
  int n1 = number_of_implicit_equations;
  int n2 = 8;
  int n = 2 * n1 + n2;
  Eigen::MatrixXd yref = DenseMatrix::Random(m, 1);

  DenseMatrix A1 = DenseMatrix::Random(n1, m);
  DenseMatrix C1 = A1 * yref;
  DenseMatrix A2 = DenseMatrix::Random(n2, m);
  DenseMatrix C2 = A2 * yref;
  C2.array() += 2;

  DenseMatrix A = DenseMatrix::Random(n, m);
  DenseMatrix C = DenseMatrix::Random(n, 1);
  A << A1, -A1, A2;

  DenseMatrix offset(n1, 1);
  offset.setConstant(distance_to_infeasible);
  C << C1, -(C1 - offset), C2;

  LinearConstraint _constraint{n, &A, &C};

  Program prog(m);
  prog.SetNumberOfVariables(m);
  prog.AddConstraint(_constraint);

  VectorXd b(2);
  VectorXd xref = VectorXd::Random(n);
  xref = xref.array().abs();
  b = A.transpose() * xref;

  DenseMatrix y(m, 1);
  Solve(b, prog, config, y.data());
}
}  // namespace conex

int main() {
  conex::EqualityConstraintFailingLDLT();
  conex::MPCFailingLDLT().Run(true /*trigger fail*/);
  srand(0);
  for (int i = 0; i < 5; i++) {
    conex::DoBadInitialization(true /*trigger fail*/);
  }
  // Triggers factorization failure
  srand(0);
  conex::LPFailSlater(1 /*num implicit eqs*/);
  // Triggers bad convergence
  srand(0);
  conex::LPFailSlater(2 /*num implicit eqs*/);
}


#include "hsi_data_reader.h"
#include "spectral.h"
#include "settings.h"
#include "write_log_file.h"
#include <iostream>
#include <Eigen/Dense>
#include <filesystem>

using namespace hsi;
using hsi::HSIData;
using hsi::HSIDataOptions;
using hsi::HSIDataReader;

int main(int argc, char** argv) {
  // Start timeing
  auto t_start = std::chrono::high_resolution_clock::now();

  // Set parameters
  int verbose = 1;

  // Get the file names for the image and header
  // from user input or use a default
  const std::string hsi_data_path = getDataFname(argc, argv);
  const std::string hsi_header_path = getHeaderFname(argc, argv);

  // Initialize the settings and log file
  settingsValues settings = createSettings(verbose, hsi_data_path);
  Logger_initialize(settings);

  // Set parameters for Eigen computations
  Logger("Number of Eigen Threads: "+std::to_string(Eigen::nbThreads()), settings);
  Eigen::initParallel();

  // Determine the metadata from the header file
  HSIDataOptions data_options(hsi_data_path);
  data_options.ReadHeaderFromFile(hsi_header_path);
  hsi::HSIDataRange data_range = getDataRange(data_options);

  // Read the image data.
  Logger("Reading image.", settings);
  HSIDataReader reader(data_options);
  reader.ReadData(data_range);
  const hsi::HSIData& hsi_data = reader.GetData();

  // Create eigen array of image from the data.
  Logger("Converting image data to Eigen matrix.", settings);
  ImData Im = HSIData2EigenData(hsi_data, data_options, settings);

  // Computing the covariance (and outputting computation time)
  Logger("Computing statistics using Eigen.", settings);
  ImStats stats = computeAnomalyCleanedImageStats(Im, settings);
  //ImStats stats = computeImageStats(Im, settings);

  // Whiten the image
  Logger("Whitening the image.", settings);
  Im.white2d = computeWhitenedImage(Im, stats, settings);

  // Compute the RX anomaly image
  Logger("Compute the RX anomaly image.", settings);
  Im.RX = computeRXimage(Im, settings);
  // Display the image if desired
  showRXimage(Im, settings);

  // Get the file names for the target library and header
  // from user input or use a default
  const std::string tgt_lib_data_path = getTgtLibDataFname(hsi_data_path);
  const std::string tgt_lib_header_path = getTgtLibHeaderFname(hsi_data_path);
  std::cout << tgt_lib_data_path << std::endl;
  std::cout << tgt_lib_header_path << std::endl;

  // Determine the metadata from the header file
  HSIDataOptions tgt_lib_data_options(tgt_lib_data_path);
  tgt_lib_data_options.ReadHeaderFromFile(tgt_lib_header_path);
  hsi::HSIDataRange tgt_lib_data_range = getDataRange(tgt_lib_data_options);

  // Read the image data.
  Logger("Reading Spectral library.", settings);
  HSIDataReader tgt_lib_reader(tgt_lib_data_options);
  tgt_lib_reader.ReadData(tgt_lib_data_range);
  const hsi::HSIData& tgt_lib_data = tgt_lib_reader.GetData();

  // Create eigen array of library from the data.
  Logger("Converting library data to Eigen data.", settings);
  LibData lib_tgt_fullres = SpecLibData2EigenData(tgt_lib_data, tgt_lib_data_options);

  // Create eigen array of library from the data.
  Logger("Resampling library to image wavelengths.", settings);
  LibData lib_tgt = resampleLibrary(lib_tgt_fullres, Im);

  lib_tgt.spectraWhite = computeWhitenedLibrary(lib_tgt, stats, settings);

  // Compute the ACE target detection image
  Eigen::MatrixXf ACE = computeACEimage(Im, lib_tgt, settings);

  // Output results to a subdirectory called \output
  showACEResults(Im, lib_tgt, ACE, settings);

  auto t_end = std::chrono::high_resolution_clock::now();
  auto secondsElapsedWhite = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start);
  Logger("Total Runtim: "+std::to_string(secondsElapsedWhite.count()/1000.)+"s", settings);
  return 0;
}


#pragma once

#include <boost/algorithm/string.hpp>

#include <parse.hpp>

BOOST_AUTO_TEST_CASE(test_parse_lin_com_arr_xy) {

    const std::vector<std::string> input = {
        "0",
        "x",
        "y",
        "x-y",
        "2x+3y",
        "2x-y",
        "x+y",
        "-x-y",
        "-x+y",
    };

    for (const auto& str : input) {
        const auto expr = parse_lin_com_arr_xy(str);
        std::ostringstream oss{};
        oss << expr;
        const auto expr_str = oss.str();
        BOOST_TEST(expr_str == str);
    }
}

BOOST_AUTO_TEST_CASE(test_parse_lin_com_arr_xypi) {
    const std::vector<std::string> input = {
        "0",
        "x",
        "y",
        "pi",
        "x-y",
        "2x+3y",
        "2x-y",
        "x+y",
        "-x-y",
        "-x+y",
        "x+2pi",
    };

    for (const auto& str : input) {
        const auto expr = parse_lin_com_arr_xypi(str);

        std::ostringstream oss{};
        oss << expr;
        const auto expr_str = oss.str();

        BOOST_TEST(expr_str == str);
    }
}

BOOST_AUTO_TEST_CASE(test_parse_lin_com_arr_xyeta) {

    const std::vector<std::string> input = {
        "0",
        "x",
        "y",
        "eta",
        "x-y",
        "2x+3y",
        "2x-y",
        "x+y",
        "-x-y",
        "-x+y",
        "x+2eta",
        "3x",
        "x+3eta",
        "-y-2eta",
    };

    for (const auto& str : input) {
        const auto expr = parse_lin_com_arr_xyeta(str);

        std::ostringstream oss{};
        oss << expr;
        const auto expr_str = oss.str();

        BOOST_TEST(expr_str == str);
    }
}

BOOST_AUTO_TEST_CASE(test_parse_lin_com_map_trig_xy) {

    // Make sure that the order of the expressions is the same as what will
    // be printed
    const std::vector<std::string> input = {
        "0",
        "trig(x)",
        "-trig(x)",
        "-8trig(y)+2trig(3x-y)",
        "-2trig(x-y)",
    };

    for (const auto& string_trig : input) {

        const auto string_sin = boost::replace_all_copy(string_trig, "trig", "sin");
        const auto expr_sin = parse_lin_com_map_sin_xy(string_sin);

        std::ostringstream oss_sin{};
        oss_sin << expr_sin;
        const auto expr_sin_str = oss_sin.str();

        BOOST_TEST(expr_sin_str == string_sin);

        const auto string_cos = boost::replace_all_copy(string_trig, "trig", "cos");
        const auto expr_cos = parse_lin_com_map_cos_xy(string_cos);

        std::ostringstream oss_cos{};
        oss_cos << expr_cos;
        const auto expr_cos_str = oss_cos.str();

        BOOST_TEST(expr_cos_str == string_cos);
    }
}


// Copyright (C) 2014-2016 Vicente J. Botet Escriba
//
//  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)

// <experimental/applicative.hpp>
// <experimental/tuple.hpp>

#include <experimental/tuple.hpp>

#include <boost/detail/lightweight_test.hpp>

int twice(int i) {
  return 2*i;
}

int main()
{
#if __cplusplus >= 201402L
  namespace stde = std::experimental;

  static_assert(stde::is_type_constructible_v<std::pair<int, int>>, "ERROR");
  static_assert(stde::is_applicative<std::tuple<stde::_t>>::value, "ERROR");

  {
    int v=1;
    std::tuple<int> x = stde::make<std::tuple>(v);
    std::tuple<int(*)(int)> f = stde::make<std::tuple>(twice);
    std::tuple<int> y = stde::applicative::ap(f, x);
    BOOST_TEST(std::get<0>(y) == 2);
  }
  {
    int v=1;
    std::tuple<int, int> x = stde::make<std::tuple>(v,v);
    auto f = stde::make<std::tuple>(twice,twice);
    std::tuple<int, int> y = stde::applicative::ap(f, x);
    BOOST_TEST(std::get<0>(y) == 2);
    BOOST_TEST(std::get<1>(y) == 2);
  }
#endif

  return ::boost::report_errors();
}


/*
 * convlayer.cpp
 *
 * Feed-forward:
 *    z(l) = a(l-1) * w(l) + b(l) // where * is conv operation and b(l) is a scalar
 *    a(l) = activation(z(l))
 *
 * Back propagation:
 *    gradient(C, a(l-1)) = full_conv(delta(l), rotate180(w(l)))
 *
 *    delta(l) = elem_prod(gradient(C, a(l)), activation_derivative(z(l)))
 *    dC/dw(l) = conv(a(l), delta(l + 1))
 *    dC/db(l) = sum_elem(delta(l + 1))
 */
#include <boost/assert.hpp>

#include "core/utils.h"
#include "core/random.h"
#include "core/functions.h"
#include "contlayer.h"
#include "convlayer.h"

using namespace std;
using namespace boost;
using namespace yann;

namespace yann {

////////////////////////////////////////////////////////////////////////////////////////////////
//
// ConvolutionalLayerContext implementation
//
class ConvolutionalLayer_Context :
    public Layer::Context
{
  typedef Layer::Context Base;

  friend class ConvolutionalLayer;

public:
  ConvolutionalLayer_Context(
      const MatrixSize & output_size,
      const MatrixSize & batch_size) :
    Base(output_size, batch_size)
  {
    _zz.resizeLike(get_output());
  }
  ConvolutionalLayer_Context(const RefVectorBatch & output) :
    Base(output)
  {
    _zz.resizeLike(get_output());
  }

protected:
  VectorBatch _zz;
}; // class ConvolutionalLayer_Context

////////////////////////////////////////////////////////////////////////////////////////////////
//
// ConvolutionalLayer_TrainingContext implementation
//
class ConvolutionalLayer_TrainingContext :
  public ConvolutionalLayer_Context
{
  typedef ConvolutionalLayer_Context Base;

  friend class ConvolutionalLayer;

public:
  ConvolutionalLayer_TrainingContext(
      const MatrixSize & output_size,
      const MatrixSize & batch_size,
      const MatrixSize & filter_size,
      const unique_ptr<Layer::Updater> & updater) :
    Base(output_size, batch_size),
    _ww_rotated(filter_size, filter_size),
    _delta_ww(filter_size, filter_size),
    _delta_bb(0),
    _ww_updater(updater->copy()),
    _bb_updater(updater->copy())
  {
    YANN_CHECK_GT(filter_size, 0);
    YANN_CHECK_GT(output_size, 0);
    YANN_CHECK_GT(batch_size, 0);

    _delta.resizeLike(_zz);
    _sigma_derivative_zz.resizeLike(_zz);

    _ww_updater->init(filter_size, filter_size);
    _bb_updater->init(1, 1);
  }
  ConvolutionalLayer_TrainingContext(
      const RefVectorBatch & output,
      const MatrixSize & filter_size,
      const unique_ptr<Layer::Updater> & updater) :
    Base(output),
    _ww_rotated(filter_size, filter_size),
    _delta_ww(filter_size, filter_size),
    _delta_bb(0),
    _ww_updater(updater->copy()),
    _bb_updater(updater->copy())
  {
    YANN_CHECK_GT(filter_size, 0);
    YANN_CHECK_GT(yann::get_batch_size(output), 0);
    YANN_CHECK_GT(yann::get_batch_item_size(output), 0);

    _delta.resizeLike(_zz);
    _sigma_derivative_zz.resizeLike(_zz);

    _ww_updater->init(filter_size, filter_size);
    _bb_updater->init(1, 1);
  }

  // Layer::Context  overwrites
  virtual void start_epoch()
  {
    YANN_SLOW_CHECK(_ww_updater);
    YANN_SLOW_CHECK(_bb_updater);

    Base::start_epoch();

    _ww_updater->start_epoch();
    _bb_updater->start_epoch();
  }

  virtual void reset_state()
  {
    YANN_SLOW_CHECK(_ww_updater);
    YANN_SLOW_CHECK(_bb_updater);

    Base::reset_state();

    _delta_ww.setZero();
    _delta_bb = 0;

    _ww_updater->reset();
    _bb_updater->reset();
  }

private:
  Matrix _ww_rotated;
  Matrix _delta_ww;
  Value  _delta_bb;

  VectorBatch _delta;
  VectorBatch _sigma_derivative_zz;

  unique_ptr<Layer::Updater> _ww_updater;
  unique_ptr<Layer::Updater> _bb_updater;
}; // class ConvolutionalLayer_TrainingContext

}; // namespace yann

////////////////////////////////////////////////////////////////////////////////////////////////
//
// yann::ConvolutionalLayer implementation
//
MatrixSize yann::ConvolutionalLayer::get_input_size(
    const MatrixSize & input_rows,
    const MatrixSize & input_cols)
{
  return input_rows * input_cols;
}

MatrixSize yann::ConvolutionalLayer::get_conv_output_rows(
    const MatrixSize & input_rows,
    const MatrixSize & filter_rows)
{
  return (input_rows - filter_rows + 1);
}
MatrixSize yann::ConvolutionalLayer::get_conv_output_cols(
    const MatrixSize & input_cols,
    const MatrixSize & filter_cols)
{
  return (input_cols - filter_cols + 1);
}
MatrixSize yann::ConvolutionalLayer::get_conv_output_size(
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    const MatrixSize & filter_rows,
    const MatrixSize & filter_cols)
{
  return get_conv_output_rows(input_rows, filter_rows)
      * get_conv_output_cols(input_cols, filter_cols);
}
MatrixSize yann::ConvolutionalLayer::get_conv_output_size(
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    const MatrixSize & filter_size)
{
  return get_conv_output_size(input_rows, input_cols, filter_size, filter_size);
}

MatrixSize yann::ConvolutionalLayer::get_full_conv_output_rows(
    const MatrixSize & input_rows,
    const MatrixSize & filter_rows)
{
  return (input_rows + filter_rows - 1);
}
MatrixSize yann::ConvolutionalLayer::get_full_conv_output_cols(
    const MatrixSize & input_cols,
    const MatrixSize & filter_cols)
{
  return (input_cols + filter_cols - 1);
}
MatrixSize yann::ConvolutionalLayer::get_full_conv_output_size(
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    const MatrixSize & filter_rows,
    const MatrixSize & filter_cols)
{
  return get_full_conv_output_rows(input_rows, filter_rows)
      * get_full_conv_output_cols(input_cols, filter_cols);
}
MatrixSize yann::ConvolutionalLayer::get_full_conv_output_size(
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    const MatrixSize & filter_size)
{
  return get_full_conv_output_size(
      input_rows, input_cols,
      filter_size, filter_size);
}

void yann::ConvolutionalLayer::plus_conv(
    const RefConstMatrix & input,
    const RefConstMatrix & filter,
    RefMatrix output,
    bool clear_output)
{
  YANN_SLOW_CHECK_GT(filter.rows(), 0);
  YANN_SLOW_CHECK_GT(filter.cols(), 0);
  YANN_SLOW_CHECK_LE(filter.rows(), input.rows());
  YANN_SLOW_CHECK_LE(filter.cols(), input.cols());

  const auto input_rows = input.rows();
  const auto filter_rows = filter.rows();
  const auto filter_cols = filter.cols();
  const auto output_rows = get_conv_output_rows(input.rows(), filter_rows);
  const auto output_cols = get_conv_output_cols(input.cols(), filter_cols);

  if(clear_output) {
    output.setZero();
  }

  YANN_SLOW_CHECK_EQ(output.rows(), output_rows);
  YANN_SLOW_CHECK_EQ(output.cols(), output_cols);
  YANN_SLOW_CHECK_LE(filter_rows, input_rows);
  YANN_SLOW_CHECK_LE(output_rows, input_rows);

  auto apply_to_row = [&](const auto & ii, const auto & filter_start_row, const auto & filter_max_rows) mutable {
    const auto in_row = input.row(ii);
    // then through the filter by rows, account for "cutoff" at bottom rows
    for(MatrixSize kk = filter_start_row, ll = ii - filter_start_row; kk < filter_max_rows; ++kk, --ll) {
      const auto filter_row = filter.row(kk);
      auto out_row = output.row(ll);
      for(MatrixSize jj = 0; jj < output_cols; ++jj) {
          const auto in_block = in_row.segment(jj, filter_cols);
          out_row(jj) += (in_block.array() * filter_row.array()).sum();
      }
    }
  };

  // iterate through the input by rows
  MatrixSize filter_start_row = 0, filter_max_rows = 0;
  for(MatrixSize ii = 0; ii < input_rows; ++ii) {
    if(ii < filter_rows) {
      // filter at the top of the input; bottom filter rows
      // don't contribute to the output
      ++filter_max_rows;
    }
    if(ii >= output_rows) {
      // filter at the bottom of the input; top filter rows
      // don't contribute to the output
      ++filter_start_row;
    }

    // and now through each col and then filter row
    apply_to_row(ii, filter_start_row, filter_max_rows);
  }
}

void yann::ConvolutionalLayer::plus_conv(
    const RefConstVectorBatch & input,
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    const RefConstMatrix & filter,
    RefVectorBatch output,
    bool clear_output)
{
  YANN_SLOW_CHECK_GT(filter.rows(), 0);
  YANN_SLOW_CHECK_GT(filter.cols(), 0);
  YANN_SLOW_CHECK_LE(filter.rows(), input_rows);
  YANN_SLOW_CHECK_LE(filter.cols(), input_cols);
  YANN_SLOW_CHECK_GT(get_batch_size(input), 0);
  YANN_SLOW_CHECK_EQ(get_batch_item_size(input), input_rows * input_cols);
  YANN_SLOW_CHECK_EQ(get_batch_size(output), get_batch_size(input));

  const auto filter_rows = filter.rows();
  const auto filter_cols = filter.cols();
  const auto output_rows = get_conv_output_rows(input_rows, filter_rows);
  const auto output_cols = get_conv_output_cols(input_cols, filter_cols);
  const auto batch_size = get_batch_size(input);
  YANN_SLOW_CHECK_EQ(get_batch_item_size(output), output_rows * output_cols);

  if(clear_output) {
    output.setZero();
  }

  // ATTENTION: this code operates on raw Matrix.data() and might be broken
  // if Matrix.data() layout changes
  for(MatrixSize ii = 0; ii < batch_size; ++ii) {
    const auto in_batch = get_batch(input, ii);
    auto out_batch = get_batch(output, ii);
    MapConstMatrix in(in_batch.data(), input_rows, input_cols);
    MapMatrix out(out_batch.data(), output_rows, output_cols);
    plus_conv(in, filter, out, false); // we already cleared output if needed
  }
}

void yann::ConvolutionalLayer::full_conv(
    const RefConstMatrix & input,
    const RefConstMatrix & filter,
    RefMatrix output)
{
  YANN_SLOW_CHECK_GT(filter.rows(), 0);
  YANN_SLOW_CHECK_GT(filter.cols(), 0);
  YANN_SLOW_CHECK_LE(filter.rows(), input.rows());
  YANN_SLOW_CHECK_LE(filter.cols(), input.cols());

  const auto input_rows = input.rows();
  const auto input_cols = input.cols();
  const auto filter_rows = filter.rows();
  const auto filter_cols = filter.cols();
  const auto output_cols = get_full_conv_output_cols(input_cols, filter_cols);

  YANN_SLOW_CHECK_EQ(output.rows(), get_full_conv_output_rows(input_rows, filter_rows));
  YANN_SLOW_CHECK_EQ(output.cols(), output_cols);

  // clear output
  output.setZero();

  MatrixSize input_row, output_col, output_last_row;
  auto apply_filter = [&](const auto & in_block, const auto & filter_block) mutable {
    // iterate through the filter by rows, account for "cutoff" at bottom rows
    for(MatrixSize kk = 0; kk < filter_rows; ++kk) {
      output(output_last_row - kk, output_col) += (in_block.array() * filter_block.row(kk).array()).sum();
    }
  };

  // iterate through the input by rows
  for(input_row = 0, output_last_row = filter_rows - 1;
      input_row < input_rows;
      ++input_row, ++output_last_row)
  {
    auto in_row = input.row(input_row);

    // iterate through output columns: we have 3 stages:
    // - filter on the left side of input with partial overlap
    // - filter over input with full overlap
    // - filter on the right side of input with partial overlap

    // on the left side from input
    MatrixSize in_start = 0, filter_start = filter_cols - 1, col_size = 1;
    for(output_col = 0; col_size < filter_cols; ++output_col, ++col_size, --filter_start) {
      // in_start == 0
      // col_size == output_col + 1
      // filter_start == filter_cols - col_size;
      auto in_block = in_row.segment(0, col_size);
      apply_filter(in_block, filter.rightCols(col_size));
    }

    // full overlap by columns
    for(; output_col < input_cols; ++output_col, ++in_start) {
      // in_start == output_col - filter_cols + 1;
      // col_size == filter_cols
      // filter_start == 0
      auto in_block = in_row.segment(in_start, col_size);
      // using here just filter instead of filter.rightCols() affects perf in a bad way
      apply_filter(in_block, filter.leftCols(col_size));
    }

    // on the right side from input
    for(--col_size; output_col < output_cols; ++output_col, ++in_start, --col_size) {
      // in_start == output_col - filter_cols + 1;
      // col_size == input_cols - in_start;
      // filter_start == 0
      auto in_block = in_row.segment(in_start, col_size);
      apply_filter(in_block, filter.leftCols(col_size));
    }
  }
}

void yann::ConvolutionalLayer::full_conv(
    const RefConstVectorBatch & input,
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    const RefConstMatrix & filter,
    RefVectorBatch output)
{
  YANN_SLOW_CHECK_GT(filter.rows(), 0);
  YANN_SLOW_CHECK_GT(filter.cols(), 0);
  YANN_SLOW_CHECK_LE(filter.rows(), input_rows);
  YANN_SLOW_CHECK_LE(filter.cols(), input_cols);
  YANN_SLOW_CHECK_GT(get_batch_size(input), 0);
  YANN_SLOW_CHECK_EQ(get_batch_item_size(input), input_rows * input_cols);
  YANN_SLOW_CHECK_EQ(get_batch_size(output), get_batch_size(input));

  const auto filter_rows = filter.rows();
  const auto filter_cols = filter.cols();
  const auto output_rows = get_full_conv_output_rows(input_rows, filter_rows);
  const auto output_cols = get_full_conv_output_cols(input_cols, filter_cols);
  const auto batch_size = get_batch_size(input);
  YANN_SLOW_CHECK_EQ(get_batch_item_size(output), output_rows * output_cols);

  // ATTENTION: this code operates on raw Matrix.data() and might be broken
  // if Matrix.data() layout changes
  for(MatrixSize ii = 0; ii < batch_size; ++ii) {
    auto in_batch = get_batch(input, ii);
    auto out_batch = get_batch(output, ii);
    MapConstMatrix in(in_batch.data(), input_rows, input_cols);
    MapMatrix out(out_batch.data(), output_rows, output_cols);
    full_conv(in, filter, out);
  }
}

void yann::ConvolutionalLayer::rotate180(const Matrix & input, Matrix & output)
{
  output.resize(input.rows(), input.cols());
  for (MatrixSize ii = 0, size1 = input.rows(), size2 = input.cols(); ii < size1;
      ++ii) {
    for (MatrixSize jj = 0; jj < size2; ++jj) {
      output(ii, jj) = input(size1 - ii - 1, size2 - jj - 1);
    }
  }
}

// Convolutional layer: broadcast from input to all the conv layers
unique_ptr<BroadcastLayer> yann::ConvolutionalLayer::create_conv_bcast_layer(
    const size_t & output_frames_num,
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    const MatrixSize & filter_size,
    const unique_ptr<ActivationFunction> & activation_function)
{
  YANN_CHECK_GT(output_frames_num, 0);

  auto bcast_layer = make_unique<BroadcastLayer>();
  YANN_CHECK(bcast_layer);
  for(auto ii = output_frames_num; ii > 0; --ii) {
    auto conv_layer = make_unique<ConvolutionalLayer>(
        input_rows,
        input_cols,
        filter_size);
    YANN_CHECK(conv_layer);

    if(activation_function) {
      conv_layer->set_activation_function(activation_function);
    }
    bcast_layer->append_layer(std::move(conv_layer));
  }

  return bcast_layer;
}

yann::ConvolutionalLayer::ConvolutionalLayer(
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    const MatrixSize & filter_size) :
  _input_rows(input_rows),
  _input_cols(input_cols),
  _filter_size(filter_size),
  _ww(filter_size, filter_size),
  _bb(0),
  _activation_function(new SigmoidFunction())
{
  YANN_CHECK_GE(input_rows, filter_size);
  YANN_CHECK_GE(input_cols, filter_size);
  YANN_CHECK_GT(filter_size, 0);
}

yann::ConvolutionalLayer::~ConvolutionalLayer()
{
}

void yann::ConvolutionalLayer::set_activation_function(
    const unique_ptr<ActivationFunction> & activation_function)
{
  YANN_CHECK(activation_function);
  _activation_function = activation_function->copy();
}

void yann::ConvolutionalLayer::set_values(const Matrix & ww, const Value & bb)
{
  YANN_CHECK(is_same_size(ww, _ww));
  _ww = ww;
  _bb = bb;
}

// Layer overwrites
bool yann::ConvolutionalLayer::is_valid() const
{
  if(!Base::is_valid()) {
    return false;
  }
  if(!_activation_function) {
    return false;
  }
  return true;
}

std::string yann::ConvolutionalLayer::get_name() const
{
  return "ConvolutionalLayer";
}

string yann::ConvolutionalLayer::get_info() const
{
  YANN_CHECK(is_valid());

  ostringstream oss;
  oss << Base::get_info()
      << " activation: " << _activation_function->get_info()
      << ", input rows: " << _input_rows
      << ", input cols: " << _input_cols
      << ", filter: " << _filter_size
      << ", output rows: " << get_output_rows()
      << ", output cols: " << get_output_cols()
  ;
  return oss.str();
}

bool yann::ConvolutionalLayer::is_equal(const Layer& other, double tolerance) const
{
  if(!Base::is_equal(other, tolerance)) {
    return false;
  }
  auto * the_other = dynamic_cast<const ConvolutionalLayer*>(&other);
  if(the_other == nullptr) {
    return false;
  }
  // TOOD: add deep compare
  if(_activation_function->get_info() != the_other->_activation_function->get_info()) {
    return false;
  }
  if(!_ww.isApprox(the_other->_ww, tolerance)) {
    return false;
  }
  if(fabs(_bb - the_other->_bb) >= tolerance) {
    return false;
  }
  return true;
}

MatrixSize yann::ConvolutionalLayer::get_input_size() const
{
  return get_input_size(_input_rows, _input_cols);
}

MatrixSize yann::ConvolutionalLayer::get_output_size() const
{
  return get_conv_output_size(_input_rows, _input_cols, _filter_size);
}

MatrixSize yann::ConvolutionalLayer::get_output_rows() const
{
  return get_conv_output_rows(_input_rows, _filter_size);
}

MatrixSize yann::ConvolutionalLayer::get_output_cols() const
{
  return get_conv_output_cols(_input_cols, _filter_size);
}

unique_ptr<Layer::Context> yann::ConvolutionalLayer::create_context(
    const MatrixSize & batch_size) const
{
  return make_unique<ConvolutionalLayer_Context>(get_output_size(), batch_size);
}
unique_ptr<Layer::Context> yann::ConvolutionalLayer::create_context(
    const RefVectorBatch & output) const
{
  return make_unique<ConvolutionalLayer_Context>(output);
}
unique_ptr<Layer::Context> yann::ConvolutionalLayer::create_training_context(
    const MatrixSize & batch_size,
    const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(updater);
  YANN_CHECK(is_valid());
  return make_unique<ConvolutionalLayer_TrainingContext>(
      get_output_size(), batch_size, _filter_size, updater);
}
unique_ptr<Layer::Context> yann::ConvolutionalLayer::create_training_context(
    const RefVectorBatch & output,
    const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(updater);
  YANN_CHECK(is_valid());
  return make_unique<ConvolutionalLayer_TrainingContext>(
      output, _filter_size, updater);
}

void yann::ConvolutionalLayer::feedforward(
    const RefConstVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  auto ctx = dynamic_cast<ConvolutionalLayer_Context *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(is_valid());

  // z(l) = conv(a(l-1))*w(l) + b(l)
  plus_conv(input, _input_rows, _input_cols, _ww, ctx->_zz);
  ctx->_zz.array() += _bb;

  // a(l) = activation(z(l))
  YANN_CHECK(is_same_size(ctx->_zz, ctx->get_output()));
  _activation_function->f(ctx->_zz, ctx->get_output(), mode);
}

void yann::ConvolutionalLayer::feedforward(
    const RefConstSparseVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  throw runtime_error("ConvolutionalLayer::feedforward() is not implemented for sparse vectors");
}

void yann::ConvolutionalLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  auto ctx = dynamic_cast<ConvolutionalLayer_TrainingContext*>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(is_valid());
  YANN_CHECK_GT(get_batch_size(gradient_output), 0);
  YANN_CHECK_EQ(get_batch_item_size(gradient_output), get_output_size());
  YANN_CHECK_EQ(get_batch_size(input), get_batch_size(gradient_output));
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK(!gradient_input || is_same_size(input, *gradient_input));

  // just to make it easier to read
  const auto & zz = ctx->_zz;
  auto & sigma_derivative_zz = ctx->_sigma_derivative_zz;
  auto & delta = ctx->_delta;
  auto & delta_ww = ctx->_delta_ww;
  auto & delta_bb = ctx->_delta_bb;
  const auto batch_size = get_batch_size(input);

  // delta(l) = elem_prod(gradient(C, a(l)), activation_derivative(z(l)))
  YANN_CHECK(is_same_size(zz, sigma_derivative_zz));
  YANN_CHECK(is_same_size(zz, gradient_output));
  _activation_function->derivative(zz, sigma_derivative_zz);
  delta.array() = gradient_output.array() * sigma_derivative_zz.array();

  // update deltas
  // dC/dw(l) = conv(a(l), delta(l + 1))
  // dC/db(l) = sum_elem(delta(l + 1))
  YANN_CHECK_EQ(batch_size, get_batch_size(delta));
  for (MatrixSize ii = 0; ii < batch_size; ++ii) {
    auto input_batch = get_batch(input, ii);
    auto delta_batch = get_batch(delta, ii);
    MapConstMatrix input_row(input_batch.data(), _input_rows, _input_cols);
    MapConstMatrix delta_row(delta_batch.data(), get_output_rows(), get_output_cols());
    plus_conv(input_row, delta_row, delta_ww, false); // delta_ww += conv()
  }
  delta_bb += delta.array().sum();

  // we don't need to calculate the first gradient(C, a(l)) for the actual inputs
  if(gradient_input) {
    // gradient(C, a(l-1)) = full_conv(delta(l), rotate180(w(l)))
    rotate180(_ww, ctx->_ww_rotated); // TODO: we can cache the rotation if _ww doesn't change
    full_conv(delta, get_output_rows(), get_output_cols(), ctx->_ww_rotated, *gradient_input);
  }
}

void yann::ConvolutionalLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstSparseVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  throw runtime_error("ConvolutionalLayer::backprop() is not implemented for sparse vectors");
}

void yann::ConvolutionalLayer::init(enum InitMode mode, optional<InitContext> init_context)
{
  switch (mode) {
  case InitMode_Zeros:
    _ww.setZero();
    _bb = 0;;
    break;
  case InitMode_Random:
    {
      unique_ptr<RandomGenerator> gen01 = RandomGenerator::normal_distribution(0, 1,
          init_context ? optional<Value>(init_context->seed()) : boost::none);
      gen01->generate(_ww);
      gen01->generate(_bb);
    }
    break;
  }
}

void yann::ConvolutionalLayer::update(Context * context, const size_t & tests_num)
{
  auto ctx = dynamic_cast<ConvolutionalLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(ctx->_ww_updater);
  YANN_CHECK(ctx->_bb_updater);
  YANN_CHECK(is_same_size(_ww, ctx->_delta_ww));

  ctx->_ww_updater->update(ctx->_delta_ww, tests_num, _ww);
  ctx->_bb_updater->update(ctx->_delta_bb, tests_num, _bb);
}

// the format is (w:<weights>,b:<bias>)
void yann::ConvolutionalLayer::read(std::istream & is)
{
  Base::read(is);

  read_char(is, '(');
  read_object(is, "w", _ww);
  read_char(is, ',');
  read_object(is, "b", _bb);
  read_char(is, ')');
}

// the format is (w:<weights>,b:<bias>)
void yann::ConvolutionalLayer::write(std::ostream & os) const
{
  Base::write(os);

  os << "(";
  write_object(os, "w", _ww);
  os << ",";
  write_object(os, "b", _bb);
  os << ")";
}


#include "testsuite.h"
#include <blitz/array.h>
#include <blitz/array.cc>
#include <blitz/tinyvec2.cc>
#include <blitz/tinymat2.h>
#include <blitz/tinymat2.cc>
// #include <blitz/array/asexpr.cc>
// #include <blitz/array/ops.h>
// #include <blitz/array/ops.cc>
// #include <blitz/array/fastiter.h>
// #include <blitz/array/methods.cc>
// #include <blitz/array/methods.cc>
// #include <blitz/array/io.cc>
// #include <blitz/ops.h>
// #include <blitz/array/funcs.h>

BZ_USING_NAMESPACE(blitz)

// Tests functors on multicomponent arrays.

static const double eps = 0.0001;

#define ISCLOSE(a, b) BZTEST(fabs((a)-(b))<eps)

typedef TinyVector<double,2> tv;
typedef TinyMatrix<double,2,2> tmt;

// types to test
typedef Array<double, 1> a1;
typedef Array<tv, 1> a1v;
typedef Array<tmt, 1> a1m;
typedef TinyVector<tv, 5> tvv;
typedef TinyMatrix<tv, 2,2> tmv;
// and the hardcore multi-level multicomponent types
typedef Array<TinyVector<tv, 2>, 1> a1vv;
typedef Array<TinyMatrix<tv, 2,2>, 1> a1mv;

class ufunc {
public:
  double operator()(double d) const { return 2*(-d)+sqrt(d*d); }
  BZ_DECLARE_FUNCTOR(ufunc);
};

class bifunc {
public:
  double operator()(double a, double b) const { return 2*(-a)+sqrt(b*b); }
  BZ_DECLARE_FUNCTOR2(bifunc);
};

class trifunc {
public:
  double operator()(double a, double b, double c) const
  { return 2*(-a)+sqrt(b*c); }
  BZ_DECLARE_FUNCTOR3(trifunc);
};

class umemfunc {
public:
  double func(double d) const { return 2*(-d)+sqrt(d*d); }
  BZ_DECLARE_MEMBER_FUNCTION(umemfunc, func);
};

class bimemfunc {
public:
  double func(double a, double b) const { return 2*(-a)+sqrt(b*b); }
  BZ_DECLARE_MEMBER_FUNCTION2(bimemfunc, func);
};

class trimemfunc {
public:
  double func(double a, double b, double c) const { return 2*(-a)+sqrt(b*c); }
  BZ_DECLARE_MEMBER_FUNCTION3(trimemfunc, func);
};

const int sz=5;

void test_results(a1& a1A, a1& a1B, a1v& a1vA, a1v& a1vB,
		  a1m& a1mA, a1m& a1mB, tvv& tvvA, tvv& tvvB,
		  tmv& tmvA, tmv& tmvB, a1vv& a1vvA, a1vv& a1vvB,
		  a1mv& a1mvA, a1mv& a1mvB)
{
  for(int i=0; i<sz; ++i) {
    ISCLOSE ( a1B(i), -a1A(i) );

    for(int j=0; j<2; ++j) {
      ISCLOSE(a1vB(i)(j), a1vA(i)(j)>0?-a1vA(i)(j):-3*a1vA(i)(j));
      ISCLOSE(tvvB(i)(j), tvvA(i)(j)>0?-tvvA(i)(j):-3*tvvA(i)(j));
      for(int k=0; k<2; ++k) {
	ISCLOSE(a1mB(i)(j,k),a1mA(i)(j,k)>0?-a1mA(i)(j,k):-3*a1mA(i)(j,k));
	ISCLOSE(a1vvB(i)(j)(k),a1vvA(i)(j)(k)>0?-a1vvA(i)(j)(k):-3*a1vvA(i)(j)(k));
	if(i<2) {
	  ISCLOSE(tmvB(j,k)(i),tmvA(j,k)(i)>0?-tmvA(j,k)(i):-3*tmvA(j,k)(i));
	}
	for(int l=0; l<2; ++l) {
	  ISCLOSE(a1mvB(i)(j,k)(l),a1mvA(i)(j,k)(l)>0?-a1mvA(i)(j,k)(l):-3*a1mvA(i)(j,k)(l));
	}
      }
    }
  }
}

int main()
{

  // create operands
  a1 a1A(sz), a1B(sz), a1C(sz);
  a1v a1vA(sz), a1vB(sz), a1vC(sz);
  a1m a1mA(sz), a1mB(sz), a1mC(sz);
  tvv tvvA, tvvB, tvvC;
  tmv tmvA, tmvB, tmvC;
  a1vv a1vvA(sz), a1vvB(sz), a1vvC(sz);
  a1mv a1mvA(sz), a1mvB(sz), a1mvC(sz);

  // fill them with data
  a1A=1,2,3,4,5;
  a1vA=tv(1,-1),tv(2,-2),tv(3,-3),tv(4),tv(5);
  a1mA=tmt(1),tmt(2),tmt(3),tmt(4),tmt(5);
  a1mA(2)=0,1,2,3;
  tvvA=tv(1,-1),tv(2,-2),tv(3,-3),tv(4),tv(5);
  tmvA=tv(1,-1),tv(2,-2),tv(3,-3),tv(4);
  a1vvA=TinyVector<TinyVector<double,2>,2>(tv(1,-1),tv(2,-2)),
    TinyVector<TinyVector<double,2>,2>(tv(3,-3),tv(3,-3)),
    TinyVector<TinyVector<double,2>,2>(tv(4,-4),tv(5,-5)),
    TinyVector<TinyVector<double,2>,2>(tv(6,6),tv(7,7)),
    TinyVector<TinyVector<double,2>,2>(-8);
  a1mvA=TinyMatrix<TinyVector<double,2>,2,2>(tv(1,-1));
  a1mvA(1)=2;
  a1mvA(2)=tv(1,-1),tv(2,-2),tv(3,-3),tv(4);
  a1mvA(3)(1,1)[1]=-42;

  ufunc uf;
  bifunc bf;
  trifunc tf;
  umemfunc umf;
  bimemfunc bmf;
  trimemfunc tmf;

  // test unary functors
  a1B=uf(a1A);
  a1vB=uf(a1vA);
  a1mB=uf(a1mA);
  tvvB=uf(tvvA);
  tmvB=uf(tmvA);
  a1vvB=uf(a1vvA);
  a1mvB=uf(a1mvA);

  test_results(a1A,a1B,a1vA,a1vB,a1mA,a1mB,tvvA,tvvB,tmvA,
	       tmvB,a1vvA,a1vvB,a1mvA,a1mvB);

  // test binary functors
  a1B=bf(a1A,a1A);
  a1vB=bf(a1vA,a1vA);
  a1mB=bf(a1mA,a1mA);
  tvvB=bf(tvvA,tvvA);
  tmvB=bf(tmvA,tmvA);
  a1vvB=bf(a1vvA,a1vvA);
  a1mvB=bf(a1mvA,a1mvA);

  test_results(a1A,a1B,a1vA,a1vB,a1mA,a1mB,tvvA,tvvB,tmvA,
	       tmvB,a1vvA,a1vvB,a1mvA,a1mvB);

  // test ternary functors
  a1B=tf(a1A,a1A,a1A);
  a1vB=tf(a1vA,a1vA,a1vA);
  a1mB=tf(a1mA,a1mA,a1mA);
  tvvB=tf(tvvA,tvvA,tvvA);
  tmvB=tf(tmvA,tmvA,tmvA);
  a1vvB=tf(a1vvA,a1vvA,a1vvA);
  a1mvB=tf(a1mvA,a1mvA,a1mvA);

  test_results(a1A,a1B,a1vA,a1vB,a1mA,a1mB,tvvA,tvvB,tmvA,
	       tmvB,a1vvA,a1vvB,a1mvA,a1mvB);

  // test unary member functions
  a1B=umf.func(a1A);
  a1vB=umf.func(a1vA);
  a1mB=umf.func(a1mA);
  tvvB=umf.func(tvvA);
  tmvB=umf.func(tmvA);
  a1vvB=umf.func(a1vvA);
  a1mvB=umf.func(a1mvA);

  test_results(a1A,a1B,a1vA,a1vB,a1mA,a1mB,tvvA,tvvB,tmvA,
	       tmvB,a1vvA,a1vvB,a1mvA,a1mvB);

  // test binary member functions
  a1B=bmf.func(a1A,a1A);
  a1vB=bmf.func(a1vA,a1vA);
  a1mB=bmf.func(a1mA,a1mA);
  tvvB=bmf.func(tvvA,tvvA);
  tmvB=bmf.func(tmvA,tmvA);
  a1vvB=bmf.func(a1vvA,a1vvA);
  a1mvB=bmf.func(a1mvA,a1mvA);

  test_results(a1A,a1B,a1vA,a1vB,a1mA,a1mB,tvvA,tvvB,tmvA,
	       tmvB,a1vvA,a1vvB,a1mvA,a1mvB);

  // test ternary member functions
  a1B=tmf.func(a1A,a1A,a1A);
  a1vB=tmf.func(a1vA,a1vA,a1vA);
  a1mB=tmf.func(a1mA,a1mA,a1mA);
  tvvB=tmf.func(tvvA,tvvA,tvvA);
  tmvB=tmf.func(tmvA,tmvA,tmvA);
  a1vvB=tmf.func(a1vvA,a1vvA,a1vvA);
  a1mvB=tmf.func(a1mvA,a1mvA,a1mvA);

  test_results(a1A,a1B,a1vA,a1vB,a1mA,a1mB,tvvA,tvvB,tmvA,
	       tmvB,a1vvA,a1vvB,a1mvA,a1mvB);

  // make sure functors can be applied to expressions too (the
  // functors and member functions use the same machinery so if one
  // works, the others should, too...)
  a1B=uf(1.0*a1A);
  a1vB=uf(1.0*a1vA);
  a1mB=uf(1.0*a1mA);
  tvvB=uf(1.0*tvvA);
  tmvB=uf(1.0*tmvA);
  a1vvB=uf(1.0*a1vvA);
  a1mvB=uf(1.0*a1mvA);

  test_results(a1A,a1B,a1vA,a1vB,a1mA,a1mB,tvvA,tvvB,tmvA,
	       tmvB,a1vvA,a1vvB,a1mvA,a1mvB);

  a1B=bmf.func(1.0*a1A,a1A+0);
  a1vB=bmf.func(1.0*a1vA,a1vA+0);
  a1mB=bmf.func(1.0*a1mA,a1mA+0);
  tvvB=bmf.func(1.0*tvvA,tvvA+0);
  tmvB=bmf.func(1.0*tmvA,tmvA+0);
  a1vvB=bmf.func(1.0*a1vvA,a1vvA+0);
  a1mvB=bmf.func(1.0*a1mvA,a1mvA+0);

  test_results(a1A,a1B,a1vA,a1vB,a1mA,a1mB,tvvA,tvvB,tmvA,
	       tmvB,a1vvA,a1vvB,a1mvA,a1mvB);

  a1B=tmf.func(1.0*a1A,a1A+0,a1A-0);
  a1vB=tmf.func(1.0*a1vA,a1vA+0,a1vA-0);
  a1mB=tmf.func(1.0*a1mA,a1mA+0,a1mA-0);
  tvvB=tmf.func(1.0*tvvA,tvvA+0,tvvA-0);
  tmvB=tmf.func(1.0*tmvA,tmvA+0,tmvA-0);
  a1vvB=tmf.func(1.0*a1vvA,a1vvA+0,a1vvA-0);
  a1mvB=tmf.func(1.0*a1mvA,a1mvA+0,a1mvA-0);

  test_results(a1A,a1B,a1vA,a1vB,a1mA,a1mB,tvvA,tvvB,tmvA,
	       tmvB,a1vvA,a1vvB,a1mvA,a1mvB);

  return 0;
}


#ifndef SIMULATION_POSTIONCONTROLLER_H
#define SIMULATION_POSTIONCONTROLLER_H

#include <ros/ros.h>
#include <ros/node_handle.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
#include <cmath>
#include <sensor_msgs/JointState.h>
/*
std_msgs/Header header
string[] name
float64[] position
float64[] velocity
float64[] effort
*/
#include <geometry_msgs/WrenchStamped.h>
/*
std_msgs/Header header
geometry_msgs/Wrench wrench
*/
#include <geometry_msgs/Point.h>
/*
float64 x
float64 y
float64 z
*/
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>

// constant
#define TOTAL_DOF 7
#define SIM_DT 0.01 //unit: sec  10ms
#define PI 3.14159265359
//Macro
#define deg2rad(deg) ((deg) * PI / 180.0)
#define rad2deg(rad) ((rad) / PI * 180.0)

const std::string JOINT_NAME[TOTAL_DOF]={"Franka_joint1","Franka_joint2","Franka_joint3","Franka_joint4",
                                        "Franka_joint5","Franka_joint6","Franka_joint7"};
class posController
{
public:
  //contructor
  posController(ros::NodeHandle nh_,double hz_);
  //destructor
  ~posController();

  //-------------- Callback Functions for subscribing topics --------------

  //joint Callback function
  void jointStateCallback(const sensor_msgs::JointStateConstPtr& msg);
  //simulation state callback 0: stopped 1:running 2: paused
  void simStateCallback(const std_msgs::Int32ConstPtr& msg);
  //simulation time callback
  void simTimeCallback(const std_msgs::Float32ConstPtr& msg);
  //simulation step done callback
  void simStepDoneCallback(const std_msgs::BoolConstPtr& msg);

  //-------------- vrep interface functions --------------

  void vrepStart();
  void vrepStop();
  void vrepStepTrigger();
  void vrepEnableSyncMode();
  void readVrep();
  void writeVrep();

  //-------------- position controller functions --------------

  void setDesiredPos(std::vector<float> des_q); //how about Eigen::Vectorxd
  void setExecTime(float t);
  void compute();
  void wait();
  void getTarget(ros::NodeHandle nh_);

private:
  //use underscore at the end to represent member variables in class

  //-------------- Publisher Variables --------------

  ros::Publisher joint_set_pub_;

  ros::Publisher sim_start_pub_;
  //ros::Publisher simPausePub_; //if u need it, use it
  ros::Publisher sim_stop_pub_;

  ros::Publisher sim_enable_sync_mode_pub_;
  ros::Publisher trigger_next_step_pub_;

  //-------------- Subscriber Variables --------------

  ros::Subscriber joint_state_sub_;
  ros::Subscriber sim_step_done_sub_;
  ros::Subscriber sim_state_sub_;
  ros::Subscriber sim_time_sub_;

  //-------------- Time or Simulation Variables --------------
  bool sim_step_done_;
  float sim_time_;
  int tick_;
  ros::Rate rate_;

  //-------------- Joint Variables --------------

  Eigen::VectorXd q_; //current joint position
  Eigen::VectorXd q_dot_; //current joint velocity

  sensor_msgs::JointState joint_cmd_;

  Eigen::VectorXd desired_q_; // controller input from cubic spline
  Eigen::VectorXd target_q_;  // target position

  Eigen::VectorXd init_q_;
  Eigen::VectorXd goal_q_err_;

  bool is_first_run_;
  double start_time_;
  double current_time_;
  double final_time_;
  int vrep_sim_status_;
  float exec_time_;
};

static double cubicSpline(double time,
                          double time_0,
                          double time_f,
                          double x_0,
                          double x_f,
                          double x_dot_0,
                          double x_dot_f)
//a0 + a1*t + a2*t^2 + a3*t^2 = u(t) 4 unknown 4 eqn(4 boundary condition)
{
  double x_t;
  if ( time < time_0 )
    x_t = x_0;
  else if (time > time_f )
    x_t = x_f;
  else
  {
    double elapsed_time = time - time_0;
    double total_time = time_f - time_0;
    double a0 = x_0;
    double a1 = x_dot_0;
    double a2 = 3.0/total_time/total_time*(x_f - x_0) - 2.0/total_time*x_dot_0 - 1.0/total_time*x_dot_f;
    double a3 = -2.0/total_time/total_time/total_time*(x_f - x_0) + 1.0/total_time/total_time*(x_dot_f + x_dot_0);

    x_t = a0 + a1*elapsed_time + a2*pow(elapsed_time,2) + a3*pow(elapsed_time,3);
  }
  return x_t;
}
#include <string>
#include <sstream>
using namespace std;

template<typename T>
string ToString(T t)
{

  stringstream ss;

  ss << t;

  return ss.str();
}
#endif


//
// Created by wang yu on 2018/6/20.
//
#include "mview.h"
#include <iostream>

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <opencv2/core.hpp>
#include <opencv/cv.hpp>
#include <opencv2/core/eigen.hpp>

static int debug_count = 0;

cv::Mat convertRgbToOpenCV(const RgbImage& rgb) {
	GrayImage r = rgb.unaryExpr([](Eigen::Vector3f rgb) { return rgb[0]; });
	GrayImage g = rgb.unaryExpr([](Eigen::Vector3f rgb) { return rgb[1]; });
	GrayImage b = rgb.unaryExpr([](Eigen::Vector3f rgb) { return rgb[2]; });

	cv::Mat r_mat;
    cv::eigen2cv(r,r_mat);
	cv::Mat g_mat;
    cv::eigen2cv(g,g_mat);
	cv::Mat b_mat;
    cv::eigen2cv(b,b_mat);

	cv::Mat arr[3] { r_mat, g_mat, b_mat };
	cv::Mat rgb_mat;
	cv::merge(arr, 3, rgb_mat);
	return rgb_mat;
}

RgbImage convertOpenCVToRgb(const cv::Mat imageMat){
    cv::Mat rgb[3];
    cv::split(imageMat, rgb);

	int width = imageMat.cols;
	int height = imageMat.rows;
    GrayImage r(height, width), g(height, width), b(height, width);

    cv::cv2eigen(rgb[0], r);
    cv::cv2eigen(rgb[1], g);
    cv::cv2eigen(rgb[2], b);

    RgbImage rgbImage (g.rows(), g.cols());

    for(int row=0;row<g.rows();row++){
        for(int col=0;col<g.cols();col++){
            rgbImage(row,col)<<r(row,col),g(row,col),b(row,col);
        }
    }

    return rgbImage;
}

void remap_rgb(cv::Mat& rgb_image, cv::Mat map1, cv::Mat map2) {
	cv::Mat rgbs[3];
	cv::split(rgb_image, rgbs);

	cv::Mat rgbs_out[3];
	for(int i = 0; i < 3; i++)
		cv::remap(rgbs[i], rgbs_out[i], map1, map2, cv::INTER_NEAREST, cv::BORDER_CONSTANT);

	cv::merge(rgbs_out, 3, rgb_image);
}

auto rectify(const Image& left, const Image& right) -> Rectified{
    //load data
    GrayImage left_gray_pixels=left.gray_pixels;
    RgbImage left_rgb_pixels=left.rgb_pixels;

    GrayImage right_gray_pixels=right.gray_pixels;
    RgbImage right_rgb_pixels=right.rgb_pixels;

	Eigen::Matrix4f left_to_right = right.extrinsics * left.extrinsics.inverse();
	for(int i = 0; i < 3; i++)
		assert(left_to_right(3, i) == 0);
	assert(left_to_right(3, 3) == 1.0);

    //rotation and transformation from left camera to right camera
    Eigen::Matrix3f R = left_to_right.block<3, 3>(0, 0);
    Eigen::Vector3f T = R.transpose() * left_to_right.block<3, 1>(0, 3);

    //convert eigen matrix to mat
    cv::Mat left_gray_mat, left_ground_mat;
    cv::eigen2cv(left_gray_pixels, left_gray_mat);
    cv::eigen2cv(left.ground_truth, left_ground_mat);
    cv::Mat left_rgb_mat = convertRgbToOpenCV(left_rgb_pixels);

    cv::Mat right_gray_mat, right_ground_mat;
    cv::eigen2cv(right_gray_pixels, right_gray_mat);
    cv::eigen2cv(right.ground_truth, right_ground_mat);
    cv::Mat right_rgb_mat = convertRgbToOpenCV(right_rgb_pixels);
    cv::Mat left_intrinsics_mat, right_intrinsics_mat, R_mat, T_mat;

    cv::eigen2cv(left.intrinsics,left_intrinsics_mat);
    cv::eigen2cv(right.intrinsics,right_intrinsics_mat);

    cv::eigen2cv(R,R_mat);
    cv::eigen2cv(T,T_mat);

	R_mat.assignTo(R_mat, CV_64F);
	T_mat.assignTo(T_mat, CV_64F);

    cv::Size imageSize = left_gray_mat.size();
	cv::Size targetSize = imageSize; // (left_gray_mat.cols/4., left_gray_mat.rows/4.);
    cv::Mat R1,R2,P1,P2,Q;
    cv::stereoRectify(left_intrinsics_mat,{},right_intrinsics_mat,{},imageSize,R_mat,T_mat,R1,R2,P1,P2,Q,
			cv::CALIB_ZERO_DISPARITY,
		   	1,targetSize,0,0);

    cv::Mat map1, map2, left_gray_out, left_ground_out;
    cv::initUndistortRectifyMap(left_intrinsics_mat,{},R1,P1,targetSize,CV_32FC1,map1,map2);
    cv::remap(left_gray_mat, left_gray_out, map1, map2, cv::INTER_NEAREST, cv::BORDER_CONSTANT);
    cv::remap(left_ground_mat, left_ground_out, map1, map2, cv::INTER_NEAREST, cv::BORDER_CONSTANT);
	left_gray_mat = left_gray_out;
	remap_rgb(left_rgb_mat, map1, map2);

    cv::Mat map3,map4, right_gray_out, right_ground_out;
    cv::initUndistortRectifyMap(right_intrinsics_mat,{},R2,P2,targetSize,CV_32FC1,map3,map4);
    cv::remap(right_gray_mat, right_gray_out, map3, map4, cv::INTER_NEAREST, cv::BORDER_CONSTANT);
    cv::remap(right_ground_mat, right_ground_out, map3, map3, cv::INTER_NEAREST, cv::BORDER_CONSTANT);
	right_gray_mat = right_gray_out;
	remap_rgb(right_rgb_mat, map3, map4);

	std::cout << "Debugging rectification results\n";
	std::cout << "R1 " << R1 << "\nR2 " << R2 << "\nP1 " << P1 << "\nP2" << P2 << "\nQ " << Q << std::endl;

#if !MVIEW_NDEBUG
	std::stringstream debug_name;
	std::string debug_left = ((debug_name << "debug.rectified" << debug_count << ".left.png"), debug_name.str()); debug_name.str("");
	std::string debug_right = ((debug_name << "debug.rectified" << debug_count << ".right.png"), debug_name.str()); debug_name.str("");
	std::string debug_depth = ((debug_name << "debug.rectified" << debug_count << ".depth.png"), debug_name.str()); debug_name.str("");
	debug_count++;

	cv::imwrite(debug_left, left_gray_mat*255.);
	cv::imwrite(debug_right, right_gray_mat*255.);
	cv::imwrite(debug_depth, left_ground_out*25.);
#endif

	Rectified rectified;

    //convert mat to eigen matrix
	rectified.pixel_left_gray = GrayImage(left_gray_mat.rows, left_gray_mat.cols);
	rectified.left_ground_truth = GrayImage(left_gray_mat.rows, left_gray_mat.cols);
    cv::cv2eigen(left_gray_mat, rectified.pixel_left_gray);
    cv::cv2eigen(left_ground_out, rectified.left_ground_truth);
	rectified.pixel_left_rgb = convertOpenCVToRgb(left_rgb_mat);

	rectified.pixel_right_gray = GrayImage (right_gray_mat.rows, right_gray_mat.cols);
	rectified.right_ground_truth = GrayImage(right_gray_mat.rows, right_gray_mat.cols);
    cv::cv2eigen(right_gray_mat, rectified.pixel_right_gray);
    cv::cv2eigen(right_ground_out, rectified.right_ground_truth);
	rectified.pixel_right_rgb = convertOpenCVToRgb(right_rgb_mat);

	rectified.extrinsics_left = left.extrinsics;
	rectified.extrinsics_right = right.extrinsics;

	rectified.R1 = R1;
	rectified.R2 = R2;
	rectified.P1 = P1;
	rectified.P2 = P2;
	rectified.Q = Q;

    return rectified;
}


// implements the solver class - Felipe Figueredo Rocha
#ifndef _solver_hpp
#define _solver_hpp

#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>
#include <vector>
#include <stdlib.h>
#include <sstream>
#include <cmath>
#include "utils.hpp"
#include "linAlg.hpp"
#include "data.hpp"
#include "simulSettings.hpp"
#include "solvergp.h"
#include <armadillo>

//~ #define DEBUG if(false)
#define DEBUG if(true)

using namespace arma;
using namespace std;

class Solver{
	public:
	// see constructor for further explanation about the variabls
	Data *d;
	SimulSettings *s;
	mat *A; // stiffness matrix
	vec *b; // force vector
	vec *Sol; // force vector
	vector<intVec*> *Coupling; // Coupling vector, works similar to flagDirich
	Solver() {}
	void init();
	void prepareMatrices(); // alloc linear system matrices and vector
	void createCoupling();
	void linsolve(); // call the routine of solving the lin System
	void enforceDirichlet(SGPrealVec *vDirich, intVec *flagDirich, double pen); // modify matrices to enforce Dirichlet
	void assembly(SGPrealMat *A_L, SGPrealVec *b_L, int e); // assembly local in global matrices
	void assemblyCoupling(intVec *Cvec, intMat *Cmat_L, int e);
	void globalToLocal(SGPrealVec *XLL,SGPrealVec *Sol0E, SGPrealVec *Sol1E,SGPrealVec *ParamE, int e); // fills the e^{th} of the mesh in a auxiliary object
	void localToGlobal(SGPrealVec *ParamE,int e); // fills the e^{th} of the mesh in a auxiliary object
	void run();
};

void Solver:: init(){
	s = new SimulSettings;
	s->loadSettings();
	d = new Data;
	d->loadSGPmesh();
	d->loadSGPdirich(s->NSubsteps);
	d->loadSGPinifile();
	d->loadSGPparam();
	prepareMatrices();
	createCoupling();
}

void Solver:: prepareMatrices(){
	int n = d->Ndof*d->Nnodes;

 	A = new mat(n,n);
	b = new vec(n);
	Sol = new vec(n);

}

void Solver:: createCoupling(){
	int n = d->Ndof*d->Nnodes;
	int NodElt = d->eNNod->max();
	int MaxLRows = NodElt*d->Ndof;
	intMat *CouplingE = new intMat(MaxLRows,MaxLRows);
	ElemGroup elG;

	Coupling = new vector<intVec*>(s->NSubsteps);
	for(int i=0; i<s->NSubsteps; i++) Coupling->at(i) = new intVec(n);

	for(int k = 0; k< s->NSubsteps; k++){

		for(int e=0; e< d->Nelem; e++){
			int iGroup = (*d->eType)(e);

			elG = s->substeps->at(k).elGroup->at(iGroup);

			(*CouplingE)=0;

			if(elG.FlagElemLib<0){
				getLocalSymbolic(&elG.ElemLib, CouplingE->v, elG.CommonPar->v,&d->Ndof, &d->Ndim, &MaxLRows);
			}

			assemblyCoupling(Coupling->at(k),CouplingE,e);
		}

		for(int i=0; i<n; i++) (*Coupling->at(k))(i) = (*Coupling->at(k))(i) - 1;
	}
}

void Solver:: linsolve(){
	solve(*Sol,*A,*b);
}

void Solver :: enforceDirichlet(SGPrealVec *vDirich, intVec *flagDirich, double pen = 99999.9){

	for(int i = 0 ; i<b->size(); i++){
		if((*flagDirich)(i)==-1){
			(*b)(i) = pen*(*vDirich)(i);
			(*A)(i,i) = pen*1.0;
			for(int j = 0 ; j<b->size(); j++){ if(j!=i) (*A)(i,j) = 0.0;}
		}
	}
}
//~
void Solver :: assembly(SGPrealMat *A_L, SGPrealVec *b_L, int e){

	int ip,jp,kip,kjp;

	int iShiftElem = (*d->eNNod_acc)(e);

	for(int i=0; i<(*d->eNNod)(e); i++){ for(int ii=0; ii<d->Ndof; ii++){
		ip = i*d->Ndof + ii;
		kip = (*d->Elem)(iShiftElem + i)*d->Ndof + ii;
		(*b)(kip) += (*b_L)(ip);
		for(int j=0; j<(*d->eNNod)(e); j++){ for(int jj=0; jj<d->Ndof; jj++){
			jp = j*d->Ndof + jj;
			kjp = (*d->Elem)(iShiftElem + j)*d->Ndof + jj;
			(*A)(kip,kjp) += (*A_L)(ip,jp);
		}
		}
	}
	}
}

void Solver :: assemblyCoupling(intVec *Cvec, intMat *Cmat_L, int e){

	int ip,kip;
	int iShiftElem = (*d->eNNod_acc)(e);

	for(int i=0; i<(*d->eNNod)(e); i++){ for(int ii=0; ii<d->Ndof; ii++){
		ip = i*d->Ndof + ii;
		kip = (*d->Elem)(iShiftElem + i)*d->Ndof + ii;
		if((*Cmat_L)(ip,ip) == 1){
			(*Cvec)(kip) = 1;
		}
	}
	}
}

void Solver :: globalToLocal(SGPrealVec *XLL,SGPrealVec *Sol0E, SGPrealVec *Sol1E, SGPrealVec *ParamE, int e){
		int ipDim,kpDim,ipDof,kpDof;
		int iShiftElem = (*d->eNNod_acc)(e);
		int iShiftMat = (*d->eMat)(e);
		int iShiftParam = (*d->eParamSize_acc)(iShiftMat);

		(*XLL) = 0.0;
		(*Sol0E) = 0.0;
		(*Sol1E) = 0.0;
		(*ParamE) = 0.0;

		for(int i=0;i<(*d->eNNod)(e);i++){
			ipDim = i*d->Ndim;
			kpDim = (*d->Elem)(iShiftElem + i)*d->Ndim;
			ipDof = i*d->Ndof;
			kpDof = (*d->Elem)(iShiftElem + i)*d->Ndof;

			for(int j=0 ; j<d->Ndim; j++) (*XLL)(ipDim + j) = (*d->X)(kpDim +j);

			for(int j=0 ; j<d->Ndof; j++){
				(*Sol0E)(ipDof + j) = (*d->Sol0)(kpDof + j);
				(*Sol1E)(ipDof + j) = (*d->Sol1)(kpDof + j);
			}
		}

		for(int k=0; k<(*d->eParamSize)(iShiftMat) ; k++) (*ParamE)(k) = (*d->Param)(iShiftParam + k);

}

void Solver :: localToGlobal(SGPrealVec *ParamE, int e){
		int iShiftMat = (*d->eMat)(e);
		int iShiftParam = (*d->eParamSize_acc)(iShiftMat);

		for(int k=0; k<(*d->eParamSize)(iShiftMat) ; k++) (*d->Param)(iShiftParam + k) = (*ParamE)(k);
}

void Solver :: run(){
	int timeStep, iGroup;
	int id_Elem_Family, MaxLRows, iDofT, NodElt;
	intVec *JParam;
	double DTm, Time;
	SGPrealVec *BE, *XLL, *Sol0E, *Sol1E, *CommonParE, *ParamE;
	SGPrealMat *AE;
	ElemGroup elG;
	double error, tol, emax = 99999.0;
	int itNL, maxit;
	bool isNL;

	NodElt = d->eNNod->max();
	MaxLRows = NodElt*d->Ndof;

	AE = new SGPrealMat(MaxLRows,MaxLRows);
	BE = new SGPrealVec(MaxLRows);
	XLL = new SGPrealVec(NodElt*d->Ndim);
	Sol0E = new SGPrealVec(MaxLRows);
	Sol1E = new SGPrealVec(MaxLRows);
	ParamE = new SGPrealVec(d->eParamSize->max());
	JParam = new intVec(3); // temporary

	Time = s->Tini;
	DTm = s->DelT;
	timeStep = 0;

	d->writeSGPdataout(timeStep,Time,DTm);

	while(Time < s->Tmax){
		DEBUG{cout << "=> Beginning Timestep = " << timeStep << ", time = " << Time << "  =====\n " << endl;}

		for(int k = 0; k< s->NSubsteps; k++){

			error = emax;
			itNL = 0;
			isNL = s->substeps->at(k).isNL;
			if(isNL){
				maxit = s->substeps->at(k).maxit;
				tol = s->substeps->at(k).tol;
			}
			else{
				maxit = 1; // force to stop with just one iteration
				tol = 0.0;
			}

			DEBUG{cout << "===> Beginning SubStep = " << k << " ===========" << endl;}

			DEBUG{if(isNL) cout << "===> Starting NonLinear Convergence, tol = " << tol << endl;}
			while(error>tol && itNL<maxit){

				DEBUG{if(isNL) cout << "===> Starting NonLinear Internal Loop, it = " << itNL << " =======" << endl;}

				A->fill(0.0);
				b->fill(0.0);

				//~ cout << "=====> Assemblying Global Matrices ================ " << endl;
				for(int e=0; e< d->Nelem; e++){

					globalToLocal(XLL,Sol0E,Sol1E,ParamE,e);

					iGroup = (*d->eType)(e);

					elG = s->substeps->at(k).elGroup->at(iGroup);

					(*AE)=0.0;
					(*BE)=0.0;

					if(elG.FlagElemLib<0){
						getLocalMatrix(&elG.ElemLib, AE->v, BE->v, &MaxLRows, XLL->v, &d->Ndim, &d->Ndof, &NodElt, Sol0E->v, Sol1E->v,
										elG.CommonPar->v, ParamE->v, JParam->v, &s->DelT, &DTm, &Time);
					}

					assembly(AE,BE,e);

					localToGlobal(ParamE,e);
				}

				DEBUG{cout << "=====> Solving Linear System ================ " << endl;}
				enforceDirichlet( d->vDirich->at(k), d->flagDirich->at(k) , 1.0);
				enforceDirichlet( d->Sol1, Coupling->at(k) , 1.0);

				linsolve();
				if(isNL){
					error = computeError(*Sol,*d->Sol1);
					DEBUG{cout << "====> Ending NonLinear Internal Loop , it= " << itNL << " , error= " << error << " =========="<< endl;}
				}

				itNL ++;
				*d->Sol1 = *Sol; // uses the operator overloading
			}
			DEBUG{if(isNL) cout << "====> Convergence Achieved with it= " << itNL-1 << " , error= " << error << " =========="<< endl;}

			DEBUG{cout << "===> Ending SubStep = " << k << " =============== \n " << endl;}
		}

		DEBUG{cout << "=> Ending Timestep = " << timeStep << ", time = " << Time << " ===== \n" << endl;}

		Time += s->DelT;
		timeStep++;

		*d->Sol0 = *d->Sol1; // uses the operator overloading

		DEBUG{cout << "=> Writing Results in File ====== \n\n" << endl;}
		d->writeSGPdataout(timeStep,Time,DTm);
		d->writeSGPparam();
	}
}

#endif


#include <time.h>
#include <boost/multi_array.hpp>

#define DIM 100
#define IX3(i, j, k, DIM) ((i)*(DIM)*(DIM) + (j)*(DIM) + (k))

#if 0
template<typename T>
class my_tensor3
{
public:
    my_tensor3( int *dims )
    {
        int data_size = 1;
        for( int i = 0; i < 3; i++ ) { data_size *= dims[i]; }
        data = new T[data_size];

        stride[0] = dims[1] * dims[2];
        stride[1] = dims[2];
    }

    ~my_tensor3()
    {
        delete[] data;
    }

    T &at( int i, int j, int k )
    {
        return data[i*stride[0] + j*stride[1] + k];
    }

private:
    int stride[2];
    T *data;
};

#else

template<typename T, int ORDER>
class my_tensor
{
public:
    my_tensor( int *dims_ )
    {
        int data_size = 1;
        for( int i = 0; i < ORDER; i++ ) { data_size *= dims_[i]; }
        data = new T[data_size];

        stride = new int[ORDER - 1];
        for( int i = 0; i < (ORDER - 1); i++ )
        {
            stride[i] = 1;
            for( int j = (i + 1); j < ORDER; j++ )
            {
                stride[i] *= dims_[j];
            }
        }

        dims = new int[ORDER];
        for( int i = 0; i < ORDER; i++ ) { dims[i] = dims_[i] }
    }

    ~my_tensor()
    {
        delete[] data;
        delete[] stride;
    }

    T &at3( int i, int j, int k )
    {
        return data[i*stride[0] + j*stride[1] + k];
    }

    int order()
    {
        return ORDER;
    }

    int dim( int i )
    {
        return dims[i];
    }

private:
    T *data;
    int *stride;
    int *dims;
};
#endif

int main()
{
    double poly_data[DIM*DIM*DIM] = { 0.0 };
    boost::multi_array_ref<double, 3> poly(poly_data, boost::extents[DIM][DIM][DIM]);

    int dims[] = { DIM, DIM, DIM };
    //my_tensor3<double> poly3( dims );
    my_tensor<double, 3> poly3( dims );

    struct timespec start, end0, end1, end2;

    clock_gettime(CLOCK_REALTIME, &start);

    double tmp;

    for( int l = 0; l < 100; l++ )
    {
        tmp = 0.0;
        for( int i = 0; i < DIM; i++ )
        {
            for( int j = 0; j < DIM; j++ )
            {
                for( int k = 0; k < DIM; k++ )
                {
                    poly_data[IX3(i,j,k,DIM)] = poly_data[IX3(i,j,k,DIM)] + tmp;
                    tmp += 1.0;
                }
            }
        }
    }

    clock_gettime(CLOCK_REALTIME, &end0);

    for( int l = 0; l < 100; l++ )
    {
        tmp = 0.0;
        for( int i = 0; i < DIM; i++ )
        {
            for( int j = 0; j < DIM; j++ )
            {
                for( int k = 0; k < DIM; k++ )
                {
                    poly[i][j][k] = poly[i][j][k] + tmp;
                    tmp += 1.0;
                }
            }
        }
    }

    clock_gettime(CLOCK_REALTIME, &end1);

    for( int l = 0; l < 100; l++ )
    {
        tmp = 0.0;
        for( int i = 0; i < DIM; i++ )
        {
            for( int j = 0; j < DIM; j++ )
            {
                for( int k = 0; k < DIM; k++ )
                {
                    //poly3.at(i, j, k) = poly3.at(i, j, k) + tmp;
                    poly3.at3(i, j, k) = poly3.at3(i, j, k) + tmp;
                    tmp += 1.0;
                }
            }
        }
    }

    clock_gettime(CLOCK_REALTIME, &end2);

    if( end0.tv_nsec < start.tv_nsec ) {
        printf("%10ld.%09ld", end0.tv_sec - start.tv_sec - 1
               ,end0.tv_nsec + 1000000000 - start.tv_nsec);
    } else {
        printf("%10ld.%09ld", end0.tv_sec - start.tv_sec
               ,end0.tv_nsec - start.tv_nsec);
    }
    printf("\n");

    if( end1.tv_nsec < end0.tv_nsec ) {
        printf("%10ld.%09ld", end1.tv_sec - end0.tv_sec - 1
               ,end1.tv_nsec + 1000000000 - end0.tv_nsec);
    } else {
        printf("%10ld.%09ld", end1.tv_sec - end0.tv_sec
               ,end1.tv_nsec - end0.tv_nsec);
    }
    printf("\n");

    if( end2.tv_nsec < end1.tv_nsec ) {
        printf("%10ld.%09ld", end2.tv_sec - end1.tv_sec - 1
               ,end2.tv_nsec + 1000000000 - end1.tv_nsec);
    } else {
        printf("%10ld.%09ld", end2.tv_sec - end1.tv_sec
               ,end2.tv_nsec - end1.tv_nsec);
    }
    printf("\n");

    return 0;
}


//Link to Boost
#define BOOST_TEST_DYN_LINK

//VERY IMPORTANT - include this last
//#include <boost/test/included/unit_test.hpp>
#include <boost/test/unit_test.hpp>

#include "test.h"
#include "../Solution.h"
using namespace std::literals::complex_literals;

// test suite
BOOST_FIXTURE_TEST_SUITE(BetheSolution_suite, SimpleTestFixture, * utf::label("BetheSolution"))

BOOST_DATA_TEST_CASE(regular_roots_test, bdata::random(4, 16) ^ bdata::xrange(20), L, index)
{
    int M = random64(L/2);
    if (M <= 1) M = 2;
    Vector roots(M);
    for (int i = 0; i < M; i++) {
        roots(i) = randomComplex(0.1, 0.9);
    }

    BetheSolution sol(roots, L, 0.0);

    BOOST_TEST(sol.size() == M);
    BOOST_TEST(sol.indexOfSingularRoot1() == -1);
    BOOST_TEST(sol.indexOfSingularRoot2() == -1);
    BOOST_TEST(sol.indexOfBlowupRoot() == -1);
    BOOST_TEST(sol.omega() == 0.0il);
    BOOST_TEST(sol.c() == 0.0il);
    for (int i = 0; i < sol.size(); i++) {
        BOOST_TEST(sol.root(i) == roots(i));
        BOOST_TEST(sol.get(i) == roots(i));
        BOOST_TEST(sol.get2(i) == roots(i));
        BOOST_TEST(sol.isSingular(i) == false);
        BOOST_TEST(sol.isLargestRoot(i) == false);
        BOOST_TEST(sol.type(i) == regular);
    }
}

BOOST_DATA_TEST_CASE(singular_roots_test_evenL, bdata::random(2, 8) ^ bdata::xrange(20), halfL, index)
{
    int L = 2 * halfL;
    int M = 2;
    Vector roots(M);
    roots(0) = -0.5il;
    roots(1) = 0.5il;
    BetheSolution sol(roots, L, 0.0);
    BOOST_TEST(sol.size() == M);

    BOOST_TEST(chop(sol.c()) == (halfL % 2 == 0 ? 2.0il : -2.0il));
    BOOST_TEST(chop(sol.epsilon()) == 0.0il);
    BOOST_TEST(sol.indexOfSingularRoot1() == 1);
    BOOST_TEST(sol.indexOfSingularRoot2() == 0);

    BOOST_TEST(sol.indexOfBlowupRoot() == -1);
    BOOST_TEST(chop(sol.omega()) == 0.0il);

    for (int i = 0; i < sol.size(); i++) {
        BOOST_TEST(chop(sol.get(i)) == roots(i));
        BOOST_TEST(chop(sol.get2(i)) == roots(i));
        BOOST_TEST(sol.isSingular(i));
        BOOST_TEST(sol.isLargestRoot(i) == false);
        BOOST_TEST(sol.type(i) == regular);
    }
}
/*
BOOST_DATA_TEST_CASE(singular_roots_test_oddL, bdata::random(2, 8) ^ bdata::xrange(20), halfL, index)
{
    int L = 2 * halfL + 1;
    int M = 2;
    Vector roots(M);
    roots(0) = -0.5il;
    roots(1) = 0.5il;
    BetheSolution sol(roots, L, 0.0);
    BOOST_TEST(sol.size() == M);

    BOOST_TEST(chop(sol.c()) == 0.0L);
    BOOST_TEST(chop(sol.epsilon()) == 0.0L);
    BOOST_TEST(sol.indexOfSingularRoot1() == -1);
    BOOST_TEST(sol.indexOfSingularRoot2() == -1);

    BOOST_TEST(sol.indexOfBlowupRoot() == -1);
    BOOST_TEST(chop(sol.omega()) == 0.0il);

    for (int i = 0; i < sol.size(); i++) {
        BOOST_TEST(chop(sol.get(i)) == roots(i));
        BOOST_TEST(chop(sol.get2(i)) == roots(i));
        BOOST_TEST(sol.isSingular(i) == false);
        BOOST_TEST(sol.isLargestRoot(i) == false);
        BOOST_TEST(sol.type(i) == regular);
    }
}*/

BOOST_DATA_TEST_CASE(blowup_roots_test, bdata::random(4, 16) ^ bdata::xrange(20), L, index)
{
    int M = 2 + random64(L/2 - 2);
    int indexOfLargest = random64(M - 1);

    Vector roots(M);
    roots(indexOfLargest) = randomComplex(blowup_root_cutoff/0.3, blowup_root_cutoff/0.2);

    std::vector<RootType> types(M, regular);
    types[indexOfLargest] = largest;
    for (int i = 0; i < M; i++) {
        if (i == indexOfLargest) continue;
        if (i % 2 == 0) {
            types[i] = large;
            roots(i) = randomComplex(blowup_root_cutoff/0.7, blowup_root_cutoff/0.6);
        } else {
            roots(i) = randomComplex(blowup_root_cutoff * 0.6, blowup_root_cutoff);
        }
    }

    BetheSolution sol(roots, L, 0.0);
    BOOST_TEST(sol.indexOfBlowupRoot() == indexOfLargest);
    BOOST_TEST(sol.isLargestRoot(indexOfLargest));
    BOOST_TEST(std::abs(sol.omega() - 1.0L/roots(indexOfLargest)) == 0, tt::tolerance(eps));

    BOOST_TEST(chop(sol.c()) == 0.0L);
    BOOST_TEST(chop(sol.epsilon()) == 0.0L);
    BOOST_TEST(sol.indexOfSingularRoot1() == -1);
    BOOST_TEST(sol.indexOfSingularRoot2() == -1);

    for (int i = 0; i < sol.size(); i++) {
        BOOST_TEST(sol.type(i) == types[i]);
        BOOST_TEST_INFO("sol.get(i)=" << std::setprecision(15) << sol.get(i) << ", roots(i)=" << roots(i));
        BOOST_TEST(std::abs(sol.get(i) - roots(i)) == 0, tt::tolerance(eps));
        BOOST_TEST(sol.isSingular(i) == false);
    }
}
/*
TODO:
BOOST_DATA_TEST_CASE(update_test, bdata::random(4, 16) ^ bdata::xrange(20), halfL, index)
{
    int L = 2 * halfL;
    int M = halfL;
    Vector roots(M);
    int indexOfSingular1 = 0;
    int indexOfSingular2 = 1;
    int indexOfLargest = 3;
    roots(indexOfSingular1) = -0.5il;
    roots(indexOfSingular2) = 0.5il;
    roots(indexOfLargest) = randomComplex(blowup_root_cutoff/0.3, blowup_root_cutoff/0.2);
    for (int i = 0; i < roots.size(); i++) {
        if (i == indexOfSingular1 || i == indexOfSingular2 || i == indexOfLargest) continue;
        roots(i) = randomComplex(blowup_root_cutoff/0.3, blowup_root_cutoff/0.2)
    }
    BetheSolution sol(roots, L, 0.0);

    int M = 2 + random64(L/2 - 2);
    int indexOfLargest = random64(M - 1);

    Vector roots(M);
    roots(indexOfLargest) = randomComplex(blowup_root_cutoff/0.3, blowup_root_cutoff/0.2);

    std::vector<RootType> types(M, regular);
    types[indexOfLargest] = largest;
    for (int i = 0; i < M; i++) {
        if (i == indexOfLargest) continue;
        if (i % 2 == 0) {
            types[i] = large;
            roots(i) = randomComplex(blowup_root_cutoff/0.7, blowup_root_cutoff/0.6);
        } else {
            roots(i) = randomComplex(blowup_root_cutoff * 0.6, blowup_root_cutoff);
        }
    }

    BetheSolution sol(roots, L, 0.0);
    BOOST_TEST(sol.indexOfBlowupRoot() == indexOfLargest);
    BOOST_TEST(sol.isLargestRoot(indexOfLargest));
    BOOST_TEST(std::abs(sol.omega() - 1.0L/roots(indexOfLargest)) == 0, tt::tolerance(eps));

    BOOST_TEST(chop(sol.c()) == 0.0L);
    BOOST_TEST(chop(sol.epsilon()) == 0.0L);
    BOOST_TEST(sol.indexOfSingularRoot1() == -1);
    BOOST_TEST(sol.indexOfSingularRoot2() == -1);

    for (int i = 0; i < sol.size(); i++) {
        BOOST_TEST(sol.type(i) == types[i]);
        BOOST_TEST_INFO("sol.get(i)=" << std::setprecision(15) << sol.get(i) << ", roots(i)=" << roots(i));
        BOOST_TEST(std::abs(sol.get(i) - roots(i)) == 0, tt::tolerance(eps));
        BOOST_TEST(sol.isSingular(i) == false);
    }
}*/

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SCALAR_ULP_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SCALAR_ULP_HPP_INCLUDED

#include <boost/simd/toolbox/ieee/functions/ulp.hpp>
#include <boost/simd/include/functions/scalar/bitwise_cast.hpp>
#include <boost/simd/include/functions/scalar/prev.hpp>
#include <boost/simd/include/functions/scalar/min.hpp>
#include <boost/simd/include/functions/scalar/is_eqz.hpp>
#include <boost/simd/include/functions/scalar/is_invalid.hpp>
#include <boost/simd/include/functions/scalar/abs.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mindenormal.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::ulp_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const &)const { return One<A0>(); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::ulp_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_integer<A0,unsigned>::type int_type;
      if (is_eqz(a0)) return Mindenormal<A0>();
      if (is_invalid(a0)) return Nan<A0>();
      const A0 x = boost::simd::abs(a0);
      int_type aa = bitwise_cast<int_type>(x);
      int_type bb = aa;
      --bb;
      ++aa;
      return boost::simd::min(x-bitwise_cast<A0>(bb), bitwise_cast<A0>(aa)-x);
    }
  };
} } }

#endif


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "maps-precomp.h"  // Precomp header
//
#include <mrpt/core/round.h>  // round()
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/math/CVectorFixed.h>
#include <mrpt/math/transform_gaussian.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservationRange.h>
#include <mrpt/random.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::random;
using namespace mrpt::poses;
using namespace std;

double COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS = 0.8;

// See docs in header
void COccupancyGridMap2D::laserScanSimulator(
	mrpt::obs::CObservation2DRangeScan& inout_Scan, const CPose2D& robotPose,
	float threshold, size_t N, float noiseStd, unsigned int decimation,
	float angleNoiseStd) const
{
	MRPT_START

	ASSERT_(decimation >= 1);
	ASSERT_(N >= 2);

	// Sensor pose in global coordinates
	CPose3D sensorPose3D = CPose3D(robotPose) + inout_Scan.sensorPose;
	// Aproximation: grid is 2D !!!
	CPose2D sensorPose(sensorPose3D);

	// Scan size:
	inout_Scan.resizeScan(N);

	double A = sensorPose.phi() +
		(inout_Scan.rightToLeft ? -0.5 : +0.5) * inout_Scan.aperture;
	const double AA =
		(inout_Scan.rightToLeft ? 1.0 : -1.0) * (inout_Scan.aperture / (N - 1));

	const float free_thres = 1.0f - threshold;

	for (size_t i = 0; i < N; i += decimation, A += AA * decimation)
	{
		bool valid;
		float out_range;
		simulateScanRay(
			sensorPose.x(), sensorPose.y(), A, out_range, valid,
			inout_Scan.maxRange, free_thres, noiseStd, angleNoiseStd);
		inout_Scan.setScanRange(i, out_range);
		inout_Scan.setScanRangeValidity(i, valid);
	}

	MRPT_END
}

void COccupancyGridMap2D::sonarSimulator(
	CObservationRange& inout_observation, const CPose2D& robotPose,
	float threshold, float rangeNoiseStd, float angleNoiseStd) const
{
	const float free_thres = 1.0f - threshold;

	for (auto itR = inout_observation.begin(); itR != inout_observation.end();
		 ++itR)
	{
		const CPose2D sensorAbsolutePose =
			CPose2D(CPose3D(robotPose) + CPose3D(itR->sensorPose));

		// For each sonar cone, simulate several rays and keep the shortest
		// distance:
		ASSERT_(inout_observation.sensorConeApperture > 0);
		size_t nRays =
			round(1 + inout_observation.sensorConeApperture / 1.0_deg);

		double direction = sensorAbsolutePose.phi() -
			0.5 * inout_observation.sensorConeApperture;
		const double Adir = inout_observation.sensorConeApperture / nRays;

		float min_detected_obs = 0;
		for (size_t i = 0; i < nRays; i++, direction += Adir)
		{
			bool valid;
			float sim_rang;
			simulateScanRay(
				sensorAbsolutePose.x(), sensorAbsolutePose.y(), direction,
				sim_rang, valid, inout_observation.maxSensorDistance,
				free_thres, rangeNoiseStd, angleNoiseStd);

			if (valid && (sim_rang < min_detected_obs || !i))
				min_detected_obs = sim_rang;
		}
		// Save:
		itR->sensedDistance = min_detected_obs;
	}
}

void COccupancyGridMap2D::simulateScanRay(
	const double start_x, const double start_y, const double angle_direction,
	float& out_range, bool& out_valid, const double max_range_meters,
	const float threshold_free, const double noiseStd,
	const double angleNoiseStd) const
{
	const double A_ = angle_direction +
		(angleNoiseStd > .0
			 ? getRandomGenerator().drawGaussian1D_normalized() * angleNoiseStd
			 : .0);

// Unit vector in the directorion of the ray:
#ifdef HAVE_SINCOS
	double Arx, Ary;
	::sincos(A_, &Ary, &Arx);
#else
	const double Arx = cos(A_);
	const double Ary = sin(A_);
#endif

	// Ray tracing, until collision, out of the map or out of range:
	const unsigned int max_ray_len = mrpt::round(max_range_meters / resolution);
	unsigned int ray_len = 0;

// Use integers for all ray tracing for efficiency
#define INTPRECNUMBIT 10
#define int_x2idx(_X) (_X >> INTPRECNUMBIT)
#define int_y2idx(_Y) (_Y >> INTPRECNUMBIT)

	auto rxi = static_cast<int64_t>(
		((start_x - x_min) / resolution) * (1L << INTPRECNUMBIT));
	auto ryi = static_cast<int64_t>(
		((start_y - y_min) / resolution) * (1L << INTPRECNUMBIT));

	const auto Arxi = static_cast<int64_t>(
		RAYTRACE_STEP_SIZE_IN_CELL_UNITS * Arx * (1L << INTPRECNUMBIT));
	const auto Aryi = static_cast<int64_t>(
		RAYTRACE_STEP_SIZE_IN_CELL_UNITS * Ary * (1L << INTPRECNUMBIT));

	cellType hitCellOcc_int = 0;  // p2l(0.5f)
	const cellType threshold_free_int = p2l(threshold_free);
	int x, y = int_y2idx(ryi);

	while ((x = int_x2idx(rxi)) >= 0 && (y = int_y2idx(ryi)) >= 0 &&
		   x < static_cast<int>(size_x) && y < static_cast<int>(size_y) &&
		   (hitCellOcc_int = map[x + y * size_x]) > threshold_free_int &&
		   ray_len < max_ray_len)
	{
		rxi += Arxi;
		ryi += Aryi;
		ray_len++;
	}

	// Store:
	// Check out of the grid?
	// Tip: if x<0, (unsigned)(x) will also be >>> size_x ;-)
	if (std::abs(hitCellOcc_int) <= 1 || static_cast<unsigned>(x) >= size_x ||
		static_cast<unsigned>(y) >= size_y)
	{
		out_valid = false;
		out_range = max_range_meters;
	}
	else
	{  // No: The normal case:
		out_range = RAYTRACE_STEP_SIZE_IN_CELL_UNITS * ray_len * resolution;
		out_valid = (ray_len < max_ray_len);  // out_range<max_range_meters;
		// Add additive Gaussian noise:
		if (noiseStd > 0 && out_valid)
			out_range +=
				noiseStd * getRandomGenerator().drawGaussian1D_normalized();
	}
}

COccupancyGridMap2D::TLaserSimulUncertaintyParams::
	TLaserSimulUncertaintyParams()

	= default;

COccupancyGridMap2D::TLaserSimulUncertaintyResult::
	TLaserSimulUncertaintyResult() = default;

struct TFunctorLaserSimulData
{
	const COccupancyGridMap2D::TLaserSimulUncertaintyParams* params;
	const COccupancyGridMap2D* grid;
};

static void func_laserSimul_callback(
	const mrpt::math::CVectorFixedDouble<3>& x_pose,
	const TFunctorLaserSimulData& fixed_param,
	mrpt::math::CVectorDouble& y_scanRanges)
{
	ASSERT_(fixed_param.params && fixed_param.grid);
	ASSERT_(fixed_param.params->decimation >= 1);
	ASSERT_(fixed_param.params->nRays >= 2);

	const size_t N = fixed_param.params->nRays;

	// Sensor pose in global coordinates
	const CPose3D sensorPose3D =
		CPose3D(x_pose[0], x_pose[1], .0, x_pose[2], .0, .0) +
		fixed_param.params->sensorPose;
	// Aproximation: grid is 2D !!!
	const CPose2D sensorPose(sensorPose3D);

	// Scan size:
	y_scanRanges.resize(N);

	double A = sensorPose.phi() +
		(fixed_param.params->rightToLeft ? -0.5 : +0.5) *
			fixed_param.params->aperture;
	const double AA = (fixed_param.params->rightToLeft ? 1.0 : -1.0) *
		(fixed_param.params->aperture / (N - 1));

	const float free_thres = 1.0f - fixed_param.params->threshold;

	for (size_t i = 0; i < N; i += fixed_param.params->decimation,
				A += AA * fixed_param.params->decimation)
	{
		bool valid;
		float range;

		fixed_param.grid->simulateScanRay(
			sensorPose.x(), sensorPose.y(), A, range, valid,
			fixed_param.params->maxRange, free_thres, .0 /*noiseStd*/,
			.0 /*angleNoiseStd*/);
		y_scanRanges[i] = valid ? range : fixed_param.params->maxRange;
	}
}

void COccupancyGridMap2D::laserScanSimulatorWithUncertainty(
	const COccupancyGridMap2D::TLaserSimulUncertaintyParams& in_params,
	COccupancyGridMap2D::TLaserSimulUncertaintyResult& out_results) const
{
	const mrpt::math::CVectorFixedDouble<3> robPoseMean =
		in_params.robotPose.mean.asVectorVal();

	TFunctorLaserSimulData simulData;
	simulData.grid = this;
	simulData.params = &in_params;

	switch (in_params.method)
	{
		case sumUnscented:
			mrpt::math::transform_gaussian_unscented(
				robPoseMean,  // x_mean
				in_params.robotPose.cov,  // x_cov
				&func_laserSimul_callback,	// void  (*functor)(const
				// VECTORLIKE1 &x,const USERPARAM
				// &fixed_param, VECTORLIKE3 &y)
				simulData,	// const USERPARAM &fixed_param,
				out_results.scanWithUncert.rangesMean,
				out_results.scanWithUncert.rangesCovar,
				nullptr,  // elem_do_wrap2pi,
				in_params.UT_alpha, in_params.UT_kappa,
				in_params.UT_beta  // alpha, K, beta
			);
			break;
		case sumMonteCarlo:
			//
			mrpt::math::transform_gaussian_montecarlo(
				robPoseMean,  // x_mean
				in_params.robotPose.cov,  // x_cov
				&func_laserSimul_callback,	// void  (*functor)(const
				// VECTORLIKE1 &x,const USERPARAM
				// &fixed_param, VECTORLIKE3 &y)
				simulData,	// const USERPARAM &fixed_param,
				out_results.scanWithUncert.rangesMean,
				out_results.scanWithUncert.rangesCovar, in_params.MC_samples);
			break;
		default:
			throw std::runtime_error(
				"[laserScanSimulatorWithUncertainty] Unknown `method` value");
			break;
	};

	// Outputs:
	out_results.scanWithUncert.rangeScan.aperture = in_params.aperture;
	out_results.scanWithUncert.rangeScan.maxRange = in_params.maxRange;
	out_results.scanWithUncert.rangeScan.rightToLeft = in_params.rightToLeft;
	out_results.scanWithUncert.rangeScan.sensorPose = in_params.sensorPose;

	out_results.scanWithUncert.rangeScan.resizeScan(in_params.nRays);
	for (unsigned i = 0; i < in_params.nRays; i++)
	{
		out_results.scanWithUncert.rangeScan.setScanRange(
			i, (float)out_results.scanWithUncert.rangesMean[i]);
		out_results.scanWithUncert.rangeScan.setScanRangeValidity(i, true);
	}

	// Add minimum uncertainty: grid cell resolution:
	out_results.scanWithUncert.rangesCovar.asEigen().diagonal().array() +=
		0.5 * resolution * resolution;
}


/*

Copyright (c) 2005-2020, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef VOLUMEDEPENDENTAVERAGEDSOURCEELLIPTICPDE_HPP_
#define VOLUMEDEPENDENTAVERAGEDSOURCEELLIPTICPDE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "NodeBasedCellPopulation.hpp"
#include "AveragedSourceEllipticPde.hpp"
#include "TetrahedralMesh.hpp"
#include "AbstractLinearEllipticPde.hpp"

/**
 * An elliptic PDE to be solved numerically using the finite element method, for
 * coupling to a cell-based simulation.
 *
 * This class inherits from AveragedSourceEllipticPde and may only be used with
 * a NodeBasedCellPopulation, since it assumes that each cell is associated with
 * a Node object.
 *
 * The PDE takes the form
 *
 * Grad.(D*Grad(u)) + k*u*rho(x) = 0,
 *
 * where the scalars D and k are specified by the members mDiffusionCoefficient and
 * mSourceCoefficient, respectively. Their values must be set in the constructor.
 *
 * The function rho(x) denotes the local density of non-apoptotic cells. This
 * quantity is computed for each element of a 'coarse' finite element mesh that is
 * passed to the method SetupSourceTerms() and stored in the member mCellDensityOnCoarseElements.
 *
 * For a point x, rho(x) is a weighted sum of the non-apoptotic cells whose centres
 * lie in each finite element containing that point, scaled by the area of that element.
 * The weighting assigned to each cell is given by the square of the radius of the
 * associated node, which is accessed using the GetRadius() method.
 *
 * \todo Consider creating a VolumeDependentAveragedSourceParabolicPde class
 */
template<unsigned DIM>
class VolumeDependentAveragedSourceEllipticPde : public AveragedSourceEllipticPde<DIM>
{
    friend class TestCellBasedEllipticPdes;

private:

    /** Needed for serialization.*/
    friend class boost::serialization::access;
    /**
     * Serialize the PDE and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
       archive & boost::serialization::base_object<AveragedSourceEllipticPde<DIM> >(*this);
    }

    /** Static cast of the NodeBasedCellPopulation. */
    NodeBasedCellPopulation<DIM>* mpStaticCastCellPopulation;

public:

    /**
     * Constructor.
     *
     * @param rCellPopulation reference to the cell population
     * @param coefficient the coefficient of consumption of nutrient by cells (defaults to 0.0)
     */
    VolumeDependentAveragedSourceEllipticPde(AbstractCellPopulation<DIM>& rCellPopulation, double coefficient=0.0);

    /**
     * Set up the source terms.
     *
     * @param rCoarseMesh reference to the coarse mesh
     * @param pCellPdeElementMap optional pointer to the map from cells to coarse elements
     */
    void SetupSourceTerms(TetrahedralMesh<DIM,DIM>& rCoarseMesh, std::map<CellPtr, unsigned>* pCellPdeElementMap=nullptr);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(VolumeDependentAveragedSourceEllipticPde)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct a VolumeDependentAveragedSourceEllipticPde.
 */
template<class Archive, unsigned DIM>
inline void save_construct_data(
    Archive & ar, const VolumeDependentAveragedSourceEllipticPde<DIM>* t, const unsigned int file_version)
{
    // Save data required to construct instance
    const AbstractCellPopulation<DIM>* p_cell_population = &(t->rGetCellPopulation());
    ar & p_cell_population;
}

/**
 * De-serialize constructor parameters and initialise a VolumeDependentAveragedSourceEllipticPde.
 */
template<class Archive, unsigned DIM>
inline void load_construct_data(
    Archive & ar, VolumeDependentAveragedSourceEllipticPde<DIM>* t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    AbstractCellPopulation<DIM>* p_cell_population;
    ar >> p_cell_population;

    // Invoke inplace constructor to initialise instance
    ::new(t)VolumeDependentAveragedSourceEllipticPde<DIM>(*p_cell_population);
}
}
} // namespace ...

#endif /*VOLUMEDEPENDENTAVERAGEDSOURCEELLIPTICPDE_HPP_*/


#ifndef __ZB_WIDGET_HPP__
#define __ZB_WIDGET_HPP__

#include <QWidget>
#include <QPaintEvent>
#include <QMouseEvent>
#include <Eigen/Eigen>
#include "Model.hpp"

class ZBWidget : public QWidget, EigenTypes {

Q_OBJECT

public:
  ZBWidget(Model *model, QWidget *parent=0);
  virtual ~ZBWidget();

public:
  static Matrix4 lookAt(float eye_x, float eye_y, float eye_z,
                        float center_x, float center_y, float center_z,
                        float up_x, float up_y, float up_z);
  static Matrix4 perspective(float fov, float aspect, float near, float far);
  static Matrix4 rotateX(float degree);
  static Matrix4 rotateY(float degree);

protected:
  virtual void paintEvent(QPaintEvent *event);
  virtual void mouseMoveEvent(QMouseEvent *event);
  virtual void mousePressEvent(QMouseEvent *event);
  virtual void mouseReleaseEvent(QMouseEvent *event);

signals:
  void repaintNeeded();

private:
  Model *m_model;
  QPoint m_lastPos;
  int m_buttons;
  float m_cameraAngleX;
  float m_cameraAngleY;
  float m_cameraDistance;

};

#endif //__ZB_WIDGET_HPP__


//
// Created by huangkun on 2020/9/22.
//

#include <Eigen/Eigen>
#include <opencv2/core/eigen.hpp>

#include <opengv2/event/EventFrame.hpp>

opengv2::EventFrame::EventFrame(opengv2::EventContainer::Ptr container, const std::pair<double, double> &duration)
        : CameraFrame(cv::Mat(), container->camera), container_(container), duration_(duration) {
    std::unordered_set<Eigen::Vector2d, EigenMatrixHash<Eigen::Vector2d>, std::equal_to<>,
            Eigen::aligned_allocator<Eigen::Vector2d>> positiveEvents, negativeEvents;
    for (auto itr = container_->container.lower_bound(duration_.first);
         itr != container_->container.upper_bound(duration_.second); itr++) {
        if (itr->second.polarity) {
            positiveEvents.insert(itr->second.location);
        } else {
            negativeEvents.insert(itr->second.location);
        }
    }

    // delete event, which have + and - event at the same pixel
    for (auto itr = positiveEvents.begin(); itr != positiveEvents.end();) {
        auto found = negativeEvents.find(*itr);
        if (found == negativeEvents.end()) {
            itr++;
        } else {
            negativeEvents.erase(found);
            itr = positiveEvents.erase(itr);
        }
    }

    std::move(positiveEvents.begin(), positiveEvents.end(), std::back_inserter(positiveEvents_));
    std::move(negativeEvents.begin(), negativeEvents.end(), std::back_inserter(negativeEvents_));
}

cv::Mat opengv2::EventFrame::undistortedImage(CameraBase::Ptr camera) const {
    cv::Mat image = cv::Mat(std::round(camera->size()[1] * 1.2), std::round(camera->size()[0] * 1.2), CV_8UC3,
                            cv::Vec3b(0, 0, 0));

    if (positiveEvents_.empty() || negativeEvents_.empty()) {
        throw std::logic_error("Events in Frame was cleared.");
    }

    for (const Eigen::Vector2d &p: positiveEvents_) {
        Eigen::Vector2d p_c = camera->undistortPoint(p);
        cv::Point loc(std::round(p_c[0] + camera->size()[0] * 0.1),
                      std::round(p_c[1] + camera->size()[1] * 0.1));
        image.at<cv::Vec3b>(loc) = cv::Vec3b(0, 0, 255);
    }
    for (const Eigen::Vector2d &p: negativeEvents_) {
        Eigen::Vector2d p_c = camera->undistortPoint(p);
        cv::Point loc(std::round(p_c[0] + camera->size()[0] * 0.1),
                      std::round(p_c[1] + camera->size()[1] * 0.1));
        image.at<cv::Vec3b>(loc) = cv::Vec3b(0, 255, 0);
    }

    return image;
}