/* Copyright 2017 Ramakrishnan Kannan */

#ifndef COMMON_TENSOR_HPP_
#define COMMON_TENSOR_HPP_

#include <cblas.h>
#include <armadillo>
#include <fstream>
#include <ios>
#include <random>
#include <string>
#include <type_traits>
#include <utility>
#include <vector>
#include "common/utils.h"

namespace planc {
/**
 * Data is stored such that the unfolding \f$Y_0\f$ is column
 * major.  This means the flattening \f$Y_{N-1}\f$ is row-major,
 * and any other flattening \f$Y_n\f$ can be represented as a set
 * of \f$\prod\limits_{k=n+1}^{N-1}I_k\f$ row major matrices, each
 * of which is \f$I_n \times \prod\limits_{k=0}^{n-1}I_k\f$.
 */

// sgemm (TRANSA, TRANSB, M, N, K, ALPHA, A, LDA, B, LDB, BETA, C, LDC)
// extern "C" void dgemm_(const char*, const char*, const int*,
//                      const int*, const int*, const double*, const double*,
//                      const int*, const double*, const int*, const double*,
//                      double*, const int*);

class Tensor {
 private:
  int m_modes;
  UVEC m_dimensions;
  UWORD m_numel;
  UVEC m_global_idx;
  unsigned int rand_seed;

  // for the time being it is used for debugging purposes
  size_t sub2ind(UVEC sub, UVEC dimensions) {
    assert(sub.n_rows == dimensions.n_rows);
    UVEC cumprod_dims = arma::cumprod(dimensions);
    UVEC cumprod_dims_shifted = arma::shift(cumprod_dims, 1);
    cumprod_dims_shifted(0) = 1;
    size_t idx = arma::dot(cumprod_dims_shifted, sub);
    return idx;
  }
  UVEC ind2sub(UVEC dimensions, size_t idx) {
    //   k = [1 cumprod(siz(1 : end - 1))];
    //   n = length(siz);
    //   idx = idx - 1;
    // for i = n : -1 : 1
    //   div = floor(idx / k(i));
    //   subs( :, i) = div + 1;
    //   idx = idx - k(i) * div;
    // end
    UVEC cumprod_dims = arma::cumprod(dimensions);
    UVEC cumprod_dims_shifted = arma::shift(cumprod_dims, 1);
    cumprod_dims_shifted(0) = 1;
    int modes = dimensions.n_elem;
    UVEC sub = arma::zeros<UVEC>(modes);
    float temp;
    for (int i = modes - 1; i >= 0; i--) {
      temp = std::floor((idx * 1.0) / (cumprod_dims_shifted(i) * 1.0));
      sub(i) = temp;
      idx = idx - cumprod_dims_shifted(i) * temp;
    }
    return sub;
  }

 public:
  std::vector<double> m_data;

  Tensor() {
    this->m_modes = 0;
    this->m_numel = 0;
  }
  /**
   * Constructor that takes only dimensions of every mode as a vector
   */
  explicit Tensor(const UVEC &i_dimensions)
      : m_modes(i_dimensions.n_rows),
        m_dimensions(i_dimensions),
        m_numel(arma::prod(i_dimensions)),
        rand_seed(103) {
    m_data.resize(m_numel);
    randu();
  }
  Tensor(const UVEC &i_dimensions, const UVEC &i_start_idx)
      : m_modes(i_dimensions.n_rows),
        m_dimensions(i_dimensions),
        m_numel(arma::prod(i_dimensions)),
        m_global_idx(i_start_idx),
        rand_seed(103) {
    m_data.resize(m_numel);
    randu();
  }
  /**
   * Need when copying from matrix to Tensor. otherwise copy constructor
   * will be called. The data will be passed in row major order
   *
   */
  Tensor(const UVEC &i_dimensions, double *i_data)
      : m_modes(i_dimensions.n_rows),
        m_dimensions(i_dimensions),
        m_numel(arma::prod(i_dimensions)),
        rand_seed(103) {
    m_data.resize(m_numel);
    memcpy(&this->m_data[0], i_data, sizeof(double) * this->m_numel);
  }
  ~Tensor() {}

  /**
   *  copy constructor
   */
  Tensor(const Tensor &src) {
    clear();
    this->m_numel = src.numel();
    this->m_modes = src.modes();
    this->m_dimensions = src.dimensions();
    this->m_global_idx = src.global_idx();
    this->rand_seed = 103;
    this->m_data = src.m_data;
  }

  Tensor &operator=(const Tensor &other) {  // copy assignment
    if (this != &other) {                   // self-assignment check expected
      clear();
      // this->m_data = new double[other.numel()];  // create storage in this
      this->m_numel = other.numel();
      this->m_modes = other.modes();
      this->m_dimensions = other.dimensions();
      this->m_global_idx = other.global_idx();
      this->m_data = other.m_data;
    }
    return *this;
  }

  void swap(Tensor &in) {
    using std::swap;
    swap(m_numel, in.m_numel);
    swap(m_modes, in.m_modes);
    swap(m_dimensions, in.m_dimensions);
    swap(m_global_idx, in.m_global_idx);
    swap(rand_seed, in.rand_seed);
    swap(m_data, in.m_data);
  }
  /**
   * Clears the data and also destroys the storage
   */

  void clear() {
    this->m_numel = 0;
    this->m_data.clear();
  }

  /// Return the number of modes. It is a scalar value.
  int modes() const { return m_modes; }
  /// Returns a vector of dimensions on every mode
  UVEC dimensions() const { return m_dimensions; }
  UVEC global_idx() const { return m_global_idx; }

  /**
   * Returns the dimension of the input mode.
   * @param[in] a mode of the tensor.
   * @return dimension of ith mode
   */

  int dimension(int i) const { return m_dimensions[i]; }
  /// Returns total number of elements
  UWORD numel() const { return m_numel; }

  void set_idx(const UVEC &i_start_idx) { m_global_idx = i_start_idx; }
  /**
   * Return the product of dimensions except mode i
   * @param[in] mode i
   * @return product of dimensions except mode i
   */
  UWORD dimensions_leave_out_one(int i) const {
    UWORD rc = arma::prod(this->m_dimensions);
    return rc - this->m_dimensions(i);
  }
  /// Zeros out the entire tensor
  void zeros() {
    for (UWORD i = 0; i < this->m_numel; i++) {
      this->m_data[i] = 0;
    }
  }
  /// set the tensor with uniform random.
  void rand() {
#pragma omp parallel for
    for (UWORD i = 0; i < this->m_numel; i++) {
      unsigned int *temp = const_cast<unsigned int *>(&rand_seed);
      this->m_data[i] =
          static_cast<double>(rand_r(temp)) / static_cast<double>(RAND_MAX);
    }
  }
  /// set the tensor with uniform int values
  void randi() {
    std::random_device rd;
    std::mt19937 gen(rd());
    int max_randi = this->m_numel;
    std::uniform_int_distribution<> dis(0, max_randi);
#pragma omp parallel for
    for (UWORD i = 0; i < this->m_numel; i++) {
      this->m_data[i] = dis(gen);
    }
  }
  /**
   * set the tensor with uniform random values starting with a seed.
   * can be used to generate the same tensor again and again.
   * @param[in] an integer seed value preferrably a relative prime number
   */
  void randu(const int i_seed = -1) {
    std::random_device rd;
    std::uniform_real_distribution<> dis(0, 1);
    if (i_seed == -1) {
      std::mt19937 gen(rand_seed);
#pragma omp parallel for
      for (unsigned int i = 0; i < this->m_numel; i++) {
        m_data[i] = dis(gen);
      }
    } else {
      std::mt19937 gen(i_seed);
#pragma omp parallel for
      for (unsigned int i = 0; i < this->m_numel; i++) {
        m_data[i] = dis(gen);
      }
    }
  }

  /**
   * size of krp must be product of all dimensions leaving out nxk.
   * o_mttkrp will be of size dimension[n]xk.
   * Memory must be allocated and freed by the caller
   * @param[in]  i_n mode number
   * @param[in]  i_krp Khatri-rao product matrix leaving out mode i_n
   * @param[out] o_mttkrp pointer to the mttkrp matrix.
   */

  void mttkrp(const int i_n, const MAT &i_krp, MAT *o_mttkrp) const {
    (*o_mttkrp).zeros();
    if (i_n == 0) {
      // if n == 1
      // Ur = khatrirao(U{2: N}, 'r');
      // Y = reshape(X.data, szn, szr);
      // V =  Y * Ur;
      // Compute number of columns of Y_n
      // Technically, we could divide the total number of entries by n,
      // but that seems like a bad decision
      // size_t ncols = arma::prod(this->m_dimensions);
      // ncols /= this->m_dimensions[0];
      // Call matrix matrix multiply
      // call dgemm (TRANSA, TRANSB, M, N, K, ALPHA, A, LDA, B, LDB, BETA, C,
      // LDC) C := alpha*op( A )*op( B ) + beta*C A, B and C are matrices, with
      // op( A ) an m by k matrix, op( B ) a k by n matrix and C an m by n
      // matrix. matricized tensor is m x k is in column major format krp is k x
      // n is in column major format output is m x n in column major format
      // char transa = 'N';
      // char transb = 'N';
      int m = this->m_dimensions[0];
      int n = i_krp.n_cols;
      int k = i_krp.n_rows;
      // int lda = m;
      // int ldb = k;
      // int ldc = m;
      double alpha = 1;
      double beta = 0;
      // sgemm (TRANSA, TRANSB, M, N, K, ALPHA, A, LDA, B, LDB, BETA, C, LDC)
      // dgemm_(&transa, &transb, &m, &n, &k, &alpha, this->m_data,
      // &lda, i_krp.memptr(), &ldb, &beta, o_mttkrp->memptr() , &ldc);
      // printf("mode=%d,i=%d,m=%d,n=%d,k=%d,alpha=%lf,T_stride=%d,lda=%d,krp_stride=%d,ldb=%d,beat=%lf,mttkrp=!!!,ldc=%d\n",0,
      // 0, m, n, k,alpha,0*k*m,m,0*n*k,i_krp.n_rows,beta,n);
      cblas_dgemm(CblasRowMajor, CblasTrans, CblasTrans, m, n, k, alpha,
                  &this->m_data[0], m, i_krp.memptr(), k, beta,
                  o_mttkrp->memptr(), n);

    } else {
      int ncols = 1;
      int nmats = 1;
      int lowrankk = i_krp.n_cols;

      // Count the number of columns
      for (int i = 0; i < i_n; i++) {
        ncols *= this->m_dimensions[i];
      }

      // Count the number of matrices
      for (int i = i_n + 1; i < this->m_modes; i++) {
        nmats *= this->m_dimensions[i];
      }
      // For each matrix...
      for (int i = 0; i < nmats; i++) {
        // char transa = 'T';
        // char transb = 'N';
        int m = this->m_dimensions[i_n];
        int n = lowrankk;
        int k = ncols;
        // int lda = k;  // not sure. could be m. higher confidence on k.
        // int ldb = i_krp.n_rows;
        // int ldc = m;
        double alpha = 1;
        double beta = (i == 0) ? 0 : 1;
        // double *A = this->m_data + i * k * m;
        // double *B = const_cast<double *>(i_krp.memptr()) + i * k;

        // for KRP move ncols*lowrankk
        // for tensor X move as n_cols*blas_n
        // For output matrix don't move anything as beta=1;
        // for reference from gram while moving input tensor like
        // Y->data()+i*nrows*ncols sgemm (TRANSA, TRANSB, M, N, K, ALPHA, A,
        // LDA, B, LDB, BETA, C, LDC)
        // dgemm_(&transa, &transb, &m, &n, &k, &alpha, A,
        // &lda, B , &ldb, &beta, (*o_mttkrp).memptr() , &ldc);
        // printf("mode=%d,i=%d,m=%d,n=%d,k=%d,alpha=%lf,T_stride=%d,lda=%d,krp_stride=%d,ldb=%d,beat=%lf,mttkrp=!!!,ldc=%d\n",i_n,
        // i, m, n, k,alpha,i*k*m,k,i*n*k,nmats*ncols,beta,n);
        cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, m, n, k, alpha,
                    &this->m_data[0] + i * k * m, ncols, i_krp.memptr() + i * k,
                    ncols * nmats, beta, o_mttkrp->memptr(), n);
      }
    }
  }
  /// prints the value of the tensor.
  void print() const {
    INFO << "Dimensions: " << this->m_dimensions;
    for (unsigned int i = 0; i < this->m_numel; i++) {
      std::cout << i << " : " << this->m_data[i] << std::endl;
    }
  }

