//
// Copyright (c) 2016-2019 CNRS INRIA
//

#ifndef __pinocchio_contact_dynamics_hxx__
#define __pinocchio_contact_dynamics_hxx__

#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/check.hpp"

#include <Eigen/Cholesky>

namespace pinocchio
{

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename TangentVectorType,
  typename ConstraintMatrixType, typename DriftVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
  forwardDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  DataTpl<Scalar,Options,JointCollectionTpl> & data,
                  const Eigen::MatrixBase<TangentVectorType> & tau,
                  const Eigen::MatrixBase<ConstraintMatrixType> & J,
                  const Eigen::MatrixBase<DriftVectorType> & gamma,
                  const Scalar inv_damping)
  {
    PINOCCHIO_CHECK_INPUT_ARGUMENT(tau.size() == model.nv);
    PINOCCHIO_CHECK_INPUT_ARGUMENT(J.cols() == model.nv);
    PINOCCHIO_CHECK_INPUT_ARGUMENT(J.rows() == gamma.size());
    assert(model.check(data) && "data is not consistent with model.");

    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typename Data::TangentVectorType & a = data.ddq;
    typename Data::VectorXs & lambda_c = data.lambda_c;

    // Compute the UDUt decomposition of data.M
    cholesky::decompose(model, data);

    // Compute the dynamic drift (control - nle)
    data.torque_residual = tau - data.nle;
    cholesky::solve(model, data, data.torque_residual);

    data.sDUiJt = J.transpose();
    // Compute U^-1 * J.T
    cholesky::Uiv(model, data, data.sDUiJt);
    for(Eigen::DenseIndex k=0;k<model.nv;++k)
      data.sDUiJt.row(k) /= sqrt(data.D[k]);

    data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;

    data.JMinvJt.diagonal().array() += inv_damping;
    data.llt_JMinvJt.compute(data.JMinvJt);

    // Compute the Lagrange Multipliers
    lambda_c.noalias() = -J*data.torque_residual;
    lambda_c -= gamma;
    data.llt_JMinvJt.solveInPlace(lambda_c);

    // Compute the joint acceleration
    a.noalias() = J.transpose() * lambda_c;
    cholesky::solve(model, data, a);
    a += data.torque_residual;

    return a;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
  typename ConstraintMatrixType, typename DriftVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
  forwardDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  DataTpl<Scalar,Options,JointCollectionTpl> & data,
                  const Eigen::MatrixBase<ConfigVectorType> & q,
                  const Eigen::MatrixBase<TangentVectorType1> & v,
                  const Eigen::MatrixBase<TangentVectorType2> & tau,
                  const Eigen::MatrixBase<ConstraintMatrixType> & J,
                  const Eigen::MatrixBase<DriftVectorType> & gamma,
                  const Scalar inv_damping)
  {
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq);
    PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);

    computeAllTerms(model, data, q, v);

    return forwardDynamics(model,data,tau,J,gamma,inv_damping);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
  typename ConstraintMatrixType, typename KKTMatrixType>
  inline void getKKTContactDynamicMatrixInverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                                const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                                const Eigen::MatrixBase<ConstraintMatrixType> & J,
                                                const Eigen::MatrixBase<KKTMatrixType> & MJtJ_inv)
  {
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
    PINOCCHIO_CHECK_INPUT_ARGUMENT(MJtJ_inv.cols() == data.JMinvJt.cols() + model.nv);
    PINOCCHIO_CHECK_INPUT_ARGUMENT(MJtJ_inv.rows() == data.JMinvJt.rows() + model.nv);
    const typename Data::MatrixXs::Index& nc = data.JMinvJt.cols();

    KKTMatrixType& MJtJ_inv_ = PINOCCHIO_EIGEN_CONST_CAST(KKTMatrixType,MJtJ_inv);

    Eigen::Block<typename Data::MatrixXs> topLeft = MJtJ_inv_.topLeftCorner(model.nv, model.nv);
    Eigen::Block<typename Data::MatrixXs> topRight = MJtJ_inv_.topRightCorner(model.nv, nc);
    Eigen::Block<typename Data::MatrixXs> bottomLeft = MJtJ_inv_.bottomLeftCorner(nc, model.nv);
    Eigen::Block<typename Data::MatrixXs> bottomRight = MJtJ_inv_.bottomRightCorner(nc, nc);

    bottomRight = -Data::MatrixXs::Identity(nc,nc);    topLeft.setIdentity();
    data.llt_JMinvJt.solveInPlace(bottomRight);    cholesky::solve(model, data, topLeft);

    bottomLeft.noalias() = J*topLeft;
    topRight.noalias() = bottomLeft.transpose() * (-bottomRight);
    topLeft.noalias() -= topRight*bottomLeft;
    bottomLeft = topRight.transpose();
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
  impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  DataTpl<Scalar,Options,JointCollectionTpl> & data,
                  const Eigen::MatrixBase<ConfigVectorType> & q,
                  const Eigen::MatrixBase<TangentVectorType> & v_before,
                  const Eigen::MatrixBase<ConstraintMatrixType> & J,
                  const Scalar r_coeff,
                  const Scalar inv_damping)
  {
    PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq);

    // Compute the mass matrix
    crba(model, data, q);

    return impulseDynamics(model,data,v_before,J,r_coeff,inv_damping);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename TangentVectorType, typename ConstraintMatrixType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
  impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                  DataTpl<Scalar,Options,JointCollectionTpl> & data,
                  const Eigen::MatrixBase<TangentVectorType> & v_before,
                  const Eigen::MatrixBase<ConstraintMatrixType> & J,
                  const Scalar r_coeff,
                  const Scalar inv_damping)
  {
    PINOCCHIO_CHECK_INPUT_ARGUMENT(v_before.size() == model.nv);
    PINOCCHIO_CHECK_INPUT_ARGUMENT(J.cols() == model.nv);
    assert(model.check(data) && "data is not consistent with model.");

    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typename Data::VectorXs & impulse_c = data.impulse_c;
    typename Data::TangentVectorType & dq_after = data.dq_after;

    // Compute the UDUt decomposition of data.M
    cholesky::decompose(model, data);

    data.sDUiJt = J.transpose();
    // Compute U^-1 * J.T
    cholesky::Uiv(model, data, data.sDUiJt);
    for(int k=0;k<model.nv;++k) data.sDUiJt.row(k) /= sqrt(data.D[k]);

    data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;

    data.JMinvJt.diagonal().array() += inv_damping;
    data.llt_JMinvJt.compute(data.JMinvJt);

    // Compute the Lagrange Multipliers related to the contact impulses
    impulse_c.noalias() = (-r_coeff - 1.) * (J * v_before);
    data.llt_JMinvJt.solveInPlace(impulse_c);

    // Compute the joint velocity after impacts
    dq_after.noalias() = J.transpose() * impulse_c;
    cholesky::solve(model, data, dq_after);
    dq_after += v_before;

    return dq_after;
  }
} // namespace pinocchio

#endif // ifndef __pinocchio_contact_dynamics_hxx__


/* Copyright 2020 CNRS-AIST JRL */

#include <chrono>
#include <iostream>
#include <vector>

#include <Eigen/Core>
#include <Eigen/QR>

#include <jrl-qp/utils/Logger.h>
#include <jrl-qp/utils/debug.h>

using namespace Eigen;
using namespace jrl::qp;

int main()
{
  MatrixXd A = MatrixXd::Random(3, 5);

  utils::Logger l(std::cout, "qp");
  l.setFlag(0x00000001);
  l.startIter(0);
  l.log(0x00000001, "test", 3);
  l.log(0x00000010, "test2", 4);
  l.log(0x00000001, "A", A);

  LOG_NEW_ITER(l, 1);
  l.log(1, "n", 5, "A", A, "r", 3.14);

  auto s = l.subLog("choice");
  s.startIter(0);
  s.log(1, "b", Vector3d(3, -1, 2));

  int c = 2;
  double d;

  LOG(l, 1, A, c);
  LOG_COMMENT(l, 1, "This is a comment");
  DBG_COMMENT(l, 1, "This is another comment");

  std::uint32_t u = 1;
  std::cout << ~u << std::endl;
  std::cout << ~decltype(u)(0) << std::endl;
}


//
// Created by Qitong Wang on 2019/10/22.
// Modified from UCR_SUITE
//

#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <queue>
#include <vector>
#include <chrono>

#include <boost/thread.hpp>

#include "bounds.h"
#include "distances.h"
#include "parameters.h"
#include "query.h"
#include "sequence.h"
#include "thread_pool.h"

using namespace std;

struct Hit {
    Hit(int location, double distance) : location(location), distance(distance) {}

    int location;
    double distance;
};

struct compare {
    bool operator()(const Hit *prior, const Hit *later) {
        return prior->distance < later->distance;
    }
};

