#include <string>
#include <iostream>
#include <iomanip>
#include <limits>
#include <unordered_map>
#include <boost/regex.hpp>
#include <boost/algorithm/string.hpp>
using namespace std;

typedef unsigned long long currency_value_type;

const string kWhitespacesPattern = "\\s+";
const string kOptionalWhitespacesPattern = "\\s*";
const string kCurrencyValuePattern = "\\b((?:[1-9][0-9]*)|0)(?:,([0-9]{1,3}))?\\b";
const string kCurrencyISOCodePattern = "([A-Z]{3})";
const string kDonorNamePattern = "((?:\\S)|(?:\\S.*\\S))";
const string kCurrencyExchangeRatePattern =
    kOptionalWhitespacesPattern +
    kCurrencyISOCodePattern +
    kWhitespacesPattern +
    kCurrencyValuePattern +
    kOptionalWhitespacesPattern;
const string kDonorLinePattern =
    kOptionalWhitespacesPattern +
    kDonorNamePattern +
    kWhitespacesPattern +
    kCurrencyValuePattern +
    kWhitespacesPattern +
    kCurrencyISOCodePattern +
    kOptionalWhitespacesPattern;
const string kQueryPattern =
    kOptionalWhitespacesPattern +
    kCurrencyValuePattern +
    kWhitespacesPattern +
    kCurrencyValuePattern +
    kOptionalWhitespacesPattern;

const currency_value_type kCurrencyMultiplyFactor = 1000;

enum class ReadingState {
  kCurrencyExchangeValues,
  kDonors,
  kQueries
};

typedef tuple<string, currency_value_type> currency_exchange_type;
typedef tuple<string, currency_value_type, string> donor_type;
typedef tuple<currency_value_type, currency_value_type> query_type;
typedef unordered_map<string, currency_value_type> exchange_rate_map_type;

bool MatchRegex(const string& sequence, boost::smatch& match_data, const string& pattern) {
  boost::regex compiledPattern(pattern);
  return boost::regex_match(sequence, match_data, compiledPattern);
}

currency_value_type ToCurrencyValue(const string& beforeComma, string afterComma) {
  while (afterComma.size() < 3)
    afterComma += '0';

  stringstream stream(beforeComma + afterComma);
  currency_value_type result;
  stream >> result;
  if (stream.fail())
    throw invalid_argument("ToCurrencyValue: invalid argument");
  return result;
}

bool TryParseCurrencyExchangeRate(const string &line, currency_exchange_type& result) {
  boost::smatch matches;
  bool matched = MatchRegex(line, matches, kCurrencyExchangeRatePattern);

  if (!matched)
    return false;

  try {
    get<0>(result) = matches[1];
    get<1>(result) = ToCurrencyValue(matches[2], matches[3]);
    return true;
  }
  catch (const invalid_argument& e) {
    return false;
  }
}

bool TryParseDonor(const string &line, donor_type& result) {
  boost::smatch matches;
  bool matched = MatchRegex(line, matches, kDonorLinePattern);

  if (!matched)
    return false;

  try {
    get<0>(result) = matches[1];
    get<1>(result) = ToCurrencyValue(matches[2], matches[3]);
    get<2>(result) = matches[4];
    return true;
  }
  catch (const invalid_argument& e) {
    return false;
  }
}

bool TryParseQuery(const string &line, query_type& result) {
  boost::smatch matches;
  bool matched = MatchRegex(line, matches, kQueryPattern);

  if (!matched)
    return false;

  try {
    get<0>(result) = ToCurrencyValue(matches[1], matches[2]);
    get<1>(result) = ToCurrencyValue(matches[3], matches[4]);
    return true;
  }
  catch (const invalid_argument& e) {
    return false;
  }
}

template<class T1, class T2>
ostream& operator<<(ostream& stream, const tuple<T1, T2>& t) {
  stream << "(" << get<0>(t) << ", " << get<1>(t) << ")";
  return stream;
}

template<class T1, class T2, class T3>
ostream& operator<<(ostream& stream, const tuple<T1, T2, T3>& t) {
  stream << "(" << get<0>(t) << ", " << get<1>(t) << ", " << get<2>(t) << ")";
  return stream;
}

currency_value_type BankersRounding(currency_value_type value, currency_value_type power) {
  currency_value_type result = value / (power / 10);
  currency_value_type lastDigit = result % 10;
  result /= 10;
  currency_value_type penultimateDigit = result % 10;
  return (lastDigit == 5 && penultimateDigit % 2) || lastDigit > 5 ? result + 1 : result;
}

currency_value_type StableMultiply(currency_value_type a, currency_value_type b, currency_value_type power) {
  currency_value_type tail1 = a % power;
  currency_value_type tail2 = b % power;
  currency_value_type head1 = a / power;
  currency_value_type head2 = b / power;
  currency_value_type rest = BankersRounding((tail1 * head2 +
                                             tail2 * head1) * power +
                                             tail1 * tail2, power);
  return head1 * head2 * power + rest;
}

currency_value_type CalculateCurrencyValue(const donor_type& donor,
                                           const exchange_rate_map_type&
                                           currency_exchange_rates) {
  currency_value_type exchange_value = currency_exchange_rates.find(get<2>(donor))->second;
  return StableMultiply(exchange_value,
                        get<1>(donor),
                        kCurrencyMultiplyFactor);
}

template<class Iterator>
pair<Iterator, Iterator> CalculateQueryRange(Iterator begin,
                                             Iterator end,
                                             const query_type& query,
                                             const exchange_rate_map_type &currency_exchange_rates) {
  Iterator result_begin = lower_bound(begin,
                           end,
                           get<0>(query),
                           [&currency_exchange_rates](const donor_type &a, currency_value_type value) {
                             return CalculateCurrencyValue(a, currency_exchange_rates) < value;
                           });

  Iterator result_end = upper_bound(begin,
                         end,
                         get<1>(query),
                         [&currency_exchange_rates](currency_value_type value, const donor_type &b) {
                           return value < CalculateCurrencyValue(b, currency_exchange_rates);
                         });

  return make_pair(result_begin, result_end);
};

bool IsCurrencyValueInRange(currency_value_type value) {
    /* value <= (kCurrencyMultiplyFactor * Max_long + kCurrencyMultiplyFactor / 2 - 1) /
                (kCurrencyMultiplyFactor * Max_int + kCurrencyMultiplyFactor - 1) */
    currency_value_type temp = numeric_limits<unsigned int>::max();
    return value <=
        (numeric_limits<unsigned long long>::max()) /
        (temp + 1) + 1 && value > 0;
}

bool IsDonorValueInRange(donor_type donor) {
    currency_value_type temp = numeric_limits<unsigned int>::max();
    currency_value_type value = get<1>(donor);
    return value <= (temp + 1) * kCurrencyMultiplyFactor - 1 && value > 0;
}

bool IsDefinedCurrency(const string &currency_name,
                       const exchange_rate_map_type &currency_exchange_rates) {
    return currency_exchange_rates.find(currency_name) != currency_exchange_rates.end();
}

int main() {
  ios::sync_with_stdio(false);

  vector<donor_type> donors;
  unordered_map<string, currency_value_type> currency_exchange_rates;
  ReadingState reading_state = ReadingState::kCurrencyExchangeValues;

  string line;
  cout << boolalpha;
  size_t lineNumber = 1;

  auto donorsComparator = [&currency_exchange_rates](donor_type a, donor_type b){
    return CalculateCurrencyValue(a, currency_exchange_rates) <
           CalculateCurrencyValue(b, currency_exchange_rates);
  };

  while (getline(cin, line)) {
    currency_exchange_type currency_exchange;
    donor_type donor;
    query_type query;

    bool valid_line = false;
    bool sorted = false;

    if (TryParseCurrencyExchangeRate(line, currency_exchange)) {
      //cout << "CurrencyExchangeRate: " << currency_exchange << endl;

      bool valid_state = reading_state == ReadingState::kCurrencyExchangeValues;
      bool valid_input = IsCurrencyValueInRange(get<1>(currency_exchange)) &&
                        !IsDefinedCurrency(get<0>(currency_exchange), currency_exchange_rates);
      if (valid_state && valid_input) {
        valid_line = true;

        currency_exchange_rates.insert(make_pair(get<0>(currency_exchange), get<1>(currency_exchange)));
      }
    }
    else if (TryParseDonor(line, donor)) {
      //cout << "Donor: " << donor << endl;

      bool valid_state = reading_state == ReadingState::kDonors ||
                         reading_state == ReadingState::kCurrencyExchangeValues;
      bool valid_input = IsDefinedCurrency(get<2>(donor), currency_exchange_rates) &&
                         IsDonorValueInRange(donor);
      if (valid_state && valid_input) {
        valid_line = true;
        reading_state = ReadingState::kDonors;

        donors.push_back(donor);
      }
    }
    else if (TryParseQuery(line, query)) {
      //cout << "Query: " << query << endl;

      if(!sorted) {
        stable_sort(donors.begin(), donors.end(), donorsComparator);
        sorted = true;
      }

      bool valid_input = get<0>(query) <= get<1>(query);
      if (valid_input) {
        valid_line = true;
        reading_state = ReadingState::kQueries;

        valid_line = get<0>(query) <= get<1>(query);

        auto range = CalculateQueryRange(donors.begin(), donors.end(), query,
                                         currency_exchange_rates);
        for (auto it = range.first; it != range.second; ++it) {
          const string& name = get<0>(*it);
          currency_value_type value = get<1>(*it);
          const string& currency = get<2>(*it);
          cout << "\"" << name << "\",\"";
          cout << value / 1000  << "," << setfill('0') << setw (3) << value % 1000;
          cout << "\"," << currency << endl;
        }
      }
    }

    if (!valid_line) {
      cerr << "Error in line " << lineNumber << ":" << line << endl;
    }

    lineNumber++;
  }
}


//
//  chol_update_r1.c
//  dusthm-cpp
//
//  Created by Brandon Kelly on 4/30/14.
//  Copyright (c) 2014 Brandon C. Kelly. All rights reserved.
//

#include <stdio.h>
#include <cmath>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>

// Global random number generator and distributions for generating random numbers on the host. The random number generator used
// is the Mersenne Twister mt19937 from the BOOST library.
boost::random::mt19937 rng;
boost::random::normal_distribution<> snorm(0.0, 1.0); // Standard normal distribution
boost::random::uniform_real_distribution<> uniform(0.0, 1.0); // Uniform distribution from 0.0 to 1.0

void chol_update_r1(double* cholfactor, double* v, int dim_v, bool downdate) {

    double sign = 1.0;
	if (downdate) {
		// Perform the downdate instead
		sign = -1.0;
	}
    int diag_index = 0;  // index of the diagonal of the cholesky factor
	for (int i=0; i<dim_v; i++) {
        // loop over the columns of the Cholesky factor
        double L_ii = cholfactor[diag_index];
        double v_i = v[i];
        double r = sqrt( L_ii * L_ii + sign * v_i * v_i);
		double c = r / L_ii;
		double s = v_i / L_ii;
		cholfactor[diag_index] = r;
        int index_ji = diag_index; // index of the cholesky factor array that points to L[j,i]
        // update the rest of the rows of the Cholesky factor for this column
        for (int j=i+1; j<dim_v; j++) {
            // loop over the rows of the i^th column of the Cholesky factor
            index_ji += j;
            cholfactor[index_ji] = (cholfactor[index_ji] + sign * s * v[j]) / c;
        }
        // update the elements of the vector v[i+1:dim_v-1]
        index_ji = diag_index;
        for (int j=i+1; j<dim_v; j++) {
            index_ji += j;
            v[j] = c * v[j] - s * cholfactor[index_ji];
        }
        diag_index += i + 2;
    }
}


//  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

#include <type_traits>
#include "test.hpp"
#include <boost/multiprecision/mpc.hpp>
#include <boost/math/constants/constants.hpp>