  void print(const UVEC &global_dims, const UVEC &global_start_sub) {
    UVEC local_sub = arma::zeros<UVEC>(global_dims.n_elem);
    // UVEC global_start_sub = ind2sub(global_dims, global_start_idx);
    UVEC global_sub = arma::zeros<UVEC>(global_dims.n_elem);
    int global_idx;
    for (unsigned int i = 0; i < this->m_numel; i++) {
      local_sub = ind2sub(this->m_dimensions, i);
      global_sub = global_start_sub + local_sub;
      global_idx = sub2ind(global_sub, global_dims);
      std::cout << i << " : " << global_idx << " : " << this->m_data[i]
                << std::endl;
    }
  }
  /// returns the frobenius norm of the tensor
  double norm() const {
    double norm_fro = 0;
    for (unsigned int i = 0; i < this->m_numel; i++) {
      norm_fro += (this->m_data[i] * this->m_data[i]);
    }
    return norm_fro;
  }
  /**
   * Computes the squared error with the input tensor
   * @param[in] b an input tensor
   */
  double err(const Tensor &b) const {
    double norm_fro = 0;
    double err_diff;
    for (unsigned int i = 0; i < this->m_numel; i++) {
      err_diff = this->m_data[i] - b.m_data[i];
      norm_fro += err_diff * err_diff;
    }
    return norm_fro;
  }
  /**
   * Scales the tensor with the constant value.
   * @param[in] scale can be an int, float or double value
   */

  template <typename NumericType>
  void scale(NumericType scale) {
    // static_assert(std::is_arithmetic<NumericType>::value,
    //               "NumericType for scale operation must be numeric");
    for (unsigned int i = 0; i < this->m_numel; i++) {
      this->m_data[i] = this->m_data[i] * scale;
    }
  }
  /**
   * Shifts (add or subtract) the tensor with the constant value.
   * If the i_shift is negative it subtracts, otherwise it adds.
   * @param[in] i_shift can be an int, float or double value
   */
  template <typename NumericType>
  void shift(NumericType i_shift) {
    // static_assert(std::is_arithmetic<NumericType>::value,
    //               "NumericType for shift operation must be numeric");
    for (unsigned int i = 0; i < this->m_numel; i++) {
      this->m_data[i] = this->m_data[i] + i_shift;
    }
  }
  /**
   * Truncate all the value between min and max. Any value beyond
   * the min and max will be truncated to min and max.
   * @param[in] min - any value less than min will be set to min
   * @param[in] max - any value greater than max will be set to max
   */
  template <typename NumericType>
  void bound(NumericType min, NumericType max) {
    // static_assert(std::is_arithmetic<NumericType>::value,
    //               "NumericType for bound operation must be numeric");
    for (unsigned int i = 0; i < this->m_numel; i++) {
      if (this->m_data[i] < min) this->m_data[i] = min;
      if (this->m_data[i] > max) this->m_data[i] = max;
    }
  }
  /**
   * Sets only the lower bound
   * @param[in] min - any value less than min will be set to min
   */

  template <typename NumericType>
  void lower_bound(NumericType min) {
    // static_assert(std::is_arithmetic<NumericType>::value,
    //               "NumericType for bound operation must be numeric");
    for (unsigned int i = 0; i < this->m_numel; i++) {
      if (this->m_data[i] < min) this->m_data[i] = min;
    }
  }

  /**
   * Write the tensor to the given filename. Compile with
   * -D_FILE_OFFSET_BITS=64 to support large files greater than 2GB
   * @param[in] filename as std::string
   */
  void write(std::string filename,
             std::ios_base::openmode mode = std::ios_base::out) {
    std::string filename_no_extension =
        filename.substr(0, filename.find_last_of("."));
    // info file always in text mode
    filename_no_extension.append(".info");
    std::ofstream ofs;
    ofs.open(filename_no_extension, std::ios_base::out);
    // write modes
    ofs << this->m_modes << std::endl;
    // dimension of modes
    for (int i = 0; i < this->m_modes; i++) {
      ofs << this->m_dimensions[i] << std::endl;
    }
    ofs << std::endl;
    ofs.close();
    FILE *fp = fopen(filename.c_str(), "wb");
    INFO << "size of the outputfile in GB "
         << (this->m_numel * 8.0) / (1024 * 1024 * 1024) << std::endl;
    size_t nwrite =
        fwrite(&this->m_data[0], sizeof(std::vector<double>::value_type),
               this->numel(), fp);
    if (nwrite != this->numel()) {
      WARN << "something wrong ::write::" << nwrite
           << "::numel::" << this->numel() << std::endl;
    }
    fclose(fp);
  }
  /**
   * Reads a tensor from the file. Compile with
   * -D_FILE_OFFSET_BITS=64 to support large files greater than 2GB
   * @param[in] filename as std::string
   */

  void read(std::string filename,
            std::ios_base::openmode mode = std::ios_base::in) {
    // clear existing tensor
    if (this->m_numel > 0) {
      this->m_data.clear();  // destroy storage in this
      this->m_numel = 0;
    }
    std::string filename_no_extension =
        filename.substr(0, filename.find_last_of("."));
    filename_no_extension.append(".info");

    std::ifstream ifs;
    // info file always in text mode
    ifs.open(filename_no_extension, std::ios_base::in);
    // write modes
    ifs >> this->m_modes;
    // dimension of modes
    this->m_dimensions = arma::zeros<UVEC>(this->m_modes);
    for (int i = 0; i < this->m_modes; i++) {
      ifs >> this->m_dimensions[i];
    }
    ifs.close();
    // ifs.open(filename, mode);
    FILE *fp = fopen(filename.c_str(), "rb");
    this->m_numel = arma::prod(this->m_dimensions);
    this->m_data.resize(this->m_numel);
    // for (int i = 0; i < this->m_numel; i++) {
    //   ifs >> this->m_data[i];
    // }
    // ifs.read(reinterpret_cast<char *>(this->m_data), sizeof(this->m_data));
    size_t nread =
        fread(&this->m_data[0], sizeof(std::vector<double>::value_type),
              this->numel(), fp);
    if (nread != this->numel()) {
      WARN << "something wrong ::write::" << nread
           << "::numel::" << this->numel() << std::endl;
    }
    // Close the file
    fclose(fp);
  }

  /**
   * Given a vector of subscripts, it return the linear index
   * in the tensor.
   * @param[in] vector of subscript
   * @return linear index within the tensor
   */

  UWORD sub2ind(UVEC sub) {
    assert(sub.n_cols == this->m_dimensions.n_cols);
    UVEC cumprod_dims = arma::cumprod(this->m_dimensions);
    UVEC cumprod_dims_shifted = arma::shift(cumprod_dims, 1);
    cumprod_dims_shifted(0) = 1;
    size_t idx = arma::dot(cumprod_dims_shifted, sub);
    return idx;
  }
  double at(UVEC sub) { return m_data[sub2ind(sub)]; }
};  // class Tensor
}  // namespace planc

void swap(planc::Tensor &x, planc::Tensor &y) { x.swap(y); }

#endif  // COMMON_TENSOR_HPP_


/*
// Licensed to DynamoBI Corporation (DynamoBI) under one
// or more contributor license agreements.  See the NOTICE file
// distributed with this work for additional information
// regarding copyright ownership.  DynamoBI licenses this file
// to you under the Apache License, Version 2.0 (the
// "License"); you may not use this file except in compliance
// with the License.  You may obtain a copy of the License at

//   http://www.apache.org/licenses/LICENSE-2.0

// Unless required by applicable law or agreed to in writing,
// software distributed under the License is distributed on an
// "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied.  See the License for the
// specific language governing permissions and limitations
// under the License.
*/

#include "fennel/common/CommonPreamble.h"
#include "fennel/test/TestBase.h"
#include "fennel/common/TraceSource.h"

#include "fennel/tuple/TupleDataWithBuffer.h"
#include "fennel/tuple/TuplePrinter.h"
#include "fennel/calculator/CalcCommon.h"
#include "fennel/common/FennelExcn.h"

#include <boost/test/test_tools.hpp>
#include <boost/scoped_array.hpp>
#include <string>
#include <limits>
#include <math.h>

using namespace fennel;
using namespace std;

class CalcExtMathTest : virtual public TestBase, public TraceSource
{
    void checkWarnings(Calculator& calc, string expected);

    void testCalcExtMathLogarithms();
    void testCalcExtMathLogarithmsFails();
    void testCalcExtMathPow();
    void testCalcExtMathPowFails();
    void testCalcExtMathAbs();

    void printOutput(
        TupleData const & tup,
        Calculator const & calc);

    string mProgramPower;
public:
    explicit CalcExtMathTest()
        : TraceSource(shared_from_this(), "CalcExtMathTest")
    {
        srand(time(NULL));
        CalcInit::instance();
        FENNEL_UNIT_TEST_CASE(CalcExtMathTest, testCalcExtMathLogarithms);
        FENNEL_UNIT_TEST_CASE(CalcExtMathTest, testCalcExtMathLogarithmsFails);
        FENNEL_UNIT_TEST_CASE(CalcExtMathTest, testCalcExtMathAbs);
        FENNEL_UNIT_TEST_CASE(CalcExtMathTest, testCalcExtMathPow);
        FENNEL_UNIT_TEST_CASE(CalcExtMathTest, testCalcExtMathPowFails);

        //~ Programs used by more than one function -------------------------
        ostringstream pg;

        pg << "O d;" << endl;
        pg << "L d;" << endl;
        pg << "C %s, %s;" << endl;
        pg << "V %s, %s;" << endl;
        pg << "T;" << endl;
        pg << "CALL 'POW(L0, C0, C1);" << endl;
        pg << "REF O0, L0;" << endl;

        mProgramPower = pg.str();
    }

    virtual ~CalcExtMathTest()
    {
    }
};

// for nitty-gritty debugging. sadly, doesn't use BOOST_MESSAGE.
void
CalcExtMathTest::printOutput(
    TupleData const & tup,
    Calculator const & calc)
{
#if 0
    TuplePrinter tuplePrinter;
    tuplePrinter.print(cout, calc.getOutputRegisterDescriptor(), tup);
    cout << endl;
#endif
}

void
CalcExtMathTest::checkWarnings(Calculator& calc, string expected)
{
    try {
        calc.exec();
    } catch (...) {
        BOOST_FAIL("An exception was thrown while running program");
    }

    int i = calc.warnings().find(expected);

    if (i < 0) {
        string msg = "Unexpected or no warning found\n";
        msg += "Expected: ";
        msg += expected;
        msg += "\nActual:  ";
        msg += calc.warnings();

        BOOST_FAIL(msg);
    }
}

void
CalcExtMathTest::testCalcExtMathLogarithms()
{
    ostringstream pg("");

    pg << "O d, d;" << endl;
    pg << "L d, d;" << endl;
    pg << "C d, d;" << endl;
    pg << "V 2.71828, 10.0;" << endl;
    pg << "T;" << endl;
    pg << "CALL 'LN(L0, C0);" << endl;
    pg << "CALL 'LOG10(L1, C1);" << endl;
    pg << "REF O0, L0;" << endl;
    pg << "REF O1, L1;" << endl;

    Calculator calc(0);

    try {
        calc.assemble(pg.str().c_str());
    } catch (FennelExcn& ex) {
        BOOST_FAIL("Assemble exception " << ex.getMessage()<< pg.str());
    }

    TupleDataWithBuffer outTuple(calc.getOutputRegisterDescriptor());
    TupleDataWithBuffer inTuple(calc.getInputRegisterDescriptor());

    calc.bind(&inTuple, &outTuple);
    calc.exec();
    printOutput(outTuple, calc);

    for (int i = 0; i < 2; i++) {
        BOOST_CHECK(
            fabs(
                *(reinterpret_cast<double*>
                  (const_cast<PBuffer>(outTuple[i].pData))) - 1.0)
            < 0.0001);
    }
}

void
CalcExtMathTest::testCalcExtMathLogarithmsFails()
{
    char buff[1024];
    const char* pg =
        "O d;\n"
        "L d;\n"
        "C %s;\n"
        "V %s;\n"
        "T;\n"
        "CALL '%s(L0, C0);\n"
        "REF O0, L0;\n";

    const char* tests[][3] = {
        { "LN", "s8", "0" },
        { "LN", "d", "0.0" },
        { "LN", "s8", "-1" },
        { "LN", "d", "-1.0" },
        { "LOG10", "s8", "0" },
        { "LOG10", "d", "0.0" },
        { "LOG10", "s8", "-1" },
        { "LOG10", "d", "-1.0" },
    };

    int n = sizeof(tests) / sizeof(tests[0]);
    for (int i = 0; i < n; i++) {
        Calculator calc(0);
        sprintf(buff, pg, tests[i][1], tests[i][2], tests[i][0]);
        try {
            calc.assemble(buff);
        } catch (FennelExcn& ex) {
            BOOST_FAIL("Assemble exception " << ex.getMessage() << ex.what());
        }

        TupleDataWithBuffer outTuple(calc.getOutputRegisterDescriptor());
        TupleDataWithBuffer inTuple(calc.getInputRegisterDescriptor());

        calc.bind(&inTuple, &outTuple);
        checkWarnings(calc, "2201E");
        if (!outTuple.containsNull()) {
            BOOST_FAIL("Result should be NULL");
        }
    }
}