void conduct_query(const Sequence *sequence, const Query *query, const Parameters *parameters, int sequence_id,
                   int query_id, boost::mutex *results_mutex, ofstream *results_ofs,
                   const chrono::time_point<chrono::high_resolution_clock> *start_time,
                   atomic<int> *processed) {
    int i, num_to_skip = 0, next = 0, epoch_size = parameters->epoch, num_epoches = 0, to_calculate, start, position_in_epoch, location;
    double sum, squared_sum, mean, std, bsf = INF, distance = 0, bound_kim = 0, bound_keogh = 0, bound_keogh_converse = 0;
    priority_queue<Hit *, vector<Hit *>, compare> bsf_pq;
    Hit *hit;
    double *tighter_bounds_keogh;
//    int skipped = 0, by_kim = 0, by_keogh = 0, by_keogh_converse = 0;

    auto buffer = (double *) malloc(sizeof(double) * parameters->epoch);
    auto upper_envelop = (double *) malloc(sizeof(double) * parameters->epoch);
    auto lower_envelop = (double *) malloc(sizeof(double) * parameters->epoch);
    auto subsequence = (double *) malloc(sizeof(double) * query->length * 2);
    auto subsequence_normalized = (double *) malloc(sizeof(double) * query->length);
    auto bounds_keogh_remaining = (double *) malloc(sizeof(double) * query->length);
    auto local_bounds_keogh = (double *) malloc(sizeof(double) * query->length);
    auto local_bounds_keogh_converse = (double *) malloc(sizeof(double) * query->length);

    if (buffer == nullptr || upper_envelop == nullptr || lower_envelop == nullptr || subsequence == nullptr ||
        subsequence_normalized == nullptr || bounds_keogh_remaining == nullptr || local_bounds_keogh == nullptr ||
        local_bounds_keogh_converse == nullptr) {
        error(1);
    }

    while (next + query->length - 1 < sequence->length) {
        if (next + parameters->epoch > sequence->length) {
            copy(sequence->values + next, sequence->values + sequence->length, buffer);
            epoch_size = sequence->length - next;
            next = sequence->length;
        } else {
            copy(sequence->values + next, sequence->values + next + parameters->epoch, buffer);
            next += (parameters->epoch - query->length + 1);
        }

        sum = 0, squared_sum = 0;
        get_envelops_lemire(buffer, epoch_size, query->warping_window, lower_envelop, upper_envelop);

        for (to_calculate = 0; to_calculate < epoch_size; ++to_calculate) {
            subsequence[to_calculate % query->length] = buffer[to_calculate];
            subsequence[(to_calculate % query->length) + query->length] = buffer[to_calculate];

            sum += buffer[to_calculate];
            squared_sum += (buffer[to_calculate] * buffer[to_calculate]);

            if (to_calculate < query->length - 1) {
                continue;
            }

            start = (to_calculate + 1) % query->length;

            // TODO skip the next following overlap sub-sequences to check is an easy but not optimal implementation
            if (num_to_skip-- > 0) {
//                skipped += 1;
                goto UPDATE_STATISTICS;
            }

            mean = sum / query->length;
            std = sqrt(squared_sum / query->length - mean * mean);

//            if (query->length <= parameters->min_length_for_kim) {
            bound_kim = get_kim(subsequence, query->normalized_values, start, query->length, mean, std, bsf);

            if (bound_kim >= bsf) {
//                    by_kim += 1;
                goto UPDATE_STATISTICS;
            }
//            }

            position_in_epoch = to_calculate + 1 - query->length;
            bound_keogh = get_keogh(query->sorted_indexes, subsequence, subsequence_normalized,
                                    query->sorted_upper_envelop, query->sorted_lower_envelop, local_bounds_keogh, start,
                                    query->length, mean, std, bsf);

            if (bound_keogh >= bsf) {
//                by_keogh += 1;
                goto UPDATE_STATISTICS;
            }

            bound_keogh_converse = get_keogh_converse(query->sorted_indexes, query->sorted_normalized_values,
                                                      local_bounds_keogh_converse, lower_envelop + position_in_epoch,
                                                      upper_envelop + position_in_epoch, query->length, mean, std, bsf);

            if (bound_keogh_converse >= bsf) {
//                by_keogh_converse += 1;
                goto UPDATE_STATISTICS;
            }

            tighter_bounds_keogh =
                    bound_keogh > bound_keogh_converse ? local_bounds_keogh : local_bounds_keogh_converse;
            copy(tighter_bounds_keogh, tighter_bounds_keogh + query->length, bounds_keogh_remaining);
            for (i = query->length - 2; i >= 0; --i) {
                bounds_keogh_remaining[i] += bounds_keogh_remaining[i + 1];
            }

            distance = dtw(subsequence_normalized, query->normalized_values, bounds_keogh_remaining, query->length,
                           query->warping_window, bsf);

            if (distance < bsf) {
                location = num_epoches * (parameters->epoch - query->length + 1) + to_calculate + 1 - query->length;
                if (bsf_pq.size() == parameters->num_neighbors) {
                    hit = bsf_pq.top();
                    bsf_pq.pop();
                    hit->distance = distance;
                    hit->location = location;
                    bsf_pq.push(hit);
                } else {
                    bsf_pq.push(new Hit(location, distance));
                }
                bsf = bsf_pq.top()->distance;
                num_to_skip = query->overlap_length;
            }

            UPDATE_STATISTICS:
            sum -= subsequence[start];
            squared_sum -= (subsequence[start] * subsequence[start]);
        }
        num_epoches += 1;
    }

    free(buffer);
    free(upper_envelop);
    free(lower_envelop);
    free(subsequence);
    free(subsequence_normalized);
    free(bounds_keogh_remaining);
    free(local_bounds_keogh);
    free(local_bounds_keogh_converse);

    {
        // TODO asynchronously write to file (but not critical)
        boost::mutex::scoped_lock lock(*results_mutex);
        for (i = 0; i < parameters->num_neighbors; ++i) {
            *results_ofs << sequence_id << " " << query_id << " " << sqrt(bsf_pq.top()->distance) << " "
                         << bsf_pq.top()->location << " " << query->length << endl;
            bsf_pq.pop();
        }
    }

    while (!bsf_pq.empty()) {
        free(bsf_pq.top());
        bsf_pq.pop();
    }

    chrono::duration<double> elapsed = std::chrono::high_resolution_clock::now() - *start_time;
    cout << ++*processed << " in " << elapsed.count() << "s" << endl;
//    cout << query->length << " " << (double) skipped / sequence->length << " " << (double) by_kim / sequence->length
//         << " " << (double) by_keogh / sequence->length << " " << (double) by_keogh_converse / sequence->length << endl;
}

