#pragma once
#include "comm_channel.hpp"
#include "matrix_multiplication.hpp"
#include "mid_layer/DamgardJurikEnc.hpp"
#include "primitives/Prg.hpp"
#include "util/blocking_queue.hpp"
#include "util/prg_adapter.hpp"
#include "util/randomize_matrix.hpp"
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/optional.hpp>
extern "C" {
    #include "obliv.h"
    #include "sparse_matrix_multiplication/sparse_matrix_multiplication.h"
}

/**
 * error_info structs for reporting input dimension in exceptions
 */
typedef boost::error_info<struct tag_TRIPLE_L,size_t> error_triple_l;
typedef boost::error_info<struct tag_TRIPLE_M,size_t> error_triple_m;
typedef boost::error_info<struct tag_TRIPLE_N,size_t> error_triple_n;
typedef boost::error_info<struct tag_DIM_A_SIZE1,size_t> error_a_size1;
typedef boost::error_info<struct tag_DIM_A_SIZE2,size_t> error_a_size2;
typedef boost::error_info<struct tag_DIM_B_SIZE1,size_t> error_b_size1;
typedef boost::error_info<struct tag_DIM_B_SIZE2,size_t> error_b_size2;
typedef boost::error_info<struct tag_K_A,size_t> error_k_A;
typedef boost::error_info<struct tag_K_B,size_t> error_k_B;
typedef boost::error_info<struct tag_CHUNK_SIZE,size_t> error_chunk_size;

template<typename T, bool is_shared = false>
class triple_provider {
protected:
  size_t l, m, n;
  int role;

public:
  using triple = std::tuple<boost::numeric::ublas::matrix<T>,
                   boost::numeric::ublas::matrix<T>,
                   boost::numeric::ublas::matrix<T> >;

  triple_provider(size_t l, size_t m, size_t n, int role):
    l(l), m(m), n(n), role(role) {};

  size_t get_l() { return l; }
  size_t get_m() { return m; }
  size_t get_n() { return n; }

  virtual triple get() = 0;
};

template<typename T, bool is_shared = false>
class fake_triple_provider : public virtual triple_provider<T, is_shared> {
using triple = typename triple_provider<T, is_shared>::triple;
private:
  blocking_queue<triple> triples;
  int seed = 34567; // seed random number generator deterministically
  std::mt19937 r;

  triple compute_fake_triple() {
      using namespace boost::numeric::ublas;
      size_t l = this->l, m = this->m, n = this->n;
      int role = this->role;

      std::uniform_int_distribution<T> dist;
      matrix<T> U(l, m), U_mask = is_shared ? matrix<T>(l, m) : zero_matrix<T>(l, m);
      matrix<T> V(m, n), V_mask = is_shared ? matrix<T>(m, n) : zero_matrix<T>(m, n);
      matrix<T> Z_mask(l, n);

      randomize_matrix(r, U);
      randomize_matrix(r, V);
      randomize_matrix(r, Z_mask);
      if(is_shared) {
        randomize_matrix(r, U_mask);
        randomize_matrix(r, V_mask);
      }

      if(role == 0) {
          return std::make_tuple(U - U_mask, V_mask, Z_mask);
      } else {
          return std::make_tuple(U_mask, V - V_mask, prod(U, V) - Z_mask);
      }
  }

public:
  // the `cap` argument allows to use a bounded queue for storing triples,
  fake_triple_provider(size_t l, size_t m, size_t n, int role, ssize_t cap = -1):
    triple_provider<T, is_shared>(l, m, n, role), triples(cap), r(seed) {};
  // blocks if capacity is bounded
  void precompute(size_t num) {
    for(size_t i = 0; i < num; i++) {
      triples.push(this->compute_fake_triple());
    }
  }
  // thread-safe!
  triple get() {
    return triples.pop();
  }
};

template<class MATRIX_A, class MATRIX_B,
  typename T = typename MATRIX_A::value_type, bool is_shared,
  typename std::enable_if<std::is_same<T, typename MATRIX_B::value_type>::value, int>::type = 0>
boost::numeric::ublas::matrix<T> matrix_multiplication(
    const boost::numeric::ublas::matrix_expression<MATRIX_A>& A_in,
    const boost::numeric::ublas::matrix_expression<MATRIX_B>& B_in,
    comm_channel& channel, int role,
    triple_provider<T, is_shared>& triples,
    ssize_t chunk_size_in = -1
) {
  using namespace boost::numeric::ublas;
  const MATRIX_A& A = A_in();
  const MATRIX_B& B = B_in();
  // A : l x m, B: m x n, C: l x n
  size_t l = A.size1(), m = A.size2(), n = B.size2();
  size_t chunk_size = chunk_size_in;
  if(chunk_size_in == -1) {
    chunk_size = l;
  }

  try {
    // check if argument sizes are valid
    if (m != B.size1()) {
        BOOST_THROW_EXCEPTION(std::invalid_argument("Matrix sizes do not match"));
    }
    if(chunk_size != triples.get_l() || triples.get_m() != m || triples.get_n() != n) {
        BOOST_THROW_EXCEPTION(boost::enable_error_info(std::invalid_argument(
            "Triple dimensions do not match matrix dimensions"))
            << error_triple_l(triples.get_l()) << error_triple_m(triples.get_m())
            << error_triple_n(triples.get_n()));
    }
    if(l % chunk_size) { //TODO: allow for different chunk sizes
        BOOST_THROW_EXCEPTION(boost::enable_error_info(std::invalid_argument(
            "`A_in.size1()` must be divisible by `chunk_size`")));
    }

    std::vector<std::function<matrix<T>()>> compute_chunks;
    for(size_t i = 0; i*chunk_size < l; i++) {
        compute_chunks.push_back([&triples, &channel, &B, &A, chunk_size, m, n, role, i]() -> matrix<T> {
        auto chunk_A = subrange(A, i*chunk_size, (i+1)*chunk_size, 0, A.size2());
        // get a multiplication triple
        matrix<T> U, V, Z;
        std::tie(U, V, Z) = triples.get();
        // role 0 sends A - U and receives B - V simultaneously
        // then compute share of the result
        matrix<T, row_major> E = (is_shared || role == 0) * (chunk_A - U), E2(chunk_size, m);
        matrix<T, row_major> F = (is_shared || role == 1) * (B - V), F2(m, n);
        if(role == 0) {
            channel.send_recv(E, F2);
            if(is_shared) {
                channel.send_recv(F, E2);
                E += E2;
            }
            F += F2;
            return prod(E, V) + prod(U, F) + Z;
        } else {
            channel.send_recv(F, E2);
            if(is_shared) {
                channel.send_recv(E, F2);
                F += F2;
            }
            E += E2;
            return prod(E, F) + prod(E, V) + prod(U, F) + Z;
        }
      });
    }
    matrix<T> result = zero_matrix<T>(l, n);
    for(size_t i = 0; i*chunk_size < l; i++) {
      subrange(result, i*chunk_size, (i+1)*chunk_size, 0, result.size2()) = compute_chunks[i]();
    }
    return result;
  } catch(boost::exception& e) {
    e << error_a_size1(A.size1()) << error_a_size2(A.size2())
      << error_b_size1(B.size1()) << error_b_size2(B.size2());
    e << error_chunk_size(chunk_size_in);
    throw;
  }
}

// register BigIntegerCiphertext class for boost::serialization
BOOST_CLASS_EXPORT_IMPLEMENT(BigIntegerCiphertext)

template<class MATRIX_A, class MATRIX_B,
  typename T = typename MATRIX_A::value_type,
  typename std::enable_if<std::is_same<T, typename MATRIX_B::value_type>::value, int>::type = 0>
boost::numeric::ublas::matrix<T> semisparse_matrix_multiplication(
    const boost::numeric::ublas::matrix_expression<MATRIX_A>& A_in,
    const boost::numeric::ublas::matrix_expression<MATRIX_B>& B_in,
    size_t k, comm_channel& channel, int role,
    size_t num_threads = boost::thread::hardware_concurrency(),
    bool fake_offline_phase = false
) {
  using namespace boost::numeric::ublas;
  const MATRIX_A& A = A_in();
  const MATRIX_B& B = B_in();
  // A : l x m, B: m x n, C: l x n
  size_t l = A.size1(), m = A.size2(), n = B.size2();

  try {
    // check if argument sizes are valid
    if (m != B.size1()) {
        BOOST_THROW_EXCEPTION(std::invalid_argument("Matrix sizes do not match"));
    }

    DJKeyGenParameterSpec spec;
    // set up ciphertext packing parameters
    // number of bits needed for padding between elements of type T
    size_t padding_bits = boost::multiprecision::msb(biginteger(k+1)) + 2 // overflows in additions
                          + sizeof(T) * 8 // overflow in multiplication
			                    + 40; // statistical security
    // number of bits needed for an element of type T (with padding for scalar mult)
    size_t packing_shift = sizeof(T) * 8 + padding_bits;
    // number of usable plaintext bits
    size_t max_plaintext_bits = spec.getModulusLength() - 1;
    // number of T elements that fit in a plaintext;
    size_t packing_factor = max_plaintext_bits / packing_shift;
    // number of packs of rows
    size_t num_row_packs = ((l - 1) / packing_factor) + 1;

    // number of threads to use
    if(!num_threads) { num_threads = 1; }
    std::vector<boost::thread> threads(num_threads);
    // duplicate thread-unsafe data structures
    std::vector<shared_ptr<PrgFromOpenSSLAES>> prg_thread(num_threads);
    for(size_t i = 0; i < num_threads; i++) {
        prg_thread[i] = make_shared<PrgFromOpenSSLAES>();
        auto key = prg_thread[i]->generateKey(128);
        prg_thread[i]->setKey(key);
    }
    // compute number of rows to give to each thread
    std::vector<size_t> num_packs_thread(num_threads);
    for(size_t i = 0; i < num_threads; i++) {
        num_packs_thread[i] = num_row_packs / num_threads;
        if(i < num_row_packs % num_threads) { // distribute evenly
            num_packs_thread[i]++;
        }
    }
    // set up encryption parameters and encryption context for each thread
    std::vector<DamgardJurikEnc> enc_thread(num_threads);
    for(size_t i = 0; i < num_threads; i++) {
        enc_thread[i] = DamgardJurikEnc(prg_thread[i]);
        enc_thread[i].setLengthParameter(1);
    }
    if (role == 0) {
        // generate key pair
        auto start = scapi_now();
        auto keypair = enc_thread[0].generateKey(&spec);
        auto public_key = keypair.first;
        auto private_key = keypair.second;
        channel.send(public_key);
        channel.flush();
        // use generated key pair
        for(auto& enc : enc_thread) {
            enc.setKey(public_key, private_key);
        }
        print_elapsed_ms(start, "Key generation");
    } else {
        shared_ptr<PublicKey> public_key;
        channel.recv(public_key);
        for(auto& enc : enc_thread) {
            enc.setKey(public_key, nullptr);
        }
    }

    // storage for the threads' results
    std::vector<matrix<BigIntegerCiphertext>> A_in_enc_thread(num_threads);
    std::vector<matrix<BigIntegerCiphertext>> share_0_enc_thread(num_threads);
    std::vector<matrix<T>> share_1_thread(num_threads);
    for(size_t i = 0; i < num_threads; i++) {
        A_in_enc_thread[i].resize(num_packs_thread[i], m);
        share_0_enc_thread[i].resize(num_packs_thread[i], n);
        share_1_thread[i].resize(num_packs_thread[i] * packing_factor, n);
    }

    // encrypted party 0's input
    matrix<BigIntegerCiphertext> A_in_enc(num_row_packs, m);
    // encrypted party's 0 share of the result
    matrix<BigIntegerCiphertext> share_0_enc(num_row_packs, n);
    // final shares of the result
    matrix<T> share_0(l, n), share_1(l, n);
    if (role == 0) {
      if(!fake_offline_phase) {
        // encrypt matrix
        auto start = scapi_now();
        size_t pack_base = 0;
        for(size_t thread_id = 0; thread_id < num_threads; thread_id++) {
            auto num_row_packs = num_packs_thread[thread_id];
            threads[thread_id] = boost::thread([=,&enc_thread,&A_in_enc_thread,&A](){
                cout << "starting thread " << thread_id << "\n";
                for (size_t col = 0; col < m; col++) {
                    // pack rows together
                    for(size_t row_pack = 0; row_pack < num_row_packs; row_pack++) {
                        biginteger current_pack(0);
                        for (size_t row_in_pack = 0; row_in_pack < packing_factor; row_in_pack++) {
                            size_t row = (pack_base + row_pack) * packing_factor + row_in_pack;
                            if(row >= l) { break; }
                            // insert current row at the correct position
                            current_pack |= biginteger(A(row, col)) << (packing_shift * row_in_pack);
                        }
                        auto plaintext = make_shared<BigIntegerPlainText>(current_pack);
                        A_in_enc_thread[thread_id](row_pack, col) = *dynamic_pointer_cast<BigIntegerCiphertext>(enc_thread[thread_id].encrypt(plaintext));
                    }
                }
            });
            pack_base += num_packs_thread[thread_id];
        }
        pack_base = 0;
        for(size_t thread_id = 0; thread_id < num_threads; thread_id++) {
            threads[thread_id].join();
            for(size_t pack = 0; pack < num_packs_thread[thread_id]; pack++) {
                for(size_t col = 0; col < m; col++) {
                    A_in_enc(pack_base + pack, col) = A_in_enc_thread[thread_id](pack, col);
                }
            }
            pack_base += num_packs_thread[thread_id];
        }
        print_elapsed_ms(start, "Matrix encryption");
        // send encrypted matrix
        start = scapi_now();
        channel.send(A_in_enc);
        channel.flush();
        print_elapsed_ms(start, "Sending encrypted matrix A");
      }
    } else {
        auto start = scapi_now();
        if(!fake_offline_phase) {
          // receive encrypted matrix
          channel.recv(A_in_enc);
          print_elapsed_ms(start, "Receiving encrypted matrix A");
        } else {
          // A_in_enc is already zeroed because of the BigIntegerCiphertext default constructor
        }

        biginteger max_element_padded = (biginteger(1) << (packing_shift - 1)) - 1;
        start = scapi_now();
        size_t pack_base = 0;
        for(size_t thread_id = 0; thread_id < num_threads; thread_id++) {
            auto num_row_packs = num_packs_thread[thread_id];
            threads[thread_id] = boost::thread([=,&prg_thread,&enc_thread,&A_in_enc,&B_in,&share_0_enc_thread,&share_1_thread](){
                for(size_t row_pack = 0; row_pack < num_row_packs; row_pack++) {
                    for (size_t j = 0; j < n; j++) {
                        // construct ciphertext of the share of the inner product
                        // generate random shares and pack them
                        biginteger pack_share_0(0);
                        for(size_t row_in_pack = 0; row_in_pack < packing_factor; row_in_pack++) {
                            size_t row = (pack_base + row_pack) * packing_factor + row_in_pack;
                            if(row >= l) { break; }
                            biginteger current_share = getRandomInRange(0, max_element_padded, prg_thread[thread_id].get());
                            // add current share and its negation to packed shares
                            pack_share_0 |= (max_element_padded - current_share + 1) << (packing_shift * row_in_pack);
                            // save our share
                            current_share &= std::numeric_limits<T>::max(); // use only lowest bits
                            share_1_thread[thread_id](row_pack * packing_factor + row_in_pack, j) = current_share.convert_to<T>();
                        }
                        // encrypt party 0's share pack
                        share_0_enc_thread[thread_id](row_pack, j) = *dynamic_pointer_cast<BigIntegerCiphertext>(enc_thread[thread_id].encrypt(
                            make_shared<BigIntegerPlainText>(pack_share_0)));
                        // compute inner product with our sparse vector
                        for (size_t k = 0; k < m; k++) {
                            if(B(k, j) == 0) { continue; } // exploit sparsity
                            biginteger b(B(k, j));
                            auto c = enc_thread[thread_id].multByConst(&A_in_enc(pack_base + row_pack, k), b);
                            share_0_enc_thread[thread_id](row_pack, j) = *dynamic_pointer_cast<BigIntegerCiphertext>(enc_thread[thread_id].add(&share_0_enc_thread[thread_id](row_pack, j), c.get()));
                        }
                    }
                }
            });
            pack_base += num_packs_thread[thread_id];
        }
        // join threads and merge share matrices
        pack_base = 0;
        for(size_t thread_id = 0; thread_id < num_threads; thread_id++) {
            threads[thread_id].join();
            for(size_t row_pack = 0; row_pack < num_packs_thread[thread_id]; row_pack++) {
                for(size_t j = 0; j < n; j++) {
                    share_0_enc(pack_base + row_pack, j) = share_0_enc_thread[thread_id](row_pack, j);
                    for(size_t row_in_pack = 0; row_in_pack < packing_factor; row_in_pack++) {
                        size_t row = (pack_base + row_pack) * packing_factor + row_in_pack;
                        if(row >= l) { break; }
                        share_1(row, j) = share_1_thread[thread_id](row_pack * packing_factor + row_in_pack, j);
                    }
                }
            }
            pack_base += num_packs_thread[thread_id];
        }
        print_elapsed_ms(start, "Homomorphic multiplication");
    }

    if (role != 0) {
        // send share of result
        auto start = scapi_now();
        channel.send(share_0_enc);
        channel.flush();
        print_elapsed_ms(start, "Sending share of result");
        return share_1;
    } else {
        // receive encrypte share of result and decrypt it
        auto start = scapi_now();
        channel.recv(share_0_enc);
        print_elapsed_ms(start, "Receiving share of result");

        start = scapi_now();
        for (size_t row_pack = 0; row_pack < num_row_packs; row_pack++) {
            for (size_t j = 0; j < n; j++) {
                auto dec = enc_thread[0].decrypt(&share_0_enc(row_pack, j));
                biginteger pack_share_0 = dynamic_pointer_cast<BigIntegerPlainText>(dec)->getX();
                // unpack share for our result
                for(size_t row_in_pack = 0; row_in_pack < packing_factor; row_in_pack++) {
                    size_t row = row_pack * packing_factor + row_in_pack;
                    if(row >= l) { break; }
                    biginteger current_share = pack_share_0 >> (packing_shift * row_in_pack);
                    current_share &= std::numeric_limits<T>::max(); // use only lowest bits
                    share_0(row, j) = current_share.convert_to<T>();
                }
            }
        }
        print_elapsed_ms(start, "Decryption");
        return share_0;
    }
  } catch(boost::exception& e) {
      e << error_a_size1(A.size1()) << error_a_size2(A.size2())
        << error_b_size1(B.size1()) << error_b_size2(B.size2()) << error_k_B(k);
      throw;
  }
}