void
CalcExtMathTest::testCalcExtMathAbs()
{
    ostringstream pg("");

    pg << "O d, d, d, s8, s8, s8;" << endl;
    pg << "L d, d, d, s8, s8, s8;" << endl;
    pg << "C d, d, d, s8, s8, s8;" << endl;
    // Pick a precision that fits in a double, but wouldn't fit in a float
    pg << "V 0.0, -1234567890123.0,  1234567890123.0,";;
    pg <<   "0, 9223372036854775807, -9223372036854775807;" << endl;
    pg << "T;" << endl;
    pg << "CALL 'ABS(L0, C0);" << endl;
    pg << "CALL 'ABS(L1, C1);" << endl;
    pg << "CALL 'ABS(L2, C2);" << endl;
    pg << "CALL 'ABS(L3, C3);" << endl;
    pg << "CALL 'ABS(L4, C4);" << endl;
    pg << "CALL 'ABS(L5, C5);" << endl;
    pg << "REF O0, L0;" << endl;
    pg << "REF O1, L1;" << endl;
    pg << "REF O2, L2;" << endl;
    pg << "REF O3, L3;" << endl;
    pg << "REF O4, L4;" << endl;
    pg << "REF O5, L5;" << endl;

    //BOOST_MESSAGE(pg.str());

    Calculator calc(0);

    try {
        calc.assemble(pg.str().c_str());
    } catch (FennelExcn& ex) {
        BOOST_FAIL("Assemble exception " << ex.getMessage() << pg.str());
    }

    TupleDataWithBuffer outTuple(calc.getOutputRegisterDescriptor());
    TupleDataWithBuffer inTuple(calc.getInputRegisterDescriptor());

    calc.bind(&inTuple, &outTuple);
    calc.exec();
    printOutput(outTuple, calc);

    double epsilon = 0.000001;

    BOOST_CHECK(
        fabs(
            *(reinterpret_cast<double*>(
                const_cast<PBuffer>(outTuple[0].pData)))
            - 0)
        < epsilon);

    BOOST_CHECK(
        fabs(
            *(reinterpret_cast<double*>(
                const_cast<PBuffer>(outTuple[1].pData)))
            - 1234567890123.0)
        < epsilon);

    BOOST_CHECK(
        fabs(
            *(reinterpret_cast<double*>(
                const_cast<PBuffer>(outTuple[2].pData)))
            - 1234567890123.0)
        < epsilon);

    BOOST_CHECK_EQUAL(
        *(reinterpret_cast<uint64_t*>
          (const_cast<PBuffer>(outTuple[3].pData))), 0);

    BOOST_CHECK_EQUAL(
        *(reinterpret_cast<uint64_t*>(
            const_cast<PBuffer>(outTuple[4].pData))),
        9223372036854775807LL);
    BOOST_CHECK_EQUAL(
        *(reinterpret_cast<uint64_t*>(
            const_cast<PBuffer>(outTuple[5].pData))),
        9223372036854775807LL);
}

void
CalcExtMathTest::testCalcExtMathPow()
{
    char buff[1024];

    const char* tests[][4] = {
        { "d", "d", "2.0", "2.2" },
        { "d", "d", "2.0", "-2.2" },
        { "d", "d", "-2.0", "2.0" },
    };

    double results[] = { 4.5947934, 0.21763764, 4};
    int n = sizeof(results) / sizeof(results[0]);
    assert(n == (sizeof(tests) / sizeof(tests[0])));
    for (int i = 0; i < n; i++) {
        sprintf(
            buff, mProgramPower.c_str(),
            tests[i][0], tests[i][1], tests[i][2], tests[i][3]);

        Calculator calc(0);
        try {
            calc.assemble(buff);
        } catch (FennelExcn& ex) {
            BOOST_FAIL(
                "Assemble exception " << ex.getMessage() << ex.what() << buff);
        }

        TupleDataWithBuffer outTuple(calc.getOutputRegisterDescriptor());
        TupleDataWithBuffer inTuple(calc.getInputRegisterDescriptor());

        calc.bind(&inTuple, &outTuple);
        calc.exec();
        printOutput(outTuple, calc);

        BOOST_CHECK(
            fabs(
                *(reinterpret_cast<double*>(
                    const_cast<PBuffer>(outTuple[0].pData))) - results[i])
            < 0.00001);
    }
}

void
CalcExtMathTest::testCalcExtMathPowFails()
{
    char buff[1024];

    const char* tests[][4] = {
        { "d", "d", "0.0", "-1.0" },
        { "d", "d", "-2.0", "2.2" },
        { "d", "d", "-2.0", "-2.2" },
    };

    int n = sizeof(tests) / sizeof(tests[0]);
    for (int i = 0; i < n; i++) {
        Calculator calc(0);
        sprintf(
            buff, mProgramPower.c_str(),
            tests[i][0], tests[i][1], tests[i][2], tests[i][3]);

        try {
            calc.assemble(buff);
        } catch (FennelExcn& ex) {
            BOOST_FAIL("Assemble exception " << ex.getMessage() << ex.what());
        }

        TupleDataWithBuffer outTuple(calc.getOutputRegisterDescriptor());
        TupleDataWithBuffer inTuple(calc.getInputRegisterDescriptor());

        calc.bind(&inTuple, &outTuple);
        checkWarnings(calc, "2201F");
        if (!outTuple.containsNull()) {
            BOOST_FAIL("Result should be NULL");
        }
    }
}

FENNEL_UNIT_TEST_SUITE(CalcExtMathTest);

// End CalcExtMathTest.cpp


#ifndef YASMIC_UTIL_LOAD_CRM_MATRIX
#define YASMIC_UTIL_LOAD_CRM_MATRIX

/*
 * load_crm_matrix.hpp
 * David Gleich
 * Stanford University
 * 25 March 2006
 */

/**
 * @file load_crm_matrix.hpp
 * Load a matrix into a crm data structure.
 */

#if _MSC_VER >= 1400
	// disable the warning for deprecated c++ commands
    #pragma warning( push )
	#pragma warning( disable : 4996 )
#endif // _MSC_VER >= 1400

#include <cctype>
#include <iostream>
#include <fstream>
#include <iterator>
#include <numeric>

#include <string>
#include <algorithm>

#include <vector>

#include <yasmic/verbose_util.hpp>

#include <boost/iterator/reverse_iterator.hpp>

#include <yasmic/ifstream_matrix.hpp>
#include <yasmic/binary_ifstream_matrix.hpp>
#include <yasmic/binary_ifstream_graph.hpp>
#include <yasmic/cluto_ifstream_matrix.hpp>
#include <yasmic/graph_ifstream_matrix.hpp>

#define BOOST_IOSTREAMS_NO_LIB
#include <boost/iostreams/filtering_stream.hpp>

#ifdef YASMIC_UTIL_LOAD_GZIP

// include the boost code to do the gzip files.
#include <yasmic/boost_mod/gzip.hpp>
// just directly include the BOOST zlib code
#include <yasmic/boost_mod/zlib.cpp>

#endif // YASMIC_UTIL_LOAD_GZIP

/**
 * This function actually loads the data from a matrix file.
 *
 * This function allocates and frees sizeof(Index)*nrows memory.
 */
template <class InputMatrix, class RAIRows, class RAICols, class RAIVals>
bool load_matrix_to_crm(InputMatrix& m,
						RAIRows rows, RAICols cols, RAIVals vals,
						bool rows_populated = false)
{
	using namespace yasmic;
	using namespace std;

	typedef typename iterator_traits<RAIRows>::value_type index_type;

	typename smatrix_traits<InputMatrix>::index_type nr = nrows(m);
    typename smatrix_traits<InputMatrix>::index_type nc = ncols(m);
	typename smatrix_traits<InputMatrix>::nz_index_type nzcount = 0;

	typename smatrix_traits<InputMatrix>::nonzero_iterator nzi, nzend;

	int warning = 0;

    typename smatrix_traits<InputMatrix>::index_type last_good_r = 0;
    typename smatrix_traits<InputMatrix>::index_type last_good_c = 0;
	typename smatrix_traits<InputMatrix>::nz_index_type last_good_nz = 0;

	if (nr < 1)
	{
		using namespace std;
		cerr << "error: invalid number of rows" << endl;
		return (false);
	}

	if (rows_populated == false)
	{
		boost::tie(nzi, nzend) = nonzeros(m);
		for (; nzi != nzend; ++nzi)
		{
			warning = warning | (row(*nzi, m) >= nr) | (column(*nzi, m) >= nc);
            last_good_r = (!warning)*row(*nzi,m);
            last_good_c = (!warning)*column(*nzi,m);
            last_good_nz = (!warning)*nzcount;
			++rows[row(*nzi, m)*(!warning)+1];
			++nzcount;
		}

		if (warning)
		{
			using namespace std;
			cerr << "error: invalid matrix data, nrows or ncols exceeded ("
                << last_good_r << "," << last_good_c << "," << last_good_nz << ")"
                << endl;
			return (false);
		}

		if (nzcount != nnz(m))
		{
			using namespace std;
			cerr << "error: number of nonzeros do not match nnz" << endl;
			return (false);
		}
	}

	// compute the reduction
	partial_sum(rows, rows+(nr+1), rows);

	index_type cr;
	index_type cc;

	nzcount = 0;
	tie(nzi, nzend) = nonzeros(m);
    for (; nzi != nzend; ++nzi)
    {
		cr = row(*nzi, m);
		cc = column(*nzi, m);
		cols[rows[cr]] = cc;
		vals[rows[cr]] = value(*nzi,m);
		++rows[cr];
		++nzcount;

		warning = warning | (cr >= nr) | (cc >= nc);
	}

	if (warning)
	{
		using namespace std;
		cerr << "error: invalid matrix data, nrows or ncols exceeded" << endl;
		return (false);
	}

	if (nzcount != nnz(m))
	{
		using namespace std;
		cerr << "error: number of nonzeros do not match nnz" << endl;
		return (false);
	}

	std::copy(boost::make_reverse_iterator(rows+nr-1),
		boost::make_reverse_iterator(rows),
		boost::make_reverse_iterator(rows+nr));

	rows[0] = 0;

	return (true);
}

/**
 * This function does most of the work loading the matrix.
 *
 * 1.  Allocate storage in the passed std::vectors.
 * 2.  Check for degrees metadata and read.
 * 3.  Load the data for the graph.
 */
template <class InputMatrix, class Index, class Value>
bool load_crm_graph_type(InputMatrix& m, std::string filename,
						 std::vector<Index>& rows,
						 std::vector<Index>& cols,
						 std::vector<Value>& vals,
						 Index& nr, Index& nc, Index& nzcount)
{
	using namespace yasmic;
	using namespace std;

	nr = nrows(m);
	nc = ncols(m);
	nzcount = nnz(m);

	//
	// 1.  Allocate storage
	//
	rows.resize(nr+1);
	cols.resize(nzcount);
	vals.resize(nzcount);

	//
	// 2.  Check for degrees metadata and read.
	//
	bool degrees_data = false;
	{
		// look at the extension...
		typedef std::string::size_type position;

		std::string filename_degrees = filename + ".degs";

		ifstream test(filename_degrees.c_str(), ios::binary);
		if (test.is_open())
		{
			// degrees CAN be binary; we test for a binary file in the following way...
			// 1.  If the filesize is exactly as predicted (sizeof(Index)*nrows)
			// 2.  If the filesize is at least as big as predicted and there
			//     are binary bytes in the first 100 characters
			ifstream::off_type begin = test.tellg();
			test.seekg (0, ios::end);
			ifstream::off_type end = test.tellg();

			bool binary1 = false;
			if (end-begin == sizeof(Index)*nr)
			{
				// passes test 1
				binary1 = true;
			}

			bool binary2 = false;
			if (!binary1 && (unsigned)(end-begin) >= sizeof(Index)*nr)
			{
				test.seekg (0, ios::end);
				// look for any non ASCII data (it would have to be a list of
				// of ints, so ...)
				for (int i = 0; i < 100; i++)
				{
					char c;
					test.get(c);
					// isspace takes care of newline, tabs, etc.
					if (!std::isdigit(c) && !std::isspace(c))
					{
						binary2 = true;
					}
				}
			}

			test.close();

			if (binary1 || binary2)
			{
				YASMIC_VERBOSE( std::cerr << "reading binary degree file..." << std::endl; )

				// the degrees file is binary
				std::ifstream degfile(filename_degrees.c_str(), ios::binary);

				typename std::vector<Index>::iterator i = rows.begin();
				typename std::vector<Index>::iterator iend = rows.end();

				// we read degrees into the second one
				++i;
				while (i != iend)
				{
					Index ci;
					degfile.read((char *)&ci, sizeof(Index));
					*i = ci;
					++i;
				}

				degrees_data = true;

			}
			else
			{
				YASMIC_VERBOSE( std::cerr << "reading ASCII degree file..." << std::endl; )

				// the degrees file is text
				std::ifstream degfile(filename_degrees.c_str());

				copy(istream_iterator<Index>(degfile),
					 istream_iterator<Index>(), ++rows.begin());

				degrees_data = true;
			}
		}
	}

	//
	// 3.  Load the matrix
	//
	return (load_matrix_to_crm(m, rows.begin(), cols.begin(), vals.begin(),
		degrees_data));
}

/**
 * Test if a file with a .graph extension is a smat or not.
 */