int main(int argc, char *argv[]) {
    boost::mutex results_mutex;
    Parameters parameters(argc, argv);

    remove(parameters.results_path.c_str());
    ifstream database_ifs(parameters.database_path);
    ifstream queries_ifs(parameters.queries_path);
    ofstream results_ofs(parameters.results_path, std::ofstream::out | std::ofstream::app);

    if (!database_ifs || !queries_ifs || !results_ofs) {
        error(2);
    }

    int size, length, id;
    auto start_time = chrono::high_resolution_clock::now();

    database_ifs >> size;
    vector<const Sequence *> sequences;
    vector<int> sequence_ids;
    for (int i = 0; i < size; ++i) {
        database_ifs >> id >> length;
        if (length > 0) {
            sequence_ids.push_back(id);
            sequences.push_back(new Sequence(database_ifs, length));
        } else {
            cout << "Empty sequence " << id << endl;
        }
    }
    database_ifs.close();

    queries_ifs >> size;
    vector<const Query *> queries;
    vector<int> query_ids;
    for (int i = 0; i < size; ++i) {
        queries_ifs >> id >> length;
        if (length > 0) {
            query_ids.push_back(id);
            queries.push_back(new Query(queries_ifs, length, (int) (parameters.warping_window * (double) length),
                                        (int) (parameters.overlap_ratio * (double) length)));
        } else {
            cout << "Empty query " << id << endl;
        }
    }
    queries_ifs.close();

    chrono::duration<double> elapsed = chrono::high_resolution_clock::now() - start_time;
    cout << "load in " << elapsed.count() << "s" << endl;

    {
        ThreadPool threadPool;
        atomic<int> processed{0};
        start_time = std::chrono::high_resolution_clock::now();

        for (int i = 0; i < (int) sequences.size(); ++i) {
            for (int j = 0; j < (int) queries.size(); ++j) {
                if ((long) queries[j]->length * parameters.min_query_length_multiple < (long) sequences[i]->length) {
                    cout << "Too exhaustive for " << query_ids[j] << " on " << sequence_ids[i] << ": "
                         << queries[j]->length << " / " << sequences[i]->length << endl;
                    continue;
                }
                threadPool.enqueue(boost::bind(conduct_query, sequences[i], queries[j], &parameters, sequence_ids[i],
                                               query_ids[j], &results_mutex, &results_ofs, &start_time, &processed));
            }
        }
    }

    results_ofs.close();
    return 0;
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Framework/TestingFramework.hpp"

#include <array>
#include <boost/functional/hash.hpp>
#include <cstddef>
#include <optional>
#include <unordered_map>
#include <utility>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tags.hpp"
#include "DataStructures/Tensor/EagerMath/DotProduct.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/CoordinateMaps/Affine.hpp"
#include "Domain/CoordinateMaps/CoordinateMap.hpp"
#include "Domain/CoordinateMaps/CoordinateMap.tpp"
#include "Domain/CoordinateMaps/ProductMaps.hpp"
#include "Domain/CoordinateMaps/ProductMaps.tpp"
#include "Domain/LogicalCoordinates.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/Element.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/Weno.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/WenoType.hpp"
#include "Evolution/DiscontinuousGalerkin/NormalVectorTags.hpp"
#include "Evolution/Systems/NewtonianEuler/Limiters/CharacteristicHelpers.hpp"
#include "Evolution/Systems/NewtonianEuler/Limiters/Weno.hpp"
#include "Evolution/Systems/NewtonianEuler/Tags.hpp"
#include "Framework/TestCreation.hpp"
#include "Framework/TestHelpers.hpp"
#include "Helpers/Evolution/DiscontinuousGalerkin/Limiters/TestHelpers.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "NumericalAlgorithms/Spectral/Spectral.hpp"
#include "PointwiseFunctions/Hydro/EquationsOfState/EquationOfState.hpp"
#include "PointwiseFunctions/Hydro/EquationsOfState/IdealFluid.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/MakeArray.hpp"
#include "Utilities/TMPL.hpp"
#include "Utilities/TaggedTuple.hpp"

namespace {

void test_neuler_weno_option_parsing() {
  INFO("Testing option parsing");
  const auto sweno =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Weno<1>>(
          "Type: SimpleWeno\n"
          "VariablesToLimit: Characteristic\n"
          "NeighborWeight: 0.001\n"
          "TvbConstant: 0.0\n"
          "KxrcfConstant: None\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");
  const auto sweno_nw =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Weno<1>>(
          "Type: SimpleWeno\n"
          "VariablesToLimit: Characteristic\n"
          "NeighborWeight: 0.02\n"
          "TvbConstant: 0.0\n"
          "KxrcfConstant: None\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");
  const auto sweno_cons =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Weno<1>>(
          "Type: SimpleWeno\n"
          "VariablesToLimit: Conserved\n"
          "NeighborWeight: 0.001\n"
          "TvbConstant: 0.0\n"
          "KxrcfConstant: None\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");
  const auto sweno_tvb =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Weno<1>>(
          "Type: SimpleWeno\n"
          "VariablesToLimit: Characteristic\n"
          "NeighborWeight: 0.001\n"
          "TvbConstant: 1.0\n"
          "KxrcfConstant: None\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");
  const auto sweno_noflat =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Weno<1>>(
          "Type: SimpleWeno\n"
          "VariablesToLimit: Characteristic\n"
          "NeighborWeight: 0.001\n"
          "TvbConstant: 0.0\n"
          "KxrcfConstant: None\n"
          "ApplyFlattener: False\n"
          "DisableForDebugging: False");
  const auto sweno_disabled =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Weno<1>>(
          "Type: SimpleWeno\n"
          "VariablesToLimit: Characteristic\n"
          "NeighborWeight: 0.001\n"
          "TvbConstant: 0.0\n"
          "KxrcfConstant: None\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: True");
  const auto hweno =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Weno<1>>(
          "Type: Hweno\n"
          "VariablesToLimit: Characteristic\n"
          "NeighborWeight: 0.001\n"
          "TvbConstant: None\n"
          "KxrcfConstant: 0.0\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");
  const auto hweno_kxrcf =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Weno<1>>(
          "Type: Hweno\n"
          "VariablesToLimit: Characteristic\n"
          "NeighborWeight: 0.001\n"
          "TvbConstant: None\n"
          "KxrcfConstant: 1.0\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");

  // Test operators == and !=
  CHECK(sweno == sweno);
  CHECK(sweno != sweno_nw);
  CHECK(sweno != sweno_cons);
  CHECK(sweno != sweno_tvb);
  CHECK(sweno != sweno_noflat);
  CHECK(sweno != sweno_disabled);
  CHECK(sweno != hweno);
  CHECK(hweno != hweno_kxrcf);

  const auto sweno_2d =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Weno<2>>(
          "Type: SimpleWeno\n"
          "VariablesToLimit: Characteristic\n"
          "NeighborWeight: 0.001\n"
          "TvbConstant: 0.0\n"
          "KxrcfConstant: None\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");
  const auto sweno_3d =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Weno<3>>(
          "Type: SimpleWeno\n"
          "VariablesToLimit: Characteristic\n"
          "NeighborWeight: 0.001\n"
          "TvbConstant: 0.0\n"
          "KxrcfConstant: None\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");

  // Test that creation from options gives correct object
  const NewtonianEuler::Limiters::Weno<1> expected_sweno(
      Limiters::WenoType::SimpleWeno,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0.001, 0.0,
      {}, true);
  const NewtonianEuler::Limiters::Weno<1> expected_sweno_nw(
      Limiters::WenoType::SimpleWeno,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0.02, 0.0, {},
      true);
  const NewtonianEuler::Limiters::Weno<1> expected_sweno_cons(
      Limiters::WenoType::SimpleWeno,
      NewtonianEuler::Limiters::VariablesToLimit::Conserved, 0.001, 0.0, {},
      true);
  const NewtonianEuler::Limiters::Weno<1> expected_sweno_tvb(
      Limiters::WenoType::SimpleWeno,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0.001, 1.0,
      {}, true);
  const NewtonianEuler::Limiters::Weno<1> expected_sweno_noflat(
      Limiters::WenoType::SimpleWeno,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0.001, 0.0,
      {}, false);
  const NewtonianEuler::Limiters::Weno<1> expected_sweno_disabled(
      Limiters::WenoType::SimpleWeno,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0.001, 0.0,
      {}, true, true);
  const NewtonianEuler::Limiters::Weno<1> expected_hweno(
      Limiters::WenoType::Hweno,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0.001, {},
      0.0, true);
  const NewtonianEuler::Limiters::Weno<1> expected_hweno_kxrcf(
      Limiters::WenoType::Hweno,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0.001, {},
      1.0, true);
  const NewtonianEuler::Limiters::Weno<2> expected_sweno_2d(
      Limiters::WenoType::SimpleWeno,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0.001, 0.0,
      {}, true);
  const NewtonianEuler::Limiters::Weno<3> expected_sweno_3d(
      Limiters::WenoType::SimpleWeno,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0.001, 0.0,
      {}, true);

  CHECK(sweno == expected_sweno);
  CHECK(sweno_nw == expected_sweno_nw);
  CHECK(sweno_cons == expected_sweno_cons);
  CHECK(sweno_tvb == expected_sweno_tvb);
  CHECK(sweno_noflat == expected_sweno_noflat);
  CHECK(sweno_disabled == expected_sweno_disabled);
  CHECK(hweno == expected_hweno);
  CHECK(hweno_kxrcf == expected_hweno_kxrcf);
  CHECK(sweno_2d == expected_sweno_2d);
  CHECK(sweno_3d == expected_sweno_3d);
}

void test_neuler_weno_serialization() {
  INFO("Testing serialization");
  const NewtonianEuler::Limiters::Weno<1> weno(
      Limiters::WenoType::SimpleWeno,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0.001, 1., {},
      true);
  test_serialization(weno);
}

template <size_t Dim>
DirectionMap<Dim, std::optional<Variables<
                      tmpl::list<evolution::dg::Tags::MagnitudeOfNormal,
                                 evolution::dg::Tags::NormalCovector<Dim>>>>>
compute_normals_and_magnitudes(
    const Mesh<Dim>& mesh,
    const ElementMap<Dim, Frame::Inertial>& element_map) {
  DirectionMap<Dim, std::optional<Variables<
                        tmpl::list<evolution::dg::Tags::MagnitudeOfNormal,
                                   evolution::dg::Tags::NormalCovector<Dim>>>>>
      normals_and_magnitudes{};
  for (const auto& dir : Direction<Dim>::all_directions()) {
    const auto boundary_mesh = mesh.slice_away(dir.dimension());
    normals_and_magnitudes[dir] =
        Variables<tmpl::list<evolution::dg::Tags::MagnitudeOfNormal,
                             evolution::dg::Tags::NormalCovector<Dim>>>(
            boundary_mesh.number_of_grid_points());
    auto& covector = get<evolution::dg::Tags::NormalCovector<Dim>>(
        normals_and_magnitudes[dir].value());
    auto& normal_magnitude = get<evolution::dg::Tags::MagnitudeOfNormal>(
        normals_and_magnitudes[dir].value());
    unnormalized_face_normal(make_not_null(&(covector)),
                             mesh.slice_away(dir.dimension()), element_map,
                             dir);
    normal_magnitude = magnitude(covector);
    for (size_t d = 0; d < Dim; ++d) {
      covector.get(d) /= get(normal_magnitude);
    }
  }
  return normals_and_magnitudes;
}

// Compare the specialized limiter to the generic limiter. The limiters are
// compared for limiting conserved/evolved and limiting characteristic
// variables.
//
// Note: comparing the specialized and generic limiters when limiting the
// characteristic variables is tedious to do thoroughly, because we need to
// call the generic limiter multiple times using characteristic fields w.r.t.
// multiple different unit vectors. In essence, the test would need to fully
// reimplement the specialized limiter. To avoid the test becoming so tedious,
// we test a simpler restricted problem, where in 2D and 3D we require
// input_momentum to be 0. With this simplification, the characteristic
// decomposition becomes independent of the choice of unit vector, and therefore
// the generic limiter can match the specialized limiter even when only one set
// of characteristic (e.g., w.r.t. \hat{x}) is used.
//
// However, this simplifying assumption doesn't play nicely with the HWENO
// limiter, because it uses the KXRCF TCI that relies on non-zero velocities to
// trigger the limiter... so we can use this function with HWENO only in 1D
// where we allow non-zero velocities.
template <size_t VolumeDim>
void test_neuler_vs_generic_weno_work(
    const Limiters::WenoType weno_type, const Scalar<DataVector>& input_density,
    const tnsr::I<DataVector, VolumeDim>& input_momentum,
    const Scalar<DataVector>& input_energy, const Mesh<VolumeDim>& mesh,
    const std::array<double, VolumeDim>& element_size,
    const Scalar<DataVector>& det_inv_logical_to_inertial_jacobian,
    const typename evolution::dg::Tags::NormalCovectorAndMagnitude<
        VolumeDim>::type& normals_and_magnitudes,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        typename NewtonianEuler::Limiters::Weno<VolumeDim>::PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data) {
  // Sanity check that
  // - input momentum satisfies simplifying assumptions
  // - limiter type is consistent with these simplifying assumptions
  if constexpr (VolumeDim > 1) {
    const auto zero_momentum = make_with_value<tnsr::I<DataVector, VolumeDim>>(
        get<0>(input_momentum), 0.);
    REQUIRE(input_momentum == zero_momentum);
    REQUIRE(weno_type == Limiters::WenoType::SimpleWeno);
  }

  CAPTURE(weno_type);

  const double neighbor_linear_weight = 0.001;
  const double tvb_constant = 0.;
  const double kxrcf_constant = 0.;
  const auto element = TestHelpers::Limiters::make_element<VolumeDim>();
  const EquationsOfState::IdealFluid<false> equation_of_state{5. / 3.};

  {
    INFO("Compare specialized vs generic Weno on conserved variables");
    auto density_generic = input_density;
    auto momentum_generic = input_momentum;
    auto energy_generic = input_energy;
    const Limiters::Weno<
        VolumeDim, tmpl::list<NewtonianEuler::Tags::MassDensityCons,
                              NewtonianEuler::Tags::MomentumDensity<VolumeDim>,
                              NewtonianEuler::Tags::EnergyDensity>>
        weno_generic(weno_type, neighbor_linear_weight, tvb_constant);
    const bool activated_generic = weno_generic(
        make_not_null(&density_generic), make_not_null(&momentum_generic),
        make_not_null(&energy_generic), mesh, element, element_size,
        neighbor_data);

    std::optional<double> opt_tvb_constant{};
    std::optional<double> opt_kxrcf_constant{};
    if (weno_type == Limiters::WenoType::SimpleWeno) {
      opt_tvb_constant = tvb_constant;
    } else if (weno_type == Limiters::WenoType::Hweno) {
      opt_kxrcf_constant = kxrcf_constant;
    }
    const bool apply_flattener = false;

    auto density_specialized = input_density;
    auto momentum_specialized = input_momentum;
    auto energy_specialized = input_energy;
    const auto vars_to_limit =
        NewtonianEuler::Limiters::VariablesToLimit::Conserved;
    const NewtonianEuler::Limiters::Weno<VolumeDim> weno_specialized(
        weno_type, vars_to_limit, neighbor_linear_weight, opt_tvb_constant,
        opt_kxrcf_constant, apply_flattener);
    const bool activated_specialized = weno_specialized(
        make_not_null(&density_specialized),
        make_not_null(&momentum_specialized),
        make_not_null(&energy_specialized), mesh, element, element_size,
        det_inv_logical_to_inertial_jacobian, normals_and_magnitudes,
        equation_of_state, neighbor_data);

    CHECK(activated_generic);
    CHECK(activated_generic == activated_specialized);
    CHECK_ITERABLE_APPROX(density_generic, density_specialized);
    CHECK_ITERABLE_APPROX(momentum_generic, momentum_specialized);
    CHECK_ITERABLE_APPROX(energy_generic, energy_specialized);
  }

  {
    INFO("Compare specialized vs generic Weno on characteristic variables");
    // Cellwise means, for cons/char transforms
    const auto mean_density =
        Scalar<double>{mean_value(get(input_density), mesh)};
    const auto mean_momentum = [&input_momentum, &mesh]() {
      tnsr::I<double, VolumeDim> result{};
      for (size_t i = 0; i < VolumeDim; ++i) {
        result.get(i) = mean_value(input_momentum.get(i), mesh);
      }
      return result;
    }();
    const auto mean_energy =
        Scalar<double>{mean_value(get(input_energy), mesh)};

    // Compute characteristic transformation using x-direction...
    // Note that in 2D and 3D we know the fluid velocity is 0, so this
    // choice is arbitrary in these higher dimensions.
    const auto unit_vector = []() {
      auto components = make_array<VolumeDim>(0.);
      components[0] = 1.;
      return tnsr::i<double, VolumeDim>(components);
    }();
    const auto right_and_left =
        NewtonianEuler::Limiters::right_and_left_eigenvectors(
            mean_density, mean_momentum, mean_energy, equation_of_state,
            unit_vector);
    const auto& right = right_and_left.first;
    const auto& left = right_and_left.second;

    // Transform tensors to characteristics
    auto char_v_minus = input_density;
    auto char_v_momentum = input_momentum;
    auto char_v_plus = input_energy;
    NewtonianEuler::Limiters::characteristic_fields(
        make_not_null(&char_v_minus), make_not_null(&char_v_momentum),
        make_not_null(&char_v_plus), input_density, input_momentum,
        input_energy, left);

    using GenericWeno =
        Limiters::Weno<VolumeDim,
                       tmpl::list<NewtonianEuler::Tags::VMinus,
                                  NewtonianEuler::Tags::VMomentum<VolumeDim>,
                                  NewtonianEuler::Tags::VPlus>>;
    std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        typename GenericWeno::PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
        neighbor_char_data{};
    for (const auto& [key, data] : neighbor_data) {
      neighbor_char_data[key].element_size = data.element_size;
      neighbor_char_data[key].mesh = data.mesh;
      NewtonianEuler::Limiters::characteristic_fields(
          make_not_null(&(neighbor_char_data[key].means)), data.means, left);
      neighbor_char_data[key].volume_data.initialize(
          mesh.number_of_grid_points());
      NewtonianEuler::Limiters::characteristic_fields(
          make_not_null(&(neighbor_char_data[key].volume_data)),
          data.volume_data, left);
    }

    const GenericWeno weno_generic(weno_type, neighbor_linear_weight,
                                   tvb_constant);
    const bool activated_generic = weno_generic(
        make_not_null(&char_v_minus), make_not_null(&char_v_momentum),
        make_not_null(&char_v_plus), mesh, element, element_size,
        neighbor_char_data);

    // Transform back to evolved variables
    // Note that because the fluid velocity is 0 and all sets of
    // characteristics are identical, we can skip the step of combining
    // different limiter results.
    auto density_generic = input_density;
    auto momentum_generic = input_momentum;
    auto energy_generic = input_energy;
    NewtonianEuler::Limiters::conserved_fields_from_characteristic_fields(
        make_not_null(&density_generic), make_not_null(&momentum_generic),
        make_not_null(&energy_generic), char_v_minus, char_v_momentum,
        char_v_plus, right);

    std::optional<double> opt_tvb_constant{};
    std::optional<double> opt_kxrcf_constant{};
    if (weno_type == Limiters::WenoType::SimpleWeno) {
      opt_tvb_constant = tvb_constant;
    } else if (weno_type == Limiters::WenoType::Hweno) {
      opt_kxrcf_constant = kxrcf_constant;
    }
    const bool apply_flattener = false;

    auto density_specialized = input_density;
    auto momentum_specialized = input_momentum;
    auto energy_specialized = input_energy;
    const auto vars_to_limit =
        NewtonianEuler::Limiters::VariablesToLimit::Characteristic;
    const NewtonianEuler::Limiters::Weno<VolumeDim> weno_specialized(
        weno_type, vars_to_limit, neighbor_linear_weight, opt_tvb_constant,
        opt_kxrcf_constant, apply_flattener);
    const bool activated_specialized = weno_specialized(
        make_not_null(&density_specialized),
        make_not_null(&momentum_specialized),
        make_not_null(&energy_specialized), mesh, element, element_size,
        det_inv_logical_to_inertial_jacobian, normals_and_magnitudes,
        equation_of_state, neighbor_data);

    CHECK(activated_generic);
    CHECK(activated_generic == activated_specialized);
    CHECK_ITERABLE_APPROX(density_generic, density_specialized);
    CHECK_ITERABLE_APPROX(momentum_generic, momentum_specialized);
    CHECK_ITERABLE_APPROX(energy_generic, energy_specialized);
  }
}

void test_neuler_vs_generic_weno_1d() {
  INFO("Testing NewtonianEuler::Limiters::Weno limiter in 1D");
  const auto mesh =
      Mesh<1>(3, Spectral::Basis::Legendre, Spectral::Quadrature::GaussLobatto);
  const auto element = TestHelpers::Limiters::make_element<1>();
  const auto logical_coords = logical_coordinates(mesh);
  using Affine = domain::CoordinateMaps::Affine;
  const Affine xi_map{-1., 1., 3.7, 4.2};
  const auto coordmap =
      domain::make_coordinate_map_base<Frame::BlockLogical, Frame::Inertial>(
          xi_map);
  const ElementMap<1, Frame::Inertial> element_map(element.id(),
                                                   coordmap->get_clone());
  const auto element_size = size_of_element(element_map);
  const auto det_inv_logical_to_inertial_jacobian =
      determinant(element_map.inv_jacobian(logical_coords));
  const auto normals_and_magnitudes =
      compute_normals_and_magnitudes(mesh, element_map);

  const auto& x = get<0>(logical_coords);
  const auto mass_density_cons = [&x]() {
    return Scalar<DataVector>{{{1. + 0.2 * x + 0.05 * square(x)}}};
  }();
  const auto momentum_density = [&x]() {
    return tnsr::I<DataVector, 1>{{{0.2 - 0.3 * x}}};
  }();
  const auto energy_density = [&mesh]() {
    return Scalar<DataVector>{DataVector(mesh.number_of_grid_points(), 1.)};
  }();

  std::unordered_map<std::pair<Direction<1>, ElementId<1>>,
                     NewtonianEuler::Limiters::Weno<1>::PackagedData,
                     boost::hash<std::pair<Direction<1>, ElementId<1>>>>
      neighbor_data{};
  const std::array<std::pair<Direction<1>, ElementId<1>>, 2> dir_keys = {
      {{Direction<1>::lower_xi(), ElementId<1>(1)},
       {Direction<1>::upper_xi(), ElementId<1>(2)}}};
  const size_t num_pts = mesh.number_of_grid_points();
  for (const auto& dir_key : dir_keys) {
    neighbor_data[dir_key].element_size = element_size;
    neighbor_data[dir_key].mesh = mesh;
    neighbor_data[dir_key].volume_data.initialize(num_pts, 0.0);
  }

  // Note: the WENO algorithms are tested in detail elsewhere, so here we don't
  // go through the trouble of setting up realistic neighbor data that varies
  // over the neighbor's grid. Instead we pass in constant volume data because
  // it's a lot easier to set up.
  using rho = NewtonianEuler::Tags::MassDensityCons;
  get(get<rho>(neighbor_data[dir_keys[0]].volume_data)) = 0.4;
  get(get<rho>(neighbor_data[dir_keys[1]].volume_data)) = 1.1;
  using mean_rho = Tags::Mean<NewtonianEuler::Tags::MassDensityCons>;
  get(get<mean_rho>(neighbor_data[dir_keys[0]].means)) = 0.4;
  get(get<mean_rho>(neighbor_data[dir_keys[1]].means)) = 1.1;

  using rhou = NewtonianEuler::Tags::MomentumDensity<1>;
  get<0>(get<rhou>(neighbor_data[dir_keys[0]].volume_data)) = 0.2;
  get<0>(get<rhou>(neighbor_data[dir_keys[1]].volume_data)) = -0.1;
  using mean_rhou = Tags::Mean<NewtonianEuler::Tags::MomentumDensity<1>>;
  get<0>(get<mean_rhou>(neighbor_data[dir_keys[0]].means)) = 0.2;
  get<0>(get<mean_rhou>(neighbor_data[dir_keys[1]].means)) = -0.1;

  using eps = NewtonianEuler::Tags::EnergyDensity;
  get(get<eps>(neighbor_data[dir_keys[0]].volume_data)) = 1.;
  get(get<eps>(neighbor_data[dir_keys[1]].volume_data)) = 1.;
  using mean_eps = Tags::Mean<NewtonianEuler::Tags::EnergyDensity>;
  get(get<mean_eps>(neighbor_data[dir_keys[0]].means)) = 1.;
  get(get<mean_eps>(neighbor_data[dir_keys[1]].means)) = 1.;

  test_neuler_vs_generic_weno_work(
      Limiters::WenoType::SimpleWeno, mass_density_cons, momentum_density,
      energy_density, mesh, element_size, det_inv_logical_to_inertial_jacobian,
      normals_and_magnitudes, neighbor_data);

  test_neuler_vs_generic_weno_work(
      Limiters::WenoType::Hweno, mass_density_cons, momentum_density,
      energy_density, mesh, element_size, det_inv_logical_to_inertial_jacobian,
      normals_and_magnitudes, neighbor_data);
}

void test_neuler_vs_generic_weno_2d() {
  INFO("Testing NewtonianEuler::Limiters::Weno limiter in 2D");
  const auto mesh =
      Mesh<2>(3, Spectral::Basis::Legendre, Spectral::Quadrature::GaussLobatto);
  const auto element = TestHelpers::Limiters::make_element<2>();
  const auto logical_coords = logical_coordinates(mesh);
  using Affine = domain::CoordinateMaps::Affine;
  using Affine2D = domain::CoordinateMaps::ProductOf2Maps<Affine, Affine>;
  const Affine xi_map{-1., 1., 3.7, 4.2};
  const Affine eta_map{-1., 1., 3.2, 4.2};
  const Affine2D map2d{xi_map, eta_map};
  const auto coordmap =
      domain::make_coordinate_map_base<Frame::BlockLogical, Frame::Inertial>(
          map2d);
  const ElementMap<2, Frame::Inertial> element_map(element.id(),
                                                   coordmap->get_clone());
  const auto element_size = size_of_element(element_map);
  const auto det_inv_logical_to_inertial_jacobian =
      determinant(element_map.inv_jacobian(logical_coords));
  const auto normals_and_magnitudes =
      compute_normals_and_magnitudes(mesh, element_map);

  const auto& x = get<0>(logical_coords);
  const auto& y = get<1>(logical_coords);
  const auto mass_density_cons = [&x, &y]() {
    return Scalar<DataVector>{
        {{1. + 0.2 * x - 0.1 * y + 0.05 * x * square(y)}}};
  }();
  const auto momentum_density = [&mesh]() {
    return tnsr::I<DataVector, 2>{DataVector(mesh.number_of_grid_points(), 0.)};
  }();
  const auto energy_density = [&x, &y]() {
    return Scalar<DataVector>{{{1.3 + 0.1 * y - 0.06 * square(x) * y}}};
  }();

  std::unordered_map<std::pair<Direction<2>, ElementId<2>>,
                     NewtonianEuler::Limiters::Weno<2>::PackagedData,
                     boost::hash<std::pair<Direction<2>, ElementId<2>>>>
      neighbor_data{};
  const std::array<std::pair<Direction<2>, ElementId<2>>, 4> dir_keys = {
      {{Direction<2>::lower_xi(), ElementId<2>(1)},
       {Direction<2>::upper_xi(), ElementId<2>(2)},
       {Direction<2>::lower_eta(), ElementId<2>(3)},
       {Direction<2>::upper_eta(), ElementId<2>(4)}}};
  const size_t num_pts = mesh.number_of_grid_points();
  for (const auto& dir_key : dir_keys) {
    neighbor_data[dir_key].element_size = element_size;
    neighbor_data[dir_key].mesh = mesh;
    neighbor_data[dir_key].volume_data.initialize(num_pts, 0.0);
  }

  using rho = NewtonianEuler::Tags::MassDensityCons;
  get(get<rho>(neighbor_data[dir_keys[0]].volume_data)) = 0.4;
  get(get<rho>(neighbor_data[dir_keys[1]].volume_data)) = 1.1;
  get(get<rho>(neighbor_data[dir_keys[2]].volume_data)) = 2.1;
  get(get<rho>(neighbor_data[dir_keys[3]].volume_data)) = 0.9;
  using mean_rho = Tags::Mean<NewtonianEuler::Tags::MassDensityCons>;
  get(get<mean_rho>(neighbor_data[dir_keys[0]].means)) = 0.4;
  get(get<mean_rho>(neighbor_data[dir_keys[1]].means)) = 1.1;
  get(get<mean_rho>(neighbor_data[dir_keys[2]].means)) = 2.1;
  get(get<mean_rho>(neighbor_data[dir_keys[3]].means)) = 0.9;

  for (const auto& dir_key : dir_keys) {
    using mean_rhou = Tags::Mean<NewtonianEuler::Tags::MomentumDensity<2>>;
    get<0>(get<mean_rhou>(neighbor_data[dir_key].means)) = 0.;
    get<1>(get<mean_rhou>(neighbor_data[dir_key].means)) = 0.;
    using mean_eps = Tags::Mean<NewtonianEuler::Tags::EnergyDensity>;
    get(get<mean_eps>(neighbor_data[dir_key].means)) = 1.;
  }

  test_neuler_vs_generic_weno_work(
      Limiters::WenoType::SimpleWeno, mass_density_cons, momentum_density,
      energy_density, mesh, element_size, det_inv_logical_to_inertial_jacobian,
      normals_and_magnitudes, neighbor_data);
}

void test_neuler_vs_generic_weno_3d() {
  INFO("Testing NewtonianEuler::Limiters::Weno limiter in 3D");
  const auto mesh =
      Mesh<3>(3, Spectral::Basis::Legendre, Spectral::Quadrature::GaussLobatto);
  const auto element = TestHelpers::Limiters::make_element<3>();
  const auto logical_coords = logical_coordinates(mesh);
  using Affine = domain::CoordinateMaps::Affine;
  using Affine3D =
      domain::CoordinateMaps::ProductOf3Maps<Affine, Affine, Affine>;
  const Affine xi_map{-1., 1., 3.7, 4.2};
  const Affine eta_map{-1., 1., 3.2, 4.2};
  const Affine zeta_map{-1., 1., 1.3, 2.1};
  const Affine3D map3d{xi_map, eta_map, zeta_map};
  const auto coordmap =
      domain::make_coordinate_map_base<Frame::BlockLogical, Frame::Inertial>(
          map3d);
  const ElementMap<3, Frame::Inertial> element_map(element.id(),
                                                   coordmap->get_clone());
  const auto element_size = size_of_element(element_map);
  const auto det_inv_logical_to_inertial_jacobian =
      determinant(element_map.inv_jacobian(logical_coords));
  const auto normals_and_magnitudes =
      compute_normals_and_magnitudes(mesh, element_map);

  const auto& x = get<0>(logical_coords);
  const auto& y = get<1>(logical_coords);
  const auto& z = get<2>(logical_coords);
  const auto mass_density_cons = [&x, &y, &z]() {
    return Scalar<DataVector>{{{1. + 0.2 * x - 0.1 * y + 0.4 * z}}};
  }();
  const auto momentum_density = [&mesh]() {
    return tnsr::I<DataVector, 3>{DataVector(mesh.number_of_grid_points(), 0.)};
  }();
  const auto energy_density = [&x, &y, &z]() {
    return Scalar<DataVector>{{{1.8 - 0.1 * square(x) * y * square(z)}}};
  }();

  std::unordered_map<std::pair<Direction<3>, ElementId<3>>,
                     NewtonianEuler::Limiters::Weno<3>::PackagedData,
                     boost::hash<std::pair<Direction<3>, ElementId<3>>>>
      neighbor_data{};
  const std::array<std::pair<Direction<3>, ElementId<3>>, 6> dir_keys = {
      {{Direction<3>::lower_xi(), ElementId<3>(1)},
       {Direction<3>::upper_xi(), ElementId<3>(2)},
       {Direction<3>::lower_eta(), ElementId<3>(3)},
       {Direction<3>::upper_eta(), ElementId<3>(4)},
       {Direction<3>::lower_zeta(), ElementId<3>(5)},
       {Direction<3>::upper_zeta(), ElementId<3>(6)}}};
  const size_t num_pts = mesh.number_of_grid_points();
  for (const auto& dir_key : dir_keys) {
    neighbor_data[dir_key].element_size = element_size;
    neighbor_data[dir_key].mesh = mesh;
    neighbor_data[dir_key].volume_data.initialize(num_pts, 0.0);
  }

  using rho = NewtonianEuler::Tags::MassDensityCons;
  get(get<rho>(neighbor_data[dir_keys[0]].volume_data)) = 0.4;
  get(get<rho>(neighbor_data[dir_keys[1]].volume_data)) = 1.1;
  get(get<rho>(neighbor_data[dir_keys[2]].volume_data)) = 2.1;
  get(get<rho>(neighbor_data[dir_keys[3]].volume_data)) = 0.9;
  get(get<rho>(neighbor_data[dir_keys[4]].volume_data)) = 0.3;
  get(get<rho>(neighbor_data[dir_keys[5]].volume_data)) = 1.3;
  using mean_rho = Tags::Mean<NewtonianEuler::Tags::MassDensityCons>;
  get(get<mean_rho>(neighbor_data[dir_keys[0]].means)) = 0.4;
  get(get<mean_rho>(neighbor_data[dir_keys[1]].means)) = 1.1;
  get(get<mean_rho>(neighbor_data[dir_keys[2]].means)) = 2.1;
  get(get<mean_rho>(neighbor_data[dir_keys[3]].means)) = 0.9;
  get(get<mean_rho>(neighbor_data[dir_keys[4]].means)) = 0.3;
  get(get<mean_rho>(neighbor_data[dir_keys[5]].means)) = 1.3;

  for (const auto& dir_key : dir_keys) {
    using rhou = NewtonianEuler::Tags::MomentumDensity<3>;
    get<0>(get<rhou>(neighbor_data[dir_key].volume_data)) = 0.;
    get<1>(get<rhou>(neighbor_data[dir_key].volume_data)) = 0.;
    using mean_rhou = Tags::Mean<NewtonianEuler::Tags::MomentumDensity<3>>;
    get<0>(get<mean_rhou>(neighbor_data[dir_key].means)) = 0.;
    get<1>(get<mean_rhou>(neighbor_data[dir_key].means)) = 0.;
    using eps = NewtonianEuler::Tags::EnergyDensity;
    get(get<eps>(neighbor_data[dir_key].volume_data)) = 1.;
    using mean_eps = Tags::Mean<NewtonianEuler::Tags::EnergyDensity>;
    get(get<mean_eps>(neighbor_data[dir_key].means)) = 1.;
  }

  test_neuler_vs_generic_weno_work(
      Limiters::WenoType::SimpleWeno, mass_density_cons, momentum_density,
      energy_density, mesh, element_size, det_inv_logical_to_inertial_jacobian,
      normals_and_magnitudes, neighbor_data);
}

template <size_t VolumeDim>
void test_neuler_weno_flattener() {
  INFO("Testing flattener use in NewtonianEuler::Limiters::Weno limiter");
  CAPTURE(VolumeDim);

  const auto mesh = Mesh<VolumeDim>(2, Spectral::Basis::Legendre,
                                    Spectral::Quadrature::GaussLobatto);
  // We use an element with no neighbors so the limiter does nothing
  const auto element = Element<VolumeDim>{ElementId<VolumeDim>(0), {}};
  const auto element_size = make_array<VolumeDim>(0.6);
  // inv_jac = logical volume / inertial volume
  const Scalar<DataVector> det_inv_logical_to_inertial_jacobian{DataVector(
      mesh.number_of_grid_points(), pow<VolumeDim>(2.) / pow<VolumeDim>(0.6))};
  const EquationsOfState::IdealFluid<false> equation_of_state{5. / 3.};

  const size_t num_points = mesh.number_of_grid_points();
  const auto input_density = [&num_points]() {
    // One negative value to trigger flattener
    Scalar<DataVector> density{DataVector(num_points, 0.8)};
    get(density)[0] = -0.2;
    return density;
  }();
  const auto input_momentum = [&num_points]() {
    return tnsr::I<DataVector, VolumeDim>{DataVector(num_points, 0.1)};
  }();
  const auto input_energy = [&num_points]() {
    return Scalar<DataVector>{DataVector(num_points, 1.4)};
  }();

  // Empty map because no neighbors
  const std::unordered_map<
      std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
      typename NewtonianEuler::Limiters::Weno<VolumeDim>::PackagedData,
      boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
      neighbor_data{};

  // First a sanity check: there should be a negative density for this test to
  // make sense... otherwise we aren't testing anything useful
  const bool has_negative_density = min(get(input_density)) < 0.;
  REQUIRE(has_negative_density);

  // With flattener off, check the limiter does nothing (because no neighbors).
  //
  // Note that the post-limiter result is unphysical: the density is still
  // negative at one point. Because of this, we have to use the generic limiter
  // when testing the limiter does nothing: the specialized limiter has an
  // ASSERT that the post-limiting solution is valid which would fail here.
  auto density = input_density;
  auto momentum = input_momentum;
  auto energy = input_energy;
  const double tvb_constant = 0.;
  const Limiters::Weno<
      VolumeDim, tmpl::list<NewtonianEuler::Tags::MassDensityCons,
                            NewtonianEuler::Tags::MomentumDensity<VolumeDim>,
                            NewtonianEuler::Tags::EnergyDensity>>
      weno_noflat(Limiters::WenoType::SimpleWeno, 0.001, tvb_constant, {});
  const bool activated_noflat = weno_noflat(
      make_not_null(&density), make_not_null(&momentum), make_not_null(&energy),
      mesh, element, element_size, neighbor_data);
  CHECK_FALSE(activated_noflat);

  // Not read from, so empty
  const DirectionMap<VolumeDim,
                     std::optional<Variables<tmpl::list<
                         evolution::dg::Tags::MagnitudeOfNormal,
                         evolution::dg::Tags::NormalCovector<VolumeDim>>>>>
      normals_and_magnitudes{};

  // With flattener on, check limiter activates
  density = input_density;
  momentum = input_momentum;
  energy = input_energy;
  const auto vars_to_limit =
      NewtonianEuler::Limiters::VariablesToLimit::Conserved;
  const NewtonianEuler::Limiters::Weno<VolumeDim> weno(
      Limiters::WenoType::SimpleWeno, vars_to_limit, 0.001, tvb_constant, {},
      true);
  const bool activated = weno(
      make_not_null(&density), make_not_null(&momentum), make_not_null(&energy),
      mesh, element, element_size, det_inv_logical_to_inertial_jacobian,
      normals_and_magnitudes, equation_of_state, neighbor_data);
  CHECK(activated);

  // Finally, check the negative density is brought back to positive
  const bool density_is_positive_everywhere = min(get(density)) > 0.;
  CHECK(density_is_positive_everywhere);
}

}  // namespace

SPECTRE_TEST_CASE("Unit.Evolution.Systems.NewtonianEuler.Limiters.Weno",
                  "[Limiters][Unit]") {
  test_neuler_weno_option_parsing();
  test_neuler_weno_serialization();

  // The package_data function for NewtonianEuler::Limiters::Weno is just a
  // direct call to the generic Weno package_data. We do not test it.

  // Compare specialized vs generic Weno limiters on simplified input data.
  // The SimpleWeno implementation is tested in all dims, but Hweno is tested
  // in 1D only because its TCI isn't compatible with the simplified input data.
  test_neuler_vs_generic_weno_1d();
  test_neuler_vs_generic_weno_2d();
  test_neuler_vs_generic_weno_3d();

  // Test the use of the post-processing flattener
  test_neuler_weno_flattener<1>();
  test_neuler_weno_flattener<2>();
  test_neuler_weno_flattener<3>();
}


/*
 * File:   image_noise_sim.cc
 * Author: ts337
 *
 * Created on 22 March 2011, 18:49
 */

#include "../image_noise_sim.h"
#include <boost/random/poisson_distribution.hpp>
#include<iostream>

namespace coela {
namespace image_noise_simulation {

uniform_random_pixel_variate::uniform_random_pixel_variate(
    const PixelRange& image_outline_,
    unuran::StreamWrapper& rns)
    : urv(0.0, image_outline_.n_pix(), rns), range(image_outline_)
{}

PixelIndex uniform_random_pixel_variate::operator()()
{
    int data_vec_index = int(urv()) ;
    return range.get_PixelIndex_for_data_vector_element(data_vec_index);
}

intensity_map_random_pixel_variate::intensity_map_random_pixel_variate(
    const PixelArray2d<double>& intensity_map,
    unuran::StreamWrapper& rns,
    const size_t /*expected_number_of_uses*/)
    :gen(NULL), range(intensity_map.range())
{
    UNUR_DISTR *distr;    /* distribution object                   */
    UNUR_PAR   *par;      /* parameter object                      */

    distr = unur_distr_discr_new();

    const vector<double>& prob_vec(intensity_map.get_raw_data());

    unur_distr_discr_set_pv(distr, &prob_vec[0] , prob_vec.size());
//    par = unur_dgt_new(distr);
//    unur_dgt_set_guidefactor(par, std::min(3.0/sqrt(prob_vec.size()), 1.0) );

    par = unur_dau_new(distr);
    unur_set_urng(par, rns.get_stream_pointer());
    gen = unur_init(par);
    unur_distr_free(distr);

    if (gen==NULL) { throw std::runtime_error("intensity_map_random_pixel_variate::intensity_map_random_pixel_variate - could not construct generator."); }

}

PixelIndex intensity_map_random_pixel_variate::operator()()
{
    int data_vec_index = unur_sample_discr(gen);
    return range.get_PixelIndex_for_data_vector_element(data_vec_index);
}

PixelArray2d<int> generate_photon_arrival_map(
    const PixelArray2d<double>& source_intensity_map,
    const double mean_total_number_of_source_photons,
    const double mean_bg_photons_per_pixel,
    unuran::StreamWrapper& rns)
{

    unuran::PoissonRandomVariate source_total_prv(
        mean_total_number_of_source_photons, rns);

    int n_source_photons = source_total_prv();
//    return photon_arrival_map_fixed_n_low_flux_optimized(intensity_map, n_photons, rns);

    PixelArray2d<int> source_photon_arrival_map(
        source_intensity_map.range().x_dim(), source_intensity_map.range().y_dim(), 0);

    intensity_map_random_pixel_variate pixel_picker(source_intensity_map, rns);

    for (int phot_index=0; phot_index!=n_source_photons; ++phot_index) {
        source_photon_arrival_map(pixel_picker())+=1;
    }

    if (mean_bg_photons_per_pixel!=0.0) {
        double mean_bg_sum_flux =
            source_photon_arrival_map.range().n_pix() * mean_bg_photons_per_pixel;

        unuran::PoissonRandomVariate bg_total_prv(mean_bg_sum_flux, rns);
        int n_bg_photons = bg_total_prv();
        uniform_random_pixel_variate bg_pixel_picker(source_photon_arrival_map.range(),
                rns);
        for (int bg_phot_gen=0; bg_phot_gen!=n_bg_photons; ++bg_phot_gen) {
            source_photon_arrival_map(bg_pixel_picker()) +=1;
        }
    }
    return source_photon_arrival_map;

}

PixelArray2d<int> EMCCD_simulated_image(
    const PixelArray2d<int> & photon_arrival_map,
    EmccdModel& model_variate,
    unuran::StreamWrapper& rns)
{
    PixelArray2d<int> sim_image(photon_arrival_map.range().x_dim(),
                                photon_arrival_map.range().y_dim(), 0);

    //Simulate photon events
    for (PixelIterator i(photon_arrival_map.range()); i!=i.end; ++i) {
        if (photon_arrival_map(i)) {
            sim_image(i) +=
                model_variate.stochastically_multiply_photons(photon_arrival_map(i));
        }
    }

    //simulate CIC events
    if (model_variate.params.serial_CIC_rate!=0) {
        int n_CIC_events;

        double expected_num_CIC_events = model_variate.params.serial_CIC_rate *
                                         sim_image.range().n_pix() ;

        unuran::PoissonRandomVariate n_CIC_events_variate(
            expected_num_CIC_events,
            rns);
        n_CIC_events = n_CIC_events_variate();
        uniform_random_pixel_variate pixel_picker(sim_image.range(), rns);

        for (int i=0; i!=n_CIC_events; ++i) {
//            int pixel_number = int( pixel_picker() ) + 1;
//            PixelIndex pix = sim_image.range().get_nth_PixelIndex(pixel_number);
            PixelIndex pix = pixel_picker();
            sim_image(pix) += model_variate.CICIR_variate();
        }
    }
    //Simulate readout
    for (PixelIterator i(sim_image.range()); i!=i.end; ++i) {
        sim_image(i)+=model_variate.readout_noise_variate();
    }

    return sim_image;
}

}//end namespace coela::image_noise_simulation
}//end namespace coela


#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <vector>
#include <iostream>
#include <memory>

int cell = 16;
int bin_num = 8;

struct Intrinsics{
    double fx;
    double fy;
    double cx;
    double cy;
};

void Get3dPointAndIntensity(int cell_size, int cell_row_id, int cell_col_id, const cv::Mat& image, const cv::Mat& depth, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >& points_3d, Eigen::VectorXd& intensity, const Eigen::Matrix4d& T_wc, Intrinsics in);

std::vector<cv::Mat> ReadGroundtruth(std::string add, std::string dataset);

class NID{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    void set_member_size();
    void ComputeHref();
    void ComputeH();

    int cell_ = 16;
    int bin_num_ ;
    Eigen::VectorXd intensity0_;
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > pw0_;
    Eigen::Matrix4d tf_;
    cv::Mat image1_;
    double fx_;
    double fy_;
    double cx_;
    double cy_;
    std::vector<double> pro_ref_, pro_current_;
    std::vector<std::vector<double> > pro_joint_;
    double sigma_ = 1e-30;
    double H_ref_ = 0.0, H_current_ = 0.0, H_joint_ = 0.0;
    Eigen::VectorXd intensity_current_;
    double nid_, MI_;

    inline double get_interpolated_pixel_value ( double x, double y )
    {
        int ix = (int)x;
        int iy = (int)y;

        double dx = x - ix;
        double dy = y - iy;
        double dxdy = dx*dy;

        double xx = x - floor ( x );
        double yy = y - floor ( y );

        return double (
        dxdy * image1_.ptr<uchar>(iy+1)[ix+1]
        + (dy - dxdy) * image1_.ptr<uchar>(iy+1)[ix]
        + (dx - dxdy) * image1_.ptr<uchar>(iy)[ix+1]
        + (1 - dx - dy + dxdy) * image1_.ptr<uchar>(iy)[ix]
        );
    }
};

int main(int argc, char** argv){
    if(argc!=2){
        std::cout<<"usage './program path_to_config.yaml', image1's timestamp should be smaller than image2"<<std::endl;
        exit(0);
    }

    std::string config_add = argv[1];
    cv::FileStorage fc;
    fc.open(config_add, cv::FileStorage::READ);
    std::string type0 = fc["image0_type"] , type1 = fc["image1_type"], id0 = fc["image0_id"], id1 = fc["image1_id"], use_gt = fc["use_groundtruth"], dataset = fc["dataset"], im_add = fc["im_address"];
    double depth_factor = 1.0/(int)fc["depth_factor"], fx = fc["fx"], fy = fc["fy"], cx = fc["cx"], cy = fc["cy"];

    cv::Mat im0,im1;
    std::string rgb0_add, rgb1_add;
    if(dataset == "eth_cvg"){
      rgb0_add = im_add + type0 + "/" + id0 + ".png";
      rgb1_add = im_add + type1 + "/" + id1 + ".png";
    }

    cv::Mat im_rgb0 = cv::imread(rgb0_add,CV_LOAD_IMAGE_UNCHANGED);//use cv::IMREAD_GRAYSCALE, the result will be different
    im0 = im_rgb0;
    cvtColor(im0,im0,CV_RGB2GRAY);

    cv::Mat im_rgb1 = cv::imread(rgb1_add,CV_LOAD_IMAGE_UNCHANGED);
    im1 = im_rgb1;
    cvtColor(im1,im1,CV_RGB2GRAY);

    std::cout<<"image size "<<im0.size()<<","<<im1.size()<<std::endl;

    //read corresponding depth
    cv::Mat depth0;
    if(dataset == "eth_cvg"){
      std::string depth_add = im_add + "depth/" + id0 + ".png";
      depth0 = cv::imread(depth_add, CV_LOAD_IMAGE_UNCHANGED);
      depth0.convertTo(depth0,CV_64F,depth_factor);
    }

    std::string pose_gt_add;
    if(dataset == "eth_cvg"){
      pose_gt_add = im_add + "groundtruth.txt";
    }
    std::vector<cv::Mat> gt = ReadGroundtruth(pose_gt_add, dataset);
    int pose_id0 = stoi(id0);
    int pose_id1 = stoi(id1);

    cv::Mat T_wc0_cv,T_wc1_cv, T_cw0_cv, T_cw1_cv;

    //pose id output from slam needs to corresponds to the id input of this program
    cv::FileStorage fp0, fp1;
    if(use_gt == "0"){
      if(fp0.open(std::to_string(pose_id0) + ".xml", cv::FileStorage::READ) && fp1.open(std::to_string(pose_id1) + ".xml", cv::FileStorage::READ)){
        std::cout<<"use the pose from etimation"<<std::endl;
        fp0["pose"]>>T_cw0_cv;
        fp1["pose"]>>T_cw1_cv;
        T_cw0_cv.convertTo(T_cw0_cv, CV_64F);
        T_cw1_cv.convertTo(T_cw1_cv, CV_64F);
        T_wc0_cv = T_cw0_cv.inv();
        T_wc1_cv = T_cw1_cv.inv();
      }
      else{
        std::cout<<"no correspoding pose, exit "<<std::endl;
        exit(0);
      }
    }
    else if(use_gt == "1"){
        std::cout<<"use groundtruth pose "<<std::endl;
        T_wc0_cv = gt[pose_id0];
        T_wc1_cv = gt[pose_id1];
    }
    else{
      std::cout<<"you must have pose provided by groundtruth or SLAM"<<std::endl;
    }

    //std::cout<<"pose id is "<<pose_id<<std::endl;
    Intrinsics in;
    in.fx = fx;
    in.fy = fy;
    in.cx = cx;
    in.cy = cy;

    std::vector<std::shared_ptr<NID> > nid_vec;
    Eigen::Matrix4d T_wc0, T_wc1;
    T_wc0<<T_wc0_cv.ptr<double>(0)[0], T_wc0_cv.ptr<double>(0)[1], T_wc0_cv.ptr<double>(0)[2], T_wc0_cv.ptr<double>(0)[3],
          T_wc0_cv.ptr<double>(1)[0], T_wc0_cv.ptr<double>(1)[1], T_wc0_cv.ptr<double>(1)[2], T_wc0_cv.ptr<double>(1)[3],
          T_wc0_cv.ptr<double>(2)[0], T_wc0_cv.ptr<double>(2)[1], T_wc0_cv.ptr<double>(2)[2], T_wc0_cv.ptr<double>(2)[3],
          0                              , 0                              , 0                              , 1;
    T_wc1<<T_wc1_cv.ptr<double>(0)[0], T_wc1_cv.ptr<double>(0)[1], T_wc1_cv.ptr<double>(0)[2], T_wc1_cv.ptr<double>(0)[3],
          T_wc1_cv.ptr<double>(1)[0], T_wc1_cv.ptr<double>(1)[1], T_wc1_cv.ptr<double>(1)[2], T_wc1_cv.ptr<double>(1)[3],
          T_wc1_cv.ptr<double>(2)[0], T_wc1_cv.ptr<double>(2)[1], T_wc1_cv.ptr<double>(2)[2], T_wc1_cv.ptr<double>(2)[3],
          0                              , 0                              , 0                              , 1;

    std::ofstream of("nid_test.csv", std::ofstream::out | std::ofstream::app);

    const double px_ori = T_wc0(0,3);
    const Eigen::Matrix4d SE3_ori= T_wc0;

    //calculate entropy of each cell
    double total_nid = 0.0;
    for(int i = 0; i<cell; i++){
        for(int j = 0; j<cell; j++){
            std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > points_3d;
            Eigen::VectorXd intensity;
            Get3dPointAndIntensity(cell, i, j, im0, depth0, points_3d, intensity, T_wc0, in);
            std::shared_ptr<NID> nid = std::make_shared<NID>();
            nid->cell_ = cell;
            nid->bin_num_ = bin_num;
            nid->image1_ = im1;
            nid->fx_ = in.fx;
            nid->fy_ = in.fy;
            nid->cx_ = in.cx;
            nid->cy_ = in.cy;
            nid->pw0_ = points_3d;
            nid->intensity0_ = intensity;
            nid->tf_ = T_wc1;
            nid->set_member_size();
            nid->ComputeHref();
            nid->ComputeH();
            total_nid += nid->nid_ * nid->nid_;
            //exit(0);
        }
    }

    std::cout<<"final nid is "<<sqrt(total_nid)<<std::endl;

    return 0;

}

void Get3dPointAndIntensity(int cell_size, int cell_row_id, int cell_col_id, const cv::Mat& image, const cv::Mat& depth, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >& points_3d, Eigen::VectorXd& intensity, const Eigen::Matrix4d& T_wc, Intrinsics in){
    int row_block = image.rows / cell_size;
    int col_block = image.cols / cell_size;
    int row_start = row_block * cell_row_id;
    int col_start = col_block * cell_col_id;
    int row_end = row_block * (cell_row_id + 1);
    int col_end = col_block * (cell_col_id + 1);
    int counter = 0;

    std::vector<double> intensity_v;
    for(int i = row_start; i < row_end; i++)
        for(int j = col_start; j < col_end; j++){
            double z_p = depth.ptr<double>(i)[j];
            counter++;
            if(z_p <0.01 || z_p > 100)
                continue;

            double x_p = z_p * (j - in.cx) / in.fx;
            double y_p = z_p * (i - in.cy) / in.fy;

            Eigen::Vector3d p_world = (T_wc*Eigen::Vector4d(x_p,y_p,z_p,1)).head(3);

            points_3d.push_back(p_world);
            intensity_v.push_back(image.ptr<uchar>(i)[j]);

        }

    intensity = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(intensity_v.data(), intensity_v.size());

    return;
};

std::vector<cv::Mat> ReadGroundtruth(std::string add, std::string dataset){
  std::vector<cv::Mat> all_gt;

  std::ifstream groundtruth_file(add.c_str());
  if(!groundtruth_file){
      printf("cannot find the file that contains groundtruth \n");
      return all_gt;
  }

  int counter = 0;
  std::string one_row_gt;

  while(getline(groundtruth_file,one_row_gt)){
      std::istringstream temp_one_row_gt(one_row_gt);
      std::string string_gt;
      Eigen::Matrix3d rm;
      double px,py,pz;
      int sequence = 0;
      if (dataset == "eth_cvg"){
      double qx,qy,qz,qw;
      while(getline(temp_one_row_gt, string_gt, ' ')){
        switch(sequence){
          case 0:{
              int msg_timestamp = atoi(string_gt.c_str());
              sequence++;
          }
              break;
          case 1:{
              //the case 1,2,3 is position x,y,z
              px = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          case 2:{
              py = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          case 3:{
              pz = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          case 4:{
              //the case 4,5,6,7 is quternion x,y,z,w
              qx = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          case 5:{
              qy = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          case 6:{
              qz = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          case 7:{
              qw = (double)atof(string_gt.c_str());
              sequence++;
          }
              break;
          default:
              break;
        }
      }
      Eigen::Quaterniond qt(qw,qx,qy,qz);
      rm = qt.toRotationMatrix();
    }

    cv::Mat one_gt(4,4,CV_64FC1);
    one_gt.ptr<double>(0)[0] = rm(0,0);
    one_gt.ptr<double>(0)[1] = rm(0,1);
    one_gt.ptr<double>(0)[2] = rm(0,2);
    one_gt.ptr<double>(1)[0] = rm(1,0);
    one_gt.ptr<double>(1)[1] = rm(1,1);
    one_gt.ptr<double>(1)[2] = rm(1,2);
    one_gt.ptr<double>(2)[0] = rm(2,0);
    one_gt.ptr<double>(2)[1] = rm(2,1);
    one_gt.ptr<double>(2)[2] = rm(2,2);
    one_gt.ptr<double>(3)[0] = 0.0;
    one_gt.ptr<double>(3)[1] = 0.0;
    one_gt.ptr<double>(3)[2] = 0.0;

    one_gt.ptr<double>(0)[3] = px;
    one_gt.ptr<double>(1)[3] = py;
    one_gt.ptr<double>(2)[3] = pz;
    one_gt.ptr<double>(3)[3] = 1.0;

    all_gt.push_back(one_gt);

  }

  //std::cout<<"size of all gt "<<all_gt.size()<<std::endl;

  return all_gt;
};

void NID::set_member_size(){
    pro_current_ = std::vector<double>(bin_num_,0.0);
    pro_ref_ = std::vector<double>(bin_num_,0.0);
    pro_joint_ = std::vector<std::vector<double> >(bin_num_);
    for(int i = 0; i < bin_num_; i++){
        pro_joint_[i] = std::vector<double>(bin_num_, 0.0);
    }
    intensity_current_ = Eigen::VectorXd::Zero(intensity0_.rows());
}

void NID::ComputeHref(){

  Eigen::Matrix4d T_cw1 = tf_.inverse();
  int ob = 0;
  for(int i = 0 ; i<intensity0_.rows(); i++){

    //decide if there is a valid mapping first, or we won't count this pixel
    Eigen::Vector4d pw;
    pw << pw0_[i](0), pw0_[i](1), pw0_[i](2), 1;
    Eigen::Vector4d p_c = T_cw1 * pw;

    //2d pixel position in current frame
    double u = fx_ * p_c(0,0) / p_c(2,0) + cx_;
    double v = fy_ * p_c(1,0) / p_c(2,0) + cy_;

    //bilinear interporlation of pixel. DSO getInterpolatedElement33() function, bilinear interpolation
    if(u >= 0 && u+3 <= image1_.cols && v >= 0 && v+3 <= image1_.rows){
      intensity_current_(i,0) = get_interpolated_pixel_value(u,v);
    }
    else{
      ob++;
      continue;
    }

    //current frame mutual information probability
    if(intensity0_(i,0) >= 255)
      intensity0_(i,0) = 254.999;
    if(intensity0_(i,0) < 0)
      intensity0_(i, 0) = 0.0;

    double bin_pos_ref =  intensity0_(i,0) * bin_num_/255.0;

    int bins_index_ref = floor(bin_pos_ref);

    if(bins_index_ref < 0 || bins_index_ref>bin_num_ -1)
        std::cout<<"the intensity is \n"<<intensity0_<<std::endl;

    pro_ref_[bins_index_ref] += 1.0;

  }

  for(int i = 0; i < bin_num_ ; i++)
    pro_ref_[i] /=  (intensity0_.rows()-ob);

  for(int i = 0; i < bin_num_ ; i++){
    if(pro_ref_[i] < sigma_)
      continue;
    H_ref_ -= pro_ref_[i] * log2(pro_ref_[i]);
  }

};

void NID::ComputeH(){
  int ob = 0;

  Eigen::Matrix4d T_cw1 = tf_.inverse();
  for(int i = 0 ; i<intensity0_.rows(); i++){

    Eigen::Vector4d pw;
    pw << pw0_[i](0), pw0_[i](1), pw0_[i](2), 1;
    Eigen::Vector4d p_c = T_cw1 * pw;

    if(intensity0_(i,0) >= 255)
      intensity0_(i,0) = 254.999;
    if(intensity0_(i,0) < 0)
      intensity0_(i, 0) = 0.0;

    double bin_pos_ref =  intensity0_(i,0)* bin_num_/255.0;
    int bins_index_ref = floor(bin_pos_ref);

    //2d pixel position in current frame
    double u = fx_ * p_c(0,0) / p_c(2,0) + cx_;
    double v = fy_ * p_c(1,0) / p_c(2,0) + cy_;

    //bilinear interporlation of pixel. DSO getInterpolatedElement33() function, bilinear interpolation
    if(u >= 0 && u+3 <= image1_.cols && v >= 0 && v+3 <= image1_.rows){
      intensity_current_(i,0) = get_interpolated_pixel_value(u,v);
    }
    else{
      ob++;
      continue;
    }

    if(intensity_current_(i,0) >= 255)
      intensity_current_(i,0) = 254.999;
    if(intensity_current_(i,0) < 0)
      intensity_current_(i, 0) = 0.0;

    double bin_pos_current = intensity_current_(i,0)* bin_num_/255.0;
    double pos_cubic_current = bin_pos_current * bin_pos_current * bin_pos_current;
    double pos_qua_current  = bin_pos_current * bin_pos_current;
    double bins_index_current = floor(bin_pos_current);

    pro_current_[bins_index_current] += 1.0;

    pro_joint_[bins_index_ref][bins_index_current] += 1.0;

  }

  //if too many points are out of image boundary, we don't use this edge
  if(intensity0_.rows() - ob < 300){
    //std::cout<<"no enough points in the image boundary"<<std::endl;
    return;
  }

  //pro_last_.size() = bin_num
  for(int i = 0; i < bin_num_ ; i++){
    pro_current_[i] /= (intensity0_.rows() - ob);
  }

  for(int i = 0; i < bin_num_; i++)
    for(int j = 0; j<bin_num_; j++){
      pro_joint_[i][j] /= (intensity0_.rows() - ob);
    }

  for(int i = 0; i < bin_num_ ; i++){
    if(pro_current_[i] < sigma_)
      continue;
    H_current_ -= pro_current_[i] * log2(pro_current_[i]);
  }

  for(int i = 0; i < bin_num_; i++)
    for(int j = 0; j<bin_num_; j++){
      if(pro_joint_[i][j] < sigma_)
        continue;
      H_joint_ -= pro_joint_[i][j] * log2(pro_joint_[i][j]);
    }

    nid_ = (2*H_joint_ - H_ref_ - H_current_)/H_joint_;
    MI_ = H_ref_ + H_current_ - H_joint_;

    //if all points from one image is mapped into only one bin so that one of the pro_current_[i] == 1, then H_currrent and H_joint will be 0;
    //For example, if we have 3 bins, pro_ref is 0 1 0, pro_current is 0 1 0 (or 0 0 1, 1 0 0), then H_ref and current will be 0. Notice we can totally infer one image's state from another. For this perfect matching, we should count the cost as 0
    if(H_ref_ == 0.0 && H_current_ == 0.0 && H_joint_ == 0.0){
      MI_ = 0.0;
      nid_ = 0.0;
    }

    std::cout<<"Href, current, joint from standard method is "<<H_ref_<<","<<H_current_<<","<<H_joint_<<", MI "<<MI_<<", NID "<<nid_<<std::endl;

}


#include <boost/simd/memory/allocator.hpp>
#include <boost/simd/memory/aligned_input_iterator.hpp>

#include <algorithm>
#include <iterator>
#include <iostream>
#include <vector>

int main()
{
  using boost::simd::pack;
  using boost::simd::allocator;
  using boost::simd::aligned_input_end;
  using boost::simd::aligned_input_begin;
  using boost::simd::aligned_input_iterator;

  std::vector<float, allocator<float> > data(16);
  typedef std::vector<float, allocator<float> >::iterator it_t;

  for(std::size_t i=0;i<data.size();++i)
    data[i]  = 1 + i;

  aligned_input_iterator<it_t,4> vb = aligned_input_begin<4>(data.begin());
  aligned_input_iterator<it_t,4> ve = aligned_input_end<4>(data.end());

  std::copy(vb, ve, std::ostream_iterator< pack<float,4> >(std::cout, " "));
  std::cout << std::endl;
}