template<class MATRIX_A, class MATRIX_B,
  typename T = typename MATRIX_A::value_type,
  typename std::enable_if<std::is_same<T, typename MATRIX_B::value_type>::value, int>::type = 0>
boost::numeric::ublas::matrix<T> sparse_matrix_multiplication(
    const boost::numeric::ublas::matrix_expression<MATRIX_A>& A_in,
    const boost::numeric::ublas::matrix_expression<MATRIX_B>& B_in,
    size_t k_A, size_t k_B, // number of non-zero entries in A and B
    comm_channel& channel, int role,
    triple_provider<T, false>& triples,
    ssize_t chunk_size = -1
) {
  using namespace boost::numeric::ublas;
  const MATRIX_A& A = A_in();
  const MATRIX_B& B = B_in();
  // A : l x m, B: m x n, C: l x n
  size_t l = A.size1(), m = A.size2(), n = B.size2();
  try {
    // check if argument sizes are valid
    if (m != B.size1()) {
        BOOST_THROW_EXCEPTION(std::invalid_argument("Matrix sizes do not match"));
    }
    if(k_A > m || k_B > m) {
        BOOST_THROW_EXCEPTION(std::invalid_argument("k_A and k_B can be at most m"));
    }
    if(n > 1) {
        BOOST_THROW_EXCEPTION(std::invalid_argument("Only matrix-vector "
          "multiplication is implemented at this time"));
    }

    // set up protocol transport object for obliv-c
    ProtocolDesc desc;

    // struct channel_transport : ProtocolTransport {
    //     comm_channel *channel;
    // };
    // channel_transport trans;
    // trans.maxParties = 2;
    // // trans.split = nullptr;
    // trans.split = [](ProtocolTransport *trans) -> ProtocolTransport* {
    //     auto self = static_cast<channel_transport*>(trans);
    //     auto trans2 = new channel_transport(*self); // copy struct
    //     trans2->channel = new comm_channel(self->channel->clone());
    //     trans2->cleanup = [](ProtocolTransport *trans2) {
    //         auto self2 = static_cast<channel_transport*>(trans2);
    //         delete self2->channel;
    //         delete self2;
    //     };
    //     return trans2;
    // };
    // trans.send = [](ProtocolTransport* trans, int, const void *data, size_t n) -> int {
    //     auto self = static_cast<channel_transport*>(trans);
    //     self->channel->write(static_cast<const char *>(data), n);
    //     return static_cast<int>(n);
    // };
    // trans.recv = [](ProtocolTransport* trans, int, void *data, size_t n) -> int {
    //     auto self = static_cast<channel_transport*>(trans);
    //     self->channel->read(static_cast<char *>(data), n);
    //     return static_cast<int>(n);
    // };
    // trans.flush = [](ProtocolTransport* trans) -> int {
    //     auto self = static_cast<channel_transport*>(trans);
    //     self->channel->flush();
    //     return 0;
    // };
    // trans.cleanup = [](ProtocolTransport*){}; // do nothing
    // trans.channel = &channel;
    // desc.trans = static_cast<ProtocolTransport*>(&trans);

    auto channel2 = channel.clone();
    int fd = channel2.get_blocking_fd();
    protocolUseTcp2PKeepAlive(&desc, fd, role!=0);
    desc.trans->split = nullptr;

    // if(channel.connect_to_oblivc(desc) == -1) {
    //   BOOST_THROW_EXCEPTION(std::runtime_error("Obliv-C: Connection failed"));
    // }

    setCurrentParty(&desc, role+1);

    // set up inputs for the use in obliv-c
    std::vector<word_t> words_A(k_A); // list of words with non-zero columns
    matrix<T> values_A = zero_matrix<T>(l, k_A); // matrix consisting of non-zero columns
    std::vector<word_index_pair> word_indices_B(k_B); // mapping from words to indices into values_B
    matrix<T> values_B = zero_matrix<T>(k_A + k_B, n); // matrix consisting of non-zero rows of B and additional zeroes (shuffled)
    std::vector<index_t> zeroes_B(k_A); // list of indices corresponding to zeros
    std::vector<index_index_pair> result(k_A);
    auto start = scapi_now();
    if(role == 0) { // set up A's inputs
        size_t count_A = 0;
        for(size_t j = 0; j < m; j++) {
            for(size_t i = 0; i < l; i++) {
                if(A(i, j) != 0 || (m - j) <= (k_A - count_A)) {
                    words_A[count_A] = j;
                    column(values_A, count_A) = column(A, j);
                    count_A++;
                    break;
                }
            }
        }
    } else { // set up B's inputs
        size_t count_B = 0;
        std::vector<T> values_B_temp(k_B);
        for(size_t i = 0; i < m; i++) {
            size_t j = 0;
            if(B(i, j) != 0 || (m - i) <= (k_B - count_B)) {
                word_indices_B[count_B].k = i;
                values_B_temp[count_B] = B(i, j);
                count_B++;
            }
        }
        // get a prg
        auto prg = PrgFromOpenSSLAES();
        auto key = prg.generateKey(128);
        prg.setKey(key);
        prg_adapter<size_t> prg_adapted(prg);
        // generate permutation to shuffle values_B
        std::vector<size_t> permutation(values_B.size1());
        std::iota(permutation.begin(), permutation.end(), 0);
        std::shuffle(permutation.begin(), permutation.end(), prg_adapted);
        // apply permutation and save indices of values and zeroes
        for(size_t i = 0; i < permutation.size(); i++) {
            size_t index = permutation[i];
            if(i < k_B) {
                values_B(index, 0) = values_B_temp[i];
                word_indices_B[i].v = index;
            } else {
                zeroes_B[i - k_B] = index;
            }
        }
    }
    print_elapsed_ms(start, "Input permuation");
    // set up args struct for sparse_matrix_multiplication
    sparse_matrix_multiplication_args args;
    args.l = l;
    args.m = m;
    args.n = n;
    args.k_A = k_A;
    args.k_B = k_B;
    args.in_A = words_A.data();
    args.in_B = word_indices_B.data();
    args.zeroes_B = zeroes_B.data();
    args.result = result.data();

    // run the protocol
    channel.sync();
    start = scapi_now();
    execYaoProtocol(&desc, sparse_matrix_multiplication, static_cast<void *>(&args));
    cleanupProtocol(&desc);
    channel.sync();
    print_elapsed_ms(start, "Circuit Execution");

    if(role == 0) {
        matrix<T> values_A_permuted = zero_matrix<T>(l, k_A + k_B);
        // permute A according to the result, so that column pair.v_A is at
        // position pair.v_B
        size_t i = 0;
        for(auto pair : result) {
          column(values_A_permuted, pair.v_B) = column(values_A, pair.v_A);
        }
        // now run dense matrix multiplication
        zero_matrix<T> empty_B(k_A + k_B, n);
        return matrix_multiplication(values_A_permuted, empty_B, channel, role, triples, chunk_size);
    } else {
        zero_matrix<T> empty_A(l, k_A + k_B);
        return matrix_multiplication(empty_A, values_B, channel, role, triples, chunk_size);
    }
  } catch(boost::exception& e) {
    e << error_a_size1(A.size1()) << error_a_size2(A.size2())
      << error_b_size1(B.size1()) << error_b_size2(B.size2())
      << error_k_A(k_A) << error_k_B(k_B);
    throw;
  }
}


#include <boost/math/tools/stats.hpp>


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#ifdef _MSC_VER
#define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/multiprecision/gmp.hpp>

#define NO_MIXED_OPS
#define BOOST_MP_NOT_TESTING_NUMBER

#include "test_arithmetic.hpp"
#include <boost/rational.hpp>

template <class T>
struct is_boost_rational<boost::rational<T> > : public boost::mpl::true_
{};

namespace boost { namespace multiprecision {

template <>
struct number_category<rational<mpz_int> > : public mpl::int_<number_kind_rational>
{};

}} // namespace boost::multiprecision

int main()
{
   test<boost::rational<boost::multiprecision::mpz_int> >();
   return boost::report_errors();
}