template <class Index, class InputStream>
bool load_crm_matrix_graph_test(InputStream& ifs)
{
    using namespace std;

    string line;
    getline(ifs,line);
    istringstream iss(line);

    Index i;
    iss >> i;
    iss >> i;
    iss >> i;

    if (iss.fail())
    {
        return (false);
    }
    else
    {
        return (true);
    }
}

/**
 * Load a CRM matrix from a file into a set of vectors.
 *
 * This operation is actually somewhat tricky.  We to do a few things.
 * 1.  Allocate storage in the passed std::vectors.
 * 2.  Check for degrees metadata and read.
 * 3.  Load the data for the graph.
 *
 * All three operations need an input matrix type, but we need
 * to do the same thing for all, so this function dumps all
 * the work on load_crm_graph_type.
 */
template <class Index, class Value>
bool load_crm_matrix(std::string filename,
					std::vector<Index>& rows, std::vector<Index>& cols,
					std::vector<Value>& vals,
					Index &nr, Index &nc, Index &nzcount)
{
	using namespace std;

	// look at the extension...
	typedef string::size_type position;

	position dot = filename.find_last_of(".");

	if (dot != string::npos)
	{
		string ext = filename.substr(dot+1);
		transform(ext.begin(), ext.end(), ext.begin(), (int(*)(int))tolower);

        // handle the filtering of the iostream
        typedef boost::iostreams::filtering_stream<boost::iostreams::input_seekable> filtered_ifstream;
        filtered_ifstream ios_fifs;
        bool ios_filter = false;

#ifdef YASMIC_UTIL_LOAD_GZIP
        if (ext.compare("gz") == 0)
        {
            position dot2 = filename.find_last_of(".",dot-1);
            if (dot == string::npos)
            {
                cerr << "Error: matrix type indeterminate." << endl;
                return (false);
            }

            string ext2 = filename.substr(dot2+1, (dot)-(dot2+1));
		    transform(ext2.begin(), ext2.end(), ext2.begin(), (int(*)(int))tolower);

            if (ext.compare("gz") == 0)
            {
                ios_filter = true;
                ios_fifs.push(boost::iostreams::gzip_decompressor());
                YASMIC_VERBOSE( std::cerr << "detected gzip format..." << std::endl; )
            }

            ext = ext2;
        }
        else
        {
        }

#endif // YASMIC_UTIL_LOAD_GZIP

        bool smat_graph = false;

        if (ext.compare("graph") == 0)
        {
            // if the first line of a .graph file has 3 entries,
            if (ios_filter)
            {
                ifstream ifs(filename.c_str(), ios_base::in | ios_base::binary);
                ios_fifs.push(ifs);
                smat_graph = load_crm_matrix_graph_test<Index>(ios_fifs);
                ios_fifs.pop();
            }
            else
            {
                ifstream ifs(filename.c_str());
                smat_graph = load_crm_matrix_graph_test<Index>(ifs);
            }
        }

		if (ext.compare("smat") == 0 || smat_graph)
		{
			YASMIC_VERBOSE( std::cerr << "using smat loader..." << std::endl; )

            if (ios_filter)
            {
                ifstream ifs(filename.c_str(), ios_base::in | ios_base::binary);
                ios_fifs.push(ifs);
			    yasmic::ifstream_matrix<> m(ios_fifs);
			    return (load_crm_graph_type(m, filename, rows, cols, vals,
				    		nr, nc, nzcount));
            }
            else
            {
                ifstream ifs(filename.c_str());
                yasmic::ifstream_matrix<> m(ifs);

			    //yasmic::ifstream_matrix<> m(ifs);
			    return (load_crm_graph_type(m, filename, rows, cols, vals,
				    		nr, nc, nzcount));
            }
		}
        else if (ext.compare("bssmat") == 0)
        {
            YASMIC_VERBOSE( std::cerr << "using bssmat loader..." << std::endl; )

            ifstream ifs(filename.c_str(), ios_base::in | ios::binary);

            if (ios_filter)
            {
                ios_fifs.push(ifs);
                yasmic::binary_ifstream_graph<> m(ios_fifs);
                return (load_crm_graph_type(m, filename, rows, cols, vals,
                            nr, nc, nzcount));
            }
            else
            {
                yasmic::binary_ifstream_graph<> m(ifs);
                return (load_crm_graph_type(m, filename, rows, cols, vals,
                            nr, nc, nzcount));
            }
        }
		else if (ext.compare("bsmat") == 0)
		{
			YASMIC_VERBOSE( std::cerr << "using bsmat loader..." << std::endl; )

            ifstream ifs(filename.c_str(), ios_base::in | ios::binary);

            if (ios_filter)
            {
            	ios_fifs.push(ifs);
                yasmic::binary_ifstream_matrix<> m(ios_fifs);
			    return (load_crm_graph_type(m, filename, rows, cols, vals,
				    		nr, nc, nzcount));
            }
            else
            {
                yasmic::binary_ifstream_matrix<> m(ifs);
			    return (load_crm_graph_type(m, filename, rows, cols, vals,
				    		nr, nc, nzcount));
            }
		}
        else if (ext.compare("mat") == 0 || ext.compare("cmat") == 0
                 || ext.compare("cgraph") == 0)
		{
			YASMIC_VERBOSE( std::cerr << "using cluto loader..." << std::endl; )

			ifstream ifs(filename.c_str());
			yasmic::cluto_ifstream_matrix<> m(ifs);
			return (load_crm_graph_type(m, filename, rows, cols, vals,
						nr, nc, nzcount));
		}
        else if (ext.compare("graph") == 0)
        {
            YASMIC_VERBOSE( std::cerr << "using graph loader..." << std::endl; )
            ifstream ifs(filename.c_str());
			yasmic::graph_ifstream_matrix<> m(ifs);
			return (load_crm_graph_type(m, filename, rows, cols, vals,
						nr, nc, nzcount));
        }
		else
		{
			cerr << "Error: matrix type unknown." << endl;
		}
	}
	else
	{
		cerr << "Error: matrix type indeterminate." << endl;
		return (false);
	}

	return (false);
}

namespace yasmic
{
namespace impl
{
	// this is a workaround for the world's most ridiculous portability
	// bug between gcc and msvc.
	int lower_case ( int c )
	{
	    return tolower ( c );
	}
}
}

template <class Index, class Value>
bool load_crm_matrix(std::string filetype_hint, std::string filename,
					std::vector<Index>& rows, std::vector<Index>& cols,
					std::vector<Value>& vals,
					Index &nr, Index &nc, Index &nzcount)
{
    using namespace std;

    // convert the filetype_hint to lower case...
    transform(filetype_hint.begin(),filetype_hint.end(),
        filetype_hint.begin(), yasmic::impl::lower_case);

    if (filetype_hint.compare("cluto") == 0)
    {
        YASMIC_VERBOSE( cerr << "using cluto loader..." << endl; )

        ifstream ifs(filename.c_str());
		yasmic::cluto_ifstream_matrix<> m(ifs);
		return (load_crm_graph_type(m, filename, rows, cols, vals,
					nr, nc, nzcount));
    }
    else if (filetype_hint.compare("graph") == 0)
    {
        YASMIC_VERBOSE( std::cerr << "using graph loader..." << std::endl; )
        ifstream ifs(filename.c_str());
		yasmic::graph_ifstream_matrix<> m(ifs);
		return (load_crm_graph_type(m, filename, rows, cols, vals,
					nr, nc, nzcount));
    }
    else if (filetype_hint.compare("smat") == 0)
    {
        YASMIC_VERBOSE( std::cerr << "using smat loader..." << std::endl; )

		ifstream ifs(filename.c_str());
		yasmic::ifstream_matrix<> m(ifs);
		return (load_crm_graph_type(m, filename, rows, cols, vals,
					nr, nc, nzcount));
    }
    else
    {
        YASMIC_VERBOSE( std::cerr << "filetype hint didn't help, trying the extension loader..." << endl; )
        return (load_crm_matrix(filename, rows, cols, vals, nr, nc, nzcount));
    }
}

#if _MSC_VER >= 1400
	// restore the warning for ifstream::read
	#pragma warning( pop )
#endif // _MSC_VER >= 1400

#endif // YASMIC_UTIL_LOAD_CRM_MATRIX


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_geometry/pinhole_camera_model.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <boost/thread.hpp>
#include <depth_image_proc/depth_traits.h>

#include <sensor_msgs/point_cloud2_iterator.h>

namespace depth_image_proc {

    using namespace message_filters::sync_policies;
    namespace enc = sensor_msgs::image_encodings;
    typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> SyncPolicy;
    typedef ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> ExactSyncPolicy;

    class PointCloudXyzRgbRadialNodelet : public nodelet::Nodelet
    {
	ros::NodeHandlePtr rgb_nh_;

	// Subscriptions
	boost::shared_ptr<image_transport::ImageTransport> rgb_it_, depth_it_;
	image_transport::SubscriberFilter sub_depth_, sub_rgb_;
	message_filters::Subscriber<sensor_msgs::CameraInfo> sub_info_;

	int queue_size_;

	// Publications
	boost::mutex connect_mutex_;
	typedef sensor_msgs::PointCloud2 PointCloud;
	ros::Publisher pub_point_cloud_;

	typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
	typedef message_filters::Synchronizer<ExactSyncPolicy> ExactSynchronizer;
	boost::shared_ptr<Synchronizer> sync_;
	boost::shared_ptr<ExactSynchronizer> exact_sync_;

	std::vector<double> D_;
	boost::array<double, 9> K_;

	int width_;
	int height_;

	cv::Mat transform_;
        image_geometry::PinholeCameraModel model_;

	virtual void onInit();

	void connectCb();

	void imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
		     const sensor_msgs::ImageConstPtr& rgb_msg_in,
		     const sensor_msgs::CameraInfoConstPtr& info_msg);

	// Handles float or uint16 depths
	template<typename T>
	void convert_depth(const sensor_msgs::ImageConstPtr& depth_msg, PointCloud::Ptr& cloud_msg);

	void convert_rgb(const sensor_msgs::ImageConstPtr &rgb_msg, PointCloud::Ptr& cloud_msg,
	  int red_offset, int green_offset, int blue_offset, int color_step);

	cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial);

    };

    cv::Mat PointCloudXyzRgbRadialNodelet::initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial)
    {
	int i,j;
	int totalsize = width*height;
	cv::Mat pixelVectors(1,totalsize,CV_32FC3);
	cv::Mat dst(1,totalsize,CV_32FC3);

	cv::Mat sensorPoints(cv::Size(height,width), CV_32FC2);
	cv::Mat undistortedSensorPoints(1,totalsize, CV_32FC2);

	std::vector<cv::Mat> ch;
	for(j = 0; j < height; j++)
	{
	    for(i = 0; i < width; i++)
	    {
		cv::Vec2f &p = sensorPoints.at<cv::Vec2f>(i,j);
		p[0] = i;
		p[1] = j;
	    }
	}

	sensorPoints = sensorPoints.reshape(2,1);

	cv::undistortPoints(sensorPoints, undistortedSensorPoints, cameraMatrix, distCoeffs);

	ch.push_back(undistortedSensorPoints);
	ch.push_back(cv::Mat::ones(1,totalsize,CV_32FC1));
	cv::merge(ch,pixelVectors);

	if(radial)
	{
	    for(i = 0; i < totalsize; i++)
	    {
		normalize(pixelVectors.at<cv::Vec3f>(i),
			  dst.at<cv::Vec3f>(i));
	    }
	    pixelVectors = dst;
	}
	return pixelVectors.reshape(3,width);
    }

    void PointCloudXyzRgbRadialNodelet::onInit()
    {
	NODELET_INFO("INIT XYZRGB RADIAL");
	ros::NodeHandle& nh         = getNodeHandle();
	ros::NodeHandle& private_nh = getPrivateNodeHandle();

	rgb_nh_.reset( new ros::NodeHandle(nh, "rgb") );
	ros::NodeHandle depth_nh(nh, "depth_registered");
	rgb_it_  .reset( new image_transport::ImageTransport(*rgb_nh_) );
	depth_it_.reset( new image_transport::ImageTransport(depth_nh) );

	// Read parameters
	private_nh.param("queue_size", queue_size_, 5);
	bool use_exact_sync;
	private_nh.param("exact_sync", use_exact_sync, false);

	// Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
	if(use_exact_sync) {
  	  exact_sync_.reset( new ExactSynchronizer(ExactSyncPolicy(queue_size_), sub_depth_, sub_rgb_, sub_info_) );
  	  exact_sync_->registerCallback(
            boost::bind(&PointCloudXyzRgbRadialNodelet::imageCb, this, _1, _2, _3));
	} else {
  	  sync_.reset( new Synchronizer(SyncPolicy(queue_size_), sub_depth_, sub_rgb_, sub_info_) );
  	  sync_->registerCallback(
            boost::bind(&PointCloudXyzRgbRadialNodelet::imageCb, this, _1, _2, _3));
	}
	// Monitor whether anyone is subscribed to the output
	ros::SubscriberStatusCallback connect_cb =
	    boost::bind(&PointCloudXyzRgbRadialNodelet::connectCb, this);
	// Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
	boost::lock_guard<boost::mutex> lock(connect_mutex_);
	pub_point_cloud_ = depth_nh.advertise<PointCloud>("points", 20, connect_cb, connect_cb);
    }

    // Handles (un)subscribing when clients (un)subscribe
    void PointCloudXyzRgbRadialNodelet::connectCb()
    {
	boost::lock_guard<boost::mutex> lock(connect_mutex_);

	if (pub_point_cloud_.getNumSubscribers() == 0)
	{
	    sub_depth_.unsubscribe();
	    sub_rgb_.unsubscribe();
	    sub_info_.unsubscribe();
	}
	else if (!sub_depth_.getSubscriber())
	{
	    ros::NodeHandle& private_nh = getPrivateNodeHandle();
	    // parameter for depth_image_transport hint
	    std::string depth_image_transport_param = "depth_image_transport";

	    // depth image can use different transport.(e.g. compressedDepth)
	    image_transport::TransportHints depth_hints("raw",ros::TransportHints(), private_nh, depth_image_transport_param);
	    sub_depth_.subscribe(*depth_it_, "image_rect",       1, depth_hints);

	    // intensity uses normal ros transport hints.
	    image_transport::TransportHints hints("raw", ros::TransportHints(), private_nh);
	    sub_rgb_.subscribe(*rgb_it_,   "image_rect_color", 1, hints);
	    sub_info_.subscribe(*rgb_nh_,   "camera_info",      1);

	    NODELET_INFO("  subscribed to: %s", sub_rgb_.getTopic().c_str());
	    NODELET_INFO("  subscribed to: %s", sub_depth_.getTopic().c_str());
	    NODELET_INFO("  subscribed to: %s", sub_info_.getTopic().c_str());
	}
    }

    void PointCloudXyzRgbRadialNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
					      const sensor_msgs::ImageConstPtr& rgb_msg_in,
					      const sensor_msgs::CameraInfoConstPtr& info_msg)
    {
	PointCloud::Ptr cloud_msg(new PointCloud);
	cloud_msg->header = depth_msg->header;
	cloud_msg->height = depth_msg->height;
	cloud_msg->width  = depth_msg->width;
	cloud_msg->is_dense = false;
	cloud_msg->is_bigendian = false;

	sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
        pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
//	pcd_modifier.setPointCloud2Fields(6,
//          "x", 1, sensor_msgs::PointField::FLOAT32,
//          "y", 1, sensor_msgs::PointField::FLOAT32,
//          "z", 1, sensor_msgs::PointField::FLOAT32,
//          "r", 1, sensor_msgs::PointField::UINT8,
//          "g", 1, sensor_msgs::PointField::UINT8,
//          "b", 1, sensor_msgs::PointField::UINT8);
	  // Check for bad inputs
  if (depth_msg->header.frame_id != rgb_msg_in->header.frame_id)
  {
    NODELET_ERROR_THROTTLE(5, "Depth image frame id [%s] doesn't match RGB image frame id [%s]",
                           depth_msg->header.frame_id.c_str(), rgb_msg_in->header.frame_id.c_str());
    return;
  }

  // Update camera model
  model_.fromCameraInfo(info_msg);

  // Check if the input image has to be resized
  sensor_msgs::ImageConstPtr rgb_msg = rgb_msg_in;
  if (depth_msg->width != rgb_msg->width || depth_msg->height != rgb_msg->height)
  {
    sensor_msgs::CameraInfo info_msg_tmp = *info_msg;
    info_msg_tmp.width = depth_msg->width;
    info_msg_tmp.height = depth_msg->height;
    float ratio = float(depth_msg->width)/float(rgb_msg->width);
    info_msg_tmp.K[0] *= ratio;
    info_msg_tmp.K[2] *= ratio;
    info_msg_tmp.K[4] *= ratio;
    info_msg_tmp.K[5] *= ratio;
    info_msg_tmp.P[0] *= ratio;
    info_msg_tmp.P[2] *= ratio;
    info_msg_tmp.P[5] *= ratio;
    info_msg_tmp.P[6] *= ratio;
    model_.fromCameraInfo(info_msg_tmp);

	    cv_bridge::CvImageConstPtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvShare(rgb_msg, rgb_msg->encoding);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    cv_bridge::CvImage cv_rsz;
    cv_rsz.header = cv_ptr->header;
    cv_rsz.encoding = cv_ptr->encoding;
    cv::resize(cv_ptr->image.rowRange(0,depth_msg->height/ratio), cv_rsz.image, cv::Size(depth_msg->width, depth_msg->height));
    if ((rgb_msg->encoding == enc::RGB8) || (rgb_msg->encoding == enc::BGR8) || (rgb_msg->encoding == enc::MONO8))
      rgb_msg = cv_rsz.toImageMsg();
    else
      rgb_msg = cv_bridge::toCvCopy(cv_rsz.toImageMsg(), enc::RGB8)->toImageMsg();

    //NODELET_ERROR_THROTTLE(5, "Depth resolution (%ux%u) does not match RGB resolution (%ux%u)",
    //                       depth_msg->width, depth_msg->height, rgb_msg->width, rgb_msg->height);
    //return;
  } else {
    rgb_msg = rgb_msg;
  }

	if(info_msg->D != D_ || info_msg->K != K_ || width_ != info_msg->width ||
	   height_ != info_msg->height)
	{
	    D_ = info_msg->D;
	    K_ = info_msg->K;
	    width_ = info_msg->width;
	    height_ = info_msg->height;
	    transform_ = initMatrix(cv::Mat_<double>(3, 3, &K_[0]),cv::Mat(D_),width_,height_,true);
	}

	if (depth_msg->encoding == enc::TYPE_16UC1)
	{
	    convert_depth<uint16_t>(depth_msg, cloud_msg);
	}
	else if (depth_msg->encoding == enc::TYPE_32FC1)
	{
	    convert_depth<float>(depth_msg, cloud_msg);
	}
	else
	{
	    NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
	    return;
	}

        int red_offset, green_offset, blue_offset, color_step;
	if(rgb_msg->encoding == enc::RGB8)
	{
		red_offset = 0;
		green_offset = 1;
		blue_offset = 2;
		color_step = 3;
	    convert_rgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step);

	}
	if(rgb_msg->encoding == enc::RGBA8)
	{
		red_offset = 0;
		green_offset = 1;
		blue_offset = 2;
		color_step = 4;
	    convert_rgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step);
	}
	else if(rgb_msg->encoding == enc::BGR8)
	{
		red_offset = 2;
		green_offset = 1;
		blue_offset = 0;
		color_step = 3;
	    convert_rgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step);
	}
	else if(rgb_msg->encoding == enc::BGRA8)
	{
		red_offset = 2;
		green_offset = 1;
		blue_offset = 0;
		color_step = 4;
	    convert_rgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step);
	}
	else if(rgb_msg->encoding == enc::MONO8)
	{
		red_offset = 0;
		green_offset = 0;
		blue_offset = 0;
		color_step = 1;
	    convert_rgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step);
	}
	else
	{
	    NODELET_ERROR_THROTTLE(5, "RGB image has unsupported encoding [%s]", rgb_msg->encoding.c_str());
	    return;
	}

	pub_point_cloud_.publish (cloud_msg);
    }

    template<typename T>
    void PointCloudXyzRgbRadialNodelet::convert_depth(const sensor_msgs::ImageConstPtr& depth_msg,
						    PointCloud::Ptr& cloud_msg)
    {
	// Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
	double unit_scaling = DepthTraits<T>::toMeters( T(1) );
	float bad_point = std::numeric_limits<float>::quiet_NaN();

	sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
	sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
	sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
	const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);

	int row_step   = depth_msg->step / sizeof(T);
	for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step)
	{
	    for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z)
	    {
		T depth = depth_row[u];

		// Missing points denoted by NaNs
		if (!DepthTraits<T>::valid(depth))
		{
		    *iter_x = *iter_y = *iter_z = bad_point;
		    continue;
		}
		const cv::Vec3f &cvPoint = transform_.at<cv::Vec3f>(u,v) * DepthTraits<T>::toMeters(depth);
		// Fill in XYZ
		*iter_x = cvPoint(0);
		*iter_y = cvPoint(1);
		*iter_z = cvPoint(2);
	    }
	}
    }

    void PointCloudXyzRgbRadialNodelet::convert_rgb(const sensor_msgs::ImageConstPtr& rgb_msg,
							PointCloud::Ptr& cloud_msg,
							int red_offset, int green_offset, int blue_offset, int color_step)
    {
	sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*cloud_msg, "r");
	sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*cloud_msg, "g");
        sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*cloud_msg, "b");

	const uint8_t* rgb = &rgb_msg->data[0];
	int rgb_skip = rgb_msg->step - rgb_msg->width * color_step;

	for (int v = 0; v < (int)cloud_msg->height; ++v, rgb += rgb_skip)
	{
	    for (int u = 0; u < (int)cloud_msg->width; ++u, rgb += color_step, ++iter_r, ++iter_g, ++iter_b)
	    {
		*iter_r = rgb[red_offset];
		*iter_g = rgb[green_offset];
		*iter_b = rgb[blue_offset];
	    }
	}
    }

} // namespace depth_image_proc

// Register as nodelet
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyzRgbRadialNodelet,nodelet::Nodelet);