using boost::multiprecision::mpc_complex_100;

template <class Complex>
void test_overloads()
{
   typedef typename Complex::value_type Real;
   Complex                              ya = {5.2, 7.4};
   Complex                              yb = {8.2, 7.3};
   Real                                 h  = 0.0001;
   auto                                 I0 = (ya + yb) * h;
   Complex                              I1 = I0 / 2 + yb * h;

   //I1 = I0;  // not supposed to work.

   Complex                      z{2, 3};
   typename Complex::value_type theta = 0.2;
   int                          n     = 2;
   using std::sin;
   Complex arg = z * sin(theta) - n * theta;

   using std::exp;
   Real    v    = 0.2;
   Real    cotv = 7.8;
   Real    cscv = 8.2;
   Complex den  = z + v * cscv * exp(-v * cotv);

   boost::multiprecision::number<boost::multiprecision::backends::mpc_complex_backend<100> > a = 2;
   boost::multiprecision::number<boost::multiprecision::backends::mpc_complex_backend<100> > b = 3;
   /*
  if (a <= b) {
    b = a;
  }*/
}

template <class F, class Real>
typename std::result_of_t<F(Real)> some_functional(F f, Real a, Real b)
{
   if (a <= -boost::math::tools::max_value<Real>())
   {
      return f(a);
   }

   return f(b);
}

template <class Complex>
void test_functional()
{
   typedef typename Complex::value_type Real;
   auto                                 f      = [](Real x) -> Complex { Complex z(x, 3); return z; };
   Real                                 a      = 0;
   Real                                 b      = 1;
   Complex                              result = some_functional(f, a, b);
}

int main()
{
   test_overloads<mpc_complex_100>();
   test_functional<mpc_complex_100>();
}


//////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::distributions::chi_squared::include.hpp //
//                                                                              //
//  (C) Copyright 2009 Erwann Rogard                                            //
//  Use, modification and distribution are subject to the                       //
//  Boost Software License, Version 1.0. (See accompanying file                 //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)            //
//////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_CHI_SQUARED_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_CHI_SQUARED_INCLUDE_HPP_ER_2009

#include <boost/math/distributions/chi_squared.hpp>

#include <boost/statistics/detail/distribution_toolkit/distributions/chi_squared/description.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/chi_squared/derivative_log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/chi_squared/is_math_distribution.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/chi_squared/log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/chi_squared/random.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/chi_squared/os.hpp>

#endif


#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>
#include <omp.h> // cxh
#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"
#include "caffe/util/sconv.hpp" // cxh
#include "caffe/util/cpu_info.hpp" // cxh