/*
 * Copyright (c) 2011-2013 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin.
 *
 * libbitcoin is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License with
 * additional permissions to the one published by the Free Software
 * Foundation, either version 3 of the License, or (at your option)
 * any later version. For more information see LICENSE.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
#include <bitcoin/utility/base58.hpp>

#include <boost/algorithm/string.hpp>
#include <bitcoin/utility/big_number.hpp>

namespace libbitcoin {

// Thanks for all the wonderful bitcoin hackers

const char* base58_chars = "123456789ABCDEFGHJKLMNPQRSTUVWXYZabcdefghijkmnopqrstuvwxyz";

std::string encode_base58(const data_chunk& unencoded_data)
{
    std::string encoded_data;
    // Expected size increase from base58 conversion is approximately 137%
    // use 138% to be safe
    encoded_data.reserve((unencoded_data.size() - 1) * 138 / 100 + 1);

    // Convert big endian data to little endian
    // Extra zero at the end make sure bignum will interpret
    // as a positive number
    data_chunk tmp_data(unencoded_data.size() + 1, 0);
    std::reverse_copy(unencoded_data.begin(), unencoded_data.end(),
        tmp_data.begin());

    big_number long_value;
    long_value.set_data(tmp_data);
    while (long_value > 0)
    {
        auto result = divmod(long_value, 58);
        long_value = result.first;
        size_t remainder = result.second.uint32();
        encoded_data += base58_chars[remainder];
    }

    // Leading zeroes encoded as base58 zeros
    for (const uint8_t unencoded_byte: unencoded_data)
    {
        if (unencoded_byte != 0)
            break;
        encoded_data += base58_chars[0];
    }

    // Convert little endian std::string to big endian
    reverse(encoded_data.begin(), encoded_data.end());
    return encoded_data;
}

data_chunk decode_base58(std::string encoded_data)
{
    // Trim spaces and newlines around the string.
    boost::algorithm::trim(encoded_data);
    // We're building a big number.
    big_number bn = 0;

    // Convert big endian string to bignum
    for (const uint8_t current_char: encoded_data)
    {
        bn *= 58;
        bn += std::string(base58_chars).find(current_char);
    }

    // Get bignum as little endian data
    data_chunk temp_data = bn.data();

    // Trim off sign byte if present
    if (temp_data.size() >= 2 &&
            temp_data.end()[-1] == 0 && temp_data.end()[-2] >= 0x80)
        temp_data.erase(temp_data.end() - 1);

    // Restore leading zeros
    int leading_zeros = 0;
    for (const uint8_t current_char: encoded_data)
    {
        if (current_char != base58_chars[0])
            break;
        leading_zeros++;
    }

    data_chunk unencoded_data;
    unencoded_data.assign(leading_zeros + temp_data.size(), 0);
    // Convert little endian data to big endian
    std::reverse_copy(temp_data.begin(), temp_data.end(),
        unencoded_data.end() - temp_data.size());
    return unencoded_data;
}

} // namespace libbitcoin


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testManifoldPreintegration.cpp
 * @brief   Unit test for the ManifoldPreintegration
 * @author  Luca Carlone
 */

#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/expressionTesting.h>

#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>

#include "imuFactorTesting.h"

namespace testing {
// Create default parameters with Z-down and above noise parameters
static std::shared_ptr<PreintegrationParams> Params() {
  auto p = PreintegrationParams::MakeSharedD(kGravity);
  p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
  p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
  p->integrationCovariance = 0.0001 * I_3x3;
  return p;
}
}

/* ************************************************************************* */
TEST(ManifoldPreintegration, BiasCorrectionJacobians) {
  testing::SomeMeasurements measurements;

  boost::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
      [=](const Vector3& a, const Vector3& w) {
        ManifoldPreintegration pim(testing::Params(), Bias(a, w));
        testing::integrateMeasurements(measurements, &pim);
        return pim.deltaRij();
      };

  boost::function<Point3(const Vector3&, const Vector3&)> deltaPij =
      [=](const Vector3& a, const Vector3& w) {
        ManifoldPreintegration pim(testing::Params(), Bias(a, w));
        testing::integrateMeasurements(measurements, &pim);
        return pim.deltaPij();
      };

  boost::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
      [=](const Vector3& a, const Vector3& w) {
        ManifoldPreintegration pim(testing::Params(), Bias(a, w));
        testing::integrateMeasurements(measurements, &pim);
        return pim.deltaVij();
      };

  // Actual pre-integrated values
  ManifoldPreintegration pim(testing::Params());
  testing::integrateMeasurements(measurements, &pim);

  EXPECT(
      assert_equal(numericalDerivative21(deltaRij, kZero, kZero),
          Matrix3(Z_3x3)));
  EXPECT(
      assert_equal(numericalDerivative22(deltaRij, kZero, kZero),
          pim.delRdelBiasOmega(), 1e-3));

  EXPECT(
      assert_equal(numericalDerivative21(deltaPij, kZero, kZero),
          pim.delPdelBiasAcc()));
  EXPECT(
      assert_equal(numericalDerivative22(deltaPij, kZero, kZero),
          pim.delPdelBiasOmega(), 1e-3));

  EXPECT(
      assert_equal(numericalDerivative21(deltaVij, kZero, kZero),
          pim.delVdelBiasAcc()));
  EXPECT(
      assert_equal(numericalDerivative22(deltaVij, kZero, kZero),
          pim.delVdelBiasOmega(), 1e-3));
}

/* ************************************************************************* */
TEST(ManifoldPreintegration, computeError) {
  ManifoldPreintegration pim(testing::Params());
  NavState x1, x2;
  imuBias::ConstantBias bias;
  Matrix9 aH1, aH2;
  Matrix96 aH3;
  pim.computeError(x1, x2, bias, aH1, aH2, aH3);
  boost::function<Vector9(const NavState&, const NavState&,
                          const imuBias::ConstantBias&)> f =
      boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3,
                  boost::none, boost::none, boost::none);
  // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
  EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
  EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
  EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


#pragma once

#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace lorad {

    class Lot {
        public:
                                            Lot();
                                            ~Lot();

            void                            setSeed(unsigned seed);
            double                          uniform();
            int                             randint(int low, int high);
            double                          normal();
            double                          gamma(double shape, double scale);
            double                          logUniform();

            typedef std::shared_ptr<Lot>    SharedPtr;

        private:

            typedef boost::variate_generator<boost::mt19937 &, boost::random::uniform_01<> >                uniform_variate_generator_t;
            typedef boost::variate_generator<boost::mt19937 &, boost::random::normal_distribution<> >       normal_variate_generator_t;
            typedef boost::variate_generator<boost::mt19937 &, boost::random::gamma_distribution<> >        gamma_variate_generator_t;
            typedef boost::variate_generator<boost::mt19937 &, boost::random::uniform_int_distribution<> >  uniform_int_generator_t;

            unsigned                                        _seed;
            boost::mt19937                                  _generator;
            std::shared_ptr<uniform_variate_generator_t>    _uniform_variate_generator;
            std::shared_ptr<normal_variate_generator_t>     _normal_variate_generator;
            std::shared_ptr<gamma_variate_generator_t>      _gamma_variate_generator;
            std::shared_ptr<uniform_int_generator_t>        _uniform_int_generator;

            double                                          _gamma_shape;
            int                                             _low;
            int                                             _high;
    };

    inline Lot::Lot() : _seed(0), _gamma_shape(1.0), _low(0), _high(100) {
        _generator.seed(static_cast<unsigned int>(std::time(0)));
        _uniform_variate_generator = std::shared_ptr<uniform_variate_generator_t>(new uniform_variate_generator_t(_generator, boost::random::uniform_01<>()));
        _normal_variate_generator = std::shared_ptr<normal_variate_generator_t>(new normal_variate_generator_t(_generator, boost::random::normal_distribution<>()));
        _gamma_variate_generator = std::shared_ptr<gamma_variate_generator_t>(new gamma_variate_generator_t(_generator, boost::random::gamma_distribution<>(_gamma_shape)));
        _uniform_int_generator = std::shared_ptr<uniform_int_generator_t>(new uniform_int_generator_t(_generator, boost::random::uniform_int_distribution<>(_low, _high)));
    }

    inline Lot::~Lot() {
        _uniform_variate_generator.reset();
        _normal_variate_generator.reset();
        _gamma_variate_generator.reset();
        _uniform_int_generator.reset();
    }

    inline void Lot::setSeed(unsigned seed) {
        _seed = seed;
        _generator.seed(_seed > 0 ? _seed : static_cast<unsigned int>(std::time(0)));
    }

    inline double Lot::uniform() {
        double u = (*_uniform_variate_generator)();
        while (u <= 0.0)
            u = (*_uniform_variate_generator)();
        return u;
    }

    inline double Lot::logUniform() {
        double u = (*_uniform_variate_generator)();
        while (u <= 0.0)
            u = (*_uniform_variate_generator)();
        return std::log(u);
    }

    inline double Lot::normal() {
        return (*_normal_variate_generator)();
    }

    inline double Lot::gamma(double shape, double scale) {
        assert(shape > 0.0);
        assert(scale > 0.0);
        if (shape != _gamma_shape) {
            _gamma_shape = shape;
            _gamma_variate_generator.reset(new gamma_variate_generator_t(_generator, boost::random::gamma_distribution<>(_gamma_shape,1)));
        }
        double deviate = (*_gamma_variate_generator)();
        return scale*deviate;
    }

    inline int Lot::randint(int low, int high) {
        if (low != _low || high != _high) {
            _low  = low;
            _high = high;
            _uniform_int_generator.reset(new uniform_int_generator_t(_generator, boost::random::uniform_int_distribution<>(_low,_high)));
        }
        return (*_uniform_int_generator)();
    }

}


// This code is licensed under the MIT License (MIT). See LICENCE.txt for details

#include "collatzOpenCL.h"
#include "arrayBignum.h"
#include "collatzConfig.h"
#include "validateErr.h"
#include "collatzGeneric.h"
#include "collatzResult.h"
#include "fileToStr.h"
#include "lookUpTable.h"
#include <algorithm>
#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>

namespace collatzOpenCL {
namespace mp = boost::multiprecision;
using BigInt = boost::multiprecision::uint512_t;

struct Numbers {
  BigInt addent;
  uint32_t powerOf3;
  BigInt multiplier;
  Numbers(const Config& settings):
    addent(settings.startLo),
    powerOf3(collatz_op(addent, sieveStep)),
    multiplier(boost::multiprecision::pow(BigInt{3}, powerOf3))
  {}
}; //struct Numbers

std::string make_build_options_from_settings(const Config& settings, const Numbers& nums)
{
  return settings.buildOptions + clh::define_constant("LUTSIZE_LOG2", settings.lutSizeLog2) + clh::define_constant("DELAYSIZE_LOG2", settings.delaySizeLog2) + clh::define_constant("INITSTEP", nums.powerOf3 + sieveStep);
}
cl_uint16 make_uint16(const BigInt& val)
{
  cl_uint16 res;
  bignum_to_array(val, res.s);
  return res;
}
cl::vector<cl_uint> make_arr(const BigInt& val)
{
  cl::vector<cl_uint> res(valLength, 0);
  bignum_to_array(val, res);
  return res;
}

struct InputBuffers {
  cl::Buffer dLUT;
  cl::Buffer dDelays;
  cl::Buffer dMultiplier;

  InputBuffers(const cl::Context& clContext, const Numbers& nums, const Config& settings)
  {
    dLUT = clh::alloc(clContext, getLUT(settings.lutSizeLog2), true);
    dDelays = clh::alloc(clContext, getDelayTable(settings.delaySizeLog2), true);
    dMultiplier = clh::alloc(clContext, make_arr(nums.multiplier), true);
  }
}; //Inputbuffers

struct CollatzQueue {
  clh::Queue queue;
  clh::Ranges ranges;
  cl::Kernel clearKernel;
  cl::Kernel collatzKernel;
  cl::vector<cl_uint> hMaxSteps;
  cl::vector<cl_ulong> hMaxPos;
  cl::vector<cl_ulong> hTotalSteps;
  cl::Buffer dMaxSteps;
  cl::Buffer dMaxPos;
  cl::Buffer dTotalSteps;

  CollatzQueue(const cl::Program& program, const Config& settings, const InputBuffers& inBuf) :
    queue{ cl::CommandQueue(program.getInfo<CL_PROGRAM_CONTEXT>()) },
    ranges{ settings.globalWorkSize, settings.localWorkSize },
    clearKernel{ program, clearKernelName },
    collatzKernel{ program, collatzKernelName },
    hMaxSteps(settings.globalWorkSize, 0),
    hMaxPos(settings.globalWorkSize, 0),
    hTotalSteps(settings.globalWorkSize, 0),
    dMaxSteps{ clh::alloc(queue.context(), clh::get_byte_size(hMaxSteps)) },
    dMaxPos{ clh::alloc(queue.context(), clh::get_byte_size(hMaxPos)) },
    dTotalSteps{ clh::alloc(queue.context(), clh::get_byte_size(hMaxPos)) }
  {
    clh::set_args(clearKernel, dMaxSteps, dMaxPos, dTotalSteps);
    clh::set_args(collatzKernel, dMaxSteps, dMaxPos, dTotalSteps, inBuf.dLUT, inBuf.dDelays, inBuf.dMultiplier);
  }

  void queue_kernels(const Config& settings, const Numbers& nums, cl_ulong& kOffset)
  {
    constexpr auto indexOfAddent = 6;
    constexpr auto indexOfkOffset = 7;
    queue.run(clearKernel, ranges);
    for (auto i = settings.kernelsPerReduction; i > 0; --i, kOffset += settings.globalWorkSize) {
      collatzKernel.setArg(indexOfAddent, make_uint16(nums.addent + nums.multiplier * kOffset));
      collatzKernel.setArg(indexOfkOffset, kOffset);
      queue.run(collatzKernel, ranges);
    }
  }