/* The MIT License

Copyright (c) 2011 Sahab Yazdani

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#ifdef _WIN32
// conflicts with std::min/max
#define NOMINMAX
#include <windows.h>
#endif // _WIN32

// stl
#include <string>
#include <memory>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <vector>
#include <cmath>
#include <stdexcept>
#include <algorithm>

// boost
#include <boost/timer.hpp>

// PNG library
#include <picopng.h>

// stippler library
#include <stippler.h>

// local
#include "parse_arguments.h"

void write_configuration( std::ostream &output, const Voronoi::StipplingParameters &parameters ) {
	using std::endl;
	using std::abs;
	using std::numeric_limits;

	output << "Generating " << parameters.points << " stipples." << endl;
	output << "Options: ";
	if ( parameters.useColour ) {
		output << "Coloured stipples";
	} else {
		output << "Black stipples";
	}

	if ( parameters.noOverlap ) {
		output << ", Non-overlapping stipples";
	} else {
		output << ", overlapping stipples";
	}

	if ( parameters.fixedRadius ) {
		output << ", Fixed radius";
	} else {
		output << ", Variable radius";
	}

	output << ", Subpixel density of " << parameters.subpixels;

	if ( abs( parameters.sizingFactor - 1.0f ) > numeric_limits<float>::epsilon() ) {
		output << ", Sizing factor of " << parameters.sizingFactor;
	}

	if ( abs( parameters.threshold - 0.1f ) > numeric_limits<float>::epsilon() ) {
		output << ", Displacement Threshold of " << parameters.threshold;
	}

	output << endl;
}

void render( STIPPLER_HANDLE stippler, const Voronoi::StipplingParameters &parameters ) {
	using std::vector;
	using std::ofstream;
	using std::stringstream;
	using std::endl;
	using std::runtime_error;

	vector<StipplePoint> points(parameters.points);
	stippler_getStipples(stippler, &points[0]);

	ofstream outputStream( parameters.outputFile.c_str() );

	if ( !outputStream.is_open() ) {
		stringstream s;
		s << "Unable to open output file " << parameters.outputFile;
		throw runtime_error(s.str());
	}

	PNG::PNGFile *png = PNG::load( parameters.inputFile );
	unsigned long w = png->w, h = png->h;
	PNG::freePng(png);

	outputStream << "<?xml version=\"1.0\" ?>" << endl;
	outputStream << "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">" << endl;
	outputStream << "<svg width=\"" << w << "\" height=\"" << h << "\" version=\"1.1\" xmlns=\"http://www.w3.org/2000/svg\">" << endl;

	for ( vector<StipplePoint>::iterator iter = points.begin(); iter != points.end(); ++iter) {
		float radius = parameters.sizingFactor;
		if ( parameters.fixedRadius ) {
			radius *= 0.5f; // gives circles with 1px diameter
		} else {
			radius *= iter->radius;
		}

		if ( !parameters.useColour ) {
			iter->r = iter->g = iter->b = 0;
		}

		outputStream << "<circle cx=\"" << iter->x << "\" cy=\"" << iter->y << "\" r=\"" << radius << "\" fill=\"rgb(" << (unsigned int)iter->r << "," << (unsigned int)iter->g << "," << (unsigned int)iter->b << ")\" />" << endl;
	}
	outputStream << "</svg>" << endl;

	outputStream.close();
}

int main( int argc, char *argv[] ) {
	using std::auto_ptr;
	using std::exception;
	using std::ofstream;
	using std::cout;
	using std::cerr;
	using std::endl;
	using std::setprecision;
	using std::setiosflags;
	using std::ios;
	using std::min;
	using boost::timer;

	timer total_profiler;

	auto_ptr<Voronoi::StipplingParameters> parameters;

	stippler_lib_init();
	::atexit(stippler_lib_destroy);

	try {
		parameters = parseArguments( argc, argv );
		if (parameters.get() == NULL) {
			stippler_lib_destroy();
			return 0;
		}
	} catch ( ... ) {
		return -1;
	}

	STIPPLER_HANDLE stippler;

	ofstream log;
	if ( parameters->createLogs ) {
		log.open( "log.txt" );
	}

	stippler = create_stippler( parameters.get() );
	if (stippler == NULL) {
		delete[] parameters.get()->inputFile;
		cerr << stippler_getLastError() << endl;

		return -1;
	}

	write_configuration( cout, *(parameters.get()) );
	if ( parameters->createLogs ) {
		write_configuration( log, *(parameters.get()) );
	}

	int iteration = 0;
	float t = parameters->threshold + 1.0f;
	timer iteration_profiler;

	do {
		iteration_profiler.restart();

		stippler_distribute(stippler);

		t = stippler_getAverageDisplacement( stippler );

		if ( parameters->createLogs ) {
			log << "Current Displacement: " << t << endl;
			cout << "Current Displacement: " << t << endl;
		}

		cout << setiosflags(ios::fixed) << setprecision(2) << min((parameters->threshold / t * 100), 100.0f) << "% Complete" << endl;

		if ( parameters->createLogs ) {
			log << "Iteration " << (++iteration) << " completed in " << iteration_profiler.elapsed() << " seconds." << endl;
			cout << "Iteration " << iteration << " completed in " << iteration_profiler.elapsed() << " seconds." << endl;
		}
	} while ( t > parameters->threshold );

	// render final result to SVG
	try {
		render( stippler, *(parameters.get()) );
	} catch (exception const &e) {
		cerr << e.what();
	}

	delete[] parameters.get()->inputFile;

	if ( parameters->createLogs ) {
		log.close();
	}

	destroy_stippler( stippler );

	cout << "Completed in " << total_profiler.elapsed() << " seconds." << endl;

	return 0;
}


// test CPU direct R solver

#include "test_common_factors.h"

#include <minisam/3rdparty/Catch2/catch.hpp>
#include <minisam/utils/testAssertions.h>

#include <minisam/nonlinear/MarginalCovariance.h>
#include <minisam/core/FactorGraph.h>
#include <minisam/core/Factor.h>
#include <minisam/core/LossFunction.h>
#include <minisam/core/Variables.h>
#include <minisam/core/Eigen.h>  // optimizing Eigen

#include <Eigen/Eigenvalues> // eigen value

using namespace minisam;

using test::PFactor;
using test::BFactor;

// random symmetric matrix positive-defined
Eigen::MatrixXd randomCovMat(int rows) {
  Eigen::MatrixXd symmat;
  double min_eig;
  do {
    symmat = Eigen::MatrixXd::Random(rows, rows);
    symmat = 0.5 * (symmat + symmat.transpose()).eval();  // make symmetric
    Eigen::VectorXcd eigs_complex = Eigen::EigenSolver<Eigen::MatrixXd>(symmat, false).eigenvalues();
    min_eig = 1e9;
    for (int i = 0; i < rows; i++)
      if (min_eig > eigs_complex[i].real())
        min_eig = eigs_complex[i].real();
  } while (min_eig < 1e-3);
  return symmat;
}

/* ************************************************************************** */
TEST_CASE("MarginalCovarianceVariableIndices1", "[nonlinear]") {

  // problem setup: all natural ordering
  // x1(3),  x2(4),    x3(5)
  // 0,1,2,  3,4,5,6,  7,8,9,10,11
  internal::LowerHessianSparsityPattern sparisty;
  sparisty.var_col = {0, 3, 7};
  sparisty.var_dim = {3, 4, 5};
  sparisty.var_ordering.push_back(key('x', 1));
  sparisty.var_ordering.push_back(key('x', 2));
  sparisty.var_ordering.push_back(key('x', 3));

  NaturalOrdering ordering(Eigen::SparseMatrix<double>(12, 12));
  std::vector<int> var_idx_exp, var_idx_act;

  var_idx_exp = {0, 1, 2};
  var_idx_act = internal::getVariableIndices(key('x', 1), ordering, sparisty);
  CHECK(assert_equal_vector(var_idx_exp, var_idx_act));

  var_idx_exp = {3, 4, 5, 6};
  var_idx_act = internal::getVariableIndices(key('x', 2), ordering, sparisty);
  CHECK(assert_equal_vector(var_idx_exp, var_idx_act));

  var_idx_exp = {7, 8, 9, 10, 11};
  var_idx_act = internal::getVariableIndices(key('x', 3), ordering, sparisty);
  CHECK(assert_equal_vector(var_idx_exp, var_idx_act));

  CHECK_THROWS_WITH(internal::getVariableIndices(key('x', 4), ordering, sparisty),
      "[VariableOrdering::searchKey] cannot find key x4 in variable ordering");
}

/* ************************************************************************** */
TEST_CASE("MarginalCovarianceVariableIndices2", "[nonlinear]") {

  // problem setup: all natural ordering
  // x3(5),       x2(4),     x1(3),
  // 4,8,10,5,2,  7,11,0,1,  3,9,6
  internal::LowerHessianSparsityPattern sparisty;
  sparisty.var_col = {0, 5, 9};
  sparisty.var_dim = {5, 4, 3};
  sparisty.var_ordering.push_back(key('x', 3));
  sparisty.var_ordering.push_back(key('x', 2));
  sparisty.var_ordering.push_back(key('x', 1));

  Eigen::VectorXi ordering_idx(12);
  ordering_idx << 4, 8, 10, 5, 2, 7, 11, 0, 1, 3, 9, 6;
  Ordering ordering(ordering_idx);
  std::vector<int> var_idx_exp, var_idx_act;

  var_idx_exp = {3, 9, 6};
  var_idx_act = internal::getVariableIndices(key('x', 1), ordering, sparisty);
  CHECK(assert_equal_vector(var_idx_exp, var_idx_act));

  var_idx_exp = {7, 11, 0, 1};
  var_idx_act = internal::getVariableIndices(key('x', 2), ordering, sparisty);
  CHECK(assert_equal_vector(var_idx_exp, var_idx_act));

  var_idx_exp = {4, 8, 10, 5, 2};
  var_idx_act = internal::getVariableIndices(key('x', 3), ordering, sparisty);
  CHECK(assert_equal_vector(var_idx_exp, var_idx_act));
}

/* ************************************************************************** */
TEST_CASE("MarginalCovariance1", "[nonlinear]") {
  MarginalCovarianceSolver mcov;

  Eigen::MatrixXd mcov_exp;
  Variables vars1, vars2;
  vars1.add(0, (Eigen::Vector3d() << 0, 0, 0).finished());
  vars2.add(0, (Eigen::Vector3d() << 3.4, 6.2, -9.1).finished());
  vars2.add(1, (Eigen::Vector3d() << 0.4, -2.2, 7.8).finished());

  // not well conditioned
  // TODO: not sure why not throw
  FactorGraph graph0;
  CHECK(MarginalCovarianceSolverStatus::SUCCESS != mcov.initialize(graph0, vars1));

  // single prior 1: unit
  FactorGraph graph1;
  graph1.add(PFactor<Eigen::Vector3d>(0, Eigen::Vector3d::Zero(), nullptr));
  CHECK(MarginalCovarianceSolverStatus::SUCCESS == mcov.initialize(graph1, vars1));
  CHECK(assert_equal(Eigen::MatrixXd(Eigen::MatrixXd::Identity(3,3)),
      mcov.marginalCovariance(0)));

  // single prior 2: random
  FactorGraph graph2;
  mcov_exp = randomCovMat(3);
  graph2.add(PFactor<Eigen::Vector3d>(0, Eigen::Vector3d::Zero(),
      GaussianLoss::Covariance(mcov_exp)));
  CHECK(MarginalCovarianceSolverStatus::SUCCESS == mcov.initialize(graph2, vars1));
  CHECK(assert_equal(mcov_exp, mcov.marginalCovariance(0)));

  // single prior 3: doubled
  FactorGraph graph3;
  mcov_exp = randomCovMat(3);
  graph3.add(PFactor<Eigen::Vector3d>(0, Eigen::Vector3d::Zero(),
      GaussianLoss::Covariance(mcov_exp)));
  graph3.add(PFactor<Eigen::Vector3d>(0, Eigen::Vector3d::Zero(),
      GaussianLoss::Covariance(mcov_exp)));
  CHECK(MarginalCovarianceSolverStatus::SUCCESS == mcov.initialize(graph3, vars1));
  mcov_exp = mcov_exp * 0.5;
  CHECK(assert_equal(mcov_exp, mcov.marginalCovariance(0)));

  // prior + between
  FactorGraph graph4;
  Eigen::MatrixXd pcov = randomCovMat(3);
  Eigen::MatrixXd bcov = randomCovMat(3);
  graph4.add(PFactor<Eigen::Vector3d>(0, Eigen::Vector3d::Zero(),
      GaussianLoss::Covariance(pcov)));
  graph4.add(BFactor<Eigen::Vector3d>(0, 1, Eigen::Vector3d::Zero(),
      GaussianLoss::Covariance(bcov)));
  CHECK(MarginalCovarianceSolverStatus::SUCCESS == mcov.initialize(graph4, vars2));
  CHECK(assert_equal(pcov, mcov.marginalCovariance(0)));
  mcov_exp = pcov + bcov;
  CHECK(assert_equal(mcov_exp, mcov.marginalCovariance(1)));
}

/* ************************************************************************** */
TEST_CASE("MarginalCovarianceJoint1", "[nonlinear]") {
  MarginalCovarianceSolver mcov;

  Eigen::MatrixXd mcov_exp;
  Variables vars1, vars2;
  vars2.add(0, (Eigen::Vector3d() << 3.4, 6.2, -9.1).finished());
  vars2.add(1, (Eigen::Vector3d() << 0.4, -2.2, 7.8).finished());

  // not well conditioned
  FactorGraph graph1;
  graph1.add(PFactor<Eigen::Vector3d>(0, Eigen::Vector3d::Zero(), nullptr));
  CHECK(MarginalCovarianceSolverStatus::SUCCESS != mcov.initialize(graph1, vars2));

  // double prior 1: unit
  FactorGraph graph2;
  graph2.add(PFactor<Eigen::Vector3d>(0, Eigen::Vector3d::Zero(), nullptr));
  graph2.add(PFactor<Eigen::Vector3d>(1, Eigen::Vector3d::Zero(), nullptr));
  CHECK(MarginalCovarianceSolverStatus::SUCCESS == mcov.initialize(graph2, vars2));
  CHECK(assert_equal(Eigen::MatrixXd(Eigen::MatrixXd::Identity(6,6)),
      mcov.jointMarginalCovariance({0, 1})));

  // double prior 2: random
  Eigen::MatrixXd rcov1 = randomCovMat(3);
  Eigen::MatrixXd rcov2 = randomCovMat(3);
  FactorGraph graph3;
  graph3.add(PFactor<Eigen::Vector3d>(0, Eigen::Vector3d::Zero(),
      GaussianLoss::Covariance(rcov1)));
  graph3.add(PFactor<Eigen::Vector3d>(1, Eigen::Vector3d::Zero(),
      GaussianLoss::Covariance(rcov2)));
  CHECK(MarginalCovarianceSolverStatus::SUCCESS == mcov.initialize(graph3, vars2));

  mcov_exp = Eigen::MatrixXd::Zero(6,6);
  mcov_exp.block<3,3>(0, 0) = rcov1;
  mcov_exp.block<3,3>(3, 3) = rcov2;
  CHECK(assert_equal(mcov_exp, mcov.jointMarginalCovariance({0, 1})));
  mcov_exp.block<3,3>(3, 3) = rcov1;
  mcov_exp.block<3,3>(0, 0) = rcov2;
  CHECK(assert_equal(mcov_exp, mcov.jointMarginalCovariance({1, 0})));
}