namespace caffe {

template<>
void caffe_cpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float* A, const float* B, const float beta,
    float* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template<>
void caffe_cpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const double alpha, const double* A, const double* B, const double beta,
    double* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

// cxh
template <>
void caffe_cpu_sparse_csrmm<float>(const int M, const int N, const int K,
		const float alpha,
		const float* A_nonzero_buf, const int* A_nonzero_idx_buf, const int* A_idx_pointerB_,const int* A_idx_pointerE_,
		const float* B,
		const float beta,float* C){
#ifdef USE_MKL
	const char *matdescra = "GXXCX";//6 bytes
	const char transa = 'N';
	mkl_scsrmm (&transa, &M , &N, &K,
			&alpha , matdescra,
			A_nonzero_buf, A_nonzero_idx_buf, A_idx_pointerB_, A_idx_pointerE_,
			B, &N,
			&beta , C, &N);
#else
	NOT_IMPLEMENTED;
#endif
}

template <>
void caffe_cpu_sparse_csrmm<double>(const int M, const int N, const int K,
		const double alpha,
		const double* A_nonzero_buf, const int* A_nonzero_idx_buf, const int* A_idx_pointerB_,const int* A_idx_pointerE_,
		const double* B,
		const double beta,double* C){
#ifdef USE_MKL
	char matdescra[6];
	matdescra[0] = 'g';
	matdescra[3] = 'c';
	const char transa = 'N';
	mkl_dcsrmm (&transa, &M , &N, &K,
			&alpha , matdescra,
			A_nonzero_buf, A_nonzero_idx_buf, A_idx_pointerB_, A_idx_pointerE_,
			B, &N,
			&beta , C, &N);
#else
	NOT_IMPLEMENTED;
#endif
}

template <>
void caffe_cpu_sparse_dense2csr<float>(const int M, const int N,
		float* A,
		float* A_nonzero_buf, int* A_nonzero_idx_buf, int* A_idx_pointer_buf){
#ifdef USE_MKL
	MKL_INT info;
	const MKL_INT job[] = {0,0,0,2,M*N,1};
	mkl_sdnscsr(job, &M , &N , A,
			&N , A_nonzero_buf, A_nonzero_idx_buf, A_idx_pointer_buf,  &info);
	if(info){
		LOG(FATAL)<<"The routine is interrupted processing the "<<
			info<<"-th row "
			<<"because there is no space in the arrays acsr and ja according to the value nzmax.";
	}
#else
	int nnz = 0;
	A_idx_pointer_buf[0] = 0;
	for(int i = 0; i < M; i ++) {
		int nnz_per_row = 0;
		for(int j = 0; j < N; j ++) {
			if(A[i*N+j] != 0) {
				A_nonzero_buf[nnz] = A[i*N+j];
				A_nonzero_idx_buf[nnz] = j;
				nnz_per_row ++;
				nnz ++;
			}
		}
		A_idx_pointer_buf[i+1] = A_idx_pointer_buf[i] + nnz_per_row;
	}
#endif
}

template <>
void caffe_cpu_sparse_dense2csr<double>(const int M, const int N,
		double* A,
		double* A_nonzero_buf, int* A_nonzero_idx_buf, int* A_idx_pointer_buf){
#ifdef USE_MKL
	MKL_INT info;
	const MKL_INT job[] = {0,0,0,2,M*N,1};
	mkl_ddnscsr(job, &M , &N , A,
			&N , A_nonzero_buf, A_nonzero_idx_buf, A_idx_pointer_buf,  &info);
	if(info){
		LOG(FATAL)<<"The routine is interrupted processing the "<<
			info<<"-th row "
			<<"because there is no space in the arrays acsr and ja according to the value nzmax.";
	}
#else
	NOT_IMPLEMENTED;
#endif
}

template <typename Dtype>
void caffe_cpu_sconv(const Dtype *input_padded, int in_channels,
		int height, int width, int pad_h, int pad_w,
		int stride_h, int stride_w, int dilation_h, int dilation_w,
		const int *rowptr, const int *colidx, const Dtype *values,
		int kernel_h, int kernel_w,
		const Dtype *bias, Dtype *output, int out_channels,
		int input_padded_len)
{
	const int output_h = (height + 2 * pad_h - (dilation_h * (kernel_h - 1) + 1)) / stride_h + 1;
	const int output_w = (width + 2 * pad_w - (dilation_w * (kernel_w - 1) + 1)) / stride_w + 1;
	//assert(output_h*output_w == N);
	int begin = 0;
	int end = out_channels;
	if (dilation_h != 1 || dilation_w != 1) {
		for (int output_row = 0; output_row < output_h; ++output_row) {
			for (int output_col = 0; output_col < output_w; ++output_col) {
				for (int out_channel = begin; out_channel < end; ++out_channel) {
					Dtype sum = 0;
					for (int j = rowptr[out_channel]; j < rowptr[out_channel + 1]; ++j) {
						int col = colidx[j];
						int kernel_col = col%(width + pad_w);
						int kernel_row = (col/(width + pad_w))%(height + pad_h);
						int in_channel = col/((width + pad_w)*(height + pad_h));
						int input_row = kernel_row * dilation_h + output_row * stride_h;
						int input_col = kernel_col * dilation_w + output_col * stride_w;
						sum += values[j] * input_padded[(in_channel * (height + pad_h) + input_row) * (width + pad_w) + input_col];
					}
					output[(out_channel * output_h + output_row) * output_w + output_col] = sum;
				}
			}
		}
	}
	else {
		for (int output_row = 0; output_row < output_h; ++output_row) {
			for (int output_col = 0; output_col < output_w; ++output_col) {
				const Dtype *in_temp2 = input_padded + output_row * stride_h * (width + pad_w) + output_col * stride_w;
				for (int out_channel = begin; out_channel < end; ++out_channel) {
					Dtype sum = 0;
					for (int j = rowptr[out_channel]; j < rowptr[out_channel + 1]; ++j) {
						assert(in_temp2 + colidx[j] - input_padded < input_padded_len);
						sum += values[j] * in_temp2[colidx[j]];
					}
					output[(out_channel * output_h + output_row) * output_w + output_col] = sum;
				}
			}
		}
	}
}

template void caffe_cpu_sconv<int>(const int *input_padded, int in_channels,
		int height, int width, int pad_h, int pad_w,
		int stride_h, int stride_w, int dilation_h, int dilation_w,
		const int *rowptr, const int *colidx, const int *values,
		int kernel_h, int kernel_w,
		const int*bias, int *output, int out_channels,
		int input_padded_len);

template void caffe_cpu_sconv<float>(const float *input_padded, int in_channels,
		int height, int width, int pad_h, int pad_w,
		int stride_h, int stride_w, int dilation_h, int dilation_w,
		const int *rowptr, const int *colidx, const float *values,
		int kernel_h, int kernel_w,
		const float *bias, float *output, int out_channels,
		int input_padded_len);

template void caffe_cpu_sconv<double>(const double *input_padded, int in_channels,
		int height, int width, int pad_h, int pad_w,
		int stride_h, int stride_w, int dilation_h, int dilation_w,
		const int *rowptr, const int *colidx, const double *values,
		int kernel_h, int kernel_w, const double *bias,
		double *output, int out_channels, int input_padded_len);

template <typename Dtype, bool FUSE_RELU>
void caffe_cpu_blocked_sconv(const Dtype *input_padded, int in_channels,
		int height, int width, int pad_h, int pad_w,
		int stride_h, int stride_w, int dilation_h, int dilation_w,
		const int *rowptr, const int *colidx, const Dtype *values,
		int kernel_h, int kernel_w, const int **rowptr_blocked,
		const int **colidx_blocked, const Dtype **values_blocked,
		int ncolblocks, const Dtype *bias, Dtype *output,
		int out_channels, Dtype *output_scratch, int ninputs)
{
	//const int output_h = (height + 2 * pad_h - (dilation_h * (kernel_h - 1) + 1)) / stride_h + 1;
	//const int output_w = (width + 2 * pad_w - (dilation_w * (kernel_w - 1) + 1)) / stride_w + 1;
	if (dilation_h != 1 || dilation_w != 1) {
		// Goto default
	} else if (stride_h == 1 && stride_w == 1 && height == width && kernel_h == kernel_w && pad_h == pad_w) {
		int num_oc_blocks = (out_channels + OC_BLOCK - 1)/OC_BLOCK;
		int oc_block_begin = 0;
		int oc_block_end = num_oc_blocks;
#ifdef USE_ICC
		cpu::OpenMpManager::getSimpleGroupedThreadPartition(
				&oc_block_begin, &oc_block_end, num_oc_blocks, ninputs);
#endif
		int oc_begin = std::min(oc_block_begin*OC_BLOCK, out_channels);
		int oc_end = std::min(oc_block_end*OC_BLOCK, out_channels);
		if (kernel_h == 2*pad_h + 1) { // matched padding
			if (kernel_h == 1) {
				// the following sizes are used by GoogLeNet
				if (height == 4) {
					sconv_unit_stride<4, 1, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
				else if (height == 7) {
					sconv_unit_stride<7, 1, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
				else if (height == 14) {
					sconv_unit_stride<14, 1, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
				else if (height == 28) {
					sconv_unit_stride<28, 1, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
				else if (height == 56) {
					sconv_unit_stride<56, 1, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
			}
			else if (kernel_h == 3) {
				if (height == 12) {
					// overfeat
					sconv_unit_stride<12, 3, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
				else if (height == 13) {
					// alexnet conv3-5
					sconv_unit_stride<13, 3, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
				// the following sizes are used by GoogLeNet
				else if (height == 7) {
					sconv_unit_stride<7, 3, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
				else if (height == 14) {
					sconv_unit_stride<14, 3, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
				else if (height == 28) {
					sconv_unit_stride<28, 3, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
				else if (height == 56) {
					sconv_unit_stride<56, 3, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
				// OCR public
				else if (height == 3) {
					sconv_unit_stride<3, 3, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
			}
			else if (kernel_h == 5) {
				// AlexNet conv2
				if (height == 27) {
					sconv_unit_stride<27, 5, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
				// the following sizes are used by GoogLeNet
				else if (height == 7) {
					sconv_unit_stride<7, 5, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
				else if (height == 14) {
					sconv_unit_stride<14, 5, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
				else if (height == 28) {
					sconv_unit_stride<28, 5, FUSE_RELU>(
							input_padded,
							rowptr_blocked, colidx_blocked, values_blocked, ncolblocks,
							bias,
							output, oc_begin, oc_end, output_scratch,
							in_channels, out_channels);
					return;
				}
			}
		}
		else if (0 == pad_h) { // zero padding
		}
/*	} else if (height == 227 && width == 227 && pad_h == 0 && pad_w == 0 && stride_h == 4 && stride_w == 4 && kernel_w == 11 && kernel_h == 11) {
		// conv1 of AlexNet
		assert(!FUSE_RELU);
		int WIDTH = 227;
		int STRIDE = 4;
		int K = 11;
		int WOUT = (WIDTH - K)/STRIDE + 1; // 55
		const int JBLOCK = 128;
		const int HBLOCK = 8;
		const int WBLOCK = 9;
		//__declspec(aligned(64)) float sum[WOUT*WOUT];
		__attribute__ ((aligned(64))) float sum[WOUT*WOUT];
		for (int out_channel = 0; out_channel < out_channels; ++out_channel) {
			int jbegin = rowptr[out_channel];
			int jend = std::min(jbegin + JBLOCK, rowptr[out_channel + 1]);
			for (int hbegin = 0; hbegin < WOUT; hbegin += HBLOCK) {
				int hend = std::min(hbegin + HBLOCK, WOUT);
				for (int wbegin = 0; wbegin < WOUT; wbegin += WBLOCK) {
					int wend = std::min(wbegin + WBLOCK, WOUT);
					for (int k = 0; k < (hend - hbegin) * (wend - wbegin); ++k) {
						sum[k] = 0;
					}
					for (int j = jbegin; j < jend; ++j) {
						float c = values[j];
						int off = colidx[j];
						int k = 0;
						for (int h = hbegin; h < hend; ++h) {
							for (int w = wbegin; w < wend; ++w, ++k) {
								sum[k] += c*input_padded[off + (h*WIDTH + w)*STRIDE];
							}
						}
					}
					int k = 0;
					for (int h = hbegin; h < hend; ++h) {
						for (int w = wbegin; w < wend; ++w, ++k) {
							output[(out_channel*WOUT + h)*WOUT + w] = sum[k];
						}
					}
				}
			}
			jbegin += JBLOCK;
			for ( ; jbegin < rowptr[out_channel + 1]; jbegin += JBLOCK) {
				int jend = std::min(jbegin + JBLOCK, rowptr[out_channel + 1]);
				for (int hbegin = 0; hbegin < WOUT; hbegin += HBLOCK) {
					int hend = std::min(hbegin + HBLOCK, WOUT);
					for (int wbegin = 0; wbegin < WOUT; wbegin += WBLOCK) {
						int wend = std::min(wbegin + WBLOCK, WOUT);
						for (int k = 0; k < (hend - hbegin) * (wend - wbegin); ++k) {
							sum[k] = 0;
						}
						for (int j = jbegin; j < jend; ++j) {
							float c = values[j];
							int off = colidx[j];
							int k = 0;
							for (int h = hbegin; h < hend; ++h) {
								for (int w = wbegin; w < wend; ++w, ++k) {
									sum[k] += c*input_padded[off + (h*WIDTH + w)*STRIDE];
								}
							}
						}
						int k = 0;
						for (int h = hbegin; h < hend; ++h) {
							for (int w = wbegin; w < wend; ++w, ++k) {
								output[(out_channel*WOUT + h)*WOUT + w] += sum[k];
							}
						}
					}
				}
			}
		}
		return;
//*/
	}
	caffe_cpu_sconv_default<FUSE_RELU>(input_padded, in_channels,
		height, width, pad_h, pad_w, stride_h, stride_w, dilation_h, dilation_w,
		rowptr, colidx, values, kernel_h, kernel_w, bias, output, out_channels);
}

template
void caffe_cpu_blocked_sconv<float,true>(const float *input_padded,
		int in_channels, int height, int width, int pad_h, int pad_w,
		int stride_h, int stride_w, int dilation_h, int dilation_w,
		const int *rowptr, const int *colidx, const float *values,
		int kernel_h, int kernel_w, const int **rowptr_blocked,
		const int **colidx_blocked, const float **values_blocked,
		int ncolblocks, const float *bias, float *output,
		int out_channels, float *output_scratch, int input_padded_len);

template
void caffe_cpu_blocked_sconv<float,false>(const float *input_padded,
		int in_channels, int height, int width, int pad_h, int pad_w,
		int stride_h, int stride_w, int dilation_h, int dilation_w,
		const int *rowptr, const int *colidx, const float *values,
		int kernel_h, int kernel_w, const int **rowptr_blocked,
		const int **colidx_blocked, const float **values_blocked,
		int ncolblocks, const float *bias, float *output,
		int out_channels, float *output_scratch, int input_padded_len);

template <>
void caffe_cpu_blocked_sconv<double,true>(const double *input_padded,
		int in_channels, int height, int width, int pad_h, int pad_w,
		int stride_h, int stride_w, int dilation_h, int dilation_w,
		const int *rowptr, const int *colidx, const double *values,
		int kernel_h, int kernel_w, const int **rowptr_blocked,
		const int **colidx_blocked, const double **values_blocked,
		int ncolblocks, const double *bias, double *output,
		int out_channels, double *output_scratch, int input_padded_len) {
	NOT_IMPLEMENTED;
}

template <>
void caffe_cpu_blocked_sconv<double,false>(const double *input_padded,
		int in_channels, int height, int width, int pad_h, int pad_w,
		int stride_h, int stride_w, int dilation_h, int dilation_w,
		const int *rowptr, const int *colidx, const double *values,
		int kernel_h, int kernel_w, const int **rowptr_blocked,
		const int **colidx_blocked, const double **values_blocked,
		int ncolblocks, const double *bias, double *output,
		int out_channels, double *output_scratch, int input_padded_len) {
	NOT_IMPLEMENTED;
}

// end cxh

template <>
void caffe_cpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float* A, const float* x,
    const float beta, float* y) {
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_cpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const double alpha, const double* A, const double* x,
    const double beta, double* y) {
  cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_axpy<float>(const int N, const float alpha, const float* X,
    float* Y) { cblas_saxpy(N, alpha, X, 1, Y, 1); }

template <>
void caffe_axpy<double>(const int N, const double alpha, const double* X,
    double* Y) { cblas_daxpy(N, alpha, X, 1, Y, 1); }

template <typename Dtype>
void caffe_set(const int N, const Dtype alpha, Dtype* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    return;
  }
  for (int i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

template void caffe_set<int>(const int N, const int alpha, int* Y);
template void caffe_set<float>(const int N, const float alpha, float* Y);
template void caffe_set<double>(const int N, const double alpha, double* Y);

template <>
void caffe_add_scalar(const int N, const float alpha, float* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <>
void caffe_add_scalar(const int N, const double alpha, double* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <typename Dtype>
void caffe_copy(const int N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
    if (Caffe::mode() == Caffe::GPU) {
#ifndef CPU_ONLY
      // NOLINT_NEXT_LINE(caffe/alt_fn)
      CUDA_CHECK(cudaMemcpy(Y, X, sizeof(Dtype) * N, cudaMemcpyDefault));
#else
      NO_GPU;
#endif
    } else {
      memcpy(Y, X, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    }
  }
}

template void caffe_copy<int>(const int N, const int* X, int* Y);
template void caffe_copy<unsigned int>(const int N, const unsigned int* X,
    unsigned int* Y);
template void caffe_copy<float>(const int N, const float* X, float* Y);
template void caffe_copy<double>(const int N, const double* X, double* Y);

template <>
void caffe_scal<float>(const int N, const float alpha, float *X) {
  cblas_sscal(N, alpha, X, 1);
}

template <>
void caffe_scal<double>(const int N, const double alpha, double *X) {
  cblas_dscal(N, alpha, X, 1);
}

template <>
void caffe_cpu_axpby<float>(const int N, const float alpha, const float* X,
                            const float beta, float* Y) {
  cblas_saxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_cpu_axpby<double>(const int N, const double alpha, const double* X,
                             const double beta, double* Y) {
  cblas_daxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_add<float>(const int n, const float* a, const float* b,
    float* y) {
  vsAdd(n, a, b, y);
}

template <>
void caffe_add<double>(const int n, const double* a, const double* b,
    double* y) {
  vdAdd(n, a, b, y);
}

template <>
void caffe_sub<float>(const int n, const float* a, const float* b,
    float* y) {
  vsSub(n, a, b, y);
}

template <>
void caffe_sub<double>(const int n, const double* a, const double* b,
    double* y) {
  vdSub(n, a, b, y);
}

template <>
void caffe_mul<float>(const int n, const float* a, const float* b,
    float* y) {
  vsMul(n, a, b, y);
}

template <>
void caffe_mul<double>(const int n, const double* a, const double* b,
    double* y) {
  vdMul(n, a, b, y);
}

template <>
void caffe_div<float>(const int n, const float* a, const float* b,
    float* y) {
  vsDiv(n, a, b, y);
}

template <>
void caffe_div<double>(const int n, const double* a, const double* b,
    double* y) {
  vdDiv(n, a, b, y);
}

template <>
void caffe_powx<float>(const int n, const float* a, const float b,
    float* y) {
  vsPowx(n, a, b, y);
}

template <>
void caffe_powx<double>(const int n, const double* a, const double b,
    double* y) {
  vdPowx(n, a, b, y);
}

template <>
void caffe_sqr<float>(const int n, const float* a, float* y) {
  vsSqr(n, a, y);
}

template <>
void caffe_sqr<double>(const int n, const double* a, double* y) {
  vdSqr(n, a, y);
}

template <>
void caffe_sqrt<float>(const int n, const float* a, float* y) {
  vsSqrt(n, a, y);
}

template <>
void caffe_sqrt<double>(const int n, const double* a, double* y) {
  vdSqrt(n, a, y);
}

template <>
void caffe_exp<float>(const int n, const float* a, float* y) {
  vsExp(n, a, y);
}

template <>
void caffe_exp<double>(const int n, const double* a, double* y) {
  vdExp(n, a, y);
}

template <>
void caffe_log<float>(const int n, const float* a, float* y) {
  vsLn(n, a, y);
}

template <>
void caffe_log<double>(const int n, const double* a, double* y) {
  vdLn(n, a, y);
}

template <>
void caffe_abs<float>(const int n, const float* a, float* y) {
    vsAbs(n, a, y);
}

template <>
void caffe_abs<double>(const int n, const double* a, double* y) {
    vdAbs(n, a, y);
}

unsigned int caffe_rng_rand() {
  return (*caffe_rng())();
}

template <typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter<Dtype>(
      b, std::numeric_limits<Dtype>::max());
}

template
float caffe_nextafter(const float b);

template
double caffe_nextafter(const double b);

template <typename Dtype>
void caffe_rng_uniform(const int n, const Dtype a, const Dtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<Dtype> random_distribution(a, caffe_nextafter<Dtype>(b));
  boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_uniform<float>(const int n, const float a, const float b,
                              float* r);

template
void caffe_rng_uniform<double>(const int n, const double a, const double b,
                               double* r);

template <typename Dtype>
void caffe_rng_gaussian(const int n, const Dtype a,
                        const Dtype sigma, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution<Dtype> random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*, boost::normal_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_gaussian<float>(const int n, const float mu,
                               const float sigma, float* r);

template
void caffe_rng_gaussian<double>(const int n, const double mu,
                                const double sigma, double* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, int* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, unsigned int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = static_cast<unsigned int>(variate_generator());
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, unsigned int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, unsigned int* r);

template <>
float caffe_cpu_strided_dot<float>(const int n, const float* x, const int incx,
    const float* y, const int incy) {
  return cblas_sdot(n, x, incx, y, incy);
}

template <>
double caffe_cpu_strided_dot<double>(const int n, const double* x,
    const int incx, const double* y, const int incy) {
  return cblas_ddot(n, x, incx, y, incy);
}

template <typename Dtype>
Dtype caffe_cpu_dot(const int n, const Dtype* x, const Dtype* y) {
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}

template
float caffe_cpu_dot<float>(const int n, const float* x, const float* y);

template
double caffe_cpu_dot<double>(const int n, const double* x, const double* y);

template <>
float caffe_cpu_asum<float>(const int n, const float* x) {
  return cblas_sasum(n, x, 1);
}

template <>
double caffe_cpu_asum<double>(const int n, const double* x) {
  return cblas_dasum(n, x, 1);
}

template <>
void caffe_cpu_scale<float>(const int n, const float alpha, const float *x,
                            float* y) {
  cblas_scopy(n, x, 1, y, 1);
  cblas_sscal(n, alpha, y, 1);
}

template <>
void caffe_cpu_scale<double>(const int n, const double alpha, const double *x,
                             double* y) {
  cblas_dcopy(n, x, 1, y, 1);
  cblas_dscal(n, alpha, y, 1);
}

}  // namespace caffe


#include <ros/ros.h>
#include <ros/package.h>
#include <nodelet/nodelet.h>

#include <mrs_lib/param_loader.h>
#include <mrs_lib/transformer.h>

#include <mrs_lib/lkf.h>

#include <mrs_msgs/PoseWithCovarianceArrayStamped.h>
#include <mrs_msgs/ReferenceStamped.h>
#include <mrs_msgs/String.h>
#include <mrs_msgs/Vec4.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <std_srvs/Trigger.h>
#include <std_srvs/SetBool.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/String.h>

#include <boost/range/adaptor/indexed.hpp>

#include <mutex>

#include <Eigen/Dense>

#define sqr(X) ((X) * (X))

#define DEFAULT_OUTPUT_FRAMERATE 20.0
#define DECAY_AGE_NORMAL 3.0
#define DECAY_AGE_UNVALIDATED 1.0
#define MIN_MEASUREMENTS_TO_VALIDATION 10
#define POS_THRESH 2.0
#define MAH_THRESH 2.0
#define YAW_THRESH 1.5
#define MATCH_LEVEL_THRESHOLD_ASSOCIATE 0.3
#define MATCH_LEVEL_THRESHOLD_REMOVE 0.5

using namespace boost::adaptors;

namespace mrs_lib
{
  const int n_states = -1;
  const int n_inputs = 0;
  const int n_measurements = 6;
  using lkf_t = LKF<n_states, n_inputs, n_measurements>;
}

using A_t = mrs_lib::lkf_t::A_t;
using B_t = mrs_lib::lkf_t::B_t;
using H_t = mrs_lib::lkf_t::H_t;
using Q_t = mrs_lib::lkf_t::Q_t;
using u_t = mrs_lib::lkf_t::u_t;
using statecov_t = mrs_lib::lkf_t::statecov_t;

namespace e = Eigen;

namespace uvdar {

  /**
   * @brief A processing lass for filtering measurements from UVDAR-based relative UAV pose estimator. This uses a vector of Kalman filter states for tracking multiple targets either if they have known identity or if they are anonymous.
   */
  class UVDARKalman {

    private:
      /* attributes //{ */

      bool _debug_ = false;

      bool initialized_ = false;
      std::string _uav_name_;

      std::string _output_frame_;
      int _input_count_;
      double _output_framerate_;
      bool _odometry_available_, _indoor_;
      double vl, vv, sn;
      bool _anonymous_measurements_;
      bool _use_velocity_;

      std::mutex meas_mutex;
      std::mutex filter_mutex;
      std::mutex transformer_mutex;

      std::shared_ptr<mrs_lib::lkf_t> filter;

      struct td_t{
        A_t A;
        B_t B;
        H_t H;
        Q_t Q;
      };
      td_t filter_matrices;

      struct FilterData{
        statecov_t filter_state;
        int update_count;
        ros::Time latest_update;
        ros::Time latest_measurement;
        unsigned long long int id;
      };

      std::vector<FilterData> fd;
      // | ----------------------- subscribers ---------------------- |

      std::vector<ros::Subscriber> sub_measurements_;

      // | ----------------------- publishers ---------------------- |
      ros::Publisher pub_filter_;
      ros::Publisher pub_filter_tent_;

      ros::Publisher pub_status_;

      ros::Timer timer;
      ros::Duration filter_update_period;
      double dt;
      unsigned long long int latest_id = 0;

      mrs_lib::Transformer transformer_;

      //}

    public:
      /**
       * @brief Constructor - loads parameters and initializes necessary structures
       *
       * @param nh Private NodeHandle of this ROS node
       */
      /* Constructor //{ */
      UVDARKalman(ros::NodeHandle nh) {
        ros::Time::waitForValid();

        mrs_lib::ParamLoader param_loader(nh, "UVDARKalman");

        param_loader.loadParam("debug", _debug_);

        param_loader.loadParam("uav_name", _uav_name_);
        param_loader.loadParam("output_frame", _output_frame_, std::string("local_origin"));
        param_loader.loadParam("output_framerate", _output_framerate_, double(DEFAULT_OUTPUT_FRAMERATE));

        param_loader.loadParam("anonymous_measurements", _anonymous_measurements_, bool(false));

        param_loader.loadParam("indoor", _indoor_, bool(false));
        param_loader.loadParam("odometry_available", _odometry_available_, bool(true));
        param_loader.loadParam("use_velocity", _use_velocity_, bool(false));

        if (_indoor_){ //lateral and vertical velocities are more limited in indoor conditions
          vl = 1;
          vv = 0.5;
        }
        else {
          vl = 2;
          vv = 1;
        }

        if (_odometry_available_){ //process noise is greater, since without odometry we can't correct for the ego-motion of the observer
          sn = 2;
        }
        else {
          sn = 4;
        }

        filter_update_period = ros::Duration(1.0 / fmax(_output_framerate_,1.0));
        dt = filter_update_period.toSec();
        timer = nh.createTimer(filter_update_period, &UVDARKalman::spin, this);

        transformer_ = mrs_lib::Transformer("UVDARKalman", _uav_name_);

        std::vector<std::string> _measured_poses_topics;
        param_loader.loadParam("measured_poses_topics", _measured_poses_topics, _measured_poses_topics);
        if (_measured_poses_topics.empty()) {
          ROS_WARN("[UVDARKalman]: No topics of measured_poses_topics were supplied. Returning.");
          return;
        }

        for (auto& topic : _measured_poses_topics) {
          ROS_INFO_STREAM("[UVDARKalman]: Subscribing to " << topic);
          sub_measurements_.push_back(nh.subscribe(topic, 3, &UVDARKalman::callbackMeasurement, this, ros::TransportHints().tcpNoDelay()));
        }

        pub_filter_ = nh.advertise<mrs_msgs::PoseWithCovarianceArrayStamped>("filtered_poses", 1);
        pub_filter_tent_ = nh.advertise<mrs_msgs::PoseWithCovarianceArrayStamped>("filtered_poses/tentative", 1);

        pub_status_ = nh.advertise<std_msgs::String>("/"+_uav_name_+"/mrs_uav_status/display_string", 1);

        if (_anonymous_measurements_ && _use_velocity_){
          ROS_WARN("[UVDARKalman]: Velocity estimation for anonymous measurements is not implemented. Returning.");
          return;
        }

        if (_anonymous_measurements_){
          filter_matrices.A.resize(6,6);
          filter_matrices.Q.resize(6,6);
          filter_matrices.H.resize(6,6);

          filter_matrices.H <<
            1,0,0,0,0,0,
            0,1,0,0,0,0,
            0,0,1,0,0,0,
            0,0,0,1,0,0,
            0,0,0,0,1,0,
            0,0,0,0,0,1;

        }
        else {
          if (_use_velocity_){
            filter_matrices.A.resize(9,9);
            filter_matrices.Q.resize(9,9);
            filter_matrices.H.resize(6,9);

            filter_matrices.H <<
              1,0,0,0,0,0,0,0,0,
              0,1,0,0,0,0,0,0,0,
              0,0,1,0,0,0,0,0,0,
              0,0,0,0,0,0,1,0,0,
              0,0,0,0,0,0,0,1,0,
              0,0,0,0,0,0,0,0,1;
          }
          else {
            filter_matrices.A.resize(6,6);
            filter_matrices.Q.resize(6,6);
            filter_matrices.H.resize(6,6);

            filter_matrices.H <<
              1,0,0,0,0,0,
              0,1,0,0,0,0,
              0,0,1,0,0,0,
              0,0,0,1,0,0,
              0,0,0,0,1,0,
              0,0,0,0,0,1;
          }
        }

        filter_matrices.B = B_t();

        filter = std::make_unique<mrs_lib::lkf_t>(filter_matrices.A, filter_matrices.B, filter_matrices.H);

        if (param_loader.loadedSuccessfully()) {
          initialized_ = true;
          ROS_INFO_STREAM("[UVDARKalman]: initiated");
        } else {
          ROS_ERROR_ONCE("[UVDARKalman]: PARAMS not loaded correctly, shutting down.");
          nh.shutdown();
        }

      }
      //}

    private:

      /**
       * @brief Callback to process input messages - array of pose estimates with covariance and identity
       *
       * @param msg
       */
      /* calllbackMeasurement //{ */
      void callbackMeasurement(const mrs_msgs::PoseWithCovarianceArrayStamped& msg){
        if ((int)(msg.poses.size()) < 1)
          return;
        if (_debug_)
          ROS_INFO_STREAM("[UVDARKalman]: Getting " << (int)(msg.poses.size()) << " measurements...");

        mrs_msgs::PoseWithCovarianceArrayStamped msg_local;
        {
          std::scoped_lock lock(meas_mutex);
          msg_local = msg;
        }

        std::optional<mrs_lib::TransformStamped> tf;
        {
          std::scoped_lock lock(transformer_mutex);
          tf = transformer_.getTransform(msg_local.header.frame_id, _output_frame_, msg_local.header.stamp);
        }
        if (!tf) {
          ROS_ERROR("[UVDARKalman]: Could not obtain transform from %s to %s",msg_local.header.frame_id.c_str(), _output_frame_.c_str());
          return;
        }
        std::vector<statecov_t> meas_converted;
        std::vector<int> ids;
        for (auto &meas : msg_local.poses){
          e::VectorXd poseVec(6);
          e::MatrixXd poseCov(6,6);

          std::optional<geometry_msgs::PoseWithCovarianceStamped> meas_t;
          geometry_msgs::PoseWithCovarianceStamped meas_s;
          meas_s.pose.pose = meas.pose;
          meas_s.pose.covariance = meas.covariance;
          if (_debug_)
            ROS_INFO_STREAM("[UVDARKalman]: Meas. cov. input: " << std::endl << rosCovarianceToEigen(meas.covariance));
          meas_s.header = msg_local.header;
          {
            std::scoped_lock lock(transformer_mutex);
            meas_t = transformer_.transform(tf.value(), meas_s);
          }
          if (!meas_t){
            ROS_INFO_STREAM("[UVDARKalman]: Failed to get transformation for measurement, returning.");
            return;
          }

          poseVec(0) = meas_t.value().pose.pose.position.x;
          poseVec(1) = meas_t.value().pose.pose.position.y;
          poseVec(2) = meas_t.value().pose.pose.position.z;
          e::Quaterniond qtemp(
              meas_t.value().pose.pose.orientation.w,
              meas_t.value().pose.pose.orientation.x,
              meas_t.value().pose.pose.orientation.y,
              meas_t.value().pose.pose.orientation.z
              );

          poseVec(3) = fixAngle(quatToRoll(qtemp), 0);
          poseVec(4) = fixAngle(quatToPitch(qtemp), 0);
          poseVec(5) = fixAngle(quatToYaw(qtemp), 0);

          if (poseVec.array().isNaN().any()){
            ROS_INFO_STREAM("[UVDARKalman]: Discarding input with ID" << meas.id << ", value includes Nans.");
            return;
          }

          /*           for (int i=0; i<6; i++){ */
          /*             for (int j=0; j<6; j++){ */
          /*               poseCov(j,i) =  [6*j+i]; */
          /*             } */
          /*           } */

          poseCov = rosCovarianceToEigen(meas_t.value().pose.covariance);

          if (poseCov.array().isNaN().any()){
            ROS_INFO("[UVDARKalman]: Discarding input, covariance includes Nans.");
            return;
          }

          bool changed = false;

          e::EigenSolver<e::Matrix3d> es(poseCov.bottomRightCorner(3,3));
          auto eigvals = es.eigenvalues();
          for (int i=0; i<(int)(eigvals.size()); i++){
            /* ROS_INFO_STREAM("[UVDARKalman]: Eigenvalues: "<< eigvals(i).real() << " im: " <<  eigvals(i).imag()); */
            if (eigvals(i).real() > sqr(M_PI/3.0)){ //angle std.dev representing no knowledge (3*sigma contains 99.7 percent)
              eigvals(i) = sqr(666.0); //arbitrarily large number, s.t. the measurement will not affect the state
              changed = true;
            }
          }

          if (changed){
            auto eigvecs = es.eigenvectors();
            /* ROS_INFO_STREAM("[UVDARKalman]: Eigenvectors: "<< std::endl << eigvecs.real() << std::endl << "im: " << std::endl <<  eigvecs.imag()); */
            poseCov.bottomRightCorner(3,3) = eigvecs.real()*eigvals.real().asDiagonal()*eigvecs.real().transpose(); //reform the covariance with new expanded eigenvalues
          }

          if (_debug_){
            ROS_INFO_STREAM("[UVDARKalman]: Transformed measurement input is: [" << poseVec.transpose() << "]");
            ROS_INFO_STREAM("[UVDARKalman]: Meas. cov. transformed: " << std::endl << poseCov);
          }

          /* poseCov.topLeftCorner(3,3) += e::Matrix3d::Identity()*0.5; //fattening the measurements before applying them - they don't represent the distribution too well and the elliptic shape of the gaussians unduly shortents the filter covariances, as if we had more information on distance than we really do. */

          meas_converted.push_back({.x=poseVec,.P=poseCov});
          ids.push_back(meas.id);
        }

        if (_anonymous_measurements_){
          applyMeasurementsAnonymous(meas_converted, msg_local.header.stamp, msg_local.header.frame_id);
        }
        else {
          applyMeasurementsWithIdentity(meas_converted, ids, msg_local.header.stamp, msg_local.header.frame_id);
        }
      }
      //}

      /**
       * @brief Thread for processing, predicting and outputting filter states at a regular rate
       *
       * @param te TimerEvent for the timer spinning this thread
       */
      /* spin //{ */
      void spin([[ maybe_unused ]] const ros::TimerEvent& te){
        std::scoped_lock lock(filter_mutex);
        removeNANs();
        if (_anonymous_measurements_){
          removeOverlaps();
        }
        for (int target=0; target<(int)(fd.size());target++){
          int targetsSeen = 0;
          double age = (ros::Time::now() - fd[target].latest_measurement).toSec();
          if (_debug_)
            ROS_INFO("[UVDARKalman]: Age of %d is %f", target, age);
          double decay_age;
          if (fd[target].update_count > MIN_MEASUREMENTS_TO_VALIDATION){
            decay_age = DECAY_AGE_NORMAL;
          }
          else {
            decay_age = DECAY_AGE_UNVALIDATED;
          }
          if (age>decay_age){
            ROS_INFO_STREAM("[UVDARKalman]: Removing state " << target << " (ID:" << fd[target].id << ") : " << fd[target].filter_state.x.transpose() << " due to old age of " << age << " s." );
            fd.erase(fd.begin()+target);
            target--;
            continue;
          }
          predictTillTime(fd.at(target), ros::Time::now(), true);

          if (_debug_){
            ROS_INFO_STREAM("[UVDARKalman]: State: ");
            ROS_INFO_STREAM("[UVDARKalman]: \n" << fd[target].filter_state.x);
          }

            /* if ( */
            /*     ((fd[target].filter_state.P.diagonal().array() < 0).any()) */
            /*    ){ */
            /*   ROS_INFO_STREAM("[UVDARKalman]: SPIN: NEGATIVE NUMBERS ON MAIN DIAGONAL!"); */
            /*   ROS_INFO_STREAM("[UVDARKalman]: State. cov: " << std::endl << fd[target].filter_state.P); */
            /* } */
        }

        publishStates();

      }
      //}

      /**
       * @brief Initiates a new filter based on a measurement
       *
       * @param x Measurement vector
       * @param C Error covariance associated with x
       * @param stamp Time of the measurement
       * @param id Identity of the target (-1 means that it is unknown or irrelevant)
       */
      /* initiateNew //{ */
      void initiateNew(e::VectorXd x, e::MatrixXd C, ros::Time stamp, int id = -1){
        if (fd.size() > 20)
          return;

        bool changed = false;
        auto eigens = C.topLeftCorner(3,3).eigenvalues();
        for (int i=0; i<3; i++){
          if (eigens(i).real() > (x.topLeftCorner(3,1).norm())){
            eigens(i) = 5.0;
            changed = true;
          }
        }

        if (changed){
          if (id == -1){
            e::EigenSolver<e::Matrix3d> es(C.topLeftCorner(3,3));
            C.topLeftCorner(3,3) = es.eigenvectors().real()*eigens.real().asDiagonal()*es.eigenvectors().real().transpose();
            x.topLeftCorner(3,1) = x.topLeftCorner(3,1).normalized()*15;
            //so that we don't initialize with the long covariances intersecting in the origin
          }
        }

          auto C_local = C; // the correlation between angle and position only exists in the measurement - the filter may have many measurements where this relation does not exist anymore
        if (!_use_velocity_){
          int index = (int)(fd.size());
          ROS_INFO_STREAM("[UVDARKalman]: Initiating state " << index << "(ID:" << ((id<0)?(latest_id++):(id)) << ")  with: " << x.transpose());
          ROS_INFO_STREAM("[UVDARKalman]: The source input of the state is " << (ros::Time::now() - stamp).toSec() << "s old.");

          C_local.topRightCorner(3,3).setZero();  // check the case of _use_velocity_ == true
          C_local.bottomLeftCorner(3,3).setZero();  // check the case of _use_velocity_ == true

          fd.push_back({.filter_state = {.x = x, .P = C_local}, .update_count = 0, .latest_update = stamp, .latest_measurement = stamp, .id = ((id<0)?(latest_id++):(id))});
        }
        else{
          ROS_WARN("[UVDARKalman]: Initialization of states with velocity is not implemented. Returning.");
          return;
        }

        /* if ( */
        /*     ((fd.back().filter_state.P.diagonal().array() < 0).any()) */
        /*    ){ */
        /*   ROS_INFO_STREAM("[UVDARKalman]: INIT: NEGATIVE NUMBERS ON MAIN DIAGONAL!"); */
        /*   ROS_INFO_STREAM("[UVDARKalman]: Generating measurement: " << std::endl << C); */
        /*   ROS_INFO_STREAM("[UVDARKalman]: Generating fixed measurement: " << std::endl << C_local); */
        /*   ROS_INFO_STREAM("[UVDARKalman]: State. cov: " << std::endl << fd.back().filter_state.P); */
        /* } */
      }
      //}

      /**
       * @brief Predicts the filter state at target_time, given that target_time is newer than the last update or measurement
       *
       * @param fd_curr The filter data structure to start from (if apply_update is true, this structure will be updated with the result)
       * @param target_time The time to which the state is to be predicted
       * @param apply_update If true, the output will be applied to the structure fd_curr. This provides more data than the return value, such as the update time
       *
       * @return The resulting state and its output covariance
       */
      /* predictTillTime //{ */
      statecov_t predictTillTime(struct FilterData &fd_curr, ros::Time target_time, bool apply_update = false){
        auto orig_state = fd_curr;
        double dt_from_last = std::fmax(std::fmin((target_time-fd_curr.latest_update).toSec(),(target_time-fd_curr.latest_measurement).toSec()),0.0);
        filter->A = A_dt(dt_from_last);
        /* ROS_INFO_STREAM("[UVDARKalman]: Filter pred. orig: " << std::endl << fd_curr.filter_state.P); */
        auto new_state =  filter->predict(fd_curr.filter_state, u_t(), Q_dt(dt_from_last), dt_from_last);
        /* ROS_INFO_STREAM("[UVDARKalman]: Filter predicted: " << std::endl << fd_curr.filter_state.P); */
        if (apply_update){
          fd_curr.filter_state = new_state;
          fd_curr.latest_update = target_time;
        }

            if (
                ((orig_state.filter_state.P.diagonal().array() < 0).any()) ||
                ((fd_curr.filter_state.P.diagonal().array() < 0).any())
               ){
              ROS_INFO_STREAM("[UVDARKalman]: NEGATIVE NUMBERS ON MAIN DIAGONAL!");
              ROS_INFO_STREAM("[UVDARKalman]: Orig. state. cov: " << std::endl << orig_state.filter_state.P);
              /* ROS_INFO_STREAM("[UVDARKalman]: dt: " << dt_from_last); */
              /* ROS_INFO_STREAM("[UVDARKalman]: Q_dt: " << Q_dt(dt_from_last)); */
              ROS_INFO_STREAM("[UVDARKalman]: New state. cov: " << std::endl << fd_curr.filter_state.P);
            }
        return new_state;
      }
      //}

      /**
       * @brief Predicts the change of the state and error covariance till meas_time, and then corrects the state with measurement
       *
       * @param fd_curr The filter data structure to start from (if apply_update is true, this structure will be updated with the result)
       * @param measurement The input measurement to correct the state with
       * @param match_level The output level of overlap between the original state fd_curr and the measurement in terms of position. This is based on the maximum of multivariate Gaussian multiplication between the measurement and the state, and represents information that is normally lost in the Kalman filter process
       * @param meas_time The time of the input measurement
       * @param prior_predict if true, the state is first brought to meas_time by predicting its change from the last update or measurement
       * @param apply_update If true, the output will be applied to the structure fd_curr. This provides more data than the return value, such as the update time
       *
       * @return The resulting state and its output covariance
       */
      /* correctWithMeasurement //{ */
      statecov_t correctWithMeasurement(struct FilterData &fd_curr, statecov_t measurement, double &match_level_pos, ros::Time meas_time, bool prior_predict, bool apply_update = false, std::string camera_frame = ""){
        auto filter_local = fd_curr;

        auto orig_state = filter_local.filter_state;
        /* ROS_INFO_STREAM("[UVDARKalman]: Original state: " << fd_curr.id << " was " << filter_local.filter_state.x.transpose()); */
        /* ROS_INFO_STREAM("[UVDARKalman]: Original cov: " << std::endl << filter_local.filter_state.P); */

        if (prior_predict){
          predictTillTime(filter_local, meas_time, true);
        }

        /* ROS_INFO_STREAM("[UVDARKalman]: Orig. angles: " << filter_local.filter_state.x.bottomRows(3)); */
        filter_local.filter_state.x[3] = fixAngle(filter_local.filter_state.x[3], measurement.x[3]);
        filter_local.filter_state.x[4] = fixAngle(filter_local.filter_state.x[4], measurement.x[4]);
        filter_local.filter_state.x[5] = fixAngle(filter_local.filter_state.x[5], measurement.x[5]);
        /* ROS_INFO_STREAM("[UVDARKalman]: Fixed. angles: " << filter_local.filter_state.x.bottomRows(3)); */

        match_level_pos = gaussJointMaxVal(
            measurement.P.topLeftCorner(3,3),
            filter_local.filter_state.P.topLeftCorner(3,3),
            measurement.x.topRows(3),
            filter_local.filter_state.x.topRows(3)
            );
        double match_level_rot = gaussJointMaxVal(
            measurement.P.bottomRightCorner(3,3),
            filter_local.filter_state.P.bottomRightCorner(3,3),
            measurement.x.bottomRows(3),
            filter_local.filter_state.x.bottomRows(3)
            );

        auto P_local = measurement.P;
        /* ROS_INFO_STREAM("[UVDARKalman]: Meas. cov: " << std::endl << P_local); */
        P_local.topRightCorner(3,3).setZero();  // check the case of _use_velocity_ == true
        P_local.bottomLeftCorner(3,3).setZero();  // check the case of _use_velocity_ == true

        /* double dt_from_last = std::fmax(std::fmin((filter_local.latest_update-meas_time).toSec(),(filter_local.latest_measurement-meas_time).toSec()),0.0); */
        /* P_local += Q_dt(dt_from_last)*dt_from_last; */

        /* ROS_INFO_STREAM("[UVDARKalman]: Scaling by: " << 1.0/match_level << " due to match level of " << match_level); */

        double volume_ratio_pos = std::min(1.0,filter_local.filter_state.P.topLeftCorner(3,3).determinant()/P_local.topLeftCorner(3,3).determinant());

        P_local.topLeftCorner(3,3) *= (1.0/(match_level_pos*volume_ratio_pos));
        /* P_local.bottomRightCorner(3,3) *= (1.0/match_level_rot); */
        /* ROS_INFO_STREAM("[UVDARKalman]: With ML: " << std::endl << P_local); */
        /* P_local *= std::max(0.1,(1.0/match_level)); */

        /* ROS_INFO_STREAM("[UVDARKalman]: Filter orig: " << std::endl << filter_local.filter_state.P); */
        try {
          filter_local.filter_state = filter->correct(filter_local.filter_state, measurement.x, P_local);
        }
        catch ([[maybe_unused]] std::exception e) {
          ROS_ERROR_STREAM("[UVDARKalman]: Attempted to correct with bad covariance (match_level_pos = " << match_level_pos << "). Will replace with big, but manabeable one.");
          /* P_local = e::MatrixXd(6,6); */
          P_local.setIdentity();
          P_local *= 10000;
          filter_local.filter_state = filter->correct(filter_local.filter_state, measurement.x, P_local);
        }
        /* ROS_INFO_STREAM("[UVDARKalman]: Filter corr: " << std::endl << filter_local.filter_state.P); */
        /* ROS_INFO_STREAM("[UVDARKalman]: Fixed to: " << std::endl << P_local); */

        /* filter_local.filter_state.P *= (1+match_level);//this makes the filter not increase certainty in case of multiple identical measurements - the mean in the covariances is more probable than the rest of its x<1*sigma space */

        auto eigens_pos = filter_local.filter_state.P.topLeftCorner(3,3).eigenvalues();
        double min_eig_pos = eigens_pos.real().minCoeff();
        auto eigens_rot = filter_local.filter_state.P.bottomRightCorner(3,3).eigenvalues();
        double min_eig_rot = eigens_rot.real().minCoeff();
        filter_local.filter_state.P.topLeftCorner(3,3) += e::MatrixXd::Identity(3,3)*(min_eig_pos*(match_level_pos));//this makes the filter not increase certainty in case of multiple identical measurements - the mean in the covariances is more probable than the rest of its x<1*sigma space
        /* filter_local.filter_state.P.bottomRightCorner(3,3) += e::MatrixXd::Identity(3,3)*(min_eig_rot*(match_level_rot));//this makes the filter not increase certainty in case of multiple identical measurements - the mean in the covariances is more probable than the rest of its x<1*sigma space */
        /* ROS_INFO_STREAM("[UVDARKalman]: Filter exp.: " << std::endl << filter_local.filter_state.P); */

        filter_local.filter_state.x[3] = fixAngle(filter_local.filter_state.x[3], 0);
        filter_local.filter_state.x[4] = fixAngle(filter_local.filter_state.x[4], 0);
        filter_local.filter_state.x[5] = fixAngle(filter_local.filter_state.x[5], 0);

        if (isInFrontOfCamera(filter_local.filter_state.x.topRows(3), camera_frame, meas_time)){
          if (apply_update){
            /* if (_debug_){ */
            /* if ( */
            /*     ((orig_state.x.topRows(3) - filter_local.filter_state.x.topRows(3)).norm() > 2.0) || */
            /*     ((orig_state.x.bottomRows(3) - filter_local.filter_state.x.bottomRows(3)).norm() > 0.2) */
            /*    ){ */
            /*   ROS_INFO_STREAM("[UVDARKalman]: Updating state: " << fd_curr.id << " with " << measurement.x.transpose()); */
            /*   ROS_INFO_STREAM("[UVDARKalman]: Meas. cov: " << std::endl << measurement.P); */
            /*   ROS_INFO_STREAM("[UVDARKalman]: Fixed to: " << std::endl << P_local); */
            /*   ROS_INFO_STREAM("[UVDARKalman]: State. cov: " << std::endl << orig_state.P); */
            /*   ROS_INFO_STREAM("[UVDARKalman]: This yelds state: " << filter_local.filter_state.x.topRows(6).transpose()); */
            /* } */
            /* if ( */
            /*     ((orig_state.P.diagonal().array() < 0).any()) || */
            /*     ((measurement.P.diagonal().array() < 0).any()) || */
            /*     ((filter_local.filter_state.P.diagonal().array() < 0).any()) */
            /*    ){ */
            /*   ROS_INFO_STREAM("[UVDARKalman]: NEGATIVE NUMBERS ON MAIN DIAGONAL!"); */
            /*   ROS_INFO_STREAM("[UVDARKalman]: Meas. cov: " << std::endl << measurement.P); */
            /*   ROS_INFO_STREAM("[UVDARKalman]: Fixed to: " << std::endl << P_local); */
            /*   ROS_INFO_STREAM("[UVDARKalman]: Orig. state. cov: " << std::endl << orig_state.P); */
            /*   ROS_INFO_STREAM("[UVDARKalman]: New state. cov: " << std::endl << filter_local.filter_state.P); */
            /* } */
            /* } */
            fd_curr.filter_state = filter_local.filter_state;
            fd_curr.latest_measurement = meas_time;
            fd_curr.update_count++;
          }
        }
        return filter_local.filter_state;
      }
      //}

      /**
       * @brief Deletes on of the active filter states
       *
       * @param index The index of the filter state to remove
       */
      /* deleteFilter //{ */
      void deleteFilter(int &index){
        if (index < 0) return;
        fd.erase(fd.begin()+index);
        index--;
      }
      //}

      /**
       * @brief Returns a value between 0 and 1 representing the level of overlap between two probability distributions in the form of multivariate Gaussians. Multiplying two Gaussians produces another Gaussian, with the value of its peak roughly corresponding to the level of the overlap between the two inputs. The ouptut of this function is the value of such peak, given that the input Gaussians have been scaled s.t. their peaks have the value of 1. Therefore, the output is 1 if the means of both inputs are identical.
       *
       * @param si0 The covariance of the first distribution
       * @param si1 The covariance of the second distribution
       * @param mu0 The mean of the first distribution
       * @param mu1 The mean of the second distribution
       *
       * @return The level of overlap between the two distribution
       */
      /* gaussJointMaxVal //{ */
      double gaussJointMaxVal(e::MatrixXd si0,e::MatrixXd si1,e::VectorXd mu0,e::VectorXd mu1){
        bool scaled = true;
        int k=mu0.size();
        auto K=si0*(si0+si1).inverse();
        auto d0=K*(mu1-mu0);
        auto d1=(K-e::MatrixXd::Identity(K.rows(),K.rows()))*(mu1-mu0);
        double N;
        if (scaled){
          auto N_v = ((-0.5)*( (d0.transpose()*(si0).inverse()*d0) + (d1.transpose()*(si1).inverse()*d1) ));
          N=exp(N_v(0));
        }
        else{
          auto N_v = ((-0.5)*( (d0.transpose()*(si0).inverse()*d0) + (d1.transpose()*(si1).inverse()*d1) ));
          N=(1.0/pow((2*M_PI),k)*sqrt((si0).determinant()*(si1).determinant()))*exp(N_v(0));
        }
        if (isnan(N))
          ROS_INFO("[UVDARKalman]: Joint Gaussian value came out NaN");

        return N;
      }
      //}

      bool isInFrontOfCamera(e::Vector3d mean, std::string camera_frame, ros::Time stamp){
        geometry_msgs::PoseStamped target_cam_view, target_filter;
        target_filter.header.frame_id = _output_frame_;
        target_filter.header.stamp = stamp;
        target_filter.pose.position.x = mean.x();
        target_filter.pose.position.y = mean.y();
        target_filter.pose.position.z = mean.z();

        auto ret = transformer_.transformSingle(camera_frame, target_filter);
        if (ret) {
          target_cam_view = ret.value();
        }
        else{
          ROS_ERROR_STREAM("[UVDARKalman]: Could not transform filter state from "<< _output_frame_ << " to camera frame " << camera_frame);
          return false;
        }

        e::Vector3d target_cam_view_vector(target_cam_view.pose.position.x, target_cam_view.pose.position.y, target_cam_view.pose.position.z);
        double norm = target_cam_view_vector.norm();
        double cos_angle = target_cam_view_vector.normalized().dot(e::Vector3d(0,0,1));

        return ((norm > 1.5) && (cos_angle > -0.173648));//cos(100 deg)

      }

      /**
       * @brief Applies relative position measurements to states that are the closest to their position component
       *
       * @param measurements A vector of measurements with error covariances
       * @param meas_time The time of the input measurements
       */
      /* applyMeasurementsAnonymous //{ */
      void applyMeasurementsAnonymous(std::vector<statecov_t> measurements, ros::Time meas_time, std::string camera_frame){
        std::scoped_lock lock(filter_mutex);
        if (fd.size() == 0){
          for (auto const& measurement_curr : measurements | indexed(0)){
            initiateNew(measurement_curr.value().x, measurement_curr.value().P, meas_time);
          }
          return;
        }

        e::MatrixXd match_matrix(measurements.size(),fd.size());
        std::vector<std::vector<FilterData>> tentative_states;

        for (auto const& measurement_curr : measurements | indexed(0)){
          tentative_states.push_back(std::vector<FilterData>());
          for (auto const& state_curr : fd | indexed(0)){

            double dt_from_last = std::fmin((meas_time-state_curr.value().latest_update).toSec(),(meas_time-state_curr.value().latest_measurement).toSec());

            tentative_states.back().push_back(
                state_curr.value()
                );
            double match_level;
            correctWithMeasurement(tentative_states.back().back(),measurement_curr.value(), match_level, meas_time, true, true, camera_frame);
            match_matrix(measurement_curr.index(),state_curr.index()) = match_level;
            double dt_s = 0.1;
            if ((ros::Time::now() - state_curr.value().latest_measurement).toSec() < dt_s){ // just in case - in simulation the camera outputs follow one another immediately, so no inflation happens in between
              tentative_states.back().back().filter_state = predictTillTime(tentative_states.back().back(), ros::Time::now()+ros::Duration(dt_s),false);
            }
          }
        }

        if (_debug_){
          ROS_INFO_STREAM("[UVDARKalman]: match_matrix: " << std::endl <<match_matrix);
        }

        std::vector<std::pair<int,int>> matches;

        int best_index;
        double best_match_level;
        double best_update_count;
        double curr_match_level;
        for (int f_i = 0; f_i < (int)(fd.size()); f_i++) {
          best_update_count = -1;
          best_index = -1;
          best_match_level = -1;
          for (int m_i = 0; m_i < (int)(measurements.size()); m_i++) {
            curr_match_level = match_matrix(m_i,f_i);
            if ((curr_match_level > MATCH_LEVEL_THRESHOLD_ASSOCIATE) && ( (fd[f_i].update_count > best_update_count) || ((fd[f_i].update_count == best_update_count) && (curr_match_level > best_match_level)) ) ){
              best_match_level = curr_match_level;
              best_index = m_i;
              best_update_count = fd[f_i].update_count;
            }
          }

          if (best_index >= 0){
            matches.push_back({best_index,f_i});
            for (int f_j = 0; f_j < (int)(fd.size()); f_j++) {
              match_matrix(best_index,f_j) = std::nan("");
            }
          }
          for (int m_j = 0; m_j < (int)(measurements.size()); m_j++) {
            if (match_matrix(m_j,f_i) > MATCH_LEVEL_THRESHOLD_ASSOCIATE){
              match_matrix(m_j,f_i) = std::nan("");
            }
          }
        }

        for( auto& match_curr : matches){
          if (_debug_){
            ROS_INFO_STREAM("[UVDARKalman]: Updating state: " << match_curr.second << " with " << measurements[match_curr.first].x.transpose());
            ROS_INFO_STREAM("[UVDARKalman]: This yelds state: " << tentative_states[match_curr.first][match_curr.second].filter_state.x.topRows(6).transpose());
          }
          fd[match_curr.second] = tentative_states[match_curr.first][match_curr.second];

        }

        if (_debug_){
          ROS_INFO_STREAM("[UVDARKalman]: match_matrix post: " << std::endl <<match_matrix);
        }
        int fd_size_orig = (int)(fd.size());
        for (int f_i = 0; f_i < fd_size_orig; f_i++) {
          for (int m_i = 0; m_i < (int)(measurements.size()); m_i++) {
            if (_debug_){
              ROS_INFO_STREAM("[UVDARKalman]: match_matrix at: [" << m_i << ":" << f_i << "] is: " << match_matrix(m_i,f_i));
            }
            if (!isnan(match_matrix(m_i,f_i))){
              initiateNew(measurements[m_i].x, measurements[m_i].P, meas_time);
              for (int f_j = 0; f_j < fd_size_orig; f_j++) {
                match_matrix(m_i,f_j) = std::nan("");
              }
              for (int m_j = 0; m_j < (int)(measurements.size()); m_j++) {
                match_matrix(m_j,f_i) = std::nan("");
              }
            }
          }
        }

      }
      //}

      /**
       * @brief Applies relative position measurements to states that match their IDs
       *
       * @param measurements A vector of measurements with error covariances
       * @param ids A vector of IDs associated with the measurements - deanonymize measurements of multiple targets
       * @param meas_time
       */
      /* applyMeasurementsWithIdentity //{ */
      void applyMeasurementsWithIdentity(std::vector<statecov_t> measurements, std::vector<int> ids, ros::Time meas_time, std::string camera_frame){
        std::scoped_lock lock(filter_mutex);

        if (measurements.size() != ids.size()){
            ROS_ERROR("[UVDARKalman]: the sizes of the input measurement vector and of the vector of their identities do not match! Returning.");
            return;
            }

        for(auto const& measurement_curr : measurements | indexed(0)){
          int id_local = ids[measurement_curr.index()] % 1000;
          int target = -1;
          for (auto const& filter_curr : fd | indexed(0)){
            if (id_local == (int)filter_curr.value().id){
              target = filter_curr.index();
            }
          }
          if (target < 0){
            initiateNew(measurement_curr.value().x, measurement_curr.value().P, meas_time, id_local);
          }
          else {
            [[ maybe_unused ]] double match_level; //for future use with multiple measurements with the same ID
            correctWithMeasurement(fd.at(target),measurement_curr.value(), match_level,meas_time,true, true, camera_frame);
          }

        }

      }
      //}

      /**
       * @brief Publishes the current set of filter states to an output topic
       */
      /* publishStates //{ */
      void publishStates(){
        mrs_msgs::PoseWithCovarianceArrayStamped msg;
        mrs_msgs::PoseWithCovarianceArrayStamped msg_tent;
        msg.header.frame_id = _output_frame_;
        msg.header.stamp = ros::Time::now();
        msg_tent.header = msg.header;

        mrs_msgs::PoseWithCovarianceIdentified temp;
        e::Quaterniond qtemp;
        for (auto const& fd_curr : fd | indexed(0)){
          temp.id = fd_curr.value().id;

          temp.pose.position.x = fd_curr.value().filter_state.x[0];
          temp.pose.position.y = fd_curr.value().filter_state.x[1];
          temp.pose.position.z = fd_curr.value().filter_state.x[2];

          qtemp = e::AngleAxisd(fd_curr.value().filter_state.x[3], e::Vector3d::UnitX()) * e::AngleAxisd(fd_curr.value().filter_state.x[4], e::Vector3d::UnitY()) * e::AngleAxisd(fd_curr.value().filter_state.x[5], e::Vector3d::UnitZ());

          temp.pose.orientation.x = qtemp.x();
          temp.pose.orientation.y = qtemp.y();
          temp.pose.orientation.z = qtemp.z();
          temp.pose.orientation.w = qtemp.w();

          /* for (int m=0; m<6; m++){ */
          /*   for (int n=0; n<6; n++){ */
          /*     temp.covariance[6*n+m] =  fd_curr.value().filter_state.P(n,m); */
          /*   } */
          /* } */
          temp.covariance = eigenCovarianceToRos(fd_curr.value().filter_state.P);

          if (fd_curr.value().update_count < MIN_MEASUREMENTS_TO_VALIDATION){
            msg_tent.poses.push_back(temp);
          }
          else{
            msg.poses.push_back(temp);
          }
        }

        std_msgs::String msg_status;
        msg_status.data = std::string("UVDAR sees "+std::to_string(msg.poses.size())+" targets").c_str();
        pub_status_.publish(msg_status);

        pub_filter_.publish(msg);
        pub_filter_tent_.publish(msg_tent);
      }
      //}

      /**
       * @brief Removes any filter state that contains NaNs.
       */
      /* removeNANs //{ */
      void removeNANs(){
        for (int i=0; i<(int)(fd.size());i++){
          if ( (fd[i].filter_state.x.array().isNaN().any() ) ||
              (fd[i].filter_state.P.array().isNaN().any() )){
            fd.erase(fd.begin()+i);
            i--;
          }
        }
      }
      //}

      /**
       * @brief Remove filter states that overlap too much with others in terms of their state vectors and associated covariances. Of each overlapping pair, the one removed is either the one that was not yet validated with sufficient number of measurements, or the one with larger covariance (less precise knowledge).
       */
      /* removeOverlaps //{ */
      void removeOverlaps(){
        double curr_match_level;
        for (int i=0; i<((int)(fd.size())-1);i++){
          bool remove_first = false;
          for (int j=i+1; j<(int)(fd.size()); j++){
            curr_match_level = gaussJointMaxVal(
                fd[i].filter_state.P.topLeftCorner(3,3),
                fd[j].filter_state.P.topLeftCorner(3,3),
                fd[i].filter_state.x.topRows(3),
                fd[j].filter_state.x.topRows(3)
                );
            if (curr_match_level > MATCH_LEVEL_THRESHOLD_REMOVE){
              auto eigens = fd[i].filter_state.P.topLeftCorner(3,3).eigenvalues();
              double size_i = (eigens.topLeftCorner(3, 1)).norm();
              eigens = fd[j].filter_state.P.topLeftCorner(3,3).eigenvalues();
              double size_j = (eigens.topLeftCorner(3, 1)).norm();
              int n = -1;
              int m = -1;
              if ( (fd[i].update_count < MIN_MEASUREMENTS_TO_VALIDATION) == (fd[j].update_count < MIN_MEASUREMENTS_TO_VALIDATION) ){
                if ( size_j > size_i ){
                  n = j;
                  m = i;
                }
                else {
                  n = i;
                  m = j;
                }
              }
              else {
                if (fd[i].update_count < MIN_MEASUREMENTS_TO_VALIDATION){
                  n = i;
                  m = j;
                }
                else {
                  n = j;
                  m = i;
                }

              }
              fd.erase(fd.begin()+n);
              ROS_INFO_STREAM("[UVDARKalman]: Removing state " << n << " (ID:" << fd[n].id << "): " << fd[n].filter_state.x.transpose() << " due to large overlap with state " << m << ": " << fd[m].filter_state.x.transpose());
              if (n<m){
                remove_first=true;
                break;
              }
              else {
                j--;
              }
            }
          }
          if (remove_first){
            i--;
          }

        }
      }
      //}

      /**
       * @brief Retrieves aviation Roll angle from a quaternion
       *
       * @param q The input quaternion
       *
       * @return The ouput angle
       */
      /* quatRotateRoll //{ */
      double quatToRoll(e::Quaterniond q){
        e::Matrix3d m = q.matrix();
        return atan2(m(2,1),m(2,2));
      }
      //}

      e::MatrixXd rosCovarianceToEigen(const boost::array<double,36> input){
        e::MatrixXd output(6,6);
        /* if ((int)(input.size()) != 36 ){ */
        /*   ROS_ERROR_STREAM("[UVDARKalman]: Covariance to be converted to Eigen matrix has " << input.size() << " elements instead of the expected 36! Returning"); */
        /*   return e::MatrixXd(); */
        /* } */

        for (int i=0; i<6; i++){
          for (int j=0; j<6; j++){
            output(j,i) =  input[6*j+i];
          }
        }

        return output;
      }

      boost::array<double,36> eigenCovarianceToRos(const e::MatrixXd input){
        boost::array<double, 36> output;
        if (((int)(input.rows()) != 6 ) || ((int)(input.cols()) != 6 )){
          ROS_ERROR_STREAM("[UVDARKalman]: Covariance to be converted to Ros has size of  " << input.rows() << "x" << input.cols() << " as opposed to the expected size of 6x6! Returning");
          /* return boost::array<double, 36>(std::nan("")); */
          double n = std::nan("");
          return {n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n,n};
        }

          for (int m=0; m<6; m++){
            for (int n=0; n<6; n++){
              output[6*n+m] = input(n,m);
            }
          }

        return output;
      }

      /**
       * @brief Retrieves aviation Pitch angle from a quaternion
       *
       * @param q The input quaternion
       *
       * @return The ouput angle
       */
      /* quatRotatePitch //{ */
      double quatToPitch(e::Quaterniond q){
        e::Matrix3d m = q.matrix();
        return atan2( -m(2,0), sqrt( m(2,1)*m(2,1) +m(2,2)*m(2,2) )  );
      }
      //}

      /**
       * @brief Retrieves aviation Yaw angle from a quaternion
       *
       * @param q The input quaternion
       *
       * @return The ouput angle
       */
      /* quatRotateYaw //{ */
      double quatToYaw(e::Quaterniond q){
        e::Matrix3d m = q.matrix();
        return atan2(m(1,0),m(0,0));
      }
      //}

      /**
       * @brief Changes the expression of the origAngle, such that if it is updated in a filtering process with newAngle, circularity issues will be avoided
       *
       * @param origAngle The angle, representing prior state, to be updated
       * @param newAngle The new angle, representing new measurement, that affects origAngle in filtering
       *
       * @return The new expression of origAngle
       */
      /* fixAngle //{ */
      double fixAngle(double origAngle, double newAngle){
        double fixedPre;
        if ( (origAngle>(2*M_PI)) || (origAngle<(-2*M_PI)) )  {
          fixedPre = fmod(origAngle,2*M_PI);
        }
        else {
          fixedPre = origAngle;
        }

        if (fixedPre>(M_PI))
          fixedPre = fixedPre - (2.0*M_PI);
        if (fixedPre < (-M_PI))
          fixedPre = fixedPre + (2.0*M_PI);

        if (fabs(newAngle-fixedPre)<M_PI)
          return fixedPre;

        if (fixedPre>newAngle)
          return (fixedPre - (2.0*M_PI));
        else
          return (fixedPre + (2.0*M_PI));
      }
      //}

      /**
       * @brief Updates the shared system matrix in case that it is time step-dependent. This is useful for non-uniform time-steps
       *
       * @param dt - The current time step of the kalman filter process
       *
       * @return The new system matrix
       */
      /* A_dt //{ */
      A_t A_dt(double dt){
        if (_anonymous_measurements_){
          filter_matrices.A <<
            1,0,0, 0,0,0,
            0,1,0, 0,0,0,
            0,0,1, 0,0,0,
            0,0,0, 1,0,0,
            0,0,0, 0,1,0,
            0,0,0, 0,0,1;
        } else {
          if (_use_velocity_){
            filter_matrices.A <<
              1,0,0,dt,0, 0, 0,0,0,
              0,1,0,0, dt,0, 0,0,0,
              0,0,1,0, 0, dt,0,0,0,
              0,0,0,1, 0, 0, 0,0,0,
              0,0,0,0, 1, 0, 0,0,0,
              0,0,0,0, 0, 1, 0,0,0,
              0,0,0,0, 0, 0, 1,0,0,
              0,0,0,0, 0, 0, 0,1,0,
              0,0,0,0, 0, 0, 0,0,1;
          }
          else{
            filter_matrices.A <<
              1,0,0, 0,0,0,
              0,1,0, 0,0,0,
              0,0,1, 0,0,0,
              0,0,0, 1,0,0,
              0,0,0, 0,1,0,
              0,0,0, 0,0,1;
          }
        }
        return filter_matrices.A;
      }
      //}

      /**
       * @brief Updates the shared process noise covariance matrix in case that it is time step-dependent. This is useful for non-uniform time-steps
       *
       * @param dt - The current time step of the kalman filter process
       *
       * @return The new process noise matrix
       */
      // Q_dt //{ */
      Q_t Q_dt(double dt){
        if (_anonymous_measurements_){
          //simplified to the process described in detail below. These two approaches will be compared
          filter_matrices.Q <<
            vl, 0 ,0, 0, 0 ,0,
            0, vl, 0, 0, 0 ,0,
            0, 0, vv, 0, 0 ,0,
            0, 0, 0,  1, 0 ,0,
            0, 0, 0,  0, 1 ,0,
            0, 0, 0,  0, 0 ,1;
        } else {
          if (_use_velocity_){
            filter_matrices.Q <<
              sn*sn, 0,     0,     0,  0,  0,  0, 0, 0,
              0,     sn*sn, 0,     0,  0,  0,  0, 0, 0,
              0,     0,     sn*sn, 0,  0,  0,  0, 0, 0,
              0,     0,     0,     vl, 0,  0,  0, 0, 0,
              0,     0,     0,     0,  vl, 0,  0, 0, 0,
              0,     0,     0,     0,  0,  vv, 0, 0, 0,
              0,     0,     0,     0,  0,  0,  1, 0, 0,
              0,     0,     0,     0,  0,  0,  0, 1, 0,
              0,     0,     0,     0,  0,  0,  0, 0, 1;
          }
          else{
            // This is an unorthodox approach.
            // I wanted for the process noise covariance Q (expressing a multivariate gaussian) to cover not only the noise in static position estimate, but to also take into account the mean of expected relative velocity, which is not a state variable in this case.
            // The noise should inflate the error covariance of the state approximately linearly. This can be demonstrated in the following example:
            //  In this filter, we can receive measurement with time-stamp after a previous step of prediction. We therefore first need to predict the effects of process noise up to the measurement time, we then correct the state with the measurement and in the next prediction step we need to expand the error covariance from the measurement time to the next step in the regular process.
            //  If the expansion was not to be linear (or at least close to linear), the two prediction steps, splitting a normal time step into two parts, would not add up to the same result as a single prediction with time step equal to the sum of the two.
            // To approach linearity without drastically changing the Kalman filter process (it must be based on multivariate Gaussians) while including an unknown velocity of uniform distribution, we need a Q that represents the sum of the influences of position estimate noise and of the drift due to unknown velocity.
            // The function used here was obtained thusly:
            // We want a normal distribution to approximate an original normal distribution (accounting only for static position noise sn) expanded by uniform (since we don't have any information on the velocity) distribution f_u(x) limited by maximum velocity v_max*dt=s_max (maximum position shift caused by unknown velocity)
            // This uniform distribution has center at mu, and its values are {f_u(x) = 1/(2*s_max) for abs(x-mu) < s_max} and {f_u(x) = 0 otherwise}, so that the integral of the distribudion is 1.
            // A normal distribution with the same standard deviation as such a sum of a normal and uniform distributions resolves (after some calculations) to one with a new standard deviation:
            //  sn_new^2 = (1/2)*sn^2 + (1/6)*v_max^2*dt^2
            // This is not linear. We may be overcomplicating the issue here, and this is a subject to testing in the future.
            filter_matrices.Q <<
              0.5*sn*sn+0.16667*vl*vl*dt*dt,0,0, 0,0,0,
              0,0.5*sn*sn+0.16667*vl*vl*dt*dt,0, 0,0,0,
              0,0,0.5*sn*sn+0.16667*vv*vv*dt*dt, 0,0,0,
              0,0,0, 0.5,0,0,
              0,0,0, 0,0.5,0,
              0,0,0, 0,0,0.5;
          }
        }
        return filter_matrices.Q;
      }
      //}

  };

} //uvdar

int main(int argc, char** argv) {
  ros::init(argc, argv, "uvdar_kalman_anonymous");
  ros::NodeHandle nh("~");
  uvdar::UVDARKalman        kl(nh);

  ROS_INFO("[UVDARKalman]: filter node initiated");

  ros::spin();

  return 0;
}


/*
    Copyright 2013 Adobe
    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*/
/**************************************************************************************************/

#ifndef ADOBE_ALGORITHM_IOTA_HPP
#define ADOBE_ALGORITHM_IOTA_HPP

#include <adobe/config.hpp>

#include <adobe/functional.hpp>

#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>

#include <algorithm>

/**************************************************************************************************/

namespace adobe {

/**************************************************************************************************/
/*!
\defgroup iota iota
\ingroup numeric

\c iota assigns sequentially increasing values to a range. That is, it assigns \c value to
\c *first, value + 1 to *(first + 1) and so on. In general, each
iterator \c i in the range [first, last) is assigned
value + (i - first).

\result the next value is returned.

\requirements
    - \c ForwardIterator is mutable.
    - \c T is Assignable.
    - If \c x is an object of type \c T, then \c ++x is defined.
    - \c T is convertible to ForwardIterator::value_type
    - [first, last) is a valid range.

\complexity
    - Linear. Exactly last - first assignments.
*/
/**************************************************************************************************/
/*!
    \ingroup iota

    \brief iota implementation
*/
template <class ForwardIterator, class T>
T iota(ForwardIterator first, ForwardIterator last, const T& value) {
    T result = value;
    while (first != last) {
        *first = result;
        ++first;
        ++result;
    }
    return result;
}

/*!
    \ingroup iota

    \brief iota implementation
*/
template <class ForwardRange, class T>
T iota(ForwardRange& range, const T& value) {
    return adobe::iota(boost::begin(range), boost::end(range), value);
}

/**************************************************************************************************/

} // namespace adobe

/**************************************************************************************************/

#endif

/**************************************************************************************************/


#include <gtest/gtest.h>
#include <vector>
#include <Eigen/Dense>

#include "opw_kinematics/opw_kinematics.h"
#include "opw_kinematics/opw_parameters_examples.h"
#include "opw_kinematics/opw_utilities.h"

const double TOLERANCE = 1e-5; // absolute tolerance for EXPECT_NEAR checks

using opw_kinematics::Transform;

/** @brief Compare every element of two eigen Isometry3 poses.
 */
template <typename T>
void comparePoses(const Transform<T> & Ta, const Transform<T> & Tb)
{
  using Matrix = Eigen::Matrix<T, 3, 3>;
  using Vector = Eigen::Matrix<T, 3, 1>;

  Matrix Ra = Ta.rotation(), Rb = Tb.rotation();
  for (int i = 0; i < Ra.rows(); ++i)
  {
    for (int j = 0; j < Ra.cols(); ++j)
    {
      EXPECT_NEAR(Ra(i, j), Rb(i, j), TOLERANCE);
    }
  }

  Vector pa = Ta.translation(), pb = Tb.translation();
  EXPECT_NEAR(pa[0], pb[0], TOLERANCE);
  EXPECT_NEAR(pa[1], pb[1], TOLERANCE);
  EXPECT_NEAR(pa[2], pb[2], TOLERANCE);
}

TEST(kuka_kr6, forward_kinematics)
{
  const auto kuka = opw_kinematics::makeKukaKR6_R700_sixx<float>();

  std::vector<float> joint_values = {0.2f, 0.2f, 0.2f, 0.2f, 0.2f, 0.2f};
  Transform<float> forward_pose = opw_kinematics::forward(kuka, &joint_values[0]);

  // Compare with copied results from forward kinematics using MoveIt!
  Transform<float> actual_pose;
  actual_pose.matrix()  << -0.5965795, 0.000371195,   0.8025539, 0,
     -0.2724458,   0.9405218,   -0.202958, 0,
     -0.7548948,  -0.3397331,  -0.5609949, 0,
      0        ,   0        ,   0        , 1;
  actual_pose.translation() << 0.7341169, -0.1520347, 0.182639;

  comparePoses(forward_pose, actual_pose);
}

TEST(kuka_kr6, inverse_kinematics)
{
  // this test assumes that the forward kinematics for joint values
  // {0.2, 0.2, 0.2, 0.2, 0.2, 0.2} are correct

  const auto kuka = opw_kinematics::makeKukaKR6_R700_sixx<float>();

  std::vector<float> joint_values = {0.2f, 0.2f, 0.2f, 0.2f, 0.2f, 0.2f};
  Transform<float> forward_pose = opw_kinematics::forward(kuka, &joint_values[0]);

  std::array<float, 6 * 8> sols;
  opw_kinematics::inverse(kuka, forward_pose, sols.data());

  Transform<float> pose;
  for (int i = 0; i < 8; ++i)
  {
    if (opw_kinematics::isValid(&sols[6 * i]))
    {
      // Forward kinematics of a solution should result in the same pose
      pose = opw_kinematics::forward(kuka, &sols[6 * i]);
      comparePoses(forward_pose, pose);
    }
  }
}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-reduction
    Function object implementing sum capabilities

    Returns the sum of the elements of the SIMD vector

    @par Semantic:

    For every parameter of type T

    @code
    scalar<T> r = sum(x);
    @endcode

    is similar to:

    @code
    scalar<T> r = Zero;
    for(result_type i = 0; i != meta::cardinal_of<T>; ++i)
      r += x[i];
    @endcode

  **/
  const boost::dispatch::functor<tag::sum_> sum = {};
} }
#endif

#include <boost/simd/function/scalar/sum.hpp>
#include <boost/simd/function/simd/sum.hpp>

#endif