  Result read_result()
  {
    queue.copy(dMaxSteps, hMaxSteps, CL_FALSE);
    queue.copy(dMaxPos, hMaxPos, CL_FALSE);
    queue.copy(dTotalSteps, hTotalSteps, CL_TRUE);
    return gather_results(hMaxSteps, hMaxPos, hTotalSteps);
  }
}; //struct CollatzQueue

Result worker_loop(cl::vector<CollatzQueue>& queues, const Config& settings, const Numbers& nums)
{
  Result result;
  cl_ulong kOffset = settings.startHi;
  for (auto& q : queues) {
    q.queue_kernels(settings, nums, kOffset);
  }
  do {
    for (auto& q : queues) {
      result = compare_and_validate(result, q.read_result(), BigInt{ settings.startLo });
      q.queue_kernels(settings, nums, kOffset);
    }
  } while (kOffset < settings.range);
  for (auto& q : queues) {
    result = compare_and_validate(result, q.read_result(), BigInt{ settings.startLo });
  }
  return result;
}

void run_kernel_impl(const cl::Device & clDevice, std::istream & configFile, std::ostream & logger, std::ostream& output)
{
  cl::Context clContext(clDevice);
  Config settings(configFile);
  Numbers nums(settings);
  InputBuffers inputBuffers(clContext, nums, settings);
  auto program = clh::build_program(clContext, file_to_str(kernelFileName), make_build_options_from_settings(settings, nums));
  cl::vector<CollatzQueue> collatzQueues
  (
    numQueues,
    CollatzQueue
    (
      program,
      settings,
      inputBuffers
    )
  );
  output << worker_loop(collatzQueues, settings, nums);
}

} //namespace collatzOpenCL


#include <random>
#include <functional>
#include <vector>
#include <chrono>
#include <boost/format.hpp>
#include <algorithm>
#include "mpi.h"
#include "array3d.h"

using namespace std;

uniform_real_distribution<double> generator { -10, 10 };
default_random_engine re;

int mpi_rank;
int mpi_size;

const double GB = 1024 * 1024 * 1024;

const double kappa = 1.25 * (sqrt(3) - 1) * 0.5 / sqrt(3);
const double bx = (1 - kappa);
const double ax = 0.5 * (1 - bx);

double dt;
double dtdx;
double dtdy;
double dtdz;
double ay, az;
double by, bz;

double gen() {
    return generator(re);
}

struct vector3d {
    double x = 0, y = 0, z = 0;

    vector3d operator-(const vector3d & other) {
        vector3d res;
        res.x = x - other.x;
        res.y = y - other.y;
        res.z = z - other.z;
        return res;
    }
};

template <typename T>
void randomize(T & a) {
    int n = a.get_n();
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
            for (int k = 0; k < n; k++){
                a(i, j, k).x = gen();
                a(i, j, k).y = gen();
                a(i, j, k).z = gen();
            }
        }
    }
}

template <typename T>
void advance_b(const T & e, T & b) {
    const int n = e.get_n();

    for(int i=2;i<n-1;i++) {
        for(int j=2;j<n-1;j++) {
            for(int k=2;k<n-1;k++) {
                b(i,j,k).x += 0.5*(   -dtdy*( bz*(e(i,j+1,k).z-e(i,j,k).z) + az*(e(i,j+1,k+1).z-e(i,j,k+1).z+e(i,j+1,k-1).z-e(i,j,k-1).z) )    +   dtdz*( by*(e(i,j,k+1).y-e(i,j,k).y) + ay*(e(i,j+1,k+1).y-e(i,j+1,k).y+e(i,j-1,k+1).y-e(i,j-1,k).y) )  );
                b(i,j,k).y += 0.5*(   dtdx*( bz*(e(i+1,j,k).z-e(i,j,k).z) + az*(e(i+1,j,k+1).z-e(i,j,k+1).z+e(i+1,j,k-1).z-e(i,j,k-1).z) )    -   dtdz*( bx*(e(i,j,k+1).x-e(i,j,k).x) + ax*(e(i+1,j,k+1).x-e(i+1,j,k).x+e(i-1,j,k+1).x-e(i-1,j,k).x) )  );
                b(i,j,k).z += 0.5*(   dtdy*( bx*(e(i,j+1,k).x-e(i,j,k).x) + ax*(e(i+1,j+1,k).x-e(i+1,j,k).x+e(i-1,j+1,k).x-e(i-1,j,k).x) )    -   dtdx*( by*(e(i+1,j,k).y-e(i,j,k).y) + ay*(e(i+1,j+1,k).y-e(i,j+1,k).y+e(i+1,j-1,k).y-e(i,j-1,k).y) )  );
            }
        }
    }
    {int i=1;
        for(int j=2;j<n-1;j++) {
            for(int k=2;k<n-1;k++) {
                b(i,j,k).y += 0.5*(   dtdx*( bz*(e(i+1,j,k).z-e(i,j,k).z) + az*(e(i+1,j,k+1).z-e(i,j,k+1).z+e(i+1,j,k-1).z-e(i,j,k-1).z) )    -   dtdz*( bx*(e(i,j,k+1).x-e(i,j,k).x) + ax*(e(i+1,j,k+1).x-e(i+1,j,k).x+e(i-1,j,k+1).x-e(i-1,j,k).x) )  );
                b(i,j,k).z += 0.5*(   dtdy*( bx*(e(i,j+1,k).x-e(i,j,k).x) + ax*(e(i+1,j+1,k).x-e(i+1,j,k).x+e(i-1,j+1,k).x-e(i-1,j,k).x) )    -   dtdx*( by*(e(i+1,j,k).y-e(i,j,k).y) + ay*(e(i+1,j+1,k).y-e(i,j+1,k).y+e(i+1,j-1,k).y-e(i,j-1,k).y) )  );
            }
        }
    }
    for(int i=2;i<n-1;i++) {
        {int j=1;
            for(int k=2;k<n-1;k++) {
                b(i,j,k).x += 0.5*(   -dtdy*( bz*(e(i,j+1,k).z-e(i,j,k).z) + az*(e(i,j+1,k+1).z-e(i,j,k+1).z+e(i,j+1,k-1).z-e(i,j,k-1).z) )    +   dtdz*( by*(e(i,j,k+1).y-e(i,j,k).y) + ay*(e(i,j+1,k+1).y-e(i,j+1,k).y+e(i,j-1,k+1).y-e(i,j-1,k).y) )  );
                b(i,j,k).z += 0.5*(   dtdy*( bx*(e(i,j+1,k).x-e(i,j,k).x) + ax*(e(i+1,j+1,k).x-e(i+1,j,k).x+e(i-1,j+1,k).x-e(i-1,j,k).x) )    -   dtdx*( by*(e(i+1,j,k).y-e(i,j,k).y) + ay*(e(i+1,j+1,k).y-e(i,j+1,k).y+e(i+1,j-1,k).y-e(i,j-1,k).y) )  );
            }
        }
    }
    for(int i=2;i<n-1;i++) {
        for(int j=2;j<n-1;j++) {
            {int k=1;
                b(i,j,k).x += 0.5*(   -dtdy*( bz*(e(i,j+1,k).z-e(i,j,k).z) + az*(e(i,j+1,k+1).z-e(i,j,k+1).z+e(i,j+1,k-1).z-e(i,j,k-1).z) )    +   dtdz*( by*(e(i,j,k+1).y-e(i,j,k).y) + ay*(e(i,j+1,k+1).y-e(i,j+1,k).y+e(i,j-1,k+1).y-e(i,j-1,k).y) )  );
                b(i,j,k).y += 0.5*(   dtdx*( bz*(e(i+1,j,k).z-e(i,j,k).z) + az*(e(i+1,j,k+1).z-e(i,j,k+1).z+e(i+1,j,k-1).z-e(i,j,k-1).z) )    -   dtdz*( bx*(e(i,j,k+1).x-e(i,j,k).x) + ax*(e(i+1,j,k+1).x-e(i+1,j,k).x+e(i-1,j,k+1).x-e(i-1,j,k).x) )  );
            }
        }
    }
}

void advance_b_z(const morton_array<vector3d> & e, morton_array<vector3d> & b) {
    const unsigned int n = e.get_size();

    for (unsigned int curr = 0; curr < n; curr++) {
        if (e.is_xmax(curr) || e.is_ymax(curr) || e.is_zmax(curr)) {
            continue;
        }

        if (e.is_xmin(curr) || e.is_ymin(curr) || e.is_zmin(curr)) {
            continue;
        }

        auto x_m = e.get_x_prev(curr);
        auto y_m = e.get_y_prev(curr);
        auto z_m = e.get_z_prev(curr);

        auto x_p = e.get_x_next(curr);
        auto y_p = e.get_y_next(curr);
        auto z_p = e.get_z_next(curr);

        auto xy_pp = e.get_y_next(x_p);
        auto xy_pm = e.get_y_prev(x_p);
        auto xy_mp = e.get_y_next(x_m);

        auto xz_pp = e.get_z_next(x_p);
        auto xz_pm = e.get_z_prev(x_p);
        auto xz_mp = e.get_z_next(x_m);

        auto yz_pp = e.get_z_next(y_p);
        auto yz_pm = e.get_z_prev(y_p);
        auto yz_mp = e.get_z_next(y_m);

        if (e.is_xmin(x_m)) {
            if (!e.is_ymin(y_m) && !e.is_zmin(z_m)) {
                b[curr].y += 0.5*(    dtdx*( bz*(e[x_p].z-e[curr].z) + az*(e[xz_pp].z-e[z_p].z+e[xz_pm].z-e[z_m].z) )    -   dtdz*( bx*(e[z_p].x-e[curr].x) + ax*(e[xz_pp].x-e[x_p].x+e[xz_mp].x-e[x_m].x) )  );
                b[curr].z += 0.5*(    dtdy*( bx*(e[y_p].x-e[curr].x) + ax*(e[xy_pp].x-e[x_p].x+e[xy_mp].x-e[x_m].x) )    -   dtdx*( by*(e[x_p].y-e[curr].y) + ay*(e[xy_pp].y-e[y_p].y+e[xy_pm].y-e[y_m].y) )  );
            }
        } else if (e.is_ymin(y_m)) {
            if (!e.is_xmin(x_m) && !e.is_zmin(z_m)) {
                b[curr].x += 0.5*(   -dtdy*( bz*(e[y_p].z-e[curr].z) + az*(e[yz_pp].z-e[z_p].z+e[yz_pm].z-e[z_m].z) )    +   dtdz*( by*(e[z_p].y-e[curr].y) + ay*(e[yz_pp].y-e[y_p].y+e[yz_mp].y-e[y_m].y) )  );
                b[curr].z += 0.5*(    dtdy*( bx*(e[y_p].x-e[curr].x) + ax*(e[xy_pp].x-e[x_p].x+e[xy_mp].x-e[x_m].x) )    -   dtdx*( by*(e[x_p].y-e[curr].y) + ay*(e[xy_pp].y-e[y_p].y+e[xy_pm].y-e[y_m].y) )  );
            }
        } else if (e.is_zmin(z_m)) {
            if (!e.is_xmin(x_m) && !e.is_ymin(y_m)) {
                b[curr].x += 0.5*(   -dtdy*( bz*(e[y_p].z-e[curr].z) + az*(e[yz_pp].z-e[z_p].z+e[yz_pm].z-e[z_m].z) )    +   dtdz*( by*(e[z_p].y-e[curr].y) + ay*(e[yz_pp].y-e[y_p].y+e[yz_mp].y-e[y_m].y) )  );
                b[curr].y += 0.5*(    dtdx*( bz*(e[x_p].z-e[curr].z) + az*(e[xz_pp].z-e[z_p].z+e[xz_pm].z-e[z_m].z) )    -   dtdz*( bx*(e[z_p].x-e[curr].x) + ax*(e[xz_pp].x-e[x_p].x+e[xz_mp].x-e[x_m].x) )  );
            }
        } else {
            b[curr].x += 0.5*(   -dtdy*( bz*(e[y_p].z-e[curr].z) + az*(e[yz_pp].z-e[z_p].z+e[yz_pm].z-e[z_m].z) )    +   dtdz*( by*(e[z_p].y-e[curr].y) + ay*(e[yz_pp].y-e[y_p].y+e[yz_mp].y-e[y_m].y) )  );
            b[curr].y += 0.5*(    dtdx*( bz*(e[x_p].z-e[curr].z) + az*(e[xz_pp].z-e[z_p].z+e[xz_pm].z-e[z_m].z) )    -   dtdz*( bx*(e[z_p].x-e[curr].x) + ax*(e[xz_pp].x-e[x_p].x+e[xz_mp].x-e[x_m].x) )  );
            b[curr].z += 0.5*(    dtdy*( bx*(e[y_p].x-e[curr].x) + ax*(e[xy_pp].x-e[x_p].x+e[xy_mp].x-e[x_m].x) )    -   dtdx*( by*(e[x_p].y-e[curr].y) + ay*(e[xy_pp].y-e[y_p].y+e[xy_pm].y-e[y_m].y) )  );
        }
    }
}