/*
==============================================================================
Kratos
A General Purpose Software for Multi-Physics Finite Element Analysis
Version 1.0 (Released on march 05, 2007).

Copyright 2007
Pooyan Dadvand, Riccardo Rossi
pooyan@cimne.upc.edu
rrossi@cimne.upc.edu
CIMNE (International Center for Numerical Methods in Engineering),
Gran Capita' s/n, 08034 Barcelona, Spain

Permission is hereby granted, free  of charge, to any person obtaining
a  copy  of this  software  and  associated  documentation files  (the
"Software"), to  deal in  the Software without  restriction, including
without limitation  the rights to  use, copy, modify,  merge, publish,
distribute,  sublicense and/or  sell copies  of the  Software,  and to
permit persons to whom the Software  is furnished to do so, subject to
the following condition:

Distribution of this code for  any  commercial purpose  is permissible
ONLY BY DIRECT ARRANGEMENT WITH THE COPYRIGHT OWNER.

The  above  copyright  notice  and  this permission  notice  shall  be
included in all copies or substantial portions of the Software.

THE  SOFTWARE IS  PROVIDED  "AS  IS", WITHOUT  WARRANTY  OF ANY  KIND,
EXPRESS OR  IMPLIED, INCLUDING  BUT NOT LIMITED  TO THE  WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT  SHALL THE AUTHORS OR COPYRIGHT HOLDERS  BE LIABLE FOR ANY
CLAIM, DAMAGES OR  OTHER LIABILITY, WHETHER IN AN  ACTION OF CONTRACT,
TORT  OR OTHERWISE, ARISING  FROM, OUT  OF OR  IN CONNECTION  WITH THE
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

==============================================================================
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: rrossi $
//   Date:                $Date: 2009-01-15 11:11:35 $
//   Revision:            $Revision: 1.5 $
//
//

// System includes

// External includes
#include <boost/python.hpp>
#include <cstring>

// Project includes
#include "includes/define.h"
#include "includes/ublas_interface.h"
#include "python/matrix_python_interface.h"
#include "python/matrix_scalar_operator_python.h"
#include "python/matrix_vector_operator_python.h"

namespace Kratos
{
namespace Python
{

  using namespace boost::python;

  void  AddScalarMatrixToPython()
  {
    /*    	  ReadonlyMatrixPythonInterface<scalar_matrix<double> >::CreateInterface("ScalarMatrix")
		  .def(init<scalar_matrix<double>::size_type, scalar_matrix<double>::size_type>())
		  .def(init<scalar_matrix<double>::size_type, scalar_matrix<double>::size_type, scalar_matrix<double>::value_type>())
	          		  .def(MatrixScalarOperatorPython<scalar_matrix<double>, double, matrix<double>, matrix<double> >())
		  .def(MatrixVectorOperatorPython<scalar_matrix<double>, vector<double> >())
		  .def(MatrixVectorOperatorPython<scalar_matrix<double>, zero_vector<double>, vector<double> >())
		  .def(MatrixVectorOperatorPython<scalar_matrix<double>, unit_vector<double>, vector<double> >())
		  .def(MatrixVectorOperatorPython<scalar_matrix<double>, scalar_vector<double>, vector<double> >())
		  .def(MatrixVectorOperatorPython<scalar_matrix<double>, mapped_vector<double>  >())
		  .def(MatrixVectorOperatorPython<scalar_matrix<double>, compressed_vector<double> >())
		  .def(MatrixVectorOperatorPython<scalar_matrix<double>, coordinate_vector<double> >())
	          .def(MatrixMatrixOperatorPython<scalar_matrix<double>, zero_matrix<double>, matrix<double> >())
	          .def(MatrixMatrixOperatorPython<scalar_matrix<double>, identity_matrix<double>, matrix<double> >())
	          .def(MatrixMatrixOperatorPython<scalar_matrix<double>, scalar_matrix<double>, matrix<double> >())
	          .def(MatrixMatrixOperatorPython<scalar_matrix<double>, matrix<double>, matrix<double> >())
	          .def(MatrixMatrixOperatorPython<scalar_matrix<double>, banded_matrix<double>, matrix<double> >())
	          .def(MatrixMatrixOperatorPython<scalar_matrix<double>, triangular_matrix<double, upper>, matrix<double> >())
	          .def(MatrixMatrixOperatorPython<scalar_matrix<double>, triangular_matrix<double, lower>, matrix<double> >())
	          .def(MatrixMatrixOperatorPython<scalar_matrix<double>, symmetric_matrix<double, upper>, matrix<double> >())
#if defined KRATOS_ADD_HERMITIAN_MATRIX_INTERFACE
	          .def(MatrixMatrixOperatorPython<scalar_matrix<double>, hermitian_matrix<double, upper>, matrix<double> >())
#endif
	          .def(MatrixMatrixOperatorPython<scalar_matrix<double>, mapped_matrix<double>, mapped_matrix<double> >())
	          .def(MatrixMatrixOperatorPython<scalar_matrix<double>, compressed_matrix<double>, compressed_matrix<double> >())
#if defined KRATOS_ADD_COORDINATE_MATRIX_INTERFACE
	          .def(MatrixMatrixOperatorPython<scalar_matrix<double>, coordinate_matrix<double>, coordinate_matrix<double> >())
#endif
		  ;
    */
 	}

}  // namespace Python.

} // Namespace Kratos


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-swar
    This function object aggregates two boost::simd::pack of basic type @c T and cardinal @c N into
    a single boost::simd::pack of cardinal @c 2*N and using a type twice smaller than @c T.

    Conversion of values during this operations are made using the C language conversion rules and
    may induce roll over.

    @par Semantic:
    For any type @c T and integral constant @c N, consider two value @c x and @c y of type
    @c boost::simd::pack<T,N>. The following code:
    @code
    auto r = group(x, y);
    @endcode
    is equivalent to:
    @code
    boost::simd::downgrade_t< boost::simd::pack<T,N> >  r{x[0], ..., x[N-1], y[0], ..., y[N-1]};
    @endcode

    @param v0 First pack to aggregate
    @param v1 Second pack to aggregate
    @return The aggregation of all values in its arguments, converted to the downgraded type.
  **/
  downgrade_t<Value> group(Value const& v0, Value const& v1);
} }
#endif

#include <boost/simd/function/simd/group.hpp>

#endif


#include "data_block.h"
#include <armadillo>
using namespace arma;

namespace multiverso
{
    namespace hybrid_logistic_regression
    {
        DataBlock::~DataBlock()
        {
            //ClearSamples();
        }

        void DataBlock::AddSamples(sp_mat& trn_x, sp_mat& tst_x, vec& trn_y, vec& tst_y)
        {
            this->trn_x = trn_x;
            this->tst_x = tst_x;
            this->trn_y = trn_y;
            this->tst_y = tst_y;
        }

	    //Get the information of the index-th Samples
        void DataBlock::GetSamples(sp_mat &trn_x, sp_mat &tst_x, vec &trn_y, vec &tst_y)
        {
            trn_x = this->trn_x;
            tst_x = this->tst_x;
            trn_y = this->trn_y;
            tst_y = this->tst_y;
        }
    }
}


#pragma once
#include <memory>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Sparse>

#include "../Univariate.hpp"
#include "../Variable.hpp"
#include "../utils.hpp"

#include "Operator.hpp"
#include "SumLocalHam.hpp"

namespace yavque
{

class SumLocalHamEvol final : public Operator, public Univariate
{
private:
	bool conjugate_ = false;
	std::shared_ptr<const detail::SumLocalHamImpl> ham_;

	void dagger_in_place_impl() override { conjugate_ = !conjugate_; }

	SumLocalHamEvol(SumLocalHamEvol&&) = default;
	SumLocalHamEvol(const SumLocalHamEvol&) = default;

public:
	explicit SumLocalHamEvol(const SumLocalHam& ham)
		: Operator(ham.dim(), "SumLocalHamEvol of " + ham.name()), ham_{ham.get_impl()}
	{
	}

	explicit SumLocalHamEvol(const SumLocalHam& ham, Variable var)
		: Operator(ham.dim(), "SumLocalHamEvol of " + ham.name()),
		  Univariate(std::move(var)), ham_{ham.get_impl()}
	{
	}

	SumLocalHamEvol& operator=(const SumLocalHamEvol&) = delete;
	SumLocalHamEvol& operator=(SumLocalHamEvol&&) = delete;

	~SumLocalHamEvol() override = default;

	[[nodiscard]] std::unique_ptr<Operator> clone() const override
	{
		auto p = std::unique_ptr<SumLocalHamEvol>{new SumLocalHamEvol(*this)};
		p->change_variable(Variable{var_.value()});
		return p;
	}

	[[nodiscard]] std::unique_ptr<Operator> log_deriv() const override
	{
		constexpr std::complex<double> I(0., 1.0);
		std::string op_name = std::string("derivative of ") + name(); // change to fmt
		cx_double constant = conjugate_ ? I : -I;
		return std::make_unique<SumLocalHam>(ham_, op_name, constant);
	}

	[[nodiscard]] Eigen::VectorXcd apply_right(const Eigen::VectorXcd& st) const override
	{
		constexpr std::complex<double> I(0., 1.0);
		Eigen::VectorXcd res = st;
		Eigen::MatrixXcd m = ham_->get_local_ham();

		double t = conjugate_ ? -var_.value() : var_.value();
		Eigen::MatrixXcd expm = ham_->local_ham_exp(-I * t);

		for(uint32_t k = 0; k < ham_->num_qubits(); ++k)
		{
			res = apply_single_qubit(res, expm, k);
		}
		return res;
	}

	[[nodiscard]] bool can_merge(const Operator& rhs) const override
	{
		if(const auto* p = dynamic_cast<const SumLocalHamEvol*>(&rhs))
		{
			if(ham_ == p->ham_)
			{
				return true;
			}
		}
		return false;
	}

	[[nodiscard]] std::string desc() const override
	{
		std::ostringstream ss;
		ss << "[" << name() << ", variable name: " << var_.name() << ", "
		   << "variable value: " << var_.value() << "]";
		return ss.str();
	}
};
} // namespace yavque


#pragma once

/* Liveness analysis */

//#include <crab/cfg/basic_block_traits.hpp>
#include <crab/domains/discrete_domains.hpp>
#include <crab/iterators/killgen_fixpoint_iterator.hpp>
#include <crab/support/debug.hpp>
#include <crab/support/stats.hpp>

#include <boost/range/iterator_range.hpp>
#include <unordered_map>