template <typename T>
inline void advance_b_point(const T & e, T & b, int i, int j, int k, int i_prev, int j_prev, int k_prev, int i_next, int j_next, int k_next) {
    b(i,j,k).x += 0.5*(   -dtdy*( bz*(e(i,j_next,k).z-e(i,j,k).z) + az*(e(i,j_next,k_next).z-e(i,j,k_next).z+e(i,j_next,k_prev).z-e(i,j,k_prev).z) )    +   dtdz*( by*(e(i,j,k_next).y-e(i,j,k).y) + ay*(e(i,j_next,k_next).y-e(i,j_next,k).y+e(i,j_prev,k_next).y-e(i,j_prev,k).y) )  );
    b(i,j,k).y += 0.5*(   dtdx*( bz*(e(i_next,j,k).z-e(i,j,k).z) + az*(e(i_next,j,k_next).z-e(i,j,k_next).z+e(i_next,j,k_prev).z-e(i,j,k_prev).z) )    -   dtdz*( bx*(e(i,j,k_next).x-e(i,j,k).x) + ax*(e(i_next,j,k_next).x-e(i_next,j,k).x+e(i_prev,j,k_next).x-e(i_prev,j,k).x) )  );
    b(i,j,k).z += 0.5*(   dtdy*( bx*(e(i,j_next,k).x-e(i,j,k).x) + ax*(e(i_next,j_next,k).x-e(i_next,j,k).x+e(i_prev,j_next,k).x-e(i_prev,j,k).x) )    -   dtdx*( by*(e(i_next,j,k).y-e(i,j,k).y) + ay*(e(i_next,j_next,k).y-e(i,j_next,k).y+e(i_next,j_prev,k).y-e(i,j_prev,k).y) )  );
}

template <typename T>
void advance_b_periodic(const T & e, T & b) {
    const int n = e.get_n();

    for(int i=0;i<n;i++) {
        for(int j=0;j<n;j++) {
            for(int k=0;k<n;k++) {
                int i_prev = (i != 0 ? i-1 : n-1);
                int j_prev = (j != 0 ? j-1 : n-1);
                int k_prev = (k != 0 ? k-1 : n-1);
                int i_next = (i != n-1 ? i+1 : 0);
                int j_next = (j != n-1 ? j+1 : 0);
                int k_next = (k != n-1 ? k+1 : 0);
                advance_b_point(e, b, i, j, k, i_prev, j_prev, k_prev, i_next, j_next, k_next);
            }
        }
    }
}

template <typename T>
void advance_b_periodic2(const T & e, T & b) {
    const int n = e.get_n();

    int i = 0;
    int j = 0;
    advance_b_point(e, b, i, j, 0, n-1, n-1, n-1, i+1, j+1, 1);

    for(int k=1;k<n-1;k++) {
        advance_b_point(e, b, i, j, k, n-1, n-1, k-1, i+1, j+1, k+1);
    }

    advance_b_point(e, b, i, j, n-1, n-1, n-1, n-2, i+1, j+1, 0);

    for(int j=1;j<n-1;j++) {
        advance_b_point(e, b, i, j, 0, n-1, j-1, n-1, i+1, j+1, 1);

        for(int k=1;k<n-1;k++) {
            advance_b_point(e, b, i, j, k, n-1, j-1, k-1, i+1, j+1, k+1);
        }

        advance_b_point(e, b, i, j, n-1, n-1, j-1, n-2, i+1, j+1, 0);
    }

    j = n-1;
    advance_b_point(e, b, i, j, 0, n-1, j-1, n-1, i+1, 0, 1);

    for(int k=1;k<n-1;k++) {
        advance_b_point(e, b, i, j, k, n-1, j-1, k-1, i+1, 0, k+1);
    }

    advance_b_point(e, b, i, j, n-1, n-1, j-1, n-2, i+1, 0, 0);

    for(int i=1;i<n-1;i++) {
        int j = 0;
        advance_b_point(e, b, i, j, 0, i-1, n-1, n-1, i+1, j+1, 1);

        for(int k=1;k<n-1;k++) {
            advance_b_point(e, b, i, j, k, i-1, n-1, k-1, i+1, j+1, k+1);
        }

        advance_b_point(e, b, i, j, n-1, i-1, n-1, n-2, i+1, j+1, 0);

        for(int j=1;j<n-1;j++) {
            advance_b_point(e, b, i, j, 0, i-1, j-1, n-1, i+1, j+1, 1);

            for(int k=1;k<n-1;k++) {
                advance_b_point(e, b, i, j, k, i-1, j-1, k-1, i+1, j+1, k+1);
            }

            advance_b_point(e, b, i, j, n-1, i-1, j-1, n-2, i+1, j+1, 0);
        }

        j = n-1;
        advance_b_point(e, b, i, j, 0, i-1, j-1, n-1, i+1, 0, 1);

        for(int k=1;k<n-1;k++) {
            advance_b_point(e, b, i, j, k, i-1, j-1, k-1, i+1, 0, k+1);
        }

        advance_b_point(e, b, i, j, n-1, i-1, j-1, n-2, i+1, 0, 0);
    }

    i = n-1;
    j = 0;
    advance_b_point(e, b, i, j, 0, i-1, n-1, n-1, 0, j+1, 1);

    for(int k=1;k<n-1;k++) {
        advance_b_point(e, b, i, j, k, i-1, n-1, k-1, 0, j+1, k+1);
    }

    advance_b_point(e, b, i, j, n-1, i-1, n-1, n-2, 0, j+1, 0);

    for(int j=1;j<n-1;j++) {
        advance_b_point(e, b, i, j, 0, i-1, j-1, n-1, 0, j+1, 1);

        for(int k=1;k<n-1;k++) {
            advance_b_point(e, b, i, j, k, i-1, j-1, k-1, 0, j+1, k+1);
        }

        advance_b_point(e, b, i, j, n-1, i-1, j-1, n-2, 0, j+1, 0);
    }

    j = n-1;
    advance_b_point(e, b, i, j, 0, i-1, j-1, n-1, 0, 0, 1);

    for(int k=1;k<n-1;k++) {
        advance_b_point(e, b, i, j, k, i-1, j-1, k-1, 0, 0, k+1);
    }

    advance_b_point(e, b, i, j, n-1, i-1, j-1, n-2, 0, 0, 0);
}

void advance_b_z_periodic(const morton_array<vector3d> & e, morton_array<vector3d> & b) {
    const unsigned int n = e.get_size();

    for (unsigned int curr = 0; curr < n; curr++) {
        auto x_p = e.get_x_next(curr);
        auto y_p = e.get_y_next(curr);
        auto z_p = e.get_z_next(curr);

        auto x_m = e.get_x_prev(curr);
        auto y_m = e.get_y_prev(curr);
        auto z_m = e.get_z_prev(curr);

        auto xy_pp = e.get_y_next(x_p);
        auto xy_pm = e.get_y_prev(x_p);
        auto xy_mp = e.get_y_next(x_m);

        auto xz_pp = e.get_z_next(x_p);
        auto xz_pm = e.get_z_prev(x_p);
        auto xz_mp = e.get_z_next(x_m);

        auto yz_pp = e.get_z_next(y_p);
        auto yz_pm = e.get_z_prev(y_p);
        auto yz_mp = e.get_z_next(y_m);

        b[curr].x += 0.5*(   -dtdy*( bz*(e[y_p].z-e[curr].z) + az*(e[yz_pp].z-e[z_p].z+e[yz_pm].z-e[z_m].z) )    +   dtdz*( by*(e[z_p].y-e[curr].y) + ay*(e[yz_pp].y-e[y_p].y+e[yz_mp].y-e[y_m].y) )  );
        b[curr].y += 0.5*(    dtdx*( bz*(e[x_p].z-e[curr].z) + az*(e[xz_pp].z-e[z_p].z+e[xz_pm].z-e[z_m].z) )    -   dtdz*( bx*(e[z_p].x-e[curr].x) + ax*(e[xz_pp].x-e[x_p].x+e[xz_mp].x-e[x_m].x) )  );
        b[curr].z += 0.5*(    dtdy*( bx*(e[y_p].x-e[curr].x) + ax*(e[xy_pp].x-e[x_p].x+e[xy_mp].x-e[x_m].x) )    -   dtdx*( by*(e[x_p].y-e[curr].y) + ay*(e[xy_pp].y-e[y_p].y+e[xy_pm].y-e[y_m].y) )  );

    }
}

template <typename T>
void advance_e(T & e, const T & b) {
    const int n = e.get_n();

    for(int i=2;i<n-1;i++) {
        for(int j=2;j<n-1;j++) {
            for(int k=2;k<n-1;k++) {
                e(i,j,k).x = e(i,j,k).x   +   dtdy*( bz*(b(i,j,k).z-b(i,j-1,k).z) + az*(b(i,j,k+1).z-b(i,j-1,k+1).z+b(i,j,k-1).z-b(i,j-1,k-1).z) )  -   dtdz*( by*(b(i,j,k).y-b(i,j,k-1).y) + ay*(b(i,j+1,k).y-b(i,j+1,k-1).y+b(i,j-1,k).y-b(i,j-1,k-1).y) );
                e(i,j,k).y = e(i,j,k).y   -   dtdx*( bz*(b(i,j,k).z-b(i-1,j,k).z) + az*(b(i,j,k+1).z-b(i-1,j,k+1).z+b(i,j,k-1).z-b(i-1,j,k-1).z) )  +   dtdz*( bx*(b(i,j,k).x-b(i,j,k-1).x) + ax*(b(i+1,j,k).x-b(i+1,j,k-1).x+b(i-1,j,k).x-b(i-1,j,k-1).x) );
                e(i,j,k).z = e(i,j,k).z   +   dtdx*( by*(b(i,j,k).y-b(i-1,j,k).y) + ay*(b(i,j+1,k).y-b(i-1,j+1,k).y+b(i,j-1,k).y-b(i-1,j-1,k).y) )  -   dtdy*( bx*(b(i,j,k).x-b(i,j-1,k).x) + ax*(b(i+1,j,k).x-b(i+1,j-1,k).x+b(i-1,j,k).x-b(i-1,j-1,k).x) );
            }
        }
    }
    {int i=1;
        for(int j=2;j<n-1;j++) {
            for(int k=2;k<n-1;k++) {
                e(i,j,k).x = e(i,j,k).x   +   dtdy*( bz*(b(i,j,k).z-b(i,j-1,k).z) + az*(b(i,j,k+1).z-b(i,j-1,k+1).z+b(i,j,k-1).z-b(i,j-1,k-1).z) )  -   dtdz*( by*(b(i,j,k).y-b(i,j,k-1).y) + ay*(b(i,j+1,k).y-b(i,j+1,k-1).y+b(i,j-1,k).y-b(i,j-1,k-1).y) );
            }
        }
    }
    for(int i=2;i<n-1;i++) {
        {int j=1;
            for(int k=2;k<n-1;k++) {
                e(i,j,k).y = e(i,j,k).y   -   dtdx*( bz*(b(i,j,k).z-b(i-1,j,k).z) + az*(b(i,j,k+1).z-b(i-1,j,k+1).z+b(i,j,k-1).z-b(i-1,j,k-1).z) )  +   dtdz*( bx*(b(i,j,k).x-b(i,j,k-1).x) + ax*(b(i+1,j,k).x-b(i+1,j,k-1).x+b(i-1,j,k).x-b(i-1,j,k-1).x) );
            }
        }
    }
    for(int i=2;i<n-1;i++) {
        for(int j=2;j<n-1;j++) {
            {int k=1;
                e(i,j,k).z = e(i,j,k).z   +   dtdx*( by*(b(i,j,k).y-b(i-1,j,k).y) + ay*(b(i,j+1,k).y-b(i-1,j+1,k).y+b(i,j-1,k).y-b(i-1,j-1,k).y) )  -   dtdy*( bx*(b(i,j,k).x-b(i,j-1,k).x) + ax*(b(i+1,j,k).x-b(i+1,j-1,k).x+b(i-1,j,k).x-b(i-1,j-1,k).x) );
            }
        }
    }
}

void advance_e_z(morton_array<vector3d> & e, const morton_array<vector3d> & b) {
    const unsigned int n = e.get_size();

    for (unsigned int curr = 0; curr < n; curr++) {
        if (b.is_xmax(curr) || b.is_ymax(curr) || b.is_zmax(curr)) {
            continue;
        }

        if (b.is_xmin(curr) || b.is_ymin(curr) || b.is_zmin(curr)) {
            continue;
        }

        auto x_p = b.get_x_next(curr);
        auto y_p = b.get_y_next(curr);
        auto z_p = b.get_z_next(curr);

        auto x_m = b.get_x_prev(curr);
        auto y_m = b.get_y_prev(curr);
        auto z_m = b.get_z_prev(curr);

        auto xy_mm = b.get_y_prev(x_m);
        auto xy_pm = b.get_y_prev(x_p);
        auto xy_mp = b.get_y_next(x_m);

        auto xz_mm = b.get_z_prev(x_m);
        auto xz_pm = b.get_z_prev(x_p);
        auto xz_mp = b.get_z_next(x_m);

        auto yz_mm = b.get_z_prev(y_m);
        auto yz_pm = b.get_z_prev(y_p);
        auto yz_mp = b.get_z_next(y_m);

        if (b.is_xmin(x_m)) {
            if (!b.is_ymin(y_m) && !b.is_zmin(z_m)) {
                e[curr].x = e[curr].x   +   dtdy*( bz*(b[curr].z-b[y_m].z) + az*(b[z_p].z-b[yz_mp].z+b[z_m].z-b[yz_mm].z) )  -   dtdz*( by*(b[curr].y-b[z_m].y) + ay*(b[y_p].y-b[yz_pm].y+b[y_m].y-b[yz_mm].y) );
            }
        } else if (b.is_ymin(y_m)) {
            if (!b.is_xmin(x_m) && !b.is_zmin(z_m)) {
                e[curr].y = e[curr].y   -   dtdx*( bz*(b[curr].z-b[x_m].z) + az*(b[z_p].z-b[xz_mp].z+b[z_m].z-b[xz_mm].z) )  +   dtdz*( bx*(b[curr].x-b[z_m].x) + ax*(b[x_p].x-b[xz_pm].x+b[x_m].x-b[xz_mm].x) );            }
        } else if (b.is_zmin(z_m)) {
            if (!b.is_xmin(x_m) && !b.is_ymin(y_m)) {
                e[curr].z = e[curr].z   +   dtdx*( by*(b[curr].y-b[x_m].y) + ay*(b[y_p].y-b[xy_mp].y+b[y_m].y-b[xy_mm].y) )  -   dtdy*( bx*(b[curr].x-b[y_m].x) + ax*(b[x_p].x-b[xy_pm].x+b[x_m].x-b[xy_mm].x) );
            }
        } else {
            e[curr].x = e[curr].x   +   dtdy*( bz*(b[curr].z-b[y_m].z) + az*(b[z_p].z-b[yz_mp].z+b[z_m].z-b[yz_mm].z) )  -   dtdz*( by*(b[curr].y-b[z_m].y) + ay*(b[y_p].y-b[yz_pm].y+b[y_m].y-b[yz_mm].y) );
            e[curr].y = e[curr].y   -   dtdx*( bz*(b[curr].z-b[x_m].z) + az*(b[z_p].z-b[xz_mp].z+b[z_m].z-b[xz_mm].z) )  +   dtdz*( bx*(b[curr].x-b[z_m].x) + ax*(b[x_p].x-b[xz_pm].x+b[x_m].x-b[xz_mm].x) );
            e[curr].z = e[curr].z   +   dtdx*( by*(b[curr].y-b[x_m].y) + ay*(b[y_p].y-b[xy_mp].y+b[y_m].y-b[xy_mm].y) )  -   dtdy*( bx*(b[curr].x-b[y_m].x) + ax*(b[x_p].x-b[xy_pm].x+b[x_m].x-b[xy_mm].x) );
        }

    }
}

template <typename T>
inline void advance_e_point(T & e, const T & b, int i, int j, int k, int i_prev, int j_prev, int k_prev, int i_next, int j_next, int k_next) {
    e(i,j,k).x = e(i,j,k).x   +   dtdy*( bz*(b(i,j,k).z-b(i,j_prev,k).z) + az*(b(i,j,k_next).z-b(i,j_prev,k_next).z+b(i,j,k_prev).z-b(i,j_prev,k_prev).z) )  -   dtdz*( by*(b(i,j,k).y-b(i,j,k_prev).y) + ay*(b(i,j_next,k).y-b(i,j_next,k_prev).y+b(i,j_prev,k).y-b(i,j_prev,k_prev).y) );
    e(i,j,k).y = e(i,j,k).y   -   dtdx*( bz*(b(i,j,k).z-b(i_prev,j,k).z) + az*(b(i,j,k_next).z-b(i_prev,j,k_next).z+b(i,j,k_prev).z-b(i_prev,j,k_prev).z) )  +   dtdz*( bx*(b(i,j,k).x-b(i,j,k_prev).x) + ax*(b(i_next,j,k).x-b(i_next,j,k_prev).x+b(i_prev,j,k).x-b(i_prev,j,k_prev).x) );
    e(i,j,k).z = e(i,j,k).z   +   dtdx*( by*(b(i,j,k).y-b(i_prev,j,k).y) + ay*(b(i,j_next,k).y-b(i_prev,j_next,k).y+b(i,j_prev,k).y-b(i_prev,j_prev,k).y) )  -   dtdy*( bx*(b(i,j,k).x-b(i,j_prev,k).x) + ax*(b(i_next,j,k).x-b(i_next,j_prev,k).x+b(i_prev,j,k).x-b(i_prev,j_prev,k).x) );
}

template <typename T>
void advance_e_periodic(T & e, const T & b) {
    const int n = e.get_n();

    for(int i=0;i<n;i++) {
        for(int j=0;j<n;j++) {
            for(int k=0;k<n;k++) {
                int i_prev = (i != 0 ? i-1 : n-1);
                int j_prev = (j != 0 ? j-1 : n-1);
                int k_prev = (k != 0 ? k-1 : n-1);
                int i_next = (i != n-1 ? i+1 : 0);
                int j_next = (j != n-1 ? j+1 : 0);
                int k_next = (k != n-1 ? k+1 : 0);
                advance_e_point(e, b, i, j, k, i_prev, j_prev, k_prev, i_next, j_next, k_next);
            }
        }
    }
}

void advance_e_z_periodic(morton_array<vector3d> & e, const morton_array<vector3d> & b) {
    const unsigned int n = e.get_size();

    for (unsigned int curr = 0; curr < n; curr++) {
        auto x_p = b.get_x_next(curr);
        auto y_p = b.get_y_next(curr);
        auto z_p = b.get_z_next(curr);

        auto x_m = b.get_x_prev(curr);
        auto y_m = b.get_y_prev(curr);
        auto z_m = b.get_z_prev(curr);

        auto xy_mm = b.get_y_prev(x_m);
        auto xy_pm = b.get_y_prev(x_p);
        auto xy_mp = b.get_y_next(x_m);

        auto xz_mm = b.get_z_prev(x_m);
        auto xz_pm = b.get_z_prev(x_p);
        auto xz_mp = b.get_z_next(x_m);

        auto yz_mm = b.get_z_prev(y_m);
        auto yz_pm = b.get_z_prev(y_p);
        auto yz_mp = b.get_z_next(y_m);

        e[curr].x = e[curr].x   +   dtdy*( bz*(b[curr].z-b[y_m].z) + az*(b[z_p].z-b[yz_mp].z+b[z_m].z-b[yz_mm].z) )  -   dtdz*( by*(b[curr].y-b[z_m].y) + ay*(b[y_p].y-b[yz_pm].y+b[y_m].y-b[yz_mm].y) );
        e[curr].y = e[curr].y   -   dtdx*( bz*(b[curr].z-b[x_m].z) + az*(b[z_p].z-b[xz_mp].z+b[z_m].z-b[xz_mm].z) )  +   dtdz*( bx*(b[curr].x-b[z_m].x) + ax*(b[x_p].x-b[xz_pm].x+b[x_m].x-b[xz_mm].x) );
        e[curr].z = e[curr].z   +   dtdx*( by*(b[curr].y-b[x_m].y) + ay*(b[y_p].y-b[xy_mp].y+b[y_m].y-b[xy_mm].y) )  -   dtdy*( bx*(b[curr].x-b[y_m].x) + ax*(b[x_p].x-b[xy_pm].x+b[x_m].x-b[xy_mm].x) );
    }
}

template <typename T>
void advance(T & e, T & b) {
    advance_b(e, b);
    advance_e(e, b);
    advance_b(e, b);
}

void advance_z(morton_array<vector3d> & e, morton_array<vector3d> & b) {
    advance_b_z(e, b);
    advance_e_z(e, b);
    advance_b_z(e, b);
}

template <typename T>
void advance_periodic(T & e, T & b) {
    advance_b_periodic(e, b);
    advance_e_periodic(e, b);
    advance_b_periodic(e, b);
}

void advance_z_periodic(morton_array<vector3d> & e, morton_array<vector3d> & b) {
    advance_b_z_periodic(e, b);
    advance_e_z_periodic(e, b);
    advance_b_z_periodic(e, b);
}

template <typename T>
void run_test(std::function<void(T&, T&)> func, const std::string & testname, int size, int iterations=10) {
    T a(size);
    T b(size);
    vector<double> times(iterations);
    for (int i = 0; i < iterations; i++) {
        randomize(a);
        randomize(b);
        MPI_Barrier(MPI_COMM_WORLD);
        chrono::steady_clock::time_point begin = chrono::steady_clock::now();
        func(a, b);
        MPI_Barrier(MPI_COMM_WORLD);
        chrono::steady_clock::time_point end = chrono::steady_clock::now();
        times[i] = chrono::duration_cast<chrono::nanoseconds>(end - begin).count() / 1e6;
    }
    if (mpi_rank == 0) {
        sort(times.begin(), times.end());
        times.pop_back();
        double avg = accumulate(times.begin(), times.end(), 0.0) / times.size();
        double avg_sq = inner_product(times.begin(), times.end(), times.begin(), 0.0) / times.size();
        double standard_deviation = sqrt(avg_sq - avg * avg);
        double throughput = (2 * static_cast<double>(mpi_size) * size * size * size * sizeof(vector3d)) / GB * 1e3 / avg; // Gb / s
        cout << boost::format("%15s ") % testname << boost::format("%9.3f ms") % avg
                << boost::format(" (+-%7.3f ms)") % standard_deviation << boost::format(" %5.2f Gb/s") % throughput
                << endl;
    }
}

template <typename T, typename V>
bool is_equal(T & a, V & b) {
    int n = a.get_n();
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
            for (int k = 0; k < n; k++) {
                if (std::fabs(b(i, j, k).x - a(i, j, k).x) > 1e-6) return false;
                if (std::fabs(b(i, j, k).y - a(i, j, k).y) > 1e-6) return false;
                if (std::fabs(b(i, j, k).z - a(i, j, k).z) > 1e-6) return false;
            }
        }
    }
    return true;
}

template <typename T>
void print_array(const T& a) {
    int n = a.get_n();
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
            for (int k = 0; k < n; k++) {
                cout << boost::format("[%6.2f,%6.2f,%6.2f]") % a(i, j, k).x % a(i, j, k).y % a(i, j, k).z;
            }
            cout << endl;
        }
        cout << endl;
    }
}

void run_checks(int size) {
    cout << "Checking size " << size << "..." << endl;

    // vector3d checks

    morton_array<vector3d> e1(size);
    morton_array<vector3d> e2(size);
    morton_array<vector3d> b1(size);
    morton_array<vector3d> b2(size);

    randomize(e1);
    e2 = e1;
    randomize(b1);
    b2 = b1;

    advance_b(e1, b1);
    advance_b_z(e2, b2);

    if (!is_equal(b1, b2)) cout << "advance_b_z is different" << endl;

    randomize(e1);
    e2 = e1;
    randomize(b1);
    b2 = b1;

    advance_e(e1, b1);
    advance_e_z(e2, b2);

    if (!is_equal(e1, e2)) cout << "advance_e_z is different" << endl;

    randomize(e1);
    e2 = e1;
    randomize(b1);
    b2 = b1;

    advance(e1, b1);
    advance_z(e2, b2);

    if (!is_equal(e1, e2) || !is_equal(b1, b2)) cout << "advance_z is different" << endl;

    randomize(e1);
    e2 = e1;
    randomize(b1);
    b2 = b1;

    advance_b_periodic(e1, b1);
    advance_b_z_periodic(e2, b2);

    if (!is_equal(b1, b2)) cout << "advance_b_z_periodic is different" << endl;

    randomize(e1);
    e2 = e1;
    randomize(b1);
    b2 = b1;

    advance_e_periodic(e1, b1);
    advance_e_z_periodic(e2, b2);

    if (!is_equal(e1, e2)) cout << "advance_e_z_periodic is different" << endl;

    randomize(e1);
    e2 = e1;
    randomize(b1);
    b2 = b1;

    advance_periodic(e1, b1);
    advance_z_periodic(e2, b2);

    if (!is_equal(e1, e2) || !is_equal(b1, b2)) cout << "advance_z_periodic is different" << endl;
}