namespace crab {

namespace analyzer {

template <typename V>
using varset_domain = ikos::discrete_domain<V>;

/**
 * Define the main operations for the liveness variable analysis:
 * compute for each basic block the set of live variables, i.e.,
 * variables that might be used in the future
 **/
template <class CFG>
class liveness_analysis_operations
    : public crab::iterators::killgen_operations_api<
          CFG, varset_domain<typename CFG::variable_t>> {

public:
  using varset_domain_t = varset_domain<typename CFG::variable_t>;
  using basic_block_label_t = typename CFG::basic_block_label_t;

private:
  using parent_type =
      crab::iterators::killgen_operations_api<CFG, varset_domain_t>;
  using binding_t = std::pair<varset_domain_t, varset_domain_t>;
  using liveness_map_t = std::unordered_map<basic_block_label_t, binding_t>;

  liveness_map_t m_liveness_map;
public:
  liveness_analysis_operations(CFG cfg) : parent_type(cfg) {}

  virtual bool is_forward() { return false; }

  virtual varset_domain_t entry() {
    varset_domain_t res = varset_domain_t::bottom();
    if (this->m_cfg.has_func_decl()) {
      auto fdecl = this->m_cfg.get_func_decl();
      for (unsigned i = 0, e = fdecl.get_num_outputs(); i < e; ++i) {
        res += fdecl.get_output_name(i);
      }
    }
    return res;
  }

  virtual varset_domain_t merge(varset_domain_t d1, varset_domain_t d2) {
    return d1 | d2;
  }

  virtual void init_fixpoint() {
    for (auto &b :
         boost::make_iterator_range(this->m_cfg.begin(), this->m_cfg.end())) {
      bool is_unreachable_block = false;
      varset_domain_t kill, gen;
      for (auto &s : boost::make_iterator_range(b.rbegin(), b.rend())) {
	if (s.is_unreachable()) {
	  is_unreachable_block = true;
	  break;
	}
        auto const &live = s.get_live();
        for (auto d :
             boost::make_iterator_range(live.defs_begin(), live.defs_end())) {
          kill += d;
          gen -= d;
        }
        for (auto u :
             boost::make_iterator_range(live.uses_begin(), live.uses_end())) {
          gen += u;
        }
      } // end for
      if (!is_unreachable_block) {
	m_liveness_map.insert(std::make_pair(b.label(), binding_t(kill, gen)));
      }
    } // end for
  }

  virtual varset_domain_t analyze(const basic_block_label_t &bb_id,
                                  varset_domain_t in) {
    auto it = m_liveness_map.find(bb_id);
    if (it != m_liveness_map.end()) {
      in -= it->second.first;
      in += it->second.second;
    } else {
      // bb_id is unreachable
      in = varset_domain_t::bottom(); // empty set (i.e., no live variables)
    }
    return in;
  }

  virtual std::string name() { return "Liveness"; }
};

/** Live variable analysis **/
template <typename CFG>
class liveness_analysis : public crab::iterators::killgen_fixpoint_iterator<
                              CFG, liveness_analysis_operations<CFG>> {

  using liveness_analysis_operations_t = liveness_analysis_operations<CFG>;
  using killgen_fixpoint_iterator_t =
      crab::iterators::killgen_fixpoint_iterator<
          CFG, liveness_analysis_operations_t>;

  liveness_analysis(const liveness_analysis<CFG> &other) = delete;
  liveness_analysis<CFG> &
  operator=(const liveness_analysis<CFG> &other) = delete;

  using basic_block_t = typename CFG::basic_block_t;

public:
  using basic_block_label_t = typename CFG::basic_block_label_t;
  using statement_t = typename CFG::statement_t;
  using varname_t = typename CFG::varname_t;
  typedef
      typename liveness_analysis_operations_t::varset_domain_t varset_domain_t;

private:
  liveness_analysis_operations_t m_liveness_op;
  bool m_release_in;

public:
  liveness_analysis(CFG cfg, bool release_in = true)
    : killgen_fixpoint_iterator_t(cfg, m_liveness_op),
      m_liveness_op(cfg), m_release_in(release_in) {}

  void exec() {
    this->run();

    CRAB_LOG("liveness-live", for (auto p
                                   : boost::make_iterator_range(
                                       this->out_begin(), this->out_end())) {
      crab::outs() << basic_block_traits<basic_block_t>::to_string(p.first)
                   << " OUT live variables=" << p.second << "\n";
      ;
    });

    if (m_release_in) {
      this->m_in_map.clear();
    }
  }

  varset_domain_t get(const basic_block_label_t &bb) const {
    auto it = this->m_out_map.find(bb);
    if (it != this->m_out_map.end()) {
      return it->second;
    } else {
      return varset_domain_t::bottom();
    }
  }

  void write(crab_os &o) const {
    o << "TODO: print liveness analysis results\n";
  }
};

template <typename CFG>
inline crab_os &operator<<(crab_os &o, const liveness_analysis<CFG> &l) {
  l.write(o);
  return o;
}

/**
 * Live and Dead variable analysis.
 **/
template <typename CFG> class live_and_dead_analysis {
public:
  using basic_block_label_t = typename CFG::basic_block_label_t;
  using basic_block_t = typename CFG::basic_block_t;
  using statement_t = typename CFG::statement_t;
  using varname_t = typename CFG::varname_t;
  using variable_t = typename CFG::variable_t;
  using varset_domain_t = varset_domain<variable_t>;

private:
  using liveness_analysis_t = liveness_analysis<CFG>;

  // the cfg
  CFG m_cfg;
  // liveness analysis
  std::unique_ptr<liveness_analysis_t> m_live;
  // precompute dead variables might be expensive so user can choose.
  bool m_ignore_dead;
  // map basic blocks to set of dead variables at the end of the
  // blocks
  std::unordered_map<basic_block_label_t, varset_domain_t> m_dead_map;

  // statistics
  unsigned m_max_live;
  unsigned m_total_live;
  unsigned m_total_blocks;

public:
  // for backward compatibility
  // XXX: maybe unused already
  using set_t = varset_domain_t;

  // If ignore_dead is true then dead symbols are not computed.
  live_and_dead_analysis(CFG cfg, bool ignore_dead = false)
      : m_cfg(cfg), m_live(new liveness_analysis_t(m_cfg)),
        m_ignore_dead(ignore_dead), m_max_live(0), m_total_live(0),
        m_total_blocks(0) {}

  live_and_dead_analysis(const live_and_dead_analysis &other) = delete;

  live_and_dead_analysis &
  operator=(const live_and_dead_analysis &other) = delete;

  void exec() {
    m_live->exec();
    if (!m_ignore_dead) {
      crab::ScopedCrabStats __st__("Liveness.precompute_dead_variables");
      /** Remove dead variables locally **/
      for (auto &bb : boost::make_iterator_range(m_cfg.begin(), m_cfg.end())) {
        varset_domain_t live_set = m_live->get(bb.label());
        if (live_set.is_bottom())
          continue;

        varset_domain_t dead_set = m_cfg.get_node(bb.label()).live();
        // dead variables = (USE(bb) U DEF(bb)) \ live_out(bb)
        dead_set -= live_set;
        CRAB_LOG("liveness",
                 crab::outs()
                     << basic_block_traits<basic_block_t>::to_string(bb.label())
                     << " dead variables=" << dead_set << "\n";);
        m_dead_map.insert(std::make_pair(bb.label(), std::move(dead_set)));
        // update statistics
        m_total_live += live_set.size();
	if (live_set.size() > m_max_live) {
	  m_max_live = live_set.size();
	}
        m_total_blocks++;
      }
    }
  }

  // Return the set of live variables at the exit of block bb
  varset_domain_t get(const basic_block_label_t &bb) const {
    return m_live->get(bb);
  }

  // Return the set of dead variables at the exit of block bb
  varset_domain_t dead_exit(const basic_block_label_t &bb) const {
    if (m_ignore_dead) {
      CRAB_WARN("Dead variables were not precomputed during liveness analysis");
    }
    auto it = m_dead_map.find(bb);
    if (it == m_dead_map.end()) {
      return varset_domain_t();
    } else {
      return it->second;
    }
  }

  void get_stats(unsigned &total_live, unsigned &max_live_per_blk,
                 unsigned &avg_live_per_blk) const {
    total_live = m_total_live;
    max_live_per_blk = m_max_live;
    avg_live_per_blk =
        (m_total_blocks == 0 ? 0 : (int)m_total_live / m_total_blocks);
  }

  void write(crab_os &o) const {
    o << "TODO: printing dead variable analysis results\n";
  }
};

template <typename CFG>
inline crab_os &operator<<(crab_os &o, const live_and_dead_analysis<CFG> &l) {
  l.write(o);
  return o;
}

} // end namespace analyzer
} // end namespace crab


/* boost random/shuffle_output.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_SHUFFLE_OUTPUT_HPP
#define BOOST_RANDOM_SHUFFLE_OUTPUT_HPP

#include <iostream>
#include <algorithm>     // std::copy
#include <cassert>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/cstdint.hpp>
#include <boost/random/linear_congruential.hpp>

namespace boost {
namespace random {

/**
 * Instatiations of class template shuffle_output model a
 * \pseudo_random_number_generator. It mixes the output
 * of some (usually \linear_congruential) \uniform_random_number_generator
 * to get better statistical properties.
 * The algorithm is described in
 *
 *  @blockquote
 *  "Improving a poor random number generator", Carter Bays
 *  and S.D. Durham, ACM Transactions on Mathematical Software,
 *  Vol 2, No. 1, March 1976, pp. 59-64.
 *  http://doi.acm.org/10.1145/355666.355670
 *  @endblockquote
 *
 * The output of the base generator is buffered in an array of
 * length k. Every output X(n) has a second role: It gives an
 * index into the array where X(n+1) will be retrieved. Used
 * array elements are replaced with fresh output from the base
 * generator.
 *
 * Template parameters are the base generator and the array
 * length k, which should be around 100. The template parameter
 * val is the validation value checked by validation.
 */
template<class UniformRandomNumberGenerator, int k,
#ifndef BOOST_NO_DEPENDENT_TYPES_IN_TEMPLATE_VALUE_PARAMETERS
  typename UniformRandomNumberGenerator::result_type
#else
  uint32_t
#endif
  val = 0>
class shuffle_output
{
public:
  typedef UniformRandomNumberGenerator base_type;
  typedef typename base_type::result_type result_type;

  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
  BOOST_STATIC_CONSTANT(int, buffer_size = k);

  /**
   * Constructs a @c shuffle_output generator by invoking the
   * default constructor of the base generator.
   *
   * Complexity: Exactly k+1 invocations of the base generator.
   */
  shuffle_output() : _rng() { init(); }
#if defined(BOOST_MSVC) && _MSC_VER < 1300
  // MSVC does not implicitly generate the copy constructor here
  shuffle_output(const shuffle_output & x)
    : _rng(x._rng), y(x.y) { std::copy(x.v, x.v+k, v); }
#endif
  /**
   * Constructs a shuffle_output generator by invoking the one-argument
   * constructor of the base generator with the parameter seed.
   *
   * Complexity: Exactly k+1 invocations of the base generator.
   */
  template<class T>
  explicit shuffle_output(T s) : _rng(s) { init(); }
  /**
   * Constructs a shuffle_output generator by using a copy
   * of the provided generator.
   *
   * Precondition: The template argument UniformRandomNumberGenerator
   * shall denote a CopyConstructible type.
   *
   * Complexity: Exactly k+1 invocations of the base generator.
   */
  explicit shuffle_output(const base_type & rng) : _rng(rng) { init(); }
  template<class It> shuffle_output(It& first, It last)
    : _rng(first, last) { init(); }
  void seed() { _rng.seed(); init(); }
  /**
   * Invokes the one-argument seed method of the base generator
   * with the parameter seed and re-initializes the internal buffer array.
   *
   * Complexity: Exactly k+1 invocations of the base generator.
   */
  template<class T>
  void seed(T s) { _rng.seed(s); init(); }
  template<class It> void seed(It& first, It last)
  {
    _rng.seed(first, last);
    init();
  }

  const base_type& base() const { return _rng; }

  result_type operator()() {
    // calculating the range every time may seem wasteful.  However, this
    // makes the information locally available for the optimizer.
    result_type range = (max)()-(min)()+1;
    int j = k*(y-(min)())/range;
    // assert(0 <= j && j < k);
    y = v[j];
    v[j] = _rng();
    return y;
  }

  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return (_rng.min)(); }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return (_rng.max)(); }
  static bool validation(result_type x) { return val == x; }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const shuffle_output& s)
  {
    os << s._rng << " " << s.y << " ";
    for(int i = 0; i < s.buffer_size; ++i)
      os << s.v[i] << " ";
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, shuffle_output& s)
  {
    is >> s._rng >> std::ws >> s.y >> std::ws;
    for(int i = 0; i < s.buffer_size; ++i)
      is >> s.v[i] >> std::ws;
    return is;
  }
#endif

  friend bool operator==(const shuffle_output& x, const shuffle_output& y)
  { return x._rng == y._rng && x.y == y.y && std::equal(x.v, x.v+k, y.v); }
  friend bool operator!=(const shuffle_output& x, const shuffle_output& y)
  { return !(x == y); }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(const shuffle_output& rhs) const
  { return _rng == rhs._rng && y == rhs.y && std::equal(v, v+k, rhs.v); }
  bool operator!=(const shuffle_output& rhs) const
  { return !(*this == rhs); }
#endif
private:
  void init()
  {
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    BOOST_STATIC_ASSERT(std::numeric_limits<result_type>::is_integer);
#endif
    result_type range = (max)()-(min)();
    assert(range > 0);      // otherwise there would be little choice
    if(static_cast<unsigned long>(k * range) <
       static_cast<unsigned long>(range))  // not a sufficient condition
      // likely overflow with bucket number computation
      assert(!"overflow will occur");

    // we cannot use std::generate, because it uses pass-by-value for _rng
    for(result_type * p = v; p != v+k; ++p)
      *p = _rng();
    y = _rng();
  }

  base_type _rng;
  result_type v[k];
  result_type y;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class UniformRandomNumberGenerator, int k,
#ifndef BOOST_NO_DEPENDENT_TYPES_IN_TEMPLATE_VALUE_PARAMETERS
  typename UniformRandomNumberGenerator::result_type
#else
  uint32_t
#endif
  val>
const bool shuffle_output<UniformRandomNumberGenerator, k, val>::has_fixed_range;

template<class UniformRandomNumberGenerator, int k,
#ifndef BOOST_NO_DEPENDENT_TYPES_IN_TEMPLATE_VALUE_PARAMETERS
  typename UniformRandomNumberGenerator::result_type
#else
  uint32_t
#endif
  val>
const int shuffle_output<UniformRandomNumberGenerator, k, val>::buffer_size;
#endif

} // namespace random

// validation by experiment from Harry Erwin's generator.h (private e-mail)
/**
 * According to Harry Erwin (private e-mail), the specialization
 * @c kreutzer1986 was suggested in:
 *
 * @blockquote
 * "System Simulation: Programming Styles and Languages (International
 * Computer Science Series)", Wolfgang Kreutzer, Addison-Wesley, December 1986.
 * @endblockquote
 */
typedef random::shuffle_output<
    random::linear_congruential<uint32_t, 1366, 150889, 714025, 0>,
  97, 139726> kreutzer1986;

} // namespace boost

#endif // BOOST_RANDOM_SHUFFLE_OUTPUT_HPP


#pragma once
#include <boost/multi_array.hpp>

namespace
{

template <typename T, std::size_t... vs> struct c_array;
template <typename T, std::size_t v, std::size_t... vs> struct c_array<T, v, vs...>
{
    typedef typename c_array<T, vs...>::type type[v];
};
template <typename T> struct c_array<T>
{
    typedef T type;
};
template <typename T, std::size_t level> struct nested_initializer_list
{
      typedef std::initializer_list<typename nested_initializer_list<T, level-1>::type> type;
};
template <typename T> struct nested_initializer_list<T, 1>
{
      typedef std::initializer_list<T> type;
};
template <typename T>
void fill_array(const T& l, typename c_array<T>::type& a)
{
      a = l;
}
template <typename T, int dim, int... dims>
void fill_array(typename nested_initializer_list<T, sizeof...(dims)+1>::type l,
                       typename c_array<T, dim, dims...>::type& a)
{
    assert(l.size() == dim); // could be a static assert in C++14
    int i=0;
    for (auto it = l.begin(); it != l.end(); ++it, ++i) {
        fill_array<T, dims...>(*it, a[i]);
    }
}
template <typename T, int... dims>
boost::multi_array<T, sizeof...(dims)>
make_multi_array(typename nested_initializer_list<T, sizeof...(dims)>::type l)
{
    typename c_array<T, dims...>::type a; // Multidimensional C array
    fill_array<T, dims...>(l, a);
    boost::const_multi_array_ref<T, sizeof...(dims)> mar(
                reinterpret_cast<const T*>(a),
                std::array<int, sizeof...(dims)>{dims...});
    return boost::multi_array<T, sizeof...(dims)>(mar);
}

}   // end of namespace