int main(int argc, char **argv) {
    dt = 0.01;
    double dx = 0.05;
    double dy = 0.05;
    double dz = 0.05;

    dtdx = dt / dx;
    dtdy = dt / dy;
    dtdz = dt / dz;

    by = 1 - kappa * dx * dx / (dy * dy);
    bz = 1 - kappa * dx * dx / (dz * dz);
    ay = 0.5 * (1 - by);
    az = 0.5 * (1 - bz);

    MPI_Init(&argc, &argv);
    MPI_Comm_size(MPI_COMM_WORLD, &mpi_size);
    MPI_Comm_rank(MPI_COMM_WORLD, &mpi_rank);

    re.seed(chrono::system_clock::now().time_since_epoch().count());

    const int iterations = 6;

    vector<int> sizes = {32, 64, 128, 256, 512};

    if (argc > 1) {
        sizes.erase(sizes.begin(), sizes.end());
        for (int i = 1; i < argc; i++) {
            sizes.push_back(atoi(argv[i]));
        }
    }

    if (mpi_rank == 0) {
        cout << "Sizes:";
        for (auto size : sizes) {
            cout << " " << size;
        }
        cout << endl;
    }

    if (mpi_rank == 0) {
        for (auto size : sizes) {
            run_checks(size);
        }
    }

    for (auto size : sizes) {
        if (mpi_rank == 0) {
            cout << "Size " << size << endl;
            cout << endl;
        }

        run_test<simple_array<vector3d>>(advance_b<simple_array<vector3d>>, "simple(b)", size, iterations);
        run_test<cached_array<vector3d>>(advance_b<cached_array<vector3d>>, "cached(b)", size, iterations);
        run_test<morton_array<vector3d>>(advance_b<morton_array<vector3d>>, "morton_bad(b)", size, iterations);
        run_test<morton_array<vector3d>>(advance_b_z, "morton(b)", size, iterations);

        run_test<simple_array<vector3d>>(advance_e<simple_array<vector3d>>, "simple(e)", size, iterations);
        run_test<cached_array<vector3d>>(advance_e<cached_array<vector3d>>, "cached(e)", size, iterations);
        run_test<morton_array<vector3d>>(advance_e<morton_array<vector3d>>, "morton_bad(e)", size, iterations);
        run_test<morton_array<vector3d>>(advance_e_z, "morton(e)", size, iterations);

        run_test<simple_array<vector3d>>(advance<simple_array<vector3d>>, "simple", size, iterations);
        run_test<cached_array<vector3d>>(advance<cached_array<vector3d>>, "cached", size, iterations);
        run_test<morton_array<vector3d>>(advance<morton_array<vector3d>>, "morton_bad", size, iterations);
        run_test<morton_array<vector3d>>(advance_z, "morton", size, iterations);

        run_test<simple_array<vector3d>>(advance_periodic<simple_array<vector3d>>, "simple_p", size, iterations);
        run_test<cached_array<vector3d>>(advance_periodic<cached_array<vector3d>>, "cached_p", size, iterations);
        run_test<morton_array<vector3d>>(advance_periodic<morton_array<vector3d>>, "morton_bad_p", size, iterations);
        run_test<morton_array<vector3d>>(advance_z_periodic, "morton_p", size, iterations);

    }

    MPI_Finalize();
    return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 complex.operator toolbox - arg/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.operator components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 18/02/2011
///
#include <nt2/arithmetic/include/functions/arg.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/include/functions/atan2.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/constant/constant.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/complex/imaginary.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/sdk/complex/meta/as_imaginary.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>

NT2_TEST_CASE_TPL ( arg_real__2_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef std::complex<T>                         cT;
  typedef native<T ,ext_t>                        vT;
  typedef native<cT ,ext_t>                      vcT;
  typedef typename nt2::meta::as_imaginary<T>::type   ciT;
  typedef native<ciT ,ext_t>                         vciT;
  typedef typename nt2::meta::as_dry<T>::type          dT;
  typedef native<dT ,ext_t>                           vdT;

  // specific values tests
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Inf<vT>(), nt2::Zero<vT>())), nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Minf<vT>(), nt2::Zero<vT>())),nt2::Pi<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Nan<vT>(), nt2::Zero<vT>())), nt2::Nan<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::One<vT>(), nt2::Zero<vT>())), nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Zero<vT>(), nt2::Zero<vT>())),nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Zero<vT>(), nt2::One<vT>())),  nt2::Pio_2<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::One<vT>(), nt2::Zero<vT>())),  nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Two<vT>(), nt2::Two<vT>())),   nt2::Pio_2<vT>()/nt2::Two<vT>());
  NT2_TEST_EQUAL(nt2::arg(vcT(nt2::Two<vT>(),nt2::Mtwo<vT>())),  -nt2::Pio_2<vT>()/nt2::Two<vT>());

  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Inf<vciT>()))  , nt2::Pio_2<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Minf<vciT>())) , -nt2::Pio_2<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Nan<vciT>()))  , nt2::Nan<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::One<vciT>()))  , nt2::Pio_2<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Zero<vciT>())) , nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Two<vciT>()))  , nt2::Pio_2<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Mone<vciT>()))  , -nt2::Pio_2<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Mzero<vciT>())) , -nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vciT(nt2::Mtwo<vciT>()))  , -nt2::Pio_2<vT>());

  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Inf<vdT>()))  , nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Minf<vdT>())) , nt2::Pi<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Nan<vdT>()))  , nt2::Nan<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::One<vdT>()))  , nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Zero<vdT>())) , nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Two<vdT>()))  , nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Mone<vdT>())) ,nt2::Pi<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Mzero<vdT>())),nt2::Zero<vT>());
  NT2_TEST_EQUAL(nt2::arg(vdT(nt2::Mtwo<vdT>())) ,nt2::Pi<vT>());

} // end of test for floating_


#include <math.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include "autoware_msgs/DetectedObject.h"
#include <autoware_msgs/DetectedObjectArray.h>
#include "zr_msgs/object_info.h"
#include "autoware_msgs/PointsImage.h"
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sensor_msgs/CameraInfo.h>
#include <string>
#include <cstdlib>
#include <vector>
#include <algorithm>
#include <boost/shared_ptr.hpp>
#include <deque>
#include <std_msgs/Int32.h>

ros::Publisher vel_L_R_pub;
ros::Publisher change_follow_flag;
static float vel_L = 0.5;
static float vel_R = 0.5; // 定义全局左右轮速度，直线行驶时使用
static float vel_turn_L = 0.3;
static float vel_turn_R = 0.3; // 定义转向时左右轮的速度
static float robot_radius = 0.3;
static float vel_diff_turn = 0.03; // 定义左右转速度的改变量的大小， 大小还需要调整
static float vel_diff_line = 0.05; //定义直行时左右轮速度改变量的大小
static float thre_off = 0.0;       //判断左右转的偏移量
float linear;
float angle;
static float thre_dist = 4.0; //定义阈值（跟踪范围）
float X;
float Y;                       //X and Y are the coordinates of the cloest person to the Camera.
float index_bear;              //用于存放检测出特殊标签的指定索引
std::deque<float> data_leader; //用于存放上一时刻leader的空间数据信息
std_msgs::Int32 follow_flag;   //1 代表跟随， 0 代表不跟随

std_msgs::Int32 change_flag;
void follow_vel_control(float &dist_person_min)
{
    float follow_range = 1.5;
    //   float vel_L = 0.3;
    //  float vel_R = 0.3;
    float y = Y;
    float x = X;

    geometry_msgs::Twist twist_follow;
    if (dist_person_min >= thre_dist) // thre_dist = 4
    {
        vel_L = 0.0;
        vel_R = 0.0;
    }
    else
    {
        //case go straight
        if (fabs(x) <= robot_radius + thre_off) //在竖直方向判断
        {
            ROS_INFO("now the robot and the leader are in the same line and the distance is in the good value!");
            if (dist_person_min > (follow_range + 0.2) && dist_person_min < thre_dist)
            {
                ROS_INFO("the leader is far with robot, the robot will speed up to make the distance smaller!!!");
                //vel_L = vel_L * (dist_person_min / follow_range) * 1.1; //这里加一个反馈控制，距离越远，车的速度越大
                // vel_R = vel_R * (dist_person_min / follow_range) * 1.1;
                vel_L += vel_diff_line * (dist_person_min / (follow_range + 0.2));
                vel_R += vel_diff_line * (dist_person_min / (follow_range + 0.2));
            }
            else
            {
                if (dist_person_min <= (follow_range + 0.2) && dist_person_min >= (follow_range - 0.2)) //定义跟随范围为1.3---1.7
                {
                    ROS_INFO("the distance betwwen the robot and leader is in good value");
                    vel_L = 0.5;
                    vel_R = 0.5; //合理范围内匀速行驶
                }
                else //situation: (dist_person_min < (follow_range - 0.2))
                {
                    if (dist_person_min < (follow_range - 0.2) && dist_person_min > 0.9)
                    {
                        ROS_INFO("the distance now is too small, please be careful!!!");
                        //  vel_R = vel_R * (dist_person_min / (follow_range + 0.5));
                        // vel_L = vel_L * (dist_person_min / (follow_range + 0.5)); //避障的优先级会高于跟随算法，若距离太近，会启动避障。
                        vel_L -= vel_diff_line * ((follow_range - 0.2) / dist_person_min);
                        vel_R -= vel_diff_line * ((follow_range - 0.2) / dist_person_min);
                    }
                    else //situation : dist_person_min <= 0.9
                    {
                        vel_L = 0;
                        vel_R = 0;
                    }
                }
            }
            std::cout << "vel_L" << vel_L << std::endl;
            std::cout << "vel_R" << vel_R << std::endl;
            linear = (vel_L + vel_R) / 2;
            angle = (vel_R - vel_L) / 0.54;
            twist_follow.linear.x = linear;
            twist_follow.angular.z = angle;
            vel_L_R_pub.publish(twist_follow);
            vel_turn_L = 0.3;
            vel_turn_R = 0.3;
        }
        else
        {
            if (x < (-robot_radius - thre_off)) //判断人是否左偏， turn left
            {
                ROS_INFO("now the leader is on the robot's left, the robot will turn left!");
                vel_turn_L -= vel_diff_turn * (fabs(x) / robot_radius); //这里diff/fabs(x)为左偏程度， 人左偏的越厉害，轮子速度改变量越大，角速度也越大
                vel_turn_R += vel_diff_turn * (fabs(x) / robot_radius); //当机器人左右转向速度过快或过慢时，可调节vel_diff_turn的值的大小
                if (!(vel_L > 0))
                {
                    vel_L = vel_R / 2;
                }
            }
            else //(x > robot_radius + 0.05) //判断人是否偏右， 偏右则右转
            {
                ROS_INFO("now the leader is on the robot's right");
                float diff = fabs(robot_radius + thre_off);
                vel_turn_L += vel_diff_turn * (fabs(x) / robot_radius);
                vel_turn_R -= vel_diff_turn * (fabs(x) / robot_radius);
                //这里diff/fabs(x)为右偏程度， 人右偏的越厉害，轮子速度改变量越大，角速度也越大

                if (!(vel_R > 0))
                {
                    vel_R = vel_L / 2;
                }
            }
        }

        linear = (vel_turn_L + vel_turn_R) / 2;
        angle = (vel_turn_R - vel_turn_L) / 0.54;
        twist_follow.linear.x = linear;
        twist_follow.angular.z = angle;
        vel_L_R_pub.publish(twist_follow);
    }
}
//如何设计让bool只判断第一帧或前几帧，而不是一直处于循环中，可以设置flag，例如前几帧flag为0， 之后flag为1
bool label_choose(std::vector<std::string> &label_raw) //bool 函数来判断传入的label里面是否含有指定标签
{
    int m = label_raw.size();
    //可以直接把j存下来，放在person_select_follow里面调用x y distance数据。
    //第一帧检测到bear时发送topic，播放语音“已连接”
    for (int j = 0; j < m; j++)
    {
        if (label_raw[j] == "bear")
        {
            index_bear = j;
            return true;
        }
    }
    return false;
}
void Person_select_follow(std::vector<float> &distance_raw, std::vector<std::string> &label_raw, std::vector<float> &x_raw, std::vector<float> &y_raw)
{
    int n = distance_raw.size();
    std::vector<std::string> label; // label of person
    std::vector<float> x;
    std::vector<float> y;
    std::vector<float> distance;
    // 此段代码为 当leader的背上带有另一个明显的标签时，筛选出标签，达到选中行人的目的
    //   若某些帧未检测出此标签 或者leader转向时由于角度原因无法正确探测背部label时，则进入最近距离筛选阶段
    if (label_choose(label_raw))
    {
        int j;
        j = index_bear;
        //等待label_choose的j数据by姜
        float distance_selected = distance_raw[j];
        X = x_raw[j];
        Y = y_raw[j]; //取出背部标签对应的空间坐标数据，其中的distance即为leader的距离
                      // std::vector<float> distance;
        data_leader.push_back(X);
        data_leader.push_back(Y);
        data_leader.push_back(distance_selected);
        while (data_leader.size() > 3)
        {
            data_leader.pop_front();
        }
        //while的作用用于总是保留上一帧的元素，而不保留其他帧元素
        if (distance_selected > 3.0)
        {
            return;
        }
        follow_vel_control(distance_selected);
    }
    else
    {
        for (int i = 0; i < n; i++)
        {
            if (label_raw[i] == "person")
            {
                distance.push_back(distance_raw[i]);
                label.push_back(label_raw[i]);
                x.push_back(x_raw[i]);
                y.push_back(y_raw[i]);
            }
        }
        if (distance.empty())
        {
            ROS_INFO("There is no leader before the robot, please make sure there is one leader");
            return;
        }
        else
        {
            ROS_INFO("now begin to find the cloest person!!");
            float dist_person_min = 99;
            float min_index = -1;
            for (int i = 0; i < num; i++)
            {
                if (dist_person_min > distance[i])
                {
                    dist_person_min = distance[i];
                    min_index = i;
                }
                else
                {
                    continue;
                }
            }
            // std::cout << "distance = " << dist_person_min << std::endl;
            X = x[min_index];
            Y = y[min_index];
            if (dist_person_min > thre_dist)
            {
                return; //可能由于某一帧距离筛选错误，所以返回下一帧继续筛选
            }
            if (data_leader.empty()) //对于第一帧来考虑， 之后的每一帧与此无关
            {
                data_leader.push_back(X);
                data_leader.push_back(Y);
                data_leader.push_back(dist_person_min);
            }
            float thre1 = fabs(dist_person_min - data_leader[2]); //data_leader为上一帧leade的空间数据，如果未检测出背上标签
            //而是选出离机器人最近距离的行人，则需要比较当前帧与上一帧的数据差异，若差异小于阈值，则认为选择正确，若大于阈值，则再次进行筛选
            float thre2 = fabs(X - data_leader[0]);
            //  float thre3 = fabd(Y - data_leader[1]);
            if (thre1 <= 0.3 && thre2 <= 0.2)
            {
                ROS_INFO("Now we select the leader correctly!!!");
                data_leader.push_back(X);
                data_leader.push_back(Y);
                data_leader.push_back(dist_person_min); //若特殊标签某一帧或某几帧未检测成功，但通过行人检测出的与上一帧差别不大的话
                //则选中此为leader的空间信息
                while (data_leader.size() > 3)
                {
                    data_leader.pop_front();
                }
                std::cout << "situation 1 : x in the space is : " << X << std::endl;
                std::cout << "situation 1 : y in the space is : " << Y << std::endl;
                std::cout << "situation 1 : distance in the space is : " << dist_person_min << std::endl;
            }
            else
            {
                ROS_INFO("now we didn't select the leader correctly, so we will choose the real leader!!! ");
                for (int i = 0; i < num; i++)
                {
                    if (fabs(data_leader[2] - distance[i]) <= 0.3 && fabs(data_leader[0] - x[i]) <= 0.2)
                    {
                        dist_person_min = distance[i];
                        X = x[i];
                        Y = y[i];
                        data_leader.push_back(X);
                        data_leader.push_back(Y);
                        data_leader.push_back(dist_person_min);
                        break;
                    }
                }
                while (data_leader.size() > 3)
                {
                    data_leader.pop_front();
                }
                std::cout << "situation 2 : x in the space is : " << X << std::endl;
                std::cout << "situation 2 : y in the space is : " << Y << std::endl;
                std::cout << "situation 2 : distance in the space is : " << dist_person_min << std::endl;
            }
            follow_vel_control(dist_person_min);
        }
        // follow_vel_control(dist_person_min);
    }
    // follow_vel_control(data_leader[2]);
}
//by jie get the disdtance of all labels
static void StartFlagCallback(const std_msgs::Int32 &msg)
{
    follow_flag.data = msg.data;
}
static void DistanceCallback(const zr_msgs::object_info &result)
{
    std::vector<float> distance_raw;    // all objects (people, car ...)
    std::vector<std::string> label_raw; //all labels
    std::vector<float> x_raw;
    std::vector<float> y_raw;
    if (result.distance.empty())
    {
        ROS_ERROR("this frame did not detect anything, it will enter into next frame!!!!!");
        return;
    }
    distance_raw = result.distance;
    label_raw = result.label;
    x_raw = result.x;
    y_raw = result.y;
    if (follow_flag.data == 0)
    {
        if (label_choose(label_raw))
        {
            ROS_INFO("now we have detected the special label, you can set flag to start following!!!"); //发送语音,  这里需要考虑通过什么样的方式可以更改flag的值
            change_flag.data = 3;
            change_follow_flag.publish(change_flag.data);
        }
        else
        {
            ROS_INFO("now we didn't detected any special label, please make sure you have the correct posture!!! you can also set the flag to 1");
            //这里如果没有检测到特殊标签，执行第二种方案，即第一帧时以选中距离最近的人为依据
            change_flag.data = 4;
            change_follow_flag.publish(change_flag.data);
        }
        return;
    }
    else
    {
        ROS_INFO("now we can start following!!!!");
        Person_select_follow(distance_raw, label_raw, x_raw, y_raw);
    }
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "follower_node_1");
    // Create node handles
    ros::NodeHandle nh;

    follow_flag.data = 1;

    vel_L_R_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    change_follow_flag = nh.advertise<std_msgs::Int32>("change_follow_flag", 10);
    // Subscribe to the following topics:
    ros::Subscriber distance_subscriber = nh.subscribe("/obj_detect_distance", 1, DistanceCallback);
    ros::Subscriber follow_flag_subscribe = nh.subscribe("start_flag", 1, StartFlagCallback);
    ros::Rate loop_rate(25);
    while (ros::ok())
    {

        ros::spinOnce();
        loop_rate.sleep();
    }
    //ros::spin();
    return 0;
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory for Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include <Utils/Math/BSplines/BSpline.h>
#include <Utils/Math/BSplines/ControlPolygonGenerator.h>
#include <gmock/gmock.h>
#include <Eigen/Core>

using namespace testing;
namespace Scine {
namespace Utils {
using namespace BSplines;
namespace Tests {

class ABSplineCorrectnessTest : public Test {
 public:
  unsigned p;
  BSpline bs;

  void SetUp() override {
    p = 3;
    Eigen::MatrixXd data(5, 2);
    data.row(0) = Eigen::Vector2d(1, 1);
    data.row(1) = Eigen::Vector2d(1.5, -0.5);
    data.row(2) = Eigen::Vector2d(3, 1);
    data.row(3) = Eigen::Vector2d(4.5, 0.5);
    data.row(4) = Eigen::Vector2d(5, -1);

    ControlPolygonGenerator bsGenerator(data, p, true);
    bs = bsGenerator.generateBSpline();
  }
};

TEST_F(ABSplineCorrectnessTest, CorrectDerivatives0) {
  Eigen::Vector2d ref(2);

  ref = Eigen::Vector2d(1., 1.);
  ASSERT_TRUE(bs.evaluate(0.0, 0).isApprox(ref));
  ref = Eigen::Vector2d(2.552, 0.312);
  ASSERT_TRUE(bs.evaluate(0.4, 0).isApprox(ref));
  ref = Eigen::Vector2d(3.448, 0.584);
  ASSERT_TRUE(bs.evaluate(0.6, 0).isApprox(ref));
  ref = Eigen::Vector2d(5., -1.);
  ASSERT_TRUE(bs.evaluate(1.0, 0).isApprox(ref));
}

TEST_F(ABSplineCorrectnessTest, CorrectDerivatives1) {
  Eigen::Vector2d ref(2);

  ref = Eigen::Vector2d(3., -9.);
  ASSERT_TRUE(bs.evaluate(0.0, 1).isApprox(ref));
  ref = Eigen::Vector2d(4.44, 2.04);
  ASSERT_TRUE(bs.evaluate(0.4, 1).isApprox(ref));
  ref = Eigen::Vector2d(4.44, 0.12);
  ASSERT_TRUE(bs.evaluate(0.6, 1).isApprox(ref));
  ref = Eigen::Vector2d(3., -9.);
  ASSERT_TRUE(bs.evaluate(1.0, 1).isApprox(ref));
}

TEST_F(ABSplineCorrectnessTest, CorrectDerivatives2) {
  Eigen::Vector2d ref(2);

  ref = Eigen::Vector2d(6., 54.);
  ASSERT_TRUE(bs.evaluate(0.0, 2).isApprox(ref));
  ref = Eigen::Vector2d(1.2, 1.2);
  ASSERT_TRUE(bs.evaluate(0.4, 2).isApprox(ref));
  ref = Eigen::Vector2d(-1.2, -15.6);
  ASSERT_TRUE(bs.evaluate(0.6, 2).isApprox(ref));
  ref = Eigen::Vector2d(-6., -30.);
  ASSERT_TRUE(bs.evaluate(1.0, 2).isApprox(ref));
}

TEST_F(ABSplineCorrectnessTest, CorrectDerivatives3) {
  Eigen::Vector2d ref(2);

  ref = Eigen::Vector2d(-12., -132.);
  ASSERT_TRUE(bs.evaluate(0.0, 3).isApprox(ref));
  ref = Eigen::Vector2d(-12., -132.);
  ASSERT_TRUE(bs.evaluate(0.4, 3).isApprox(ref));
  ref = Eigen::Vector2d(-12., -36.);
  ASSERT_TRUE(bs.evaluate(0.6, 3).isApprox(ref));
  ref = Eigen::Vector2d(-12., -36.);
  ASSERT_TRUE(bs.evaluate(1.0, 3).isApprox(ref));
}

TEST_F(ABSplineCorrectnessTest, CorrectDerivatives4) {
  Eigen::Vector2d ref(2);

  ref = Eigen::Vector2d(0., 0.);
  ASSERT_TRUE(bs.evaluate(0.0, 4).isApprox(ref));
  ref = Eigen::Vector2d(0., 0.);
  ASSERT_TRUE(bs.evaluate(0.4, 4).isApprox(ref));
  ref = Eigen::Vector2d(0., 0.);
  ASSERT_TRUE(bs.evaluate(0.6, 4).isApprox(ref));
  ref = Eigen::Vector2d(0., 0.);
  ASSERT_TRUE(bs.evaluate(1.0, 4).isApprox(ref));
}

TEST_F(ABSplineCorrectnessTest, CorrectDerivatives5) {
  Eigen::Vector2d ref(2);

  ref = Eigen::Vector2d(0., 0.);
  ASSERT_TRUE(bs.evaluate(0.0, 5).isApprox(ref));
  ref = Eigen::Vector2d(0., 0.);
  ASSERT_TRUE(bs.evaluate(0.4, 5).isApprox(ref));
  ref = Eigen::Vector2d(0., 0.);
  ASSERT_TRUE(bs.evaluate(0.6, 5).isApprox(ref));
  ref = Eigen::Vector2d(0., 0.);
  ASSERT_TRUE(bs.evaluate(1.0, 5).isApprox(ref));
}

TEST_F(ABSplineCorrectnessTest, CompareEvaluationMethods) {
  for (unsigned k = 0; k < p; ++k) {
    ASSERT_TRUE(bs.evaluateNaive(0.0, k).isApprox(bs.evaluate(0.0, k)));
  }
  for (unsigned k = 0; k < p; ++k) {
    ASSERT_TRUE(bs.evaluateNaive(1 / 3.0, k).isApprox(bs.evaluate(1 / 3.0, k)));
  }
  for (unsigned k = 0; k < p; ++k) {
    ASSERT_TRUE(bs.evaluateNaive(0.5, k).isApprox(bs.evaluate(0.5, k)));
  }
  for (unsigned k = 0; k < p; ++k) {
    ASSERT_TRUE(bs.evaluateNaive(0.6, k).isApprox(bs.evaluate(0.6, k)));
  }
  for (unsigned k = 0; k < p; ++k) {
    ASSERT_TRUE(bs.evaluateNaive(1.0, k).isApprox(bs.evaluate(1.0, k)));
  }
}

} // namespace Tests
} // namespace Utils
} // namespace Scine


#include <frovedis.hpp>
#include <frovedis/ml/neighbors/knn_supervised.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;

template <class T>
void pretty_print(rowmajor_matrix_local<T>& mat) {
  auto nrow = mat.local_num_row;
  auto ncol = mat.local_num_col;
  for(size_t i = 0; i < nrow; ++i) {
    for(size_t j = 0; j < ncol; ++j) {
      std::cout << mat.val[i * ncol + j] << " ";
    }
    std::cout << std::endl;
  }
}

int main(int argc, char** argv) {
  use_frovedis use(argc,argv);
  using namespace boost::program_options;

  options_description opt("option");
  opt.add_options()
    ("help,h", "print help")
    ("input,i", value<std::string>(), "path for input dense feature data")
    ("label,l", value<std::string>(), "path for input label")
    ("target,t", value<std::string>(), "target: classification or regression")
    ("neighbors,k", value<int>(), "number of required neighbors (default: 2)")
    ("metric,m", value<std::string>(), "metric for distance calculation (default: euclidean)")
    ("algorithm,a", value<std::string>(), "algorithm for knn (default: brute)")
    ("save_proba,p", value<int>(), "whether to save probability_matrix (1/0) (default: 0)")
    //("need_distance,d", value<int>(), "need distance as output (1/0) (default: 1)")
    ("chunk_size,s", value<float>(), "chunk size in MB for controlling runtime memory requirement (default: 1.0)");

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().
        run(), argmap);
  notify(argmap);

  std::string input, label, target;
  int k = 2;
  float chunk_size = 1.0;
  std::string metric = "euclidean";
  std::string algorithm = "brute";
  bool save_proba = false;
  //bool need_distance = true;

  if(argmap.count("help")){
    std::cerr << opt << std::endl;
    exit(1);
  }

  if(argmap.count("input")){
    input = argmap["input"].as<std::string>();
  } else {
    std::cerr << "input feature data path is not specified!" << std::endl;
    std::cerr << opt << std::endl;
    exit(1);
  }
  auto data = make_rowmajor_matrix_load<double>(input);

  if(argmap.count("label")){
    label = argmap["label"].as<std::string>();
  } else {
    std::cerr << "input label data path is not specified!" << std::endl;
    std::cerr << opt << std::endl;
    exit(1);
  }
  auto lbl = make_dvector_loadline<double>(label);

  if(argmap.count("target")){
    target = argmap["target"].as<std::string>();
  } else {
    std::cerr << "target: classification/regression is not specified!" << std::endl;
    std::cerr << opt << std::endl;
    exit(1);
  }

  if(argmap.count("neighbors")){
    k = argmap["neighbors"].as<int>();
  }

  if(argmap.count("metric")){
    metric = argmap["metric"].as<std::string>();
  }

  if(argmap.count("algorithm")){
    algorithm = argmap["algorithm"].as<std::string>();
  }

  if(argmap.count("save_proba")){
    save_proba = argmap["save_proba"].as<int>() == 1;
  }

/*
  if(argmap.count("need_distance")){
    need_distance = argmap["need_distance"].as<int>() == 1;
  }
*/

  if(argmap.count("chunk_size")){
    chunk_size = argmap["chunk_size"].as<float>();
  }

  time_spent calc_knn(INFO);
  if (target == "classification") {
    calc_knn.lap_start();
    kneighbors_classifier<double, rowmajor_matrix<double>> obj(k, algorithm, metric, chunk_size);
    obj.fit(data, lbl);
    auto pred = obj.predict(data, save_proba);
    calc_knn.lap_stop();
    calc_knn.show_lap("total knn fit-predict time: ");
    std::cout << "prediction: "; debug_print_vector(pred.gather());
    std::cout << "score: " << obj.score(data, lbl) << std::endl;
  }
  else if (target == "regression") {
    calc_knn.lap_start();
    kneighbors_regressor<double, rowmajor_matrix<double>> obj(k, algorithm, metric, chunk_size);
    obj.fit(data, lbl);
    auto pred = obj.predict(data);
    calc_knn.lap_stop();
    calc_knn.show_lap("total knn fit-predict time: ");
    std::cout << "prediction: "; debug_print_vector(pred.gather());
    std::cout << "score: " << obj.score(data, lbl) << std::endl;
  }
  else {
    std::cerr << "Unknown target: " << target << " for Supervised KNN!\n";
    std::cerr << opt << std::endl;
    exit(1);
  }

  return 0;
}