/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * Licensed 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 <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>

#include "SiconosVector.hpp"
#include "SimpleMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "BlockMatrix.hpp"

#include "SiconosAlgebra.hpp"

using namespace Siconos;

void subprod(const SiconosMatrix& A, const SiconosVector& x, SiconosVector& y, const Index& coord, bool init)
{
  // To compute subY = subA * subX in an "optimized" way (in comparison with y = prod(A,x) )
  // or subY += subA*subX if init = false.

  // coord is [r0A r1A c0A c1A r0x r1x r0y r1y]
  //
  // subA is the sub-matrix of A, for row numbers between r0A and r1A-1 and columns between c0A and c1A-1.
  // The same for x and y with rix and riy.

  // Check dims
  unsigned int rowA = coord[1] - coord[0];
  unsigned int colA = coord[3] - coord[2];
  unsigned int dimX = coord[5] - coord[4];
  unsigned int dimY = coord[7] - coord[6];
  if (colA != dimX)
    SiconosMatrixException::selfThrow("subprod(A,x,y) error: inconsistent sizes between A and x.");

  if (rowA != dimY)
    SiconosMatrixException::selfThrow("subprod(A,x,y) error: inconsistent sizes between A and y.");

  if (dimX > x.size() || dimY > y.size() || rowA > A.size(0) || colA > A.size(1))
    SiconosMatrixException::selfThrow("subprod(A,x,y) error: input index too large.");

  unsigned int numA = A.num();
  unsigned int numX = x.num();
  unsigned int numY = y.num();

  if (numA == 0)  // If A,x or y is Block
    SiconosMatrixException::selfThrow("subprod(A,x,y) error: not yet implemented for A block matrices.");

  if (numA == 6) // A = 0
  {
    if (init)
    {
      if (numY == 1)
        ublas::subrange(*y.dense(), coord[6], coord[7]) *= 0.0;
      else //if(numY==4)
        ublas::subrange(*y.sparse(), coord[6], coord[7]) *= 0.0;
    }
    //else nothing
  }
  else if (numA == 7) // A = identity
  {
    if (!init)
      ublas::subrange(*y.dense(), coord[6], coord[7]) += ublas::subrange(*x.dense(), coord[4], coord[5]);
    else
    {
      // if x and y do not share memory (ie are different objects)
      if (&x != &y)
        noalias(ublas::subrange(*y.dense(), coord[6], coord[7])) = ublas::subrange(*x.dense(), coord[4], coord[5]);

      // else nothing
    }
  }

  else // A is not 0 or identity
  {
    {
      if (init)
      {
        if (&x != &y) // if no common memory between x and y.
        {
          if (numX == 1)
          {
            ublas::vector_range<DenseVect> subX(*x.dense(), ublas::range(coord[4], coord[5]));

            if (numY != 1)
              SiconosMatrixException::selfThrow("prod(A,x,y) error: y (output) must be a dense vector.");
            ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[6], coord[7]));

            if (numA == 1)
            {
              ublas::matrix_range<DenseMat> subA(*A.dense(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) = ublas::prod(subA, subX);
            }
            else if (numA == 2)
            {
              ublas::matrix_range<TriangMat> subA(*A.triang(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) = ublas::prod(subA, subX);
            }
            else if (numA == 3)
            {
              ublas::matrix_range<SymMat> subA(*A.sym(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) = ublas::prod(subA, subX);
            }
            else if (numA == 4)
            {
#ifdef BOOST_LIMITATION
              SiconosMatrixException("SimpleMatrix::subprod warning - ublas::matrix_range<SparseMat> does not exist for your boost distribution and your architecture.");
#else
              ublas::matrix_range<SparseMat> subA(*A.sparse(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) = ublas::prod(subA, subX);
#endif
            }
            else //if(numA==5)
            {
              ublas::matrix_range<BandedMat> subA(*A.banded(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) = ublas::prod(subA, subX);
            }
          }
          else //if(numX == 4)
          {
            ublas::vector_range<SparseVect> subX(*x.sparse(), ublas::range(coord[4], coord[5]));
            if (numY != 1 && numA != 4)
              SiconosMatrixException::selfThrow("prod(A,x,y) error: y (output) must be a dense vector.");

            if (numA == 1)
            {
              ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[6], coord[7]));
              ublas::matrix_range<DenseMat> subA(*A.dense(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) = ublas::prod(subA, subX);
            }
            else if (numA == 2)
            {
              ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[6], coord[7]));
              ublas::matrix_range<TriangMat> subA(*A.triang(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) = ublas::prod(subA, subX);
            }
            else if (numA == 3)
            {
              ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[6], coord[7]));
              ublas::matrix_range<SymMat> subA(*A.sym(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) = ublas::prod(subA, subX);
            }
            else if (numA == 4)
            {
#ifdef BOOST_LIMITATION
              SiconosMatrixException("SimpleMatrix::subprod warning - ublas::matrix_range<SparseMat> does not exist for your boost distribution and your architecture.");
#else
              ublas::matrix_range<SparseMat> subA(*A.sparse(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));

              if (numY == 1)
              {
                ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[6], coord[7]));
                noalias(subY) = ublas::prod(subA, subX);
              }
              else
              {
                ublas::vector_range<SparseVect> subY(*y.sparse(), ublas::range(coord[6], coord[7]));
                noalias(subY) = ublas::prod(subA, subX);
              }
#endif
            }
            else //if(numA==5)
            {
              ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[6], coord[7]));
              ublas::matrix_range<BandedMat> subA(*A.banded(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) = ublas::prod(subA, subX);
            }
          }
        }
        else // if x and y are the same object => alias
        {
          if (numX == 1)
          {
            ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[4], coord[5]));
            if (numA == 1)
            {
              ublas::matrix_range<DenseMat> subA(*A.dense(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY = ublas::prod(subA, subY);
            }
            else if (numA == 2)
            {
              ublas::matrix_range<TriangMat> subA(*A.triang(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY = ublas::prod(subA, subY);
            }
            else if (numA == 3)
            {
              ublas::matrix_range<SymMat> subA(*A.sym(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY = ublas::prod(subA, subY);
            }
            else if (numA == 4)
            {
#ifdef BOOST_LIMITATION
              SiconosMatrixException("SimpleMatrix::subprod warning - ublas::matrix_range<SparseMat> and vector_range<SparseVect> does not exist for your boost distribution and your architecture.");
#else
              ublas::matrix_range<SparseMat> subA(*A.sparse(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY = ublas::prod(subA, subY);
#endif
            }
            else //if(numA==5)
            {
              ublas::matrix_range<BandedMat> subA(*A.banded(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY = ublas::prod(subA, subY);
            }
          }
          else //if(numX == 4)
          {
            ublas::vector_range<SparseVect> subY(*y.sparse(), ublas::range(coord[4], coord[5]));
            if (numA == 1)
            {
              ublas::matrix_range<DenseMat> subA(*A.dense(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY = ublas::prod(subA, subY);
            }
            else if (numA == 2)
            {
              ublas::matrix_range<TriangMat> subA(*A.triang(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY = ublas::prod(subA, subY);
            }
            else if (numA == 3)
            {
              ublas::matrix_range<SymMat> subA(*A.sym(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY = ublas::prod(subA, subY);
            }
            else if (numA == 4)
            {
#ifdef BOOST_LIMITATION
              SiconosMatrixException("SimpleMatrix::subprod warning - ublas::matrix_range<SparseMat> does not exist for your boost distribution and your architecture.");
#else
              ublas::matrix_range<SparseMat> subA(*A.sparse(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY = ublas::prod(subA, subY);
#endif
            }
            else //if(numA==5)
            {
              ublas::matrix_range<BandedMat> subA(*A.banded(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY = ublas::prod(subA, subY);
            }
          }
        }
      }
      else // += case
      {
        if (&x != &y) // if no common memory between x and y.
        {
          if (numX == 1)
          {
            ublas::vector_range<DenseVect> subX(*x.dense(), ublas::range(coord[4], coord[5]));

            if (numY != 1)
              SiconosMatrixException::selfThrow("prod(A,x,y) error: y (output) must be a dense vector.");
            ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[6], coord[7]));

            if (numA == 1)
            {
              ublas::matrix_range<DenseMat> subA(*A.dense(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) += ublas::prod(subA, subX);
            }
            else if (numA == 2)
            {
              ublas::matrix_range<TriangMat> subA(*A.triang(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) += ublas::prod(subA, subX);
            }
            else if (numA == 3)
            {
              ublas::matrix_range<SymMat> subA(*A.sym(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) += ublas::prod(subA, subX);
            }
            else if (numA == 4)
            {
#ifdef BOOST_LIMITATION
              SiconosMatrixException("SimpleMatrix::subprod warning - ublas::matrix_range<SparseMat> does not exist for your boost distribution and your architecture.");
#else
              ublas::matrix_range<SparseMat> subA(*A.sparse(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) += ublas::prod(subA, subX);
#endif
            }
            else //if(numA==5)
            {
              ublas::matrix_range<BandedMat> subA(*A.banded(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) += ublas::prod(subA, subX);
            }
          }
          else //if(numX == 4)
          {
            ublas::vector_range<SparseVect> subX(*x.sparse(), ublas::range(coord[4], coord[5]));
            if (numY != 1 && numA != 4)
              SiconosMatrixException::selfThrow("prod(A,x,y) error: y (output) must be a dense vector.");

            if (numA == 1)
            {
              ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[6], coord[7]));
              ublas::matrix_range<DenseMat> subA(*A.dense(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) += ublas::prod(subA, subX);
            }
            else if (numA == 2)
            {
              ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[6], coord[7]));
              ublas::matrix_range<TriangMat> subA(*A.triang(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) += ublas::prod(subA, subX);
            }
            else if (numA == 3)
            {
              ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[6], coord[7]));
              ublas::matrix_range<SymMat> subA(*A.sym(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) += ublas::prod(subA, subX);
            }
            else if (numA == 4)
            {
#ifdef BOOST_LIMITATION
              SiconosMatrixException("SimpleMatrix::subprod warning - ublas::matrix_range<SparseMat> does not exist for your boost distribution and your architecture.");
#else
              ublas::matrix_range<SparseMat> subA(*A.sparse(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              if (numY == 1)
              {
                ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[6], coord[7]));
                noalias(subY) += ublas::prod(subA, subX);
              }
              else
              {
                ublas::vector_range<SparseVect> subY(*y.sparse(), ublas::range(coord[6], coord[7]));
                noalias(subY) += ublas::prod(subA, subX);
              }
#endif
            }
            else //if(numA==5)
            {
              ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[6], coord[7]));
              ublas::matrix_range<BandedMat> subA(*A.banded(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              noalias(subY) += ublas::prod(subA, subX);
            }
          }
        }
        else // if x and y are the same object => alias
        {
          if (numX == 1)
          {
            ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[4], coord[5]));
            if (numA == 1)
            {
              ublas::matrix_range<DenseMat> subA(*A.dense(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY += ublas::prod(subA, subY);
            }
            else if (numA == 2)
            {
              ublas::matrix_range<TriangMat> subA(*A.triang(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY += ublas::prod(subA, subY);
            }
            else if (numA == 3)
            {
              ublas::matrix_range<SymMat> subA(*A.sym(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY += ublas::prod(subA, subY);
            }
            else if (numA == 4)
            {
#ifdef BOOST_LIMITATION
              SiconosMatrixException("SimpleMatrix::subprod warning - ublas::matrix_range<SparseMat> does not exist for your boost distribution and your architecture.");
#else
              ublas::matrix_range<SparseMat> subA(*A.sparse(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY += ublas::prod(subA, subY);
#endif
            }
            else //if(numA==5)
            {
              ublas::matrix_range<BandedMat> subA(*A.banded(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY += ublas::prod(subA, subY);
            }
          }
          else //if(numX == 4)
          {
            ublas::vector_range<SparseVect> subY(*y.sparse(), ublas::range(coord[4], coord[5]));
            if (numA == 1)
            {
              ublas::matrix_range<DenseMat> subA(*A.dense(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY += ublas::prod(subA, subY);
            }
            else if (numA == 2)
            {
              ublas::matrix_range<TriangMat> subA(*A.triang(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY += ublas::prod(subA, subY);
            }
            else if (numA == 3)
            {
              ublas::matrix_range<SymMat> subA(*A.sym(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY += ublas::prod(subA, subY);
            }
            else if (numA == 4)
            {
#ifdef BOOST_LIMITATION
              SiconosMatrixException("SimpleMatrix::subprod warning - ublas::matrix_range<SparseMat> does not exist for your boost distribution and your architecture.");
#else
              ublas::matrix_range<SparseMat> subA(*A.sparse(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY += ublas::prod(subA, subY);
#endif
            }
            else //if(numA==5)
            {
              ublas::matrix_range<BandedMat> subA(*A.banded(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
              subY += ublas::prod(subA, subY);
            }
          }
        }
      }
    }
  }
}


#ifndef SEQUENTIAL_LINE_SEARCH_REGRESSOR_HPP
#define SEQUENTIAL_LINE_SEARCH_REGRESSOR_HPP

#include <Eigen/Core>
#include <sequential-line-search/kernel-type.hpp>
#include <vector>

namespace sequential_line_search
{
    class Regressor
    {
    public:
        Regressor(const KernelType kernel_type);
        virtual ~Regressor() {}

        unsigned GetNumDims() const { return GetLargeX().rows(); }

        virtual double PredictMu(const Eigen::VectorXd& x) const    = 0;
        virtual double PredictSigma(const Eigen::VectorXd& x) const = 0;

        virtual Eigen::VectorXd PredictMuDerivative(const Eigen::VectorXd& x) const    = 0;
        virtual Eigen::VectorXd PredictSigmaDerivative(const Eigen::VectorXd& x) const = 0;

        virtual const Eigen::VectorXd& GetKernelHyperparams() const = 0;
        virtual double                 GetNoiseHyperparam() const   = 0;

        virtual const Eigen::MatrixXd& GetLargeX() const = 0;
        virtual const Eigen::VectorXd& GetSmallY() const = 0;

        Eigen::VectorXd PredictMaximumPointFromData() const;

        Kernel                   GetKernel() const { return m_kernel; }
        KernelThetaDerivative    GetKernelThetaDerivative() const { return m_kernel_theta_derivative; }
        KernelFirstArgDerivative GetKernelFirstArgDerivative() const { return m_kernel_first_arg_derivative; }

    protected:
        Kernel                   m_kernel;
        KernelThetaDerivative    m_kernel_theta_derivative;
        KernelFirstArgDerivative m_kernel_first_arg_derivative;
    };

    // k
    Eigen::VectorXd CalcSmallK(const Eigen::VectorXd& x,
                               const Eigen::MatrixXd& X,
                               const Eigen::VectorXd& kernel_hyperparameters,
                               const Kernel           kernel);

    // K_y = K_f + sigma^{2} I
    Eigen::MatrixXd CalcLargeKY(const Eigen::MatrixXd& X,
                                const Eigen::VectorXd& kernel_hyperparameters,
                                const double           noise_level,
                                const Kernel           kernel);

    // K_f
    Eigen::MatrixXd
    CalcLargeKF(const Eigen::MatrixXd& X, const Eigen::VectorXd& kernel_hyperparameters, const Kernel kernel);

    // partial k / partial x
    Eigen::MatrixXd CalcSmallKSmallXDerivative(const Eigen::VectorXd&         x,
                                               const Eigen::MatrixXd&         X,
                                               const Eigen::VectorXd&         kernel_hyperparameters,
                                               const KernelFirstArgDerivative kernel_first_arg_derivative);

    // partial K_y / partial theta
    std::vector<Eigen::MatrixXd> CalcLargeKYThetaDerivative(const Eigen::MatrixXd&      X,
                                                            const Eigen::VectorXd&      kernel_hyperparameters,
                                                            const KernelThetaDerivative kernel_theta_derivative);

    // partial K_y / partial sigma^{2}
    Eigen::MatrixXd CalcLargeKYNoiseLevelDerivative(const Eigen::MatrixXd& X,
                                                    const Eigen::VectorXd& kernel_hyperparameters,
                                                    const double           noise_level);

} // namespace sequential_line_search

#endif // SEQUENTIAL_LINE_SEARCH_REGRESSOR_HPP


#include <benchmark/benchmark.h>
#include <functional>
#include <random>
#include <unordered_map>
#include <absl/random/random.h>
#include <boost/random.hpp>
#include "pcg_random.hpp"
#include "pcg_uint128.hpp"
#include "pcg_extras.hpp"

static void BenchCRandom(benchmark::State &state) {
    srand(std::random_device{}());
    for (auto _ : state) {
        benchmark::DoNotOptimize(rand());
    }
}

template<typename G, typename D>
static void BenchStdRandom(benchmark::State &state) {
    G gen{std::random_device{}()};
    for (auto _ : state) {
        D dis(INT_MIN, INT_MAX);
        benchmark::DoNotOptimize(dis(gen));
    }
}

BENCHMARK(BenchCRandom);

BENCHMARK_TEMPLATE(BenchStdRandom, std::minstd_rand0, std::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchStdRandom, std::minstd_rand0, std::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchStdRandom, std::minstd_rand0, std::normal_distribution<>);

BENCHMARK_TEMPLATE(BenchStdRandom, std::minstd_rand, std::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchStdRandom, std::minstd_rand, std::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchStdRandom, std::minstd_rand, std::normal_distribution<>);

BENCHMARK_TEMPLATE(BenchStdRandom, std::mt19937, std::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchStdRandom, std::mt19937, std::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchStdRandom, std::mt19937, std::normal_distribution<>);

BENCHMARK_TEMPLATE(BenchStdRandom, std::mt19937_64, std::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchStdRandom, std::mt19937_64, std::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchStdRandom, std::mt19937_64, std::normal_distribution<>);

BENCHMARK_TEMPLATE(BenchStdRandom, pcg32_fast, std::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchStdRandom, pcg32_fast, std::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchStdRandom, pcg32_fast, std::normal_distribution<>);

BENCHMARK_TEMPLATE(BenchStdRandom, pcg64_fast, std::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchStdRandom, pcg64_fast, std::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchStdRandom, pcg64_fast, std::normal_distribution<>);

template<typename G, typename D>
static void BenchBoostRandom(benchmark::State &state) {
    G gen{std::random_device{}()};
    for (auto _ : state) {
        D dis(INT_MIN, INT_MAX);
        benchmark::DoNotOptimize(dis(gen));
    }
}

BENCHMARK_TEMPLATE(BenchBoostRandom, boost::minstd_rand0, boost::random::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchBoostRandom, boost::minstd_rand0, boost::random::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchBoostRandom, boost::minstd_rand0, boost::random::normal_distribution<>);

BENCHMARK_TEMPLATE(BenchBoostRandom, boost::minstd_rand, boost::random::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchBoostRandom, boost::minstd_rand, boost::random::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchBoostRandom, boost::minstd_rand, boost::random::normal_distribution<>);

BENCHMARK_TEMPLATE(BenchBoostRandom, boost::mt19937, boost::random::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchBoostRandom, boost::mt19937, boost::random::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchBoostRandom, boost::mt19937, boost::random::normal_distribution<>);

BENCHMARK_TEMPLATE(BenchBoostRandom, boost::mt19937_64, boost::random::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchBoostRandom, boost::mt19937_64, boost::random::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchBoostRandom, boost::mt19937_64, boost::random::normal_distribution<>);

BENCHMARK_TEMPLATE(BenchBoostRandom, boost::mt11213b, boost::random::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchBoostRandom, boost::mt11213b, boost::random::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchBoostRandom, boost::mt11213b, boost::random::normal_distribution<>);

template<typename P, typename D>
static void BenchAbRandom(benchmark::State &state) {
    P p{std::random_device{}()};
    for (auto _ : state) {
        D dis(INT_MIN,INT_MAX);
        benchmark::DoNotOptimize(dis(p));
    }
}

BENCHMARK_TEMPLATE(BenchAbRandom,absl::random_internal::pcg32_2018_engine,absl::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchAbRandom,absl::random_internal::pcg32_2018_engine,absl::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchAbRandom,absl::random_internal::pcg32_2018_engine,absl::log_uniform_int_distribution<>);

BENCHMARK_TEMPLATE(BenchAbRandom,absl::random_internal::pcg64_2018_engine,absl::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchAbRandom,absl::random_internal::pcg64_2018_engine,absl::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchAbRandom,absl::random_internal::pcg64_2018_engine,absl::log_uniform_int_distribution<>);

BENCHMARK_TEMPLATE(BenchAbRandom,absl::random_internal::randen_engine<uint32_t>,absl::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchAbRandom,absl::random_internal::randen_engine<uint32_t>,absl::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchAbRandom,absl::random_internal::randen_engine<uint32_t>,absl::log_uniform_int_distribution<>);

BENCHMARK_TEMPLATE(BenchAbRandom,absl::random_internal::randen_engine<uint64_t>,absl::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchAbRandom,absl::random_internal::randen_engine<uint64_t>,absl::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchAbRandom,absl::random_internal::randen_engine<uint64_t>,absl::log_uniform_int_distribution<>);

BENCHMARK_TEMPLATE(BenchAbRandom,pcg32_fast,absl::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchAbRandom,pcg32_fast,absl::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchAbRandom,pcg32_fast,absl::log_uniform_int_distribution<>);

BENCHMARK_TEMPLATE(BenchAbRandom,pcg64_fast,absl::uniform_int_distribution<>);
BENCHMARK_TEMPLATE(BenchAbRandom,pcg64_fast,absl::uniform_real_distribution<>);
BENCHMARK_TEMPLATE(BenchAbRandom,pcg64_fast,absl::log_uniform_int_distribution<>);

int main(int argc, char **argv) {
    benchmark::Initialize(&argc, argv);
    benchmark::RunSpecifiedBenchmarks();
    benchmark::Shutdown();
    return 0;
}


#include <blitz/array.h>
#ifdef BZ_HAVE_STD
	#include <fstream>
#else
	#include <fstream.h>
#endif

BZ_USING_NAMESPACE(blitz)

const char* filename = "io.data";

void write_arrays()
{
    ofstream ofs(filename);
    if (ofs.bad())
    {
        cerr << "Unable to write to file: " << filename << endl;
        exit(1);
    }

    Array<float,3> A(3,4,5);
    A = 111 + tensor::i + 10 * tensor::j + 100 * tensor::k;
    ofs << A << endl;

    Array<float,2> B(3,4);
    B = 11 + tensor::i + 10 * tensor::j;
    ofs << B << endl;

    Array<float,1> C(4);
    C = 1 + tensor::i;
    ofs << C << endl;
}

int main()
{
    write_arrays();

    ifstream ifs(filename);
    if (ifs.bad())
    {
        cerr << "Unable to open file: " << filename << endl;
        exit(1);
    }

    Array<float,3> A;
    Array<float,2> B;
    Array<float,1> C;

    ifs >> A >> B >> C;

    cout << "Arrays restored from file: " << A << B << C << endl;

    return 0;
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2018 INRIA.
 *
 * Licensed 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 "NewtonEulerFrom3DLocalFrameR.hpp"
#include "NewtonEulerDS.hpp"
#include <boost/math/quaternion.hpp>
#include "Interaction.hpp"
#include "BlockVector.hpp"

#include "op3x3.h"

// #define DEBUG_NOCOLOR
// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include "debug.h"

/*
See devNotes.pdf for details. A detailed documentation is available in DevNotes.pdf: chapter 'NewtonEulerR: computation of \nabla q H'. Subsection 'Case FC3D: using the local frame local velocities'
*/
void NewtonEulerFrom3DLocalFrameR::initialize(Interaction& inter)
{
  NewtonEulerFrom1DLocalFrameR::initialize(inter);
  unsigned int qSize = 7 * (inter.getSizeOfDS() / 6);
  /*keep only the distance.*/
  _jachq.reset(new SimpleMatrix(3, qSize));

  _RotationAbsToContactFrame.reset(new SimpleMatrix(3, 3));
  _AUX2.reset(new SimpleMatrix(3, 3));
  //  _isContact=1;
}
void NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts(SP::SiconosVector q1)
{
  DEBUG_BEGIN("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts(SP::SiconosVector q1)\n");
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);

  DEBUG_PRINT("contact normal:\n");
  DEBUG_EXPR(_Nc->display(););
  DEBUG_PRINTF("_Nc->norm2() -1.0 = %e\n",_Nc->norm2()-1.0);
  DEBUG_PRINT("contact point :\n");
  DEBUG_EXPR(_Pc1->display(););
  DEBUG_PRINT("center of mass :\n");
  DEBUG_EXPR(q1->display(););

  assert(_Nc->norm2() >0.0
         && std::abs(_Nc->norm2()-1.0) < 1e-6
         && "NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts. Normal vector not consistent ") ;

  double t[6];
  double * pt = t;

  if (orthoBaseFromVector(&Nx, &Ny, &Nz, pt, pt + 1, pt + 2, pt + 3, pt + 4, pt + 5))
    RuntimeException::selfThrow("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts. Problem in calling orthoBaseFromVector");
  pt = t;
  _RotationAbsToContactFrame->setValue(0, 0, Nx);
  _RotationAbsToContactFrame->setValue(1, 0, *pt);
  _RotationAbsToContactFrame->setValue(2, 0, *(pt + 3));
  _RotationAbsToContactFrame->setValue(0, 1, Ny);
  _RotationAbsToContactFrame->setValue(1, 1, *(pt + 1));
  _RotationAbsToContactFrame->setValue(2, 1, *(pt + 4));
  _RotationAbsToContactFrame->setValue(0, 2, Nz);
  _RotationAbsToContactFrame->setValue(1, 2, *(pt + 2));
  _RotationAbsToContactFrame->setValue(2, 2, *(pt + 5));

  DEBUG_PRINT("_RotationAbsToContactFrame:\n");
  DEBUG_EXPR(_RotationAbsToContactFrame->display(););

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

  DEBUG_PRINT("lever arm skew matrix :\n");
  DEBUG_EXPR(_NPG1->display(););

  computeRotationMatrix(q1,_rotationMatrixAbsToBody);
  DEBUG_EXPR(_rotationMatrixAbsToBody->display(););

  prod(*_NPG1, *_rotationMatrixAbsToBody, *_AUX1, true);

  DEBUG_EXPR(_rotationMatrixAbsToBody->display(););
  DEBUG_EXPR(_AUX1->display(););

  prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);

  DEBUG_EXPR(_RotationAbsToContactFrame->display(););
  DEBUG_EXPR(_AUX1->display(););
  DEBUG_EXPR(_AUX2->display(););

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 0; jj < 3; jj++)
      _jachqT->setValue(ii, jj, _RotationAbsToContactFrame->getValue(ii, jj));

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 3; jj < 6; jj++)
      _jachqT->setValue(ii, jj, _AUX2->getValue(ii, jj - 3));

  DEBUG_EXPR(_jachqT->display(););
  // DEBUG_EXPR_WE(
  //   SP::SimpleMatrix jaux(new SimpleMatrix(*_jachqT));
  //   jaux->trans();
  //   SP::SiconosVector v(new SiconosVector(3));
  //   SP::SiconosVector vRes(new SiconosVector(6));
  //   v->zero();
  //   v->setValue(0, 1);
  //   prod(*jaux, *v, *vRes, true);
  //   vRes->display();
  //   v->zero();
  //   v->setValue(1, 1);
  //   prod(*jaux, *v, *vRes, true);
  //   vRes->display();
  //   v->zero();
  //   v->setValue(2, 1);
  //   prod(*jaux, *v, *vRes, true);
  //   vRes->display();
  //   );
  DEBUG_END("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts(SP::SiconosVector q1)\n");
}

void NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts(SP::SiconosVector q1, SP::SiconosVector q2)
{
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);
  double G2x = q2->getValue(0);
  double G2y = q2->getValue(1);
  double G2z = q2->getValue(2);

  DEBUG_PRINT("contact normal:\n");
  DEBUG_EXPR(_Nc->display(););
  DEBUG_PRINT("contact point :\n");
  DEBUG_EXPR(_Pc1->display(););
  DEBUG_PRINT("center of mass :\n");
  DEBUG_EXPR(q1->display(););

  double t[6];
  double * pt = t;
  if(orthoBaseFromVector(&Nx, &Ny, &Nz, pt, pt + 1, pt + 2, pt + 3, pt + 4, pt + 5))
    RuntimeException::selfThrow("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts. Problem in calling orthoBaseFromVector");
  pt = t;
  _RotationAbsToContactFrame->setValue(0, 0, Nx);
  _RotationAbsToContactFrame->setValue(1, 0, *pt);
  _RotationAbsToContactFrame->setValue(2, 0, *(pt + 3));
  _RotationAbsToContactFrame->setValue(0, 1, Ny);
  _RotationAbsToContactFrame->setValue(1, 1, *(pt + 1));
  _RotationAbsToContactFrame->setValue(2, 1, *(pt + 4));
  _RotationAbsToContactFrame->setValue(0, 2, Nz);
  _RotationAbsToContactFrame->setValue(1, 2, *(pt + 2));
  _RotationAbsToContactFrame->setValue(2, 2, *(pt + 5));

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

  _NPG2->zero();

  (*_NPG2)(0, 0) = 0;
  (*_NPG2)(0, 1) = -(G2z - Pz);
  (*_NPG2)(0, 2) = (G2y - Py);
  (*_NPG2)(1, 0) = (G2z - Pz);
  (*_NPG2)(1, 1) = 0;
  (*_NPG2)(1, 2) = -(G2x - Px);
  (*_NPG2)(2, 0) = -(G2y - Py);
  (*_NPG2)(2, 1) = (G2x - Px);
  (*_NPG2)(2, 2) = 0;

  computeRotationMatrix(q1,_rotationMatrixAbsToBody);
  prod(*_NPG1, *_rotationMatrixAbsToBody, *_AUX1, true);
  prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 0; jj < 3; jj++)
      _jachqT->setValue(ii, jj, _RotationAbsToContactFrame->getValue(ii, jj));

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 3; jj < 6; jj++)
      _jachqT->setValue(ii, jj, _AUX2->getValue(ii, jj - 3));

  computeRotationMatrix(q2,_rotationMatrixAbsToBody);
  prod(*_NPG2, *_rotationMatrixAbsToBody, *_AUX1, true);
  prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 0; jj < 3; jj++)
      _jachqT->setValue(ii, jj + 6, -_RotationAbsToContactFrame->getValue(ii, jj));

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 3; jj < 6; jj++)
      _jachqT->setValue(ii, jj + 6, -_AUX2->getValue(ii, jj - 3));

}

void NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter, SP::BlockVector q0)
{
  DEBUG_BEGIN("NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter,  SP::BlockVector q0)\n");
  if (q0->numberOfBlocks()>1)
  {
    FC3DcomputeJachqTFromContacts((q0->getAllVect())[0], (q0->getAllVect())[1]);
  }
  else
  {
    FC3DcomputeJachqTFromContacts((q0->getAllVect())[0]);
  }
  DEBUG_END("NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter,  SP::BlockVector q0)\n");

}


#ifndef FFTW_BLITZ
#define FFTW_BLITZ
#include <cassert>
#include <complex>
#include <blitz/array.h>
#include <fftw3.h>
#include <iostream>
#include <cstdlib>
#ifndef NFFTW_THREADS
//#define NFFTW_THREADS 8
#endif

namespace fftw_blitz
{

  template <typename T>
  class fftw_blitz_trait
  {
  public:
    static void test()
    {
      std::cerr<<"Error, this type has not been implemented"<<std::endl;
      assert(0);
    }

  };

  template <>
  class fftw_blitz_trait<double>
  {
  public:
    typedef fftw_complex fftw_complex_type;
    typedef fftw_plan fftw_plan_type;
    static void test()
    {
    }
    static fftw_plan fft_plan_c2c(int, const int*,
				  fftw_complex*, fftw_complex*,
				  int, unsigned int);
    static fftw_plan fft_plan_r2c(int, const int*,
				  double*, fftw_complex*,
				  unsigned int);
    static fftw_plan fft_plan_c2r(int, const int*,
				  fftw_complex*, double*,
				  unsigned int);
    static void fftw_init_threads();
    static void fftw_plan_with_nthreads(int nthreads);
    static void fftw_execute(const fftw_plan plan);
    static void fftw_destroy_plan(fftw_plan plan);
    static void fftw_cleanup_threads();
  };

  template <>
  class fftw_blitz_trait<float>
  {
  public:
    typedef fftwf_complex fftw_complex_type;
    typedef fftwf_plan fftw_plan_type;
    static void test()
    {
    }
    static fftwf_plan fft_plan_c2c(int, const int*,
				  fftwf_complex*, fftwf_complex*,
				  int, unsigned int);
    static fftwf_plan fft_plan_r2c(int, const int*,
				  float*, fftwf_complex*,
				  unsigned int);
    static fftwf_plan fft_plan_c2r(int, const int*,
				  fftwf_complex*, float*,
				  unsigned int);
    static void fftw_init_threads();
    static void fftw_plan_with_nthreads(int nthreads);
    static void fftw_execute(const fftwf_plan plan);
    static void fftw_destroy_plan(fftwf_plan plan);
    static void fftw_cleanup_threads();
  };

  template <typename T,int N>
  blitz::Array<std::complex<T>,N> fft_c2c(blitz::Array<std::complex<T>,N> in, int sign=FFTW_FORWARD, int flags=FFTW_ESTIMATE)
  {
    fftw_blitz_trait<T>::test();
    blitz::Array<std::complex<T>,N> out(in.shape());
    typename fftw_blitz_trait<T>::fftw_plan_type p;
    blitz::TinyVector<int, N> ranks=in.shape();
#ifdef NFFTW_THREADS
    fftw_blitz_trait<T>::fftw_init_threads();
    fftw_blitz_trait<T>::fftw_plan_with_nthreads(NFFTW_THREADS);
#endif
    p = fftw_blitz_trait<T>::fft_plan_c2c(N, ranks.data(),reinterpret_cast<typename fftw_blitz_trait<T>::fftw_complex_type*>(in.data()),
					  reinterpret_cast<typename fftw_blitz_trait<T>::fftw_complex_type*>(out.data()), sign, flags);
    fftw_blitz_trait<T>::fftw_execute(p);
    fftw_blitz_trait<T>::fftw_destroy_plan(p);
#ifdef NFFTW_THREADS
    fftw_blitz_trait<T>::fftw_cleanup_threads();
#endif
    return out;
  }

  template <typename T,int N>
  blitz::Array<std::complex<T>,N> ifft_c2c_normed(blitz::Array<std::complex<T>,N> in, int flags=FFTW_ESTIMATE)
  {
    T norm=1;
    for(int i=0;i<N;++i)
      {
	norm*=in.extent(i);
      }
    blitz::Array<std::complex<T>,N> result(fft_c2c(in,FFTW_BACKWARD,flags));
    for(typename blitz::Array<std::complex<T>,N>::iterator i=result.begin();i!=result.end();++i)
      {
	(*i)/=norm;
      }
    return result;
  }

  template <typename T,int N>
  blitz::Array<std::complex<T>,N> fft_r2c(blitz::Array<T,N> in,int flags=FFTW_ESTIMATE)
  {
    fftw_blitz_trait<T>::test();
    blitz::Array<std::complex<T>,N> out(in.shape());
    typename fftw_blitz_trait<T>::fftw_plan_type p;
    blitz::TinyVector<int, N> ranks=in.shape();
#ifdef NFFTW_THREADS
    fftw_blitz_trait<T>::fftw_init_threads();
    fftw_blitz_trait<T>::fftw_plan_with_nthreads(NFFTW_THREADS);
#endif
    p = fftw_blitz_trait<T>::fft_plan_r2c(N, ranks.data(),in.data(),
					  reinterpret_cast<typename fftw_blitz_trait<T>::fftw_complex_type*>(out.data()),flags);
    fftw_blitz_trait<T>::fftw_execute(p);
    fftw_blitz_trait<T>::fftw_destroy_plan(p);
#ifdef NFFTW_THREADS
    fftw_blitz_trait<T>::fftw_cleanup_threads();
#endif
    return out;
  }

  template <typename T,int N>
  blitz::Array<T,N> fft_c2r(blitz::Array<std::complex<T>,N> in,int flags=FFTW_ESTIMATE)
  {
    fftw_blitz_trait<T>::test();
    blitz::Array<T,N> out(in.shape());
    typename fftw_blitz_trait<T>::fftw_plan_type p;
    blitz::TinyVector<int, N> ranks=in.shape();
#ifdef NFFTW_THREADS
    fftw_blitz_trait<T>::fftw_init_threads();
    fftw_blitz_trait<T>::fftw_plan_with_nthreads(NFFTW_THREADS);
#endif
    p = fftw_blitz_trait<T>::fft_plan_c2r(N, ranks.data(), reinterpret_cast<typename fftw_blitz_trait<T>::fftw_complex_type*>(in.data()),
					  out.data(),flags);
    fftw_blitz_trait<T>::fftw_execute(p);
    fftw_blitz_trait<T>::fftw_destroy_plan(p);
#ifdef NFFTW_THREADS
    fftw_blitz_trait<T>::fftw_cleanup_threads();
#endif
    return out;
  }

  /*
    template <typename T,int N>
    void fft_shift(blitz::Array<T,N>& mx)
  {
  blitz::TinyVector<T,N> idx1,idx2;
  for(int cnt=0;cnt<N;++cnt)
  {
  for(typename blitz::Array<T,N>::iterator i=mx.begin();
  i!=mx.end();++i)
  {
  if(i.position()[cnt]<mx.extent(cnt)/2)
  {
  blitz::TinyVector<int,N> idx=i.position();
  idx(cnt)+=mx.extent(cnt)/2;
  T temp=mx(idx);
  mx(idx)=*i;
  *i=temp;
  }
  }
  }
  }
  */
  template <typename T,int N>
  void swap_by_dim(blitz::Array<T,N>& mx,int dim)
  {
    for(typename blitz::Array<T,N>::iterator i=mx.begin();i!=mx.end();++i)
      {
	if(i.position()(dim)*2>=mx.extent(dim))
	  {
	    continue;
	  }
	blitz::TinyVector<int,N> source_idx=i.position();
	blitz::TinyVector<int,N> target_idx=i.position();
	target_idx(dim)+=mx.extent(dim)/2;
	T temp=mx(source_idx);
	mx(source_idx)=mx(target_idx);
	mx(target_idx)=temp;
      }
  }

  template <typename T,int N>
  void fft_shift(blitz::Array<T,N>& mx)
  {
    for(int i=0;i<N;++i)
      {
	swap_by_dim(mx,i);
      }
  }

  template <typename T,int N>
  blitz::Array<std::complex<T>,N> shifted_fft_c2c(blitz::Array<std::complex<T>,N> in, int sign=FFTW_FORWARD, int flags=FFTW_ESTIMATE)
  {
    fft_shift(in);
    blitz::Array<std::complex<T>,N> result(fft_c2c(in,sign,flags));
    fft_shift(in);
    fft_shift(result);
    return result;
  }

  template <typename T,int N>
  blitz::Array<std::complex<T>,N> shifted_ifft_c2c_normed(blitz::Array<std::complex<T>,N> in, int flags=FFTW_ESTIMATE)
  {
    fft_shift(in);
    blitz::Array<std::complex<T>,N> result(fft_c2c(in,FFTW_BACKWARD,flags));
    fft_shift(in);
    fft_shift(result);
    T norm=1;
    for(int i=0;i<N;++i)
      {
	norm*=in.extent(i);
      }
    for(typename blitz::Array<std::complex<T>,N>::iterator i=result.begin();i!=result.end();++i)
      {
	(*i)/=norm;
      }
    return result;
  }

};

#endif


#include <boost/accumulators/statistics/tail_quantile.hpp>


#include <typeinfo>
#include <iostream>
#include <Eigen/Core>
#include "BenchTimer.h"
using namespace Eigen;
using namespace std;

template<typename T>
EIGEN_DONT_INLINE typename T::Scalar sqsumNorm(T& v)
{
  return v.norm();
}

template<typename T>
EIGEN_DONT_INLINE typename T::Scalar stableNorm(T& v)
{
  return v.stableNorm();
}

template<typename T>
EIGEN_DONT_INLINE typename T::Scalar hypotNorm(T& v)
{
  return v.hypotNorm();
}

template<typename T>
EIGEN_DONT_INLINE typename T::Scalar blueNorm(T& v)
{
  return v.blueNorm();
}

template<typename T>
EIGEN_DONT_INLINE typename T::Scalar lapackNorm(T& v)
{
  typedef typename T::Scalar Scalar;
  int n = v.size();
  Scalar scale = 0;
  Scalar ssq = 1;
  for (int i=0;i<n;++i)
  {
    Scalar ax = std::abs(v.coeff(i));
    if (scale >= ax)
    {
      ssq += numext::abs2(ax/scale);
    }
    else
    {
      ssq = Scalar(1) + ssq * numext::abs2(scale/ax);
      scale = ax;
    }
  }
  return scale * std::sqrt(ssq);
}

template<typename T>
EIGEN_DONT_INLINE typename T::Scalar twopassNorm(T& v)
{
  typedef typename T::Scalar Scalar;
  Scalar s = v.array().abs().maxCoeff();
  return s*(v/s).norm();
}

template<typename T>
EIGEN_DONT_INLINE typename T::Scalar bl2passNorm(T& v)
{
  return v.stableNorm();
}

template<typename T>
EIGEN_DONT_INLINE typename T::Scalar divacNorm(T& v)
{
  int n =v.size() / 2;
  for (int i=0;i<n;++i)
    v(i) = v(2*i)*v(2*i) + v(2*i+1)*v(2*i+1);
  n = n/2;
  while (n>0)
  {
    for (int i=0;i<n;++i)
      v(i) = v(2*i) + v(2*i+1);
    n = n/2;
  }
  return std::sqrt(v(0));
}

namespace Eigen {
namespace internal {
#ifdef EIGEN_VECTORIZE
Packet4f plt(const Packet4f& a, Packet4f& b) { return _mm_cmplt_ps(a,b); }
Packet2d plt(const Packet2d& a, Packet2d& b) { return _mm_cmplt_pd(a,b); }

Packet4f pandnot(const Packet4f& a, Packet4f& b) { return _mm_andnot_ps(a,b); }
Packet2d pandnot(const Packet2d& a, Packet2d& b) { return _mm_andnot_pd(a,b); }
#endif
}
}

template<typename T>
EIGEN_DONT_INLINE typename T::Scalar pblueNorm(const T& v)
{
  #ifndef EIGEN_VECTORIZE
  return v.blueNorm();
  #else
  typedef typename T::Scalar Scalar;

  static int nmax = 0;
  static Scalar b1, b2, s1m, s2m, overfl, rbig, relerr;
  int n;

  if(nmax <= 0)
  {
    int nbig, ibeta, it, iemin, iemax, iexp;
    Scalar abig, eps;

    nbig  = NumTraits<int>::highest();          // largest integer
    ibeta = std::numeric_limits<Scalar>::radix; // NumTraits<Scalar>::Base;                    // base for floating-point numbers
    it    = NumTraits<Scalar>::digits();        // NumTraits<Scalar>::Mantissa;                // number of base-beta digits in mantissa
    iemin = NumTraits<Scalar>::min_exponent();  // minimum exponent
    iemax = NumTraits<Scalar>::max_exponent();  // maximum exponent
    rbig  = NumTraits<Scalar>::highest();       // largest floating-point number

    // Check the basic machine-dependent constants.
    if(iemin > 1 - 2*it || 1+it>iemax || (it==2 && ibeta<5)
      || (it<=4 && ibeta <= 3 ) || it<2)
    {
      eigen_assert(false && "the algorithm cannot be guaranteed on this computer");
    }
    iexp  = -((1-iemin)/2);
    b1    = std::pow(ibeta, iexp);  // lower boundary of midrange
    iexp  = (iemax + 1 - it)/2;
    b2    = std::pow(ibeta,iexp);   // upper boundary of midrange

    iexp  = (2-iemin)/2;
    s1m   = std::pow(ibeta,iexp);   // scaling factor for lower range
    iexp  = - ((iemax+it)/2);
    s2m   = std::pow(ibeta,iexp);   // scaling factor for upper range

    overfl  = rbig*s2m;          // overflow boundary for abig
    eps     = std::pow(ibeta, 1-it);
    relerr  = std::sqrt(eps);      // tolerance for neglecting asml
    abig    = 1.0/eps - 1.0;
    if (Scalar(nbig)>abig)  nmax = abig;  // largest safe n
    else                    nmax = nbig;
  }

  typedef typename internal::packet_traits<Scalar>::type Packet;
  const int ps = internal::packet_traits<Scalar>::size;
  Packet pasml = internal::pset1<Packet>(Scalar(0));
  Packet pamed = internal::pset1<Packet>(Scalar(0));
  Packet pabig = internal::pset1<Packet>(Scalar(0));
  Packet ps2m = internal::pset1<Packet>(s2m);
  Packet ps1m = internal::pset1<Packet>(s1m);
  Packet pb2  = internal::pset1<Packet>(b2);
  Packet pb1  = internal::pset1<Packet>(b1);
  for(int j=0; j<v.size(); j+=ps)
  {
    Packet ax = internal::pabs(v.template packet<Aligned>(j));
    Packet ax_s2m = internal::pmul(ax,ps2m);
    Packet ax_s1m = internal::pmul(ax,ps1m);
    Packet maskBig = internal::plt(pb2,ax);
    Packet maskSml = internal::plt(ax,pb1);

//     Packet maskMed = internal::pand(maskSml,maskBig);
//     Packet scale = internal::pset1(Scalar(0));
//     scale = internal::por(scale, internal::pand(maskBig,ps2m));
//     scale = internal::por(scale, internal::pand(maskSml,ps1m));
//     scale = internal::por(scale, internal::pandnot(internal::pset1(Scalar(1)),maskMed));
//     ax = internal::pmul(ax,scale);
//     ax = internal::pmul(ax,ax);
//     pabig = internal::padd(pabig, internal::pand(maskBig, ax));
//     pasml = internal::padd(pasml, internal::pand(maskSml, ax));
//     pamed = internal::padd(pamed, internal::pandnot(ax,maskMed));

    pabig = internal::padd(pabig, internal::pand(maskBig, internal::pmul(ax_s2m,ax_s2m)));
    pasml = internal::padd(pasml, internal::pand(maskSml, internal::pmul(ax_s1m,ax_s1m)));
    pamed = internal::padd(pamed, internal::pandnot(internal::pmul(ax,ax),internal::pand(maskSml,maskBig)));
  }
  Scalar abig = internal::predux(pabig);
  Scalar asml = internal::predux(pasml);
  Scalar amed = internal::predux(pamed);
  if(abig > Scalar(0))
  {
    abig = std::sqrt(abig);
    if(abig > overfl)
    {
      eigen_assert(false && "overflow");
      return rbig;
    }
    if(amed > Scalar(0))
    {
      abig = abig/s2m;
      amed = std::sqrt(amed);
    }
    else
    {
      return abig/s2m;
    }

  }
  else if(asml > Scalar(0))
  {
    if (amed > Scalar(0))
    {
      abig = std::sqrt(amed);
      amed = std::sqrt(asml) / s1m;
    }
    else
    {
      return std::sqrt(asml)/s1m;
    }
  }
  else
  {
    return std::sqrt(amed);
  }
  asml = std::min(abig, amed);
  abig = std::max(abig, amed);
  if(asml <= abig*relerr)
    return abig;
  else
    return abig * std::sqrt(Scalar(1) + numext::abs2(asml/abig));
  #endif
}

#define BENCH_PERF(NRM) { \
  float af = 0; double ad = 0; std::complex<float> ac = 0; \
  Eigen::BenchTimer tf, td, tcf; tf.reset(); td.reset(); tcf.reset();\
  for (int k=0; k<tries; ++k) { \
    tf.start(); \
    for (int i=0; i<iters; ++i) { af += NRM(vf); } \
    tf.stop(); \
  } \
  for (int k=0; k<tries; ++k) { \
    td.start(); \
    for (int i=0; i<iters; ++i) { ad += NRM(vd); } \
    td.stop(); \
  } \
  /*for (int k=0; k<std::max(1,tries/3); ++k) { \
    tcf.start(); \
    for (int i=0; i<iters; ++i) { ac += NRM(vcf); } \
    tcf.stop(); \
  } */\
  std::cout << #NRM << "\t" << tf.value() << "   " << td.value() <<  "    " << tcf.value() << "\n"; \
}

void check_accuracy(double basef, double based, int s)
{
  double yf = basef * std::abs(internal::random<double>());
  double yd = based * std::abs(internal::random<double>());
  VectorXf vf = VectorXf::Ones(s) * yf;
  VectorXd vd = VectorXd::Ones(s) * yd;

  std::cout << "reference\t" << std::sqrt(double(s))*yf << "\t" << std::sqrt(double(s))*yd << "\n";
  std::cout << "sqsumNorm\t" << sqsumNorm(vf) << "\t" << sqsumNorm(vd) << "\n";
  std::cout << "hypotNorm\t" << hypotNorm(vf) << "\t" << hypotNorm(vd) << "\n";
  std::cout << "blueNorm\t" << blueNorm(vf) << "\t" << blueNorm(vd) << "\n";
  std::cout << "pblueNorm\t" << pblueNorm(vf) << "\t" << pblueNorm(vd) << "\n";
  std::cout << "lapackNorm\t" << lapackNorm(vf) << "\t" << lapackNorm(vd) << "\n";
  std::cout << "twopassNorm\t" << twopassNorm(vf) << "\t" << twopassNorm(vd) << "\n";
  std::cout << "bl2passNorm\t" << bl2passNorm(vf) << "\t" << bl2passNorm(vd) << "\n";
}

void check_accuracy_var(int ef0, int ef1, int ed0, int ed1, int s)
{
  VectorXf vf(s);
  VectorXd vd(s);
  for (int i=0; i<s; ++i)
  {
    vf[i] = std::abs(internal::random<double>()) * std::pow(double(10), internal::random<int>(ef0,ef1));
    vd[i] = std::abs(internal::random<double>()) * std::pow(double(10), internal::random<int>(ed0,ed1));
  }

  //std::cout << "reference\t" << internal::sqrt(double(s))*yf << "\t" << internal::sqrt(double(s))*yd << "\n";
  std::cout << "sqsumNorm\t"  << sqsumNorm(vf)  << "\t" << sqsumNorm(vd)  << "\t" << sqsumNorm(vf.cast<long double>()) << "\t" << sqsumNorm(vd.cast<long double>()) << "\n";
  std::cout << "hypotNorm\t"  << hypotNorm(vf)  << "\t" << hypotNorm(vd)  << "\t" << hypotNorm(vf.cast<long double>()) << "\t" << hypotNorm(vd.cast<long double>()) << "\n";
  std::cout << "blueNorm\t"   << blueNorm(vf)   << "\t" << blueNorm(vd)   << "\t" << blueNorm(vf.cast<long double>()) << "\t" << blueNorm(vd.cast<long double>()) << "\n";
  std::cout << "pblueNorm\t"  << pblueNorm(vf)  << "\t" << pblueNorm(vd)  << "\t" << blueNorm(vf.cast<long double>()) << "\t" << blueNorm(vd.cast<long double>()) << "\n";
  std::cout << "lapackNorm\t" << lapackNorm(vf) << "\t" << lapackNorm(vd) << "\t" << lapackNorm(vf.cast<long double>()) << "\t" << lapackNorm(vd.cast<long double>()) << "\n";
  std::cout << "twopassNorm\t" << twopassNorm(vf) << "\t" << twopassNorm(vd) << "\t" << twopassNorm(vf.cast<long double>()) << "\t" << twopassNorm(vd.cast<long double>()) << "\n";
//   std::cout << "bl2passNorm\t" << bl2passNorm(vf) << "\t" << bl2passNorm(vd) << "\t" << bl2passNorm(vf.cast<long double>()) << "\t" << bl2passNorm(vd.cast<long double>()) << "\n";
}

int main(int argc, char** argv)
{
  int tries = 10;
  int iters = 100000;
  double y = 1.1345743233455785456788e12 * internal::random<double>();
  VectorXf v = VectorXf::Ones(1024) * y;

// return 0;
  int s = 10000;
  double basef_ok = 1.1345743233455785456788e15;
  double based_ok = 1.1345743233455785456788e95;

  double basef_under = 1.1345743233455785456788e-27;
  double based_under = 1.1345743233455785456788e-303;

  double basef_over = 1.1345743233455785456788e+27;
  double based_over = 1.1345743233455785456788e+302;

  std::cout.precision(20);

  std::cerr << "\nNo under/overflow:\n";
  check_accuracy(basef_ok, based_ok, s);

  std::cerr << "\nUnderflow:\n";
  check_accuracy(basef_under, based_under, s);

  std::cerr << "\nOverflow:\n";
  check_accuracy(basef_over, based_over, s);

  std::cerr << "\nVarying (over):\n";
  for (int k=0; k<1; ++k)
  {
    check_accuracy_var(20,27,190,302,s);
    std::cout << "\n";
  }

  std::cerr << "\nVarying (under):\n";
  for (int k=0; k<1; ++k)
  {
    check_accuracy_var(-27,20,-302,-190,s);
    std::cout << "\n";
  }

  y = 1;
  std::cout.precision(4);
  int s1 = 1024*1024*32;
  std::cerr << "Performance (out of cache, " << s1 << "):\n";
  {
    int iters = 1;
    VectorXf vf = VectorXf::Random(s1) * y;
    VectorXd vd = VectorXd::Random(s1) * y;
    VectorXcf vcf = VectorXcf::Random(s1) * y;
    BENCH_PERF(sqsumNorm);
    BENCH_PERF(stableNorm);
    BENCH_PERF(blueNorm);
    BENCH_PERF(pblueNorm);
    BENCH_PERF(lapackNorm);
    BENCH_PERF(hypotNorm);
    BENCH_PERF(twopassNorm);
    BENCH_PERF(bl2passNorm);
  }

  std::cerr << "\nPerformance (in cache, " << 512 << "):\n";
  {
    int iters = 100000;
    VectorXf vf = VectorXf::Random(512) * y;
    VectorXd vd = VectorXd::Random(512) * y;
    VectorXcf vcf = VectorXcf::Random(512) * y;
    BENCH_PERF(sqsumNorm);
    BENCH_PERF(stableNorm);
    BENCH_PERF(blueNorm);
    BENCH_PERF(pblueNorm);
    BENCH_PERF(lapackNorm);
    BENCH_PERF(hypotNorm);
    BENCH_PERF(twopassNorm);
    BENCH_PERF(bl2passNorm);
  }
}


#include "Poly2d.h"

#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <sstream>
#include <vector>

#include <Eigen/Dense>

#include <isce3/core/Poly2d.h>
#include <isce3/except/Error.h>

// Aliases
namespace py = pybind11;
using isce3::core::Poly2d;
using isce3::except::InvalidArgument;
using isce3::except::LengthError;

using RowArrayXXd =
        Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

void addbinding(pybind11::class_<isce3::core::Poly2d>& pyPoly2d)
{
    pyPoly2d.doc() = R"(
        A 2-dimensional power series.

        The Poly2d class represents a polynomial function of range
        (:math:`x`) and azimuth (:math:`y`), defined as follows:

        .. math::

            f\left( y, x \right) = \sum_{i=0}^{N_y} \sum_{j=0}^{N_x} a_{ij}
                \cdot \left( \frac{y-\mu_y}{\sigma_y} \right)^i  \cdot
                \left( \frac{x-\mu_x}{\sigma_x} \right)^j

        Attributes
        ----------
        coeffs : numpy.ndarray
            A 2-D array of polynomial coefficients in order of increasing
            degree, with shape `(y_order+1, x_order+1)`.
        x_mean, y_mean : float
            x/range and y/azimuth offsets.
        x_std, y_std : float
            x/range and y/azimuth scale factors.
        )";

        pyPoly2d
        // constructor
        .def(py::init([](const Eigen::Ref<const RowArrayXXd>& coeffs,
                         double x_mean = 0.0, double y_mean = 0.0,
                         double x_std = 1.0, double y_std = 1.0) {
             if (!(x_std > 0.0))
                 throw InvalidArgument(ISCE_SRCINFO(),
                         "x/range STD must be a positive value");
             if (!(y_std > 0.0))
                 throw InvalidArgument(ISCE_SRCINFO(),
                          "y/azimuth STD must be a positive value");
             auto pf_obj = Poly2d(coeffs.cols() - 1, coeffs.rows() - 1, x_mean,
                     y_mean, x_std, y_std);
             pf_obj.coeffs = std::vector<double>(coeffs.data(),
                     coeffs.data() + coeffs.size());
             return pf_obj;
        }),
        py::arg("coeffs"),
        py::arg("x_mean") = 0.0,
        py::arg("y_mean") = 0.0,
        py::arg("x_std") = 1.0,
        py::arg("y_std") = 1.0)

        // dunder methods
        .def("__repr__",
                [](const Poly2d& self) {
                    std::stringstream os;
                    os << "Poly2d(x_order=" << self.xOrder
                       << ", y_order=" << self.yOrder
                       << ", x_mean=" << self.xMean
                       << ", y_mean=" << self.yMean
                       << ", x_std=" << self.xNorm
                       << ", y_std=" << self.yNorm
                       << ")";
                    return os.str();
                })

        // methods
        .def("eval", &Poly2d::eval, py::arg("y"), py::arg("x"),
                R"(
            Evaluate polynomial at given `y` and `x`.
                )")

        .def("eval",
                [](const Poly2d& self, const Eigen::Ref<const Eigen::ArrayXd>& y,
                   const Eigen::Ref<const Eigen::ArrayXd>& x) {
                    // Check that inputs have compatible shapes.
                    if (x.size() != y.size()) {
                        throw InvalidArgument(ISCE_SRCINFO(),
                                "x & y arrays must have the same shape");
                    }

                    // Evaluate the polynomial at each (x,y) pair.
                    const auto n = x.size();
                    auto z = Eigen::ArrayXd(n);
                    for (Eigen::Index i = 0; i < n; ++i) {
                        z[i] = self.eval(y[i], x[i]);
                    }

                    return z;
                },
                py::arg("y"), py::arg("x"), R"(
                Evaluate the polynomial at the given (y, x) coordinates.

                `y` and `x` must have the same shape.
                )")

        .def("eval",
                [](const Poly2d& self, const Eigen::Ref<const RowArrayXXd>& y,
                   const Eigen::Ref<const RowArrayXXd>& x) {
                    // Check that inputs have compatible shapes.
                    if ((x.rows() != y.rows()) or (x.cols() != y.cols())) {
                        throw InvalidArgument(ISCE_SRCINFO(),
                                "x & y arrays must have the same shape");
                    }

                    // Evaluate the polynomial at each (x,y) pair.
                    const auto m = x.rows();
                    const auto n = x.cols();
                    auto z = RowArrayXXd(m, n);
                    for (Eigen::Index i = 0; i < m; ++i) {
                        for (Eigen::Index j = 0; j < n; ++j) {
                            z(i, j) = self.eval(y(i, j), x(i, j));
                        }
                    }

                    return z;
                },
                py::arg("y"), py::arg("x"), R"(
                Evaluate the polynomial at the given (y, x) coordinates.

                `y` and `x` must have the same shape.
                )")

        .def("evalgrid",
                [](const Poly2d& self,
                   const Eigen::Ref<const Eigen::ArrayXd>& y_vect,
                   const Eigen::Ref<const Eigen::ArrayXd>& x_vect) {
                    // Validate inputs
                    if (x_vect.size() < 1 || y_vect.size() < 1)
                        throw LengthError(ISCE_SRCINFO(),
                                "The size of the vectors must be nonzero");

                    // Evaluate over x and y mesh
                    Eigen::ArrayXXd z(x_vect.size(), y_vect.size());
                    for (Eigen::Index i = 0; i < z.rows(); ++i) {
                        for (Eigen::Index j = 0; j < z.cols(); ++j) {
                            z(i, j) = self.eval(y_vect(j), x_vect(i));
                       }
                    }
                    return z;
                    },
                    py::arg("y_vect"), py::arg("x_vect"),
                    R"(
                Evaluate the polynomial on the Cartesian product of `x_vect` and `y_vect`.
                    )")

        // properties
        .def_property_readonly("x_order",
                [](const Poly2d& self) {return self.xOrder; })
        .def_property_readonly("y_order",
                [](const Poly2d& self) {return self.yOrder; })
        .def_property_readonly("x_mean",
                [](const Poly2d& self) {return self.xMean; })
        .def_property_readonly("y_mean",
                [](const Poly2d& self) {return self.yMean; })
        .def_property_readonly("x_std",
                [](const Poly2d& self) {return self.xNorm; })
        .def_property_readonly("y_std",
                [](const Poly2d& self) {return self.yNorm; })
        .def_property_readonly("coeffs",
                [](const Poly2d& self) {
                    Eigen::Map<const RowArrayXXd> coeffs(
                            self.coeffs.data(), self.yOrder + 1, self.xOrder + 1);
                    return coeffs;
                })
        ;
}


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

#ifndef GEN_GENERIC_HPP_
#define GEN_GENERIC_HPP_

#include <boost/random.hpp>
#include "total_unimodularity.hpp"
#include "matrix_transposed.hpp"
#include "matrix_reorder.hpp"
#include "permutations.hpp"
#include "matrix_transposed_permuted.hpp"

class matrix_generator
{
protected:
  size_t _height;
  size_t _width;
  unimod::integer_matrix _matrix;
  boost::mt19937 _rng;
  unimod::log_level _level;
  const char* _name;

public:
  matrix_generator(const char* name, size_t height, size_t width, unimod::log_level level) :
    _height(height), _width(width), _matrix(height, width), _rng(time(NULL)), _level(level), _name(name)
  {
  }

  virtual ~matrix_generator()
  {

  }

  virtual void generate() = 0;

  void log_generate_start()
  {
    if (_level != unimod::LOG_QUIET)
      std::cerr << "Generating " << _height << " x " << _width << " " << _name << " matrix..." << std::flush;
  }

  void log_generate_end()
  {
    if (_level != unimod::LOG_QUIET)
      std::cerr << " done." << std::endl;
  }

  virtual bool do_pivot(size_t row, size_t column)
  {
    return true;
  }

  template <typename MatrixType>
  void permute_rows(MatrixType& matrix)
  {
    unimod::permutation perm(matrix.size1(), _rng);
    unimod::matrix_apply_row_permutation(matrix, perm);
  }

  void permute_rows()
  {
    permute_rows(_matrix);
  }

  void permute_columns()
  {
    unimod::matrix_transposed <unimod::integer_matrix> transposed(_matrix);
    permute_rows(transposed);
  }

  virtual void randomize()
  {
    permute_rows();
    permute_columns();
  }

  virtual void sign()
  {
    unimod::sign_matrix(_matrix);
  }

  virtual void print()
  {
    std::cout << _height << " " << _width << "\n\n";
    for (size_t row = 0; row < _height; ++row)
    {
      for (size_t column = 0; column < _width; ++column)
      {
        std::cout << std::setw(3) << _matrix(row, column);
      }
      std::cout << "\n";
    }
    std::cout << std::flush;
  }
};

#endif


//==================================================================================================
/*!
  @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_CONSTANT_DEFINITION_GOLDBAR_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_DEFINITION_GOLDBAR_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    struct goldbar_ : boost::dispatch::constant_value_<goldbar_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,goldbar_,boost::dispatch::constant_value_<goldbar_>);
      BOOST_SIMD_REGISTER_CONSTANT(0, 0xbf1e377aLL, 0xbfe3c6ef372fe950ULL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, goldbar_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::goldbar_,goldbar);
  }

  template<typename T> BOOST_FORCEINLINE auto Goldbar()
  BOOST_NOEXCEPT_DECLTYPE(detail::goldbar( boost::dispatch::as_<T>{}))
  {
    return detail::goldbar( boost::dispatch::as_<T>{} );
  }
} }

#endif


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.

#include "cast_to_gfloat.hpp"
#include "poputil/TileMapping.hpp"
#include "poputil/VertexTemplates.hpp"
#include <boost/multi_array.hpp>
#include <boost/program_options.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <iomanip>
#include <iostream>
#include <memory>
#include <popfloat/experimental/CastToGfloat.hpp>
#include <popfloat/experimental/CastToHalf.hpp>
#include <popfloat/experimental/codelets.hpp>
#include <poplar/Engine.hpp>
#include <popops/codelets.hpp>
#include <random>

#include <poplibs_support/Compiler.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_test/Pass.hpp>
#include <poplibs_test/Util.hpp>

#include <poplar/CSRFunctions.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace poplibs_test::util;
using namespace popfloat::experimental;
using namespace poputil;
using namespace poplibs_support;
using poplibs_test::Pass;

const OptionFlags simDebugOptions{{"debug.trace", "false"}};

template <typename T, bool pack>
bool castNativeToGfloatCheck(float *inVec, T *outVec, unsigned sizeVec,
                             const GfloatCast::FormatConfig &gfFormatCfg,
                             const GfloatCast::CastConfig &gfCastCfg) {
  int32_t minExp = 1 - gfFormatCfg.getExponentBias();
  if (gfFormatCfg.isDenormEnabled()) {
    minExp -= gfFormatCfg.getNumMantissaBits();
  }
  auto formatType = gfFormatCfg.getFormatType();

  if (gfFormatCfg.getCalculationType() == HALF) {
    minExp = gfFormatCfg.isDenormEnabled()
                 ? -(14 + gfFormatCfg.getNumMantissaBits())
                 : -14;
    minExp -= (formatType == FormatType::MAX_NORM_ALIGN_GF8);
  }

  float minValue = std::pow(2.0, minExp);

  int32_t maxExp = (1 << gfFormatCfg.getNumExponentBits());
  maxExp -= 1 + gfFormatCfg.getExponentBias();
  if (gfFormatCfg.getCalculationType() == HALF) {
    maxExp = (1 << gfFormatCfg.getNumExponentBits()) - 1 - expBiasFp16;
  }

  if (gfFormatCfg.infAndNansEnabled() && !gfFormatCfg.isBlockFloat()) {
    --maxExp;
  }
  float maxValue = (float)((1 << (gfFormatCfg.getNumMantissaBits() + 1)) - 1);
  if (gfFormatCfg.isBlockFloat()) {
    maxValue = (float)((1 << gfFormatCfg.getNumMantissaBits()) - 1);
    maxValue *= std::pow(2.0, minExp);
  } else {
    maxExp -= gfFormatCfg.getNumMantissaBits();
    maxValue *= std::pow(2.0, maxExp);
  }

  float scale = 1.0;

  if (gfFormatCfg.getCalculationType() == HALF) {
    if ((formatType == FormatType::MAX_NORM_ALIGN_GF8) ||
        (!gfFormatCfg.infAndNansEnabled() &&
         (gfFormatCfg.getNumExponentBits() == expSizeFp16))) {
      scale = std::pow(2.0, gfFormatCfg.getExponentBias() - 16);
    } else {
      scale = std::pow(2.0, gfFormatCfg.getExponentBias() - 15);
    }
  }

  unsigned fpSize = 31;
  if ((gfCastCfg.getStorageType() == HALF) ||
      (gfCastCfg.getStorageType() == SHORT)) {
    fpSize = 15;
  } else if (gfCastCfg.getStorageType() == SIGNED_CHAR) {
    fpSize = 7;
  }

  unsigned manSize = fpSize - gfFormatCfg.getNumExponentBits();

  unsigned manExpMask = (1 << fpSize) - 1;

  unsigned fpMask = (1 << (fpSize + 1)) - 1;
  unsigned alignShr = manSizeFp32 - manSize;

  int32_t expBias = gfFormatCfg.getExponentBias();
  if (gfFormatCfg.getCalculationType() == HALF) {
    expBias = (formatType == FormatType::MAX_NORM_ALIGN_GF8) ? (expBiasFp16 + 1)
                                                             : expBiasFp16;
  }
  int32_t minNormExp0 = 1 - expBias;

  bool pass = true;

  int32_t minNormExp1 = 1 - gfFormatCfg.getExponentBias();
  if (gfFormatCfg.getCalculationType() == HALF) {
    minNormExp1 = 1 - expBiasFp16;
  }

  int32_t qNan = qnanFp32;
  if ((gfFormatCfg.getCalculationType() == HALF) ||
      (gfCastCfg.getStorageType() == HALF)) {
    float _qnan = halfToSingle(qnanFp16);
    std::memcpy(&qNan, &_qnan, sizeof(qNan));
  }

  // Quantised FP16 clip input before scaling if NaNOO mode is disable
  uint16_t maxBits = 0x7BFF;
  if (gfFormatCfg.getCalculationType() == HALF) {
    maxBits >>= (manSizeFp16 - gfFormatCfg.getNumMantissaBits());
    maxBits <<= (manSizeFp16 - gfFormatCfg.getNumMantissaBits());
  }

  float maxAbs = halfToSingle(maxBits);
  for (unsigned j = 0; j != sizeVec; ++j) {
    float input = inVec[j] * scale;
    int32_t inBits;

    if (gfFormatCfg.getCalculationType() == HALF) {
      if (!gfCastCfg.isNanooModeEnabled()) {
        if (std::abs(input) > maxAbs) {
          input = (input > 0) ? maxAbs : (-1.0 * maxAbs);
        }
      }
      auto inBits16 = singleToHalf(input, gfCastCfg.isNanooModeEnabled());
      input = halfToSingle(inBits16);
    }
    std::memcpy(&inBits, &input, sizeof(inBits));

    int32_t m_single = inBits & manMaskFp32;
    int32_t e_single = ((inBits & expMaskFp32) >> manSizeFp32);
    if (e_single != 0) {
      m_single |= (1 << manSizeFp32);
    }
    e_single -= expBiasFp32;
    int32_t s_single = (inBits & sgnMaskFp32);

    int32_t masklen;
    masklen = (minNormExp0 - e_single);
    masklen = (masklen < 0) ? 0 : masklen;
    masklen += manSizeFp32 - gfFormatCfg.getNumMantissaBits();
    masklen = std::min<uint32_t>(masklen, manSizeFp32 + 1);

    if ((gfFormatCfg.getCalculationType() == HALF) &&
        (std::abs(input) > maxAbs)) {
      m_single = m_single >> masklen;
      m_single = m_single << masklen;
    } else if (gfCastCfg.getRoundMode() == RoundType::RZ) {
      m_single = m_single >> masklen;
      m_single = m_single << masklen;
    } else if (gfCastCfg.getRoundMode() == RoundType::RN) {
      bool msfBitVal = (m_single >> (masklen - 1)) & 1;
      bool lsbs = (m_single & ((1 << (masklen - 1)) - 1)) != 0;
      bool lsBitVal = (m_single >> masklen) & 1;
      m_single = (m_single >> masklen);
      if (msfBitVal && (lsBitVal || lsbs)) {
        m_single += 1;
      }
      m_single = m_single << masklen;
    } else if (gfCastCfg.getRoundMode() == RoundType::RA) {
      m_single = m_single + ((1 << masklen) >> 1);
      m_single = m_single >> masklen;
      m_single = m_single << masklen;
    } else if (gfCastCfg.getRoundMode() == RoundType::RU) {
      uint32_t corr = (s_single == 0) ? ((1 << masklen) - 1) : 0;
      m_single = m_single + corr;
      m_single = m_single >> masklen;
      m_single = m_single << masklen;
    } else if (gfCastCfg.getRoundMode() == RoundType::RD) {
      uint32_t corr = (s_single == 0) ? 0 : ((1 << masklen) - 1);
      m_single = m_single + corr;
      m_single = m_single >> masklen;
      m_single = m_single << masklen;
    } else {
      std::cout << "Rounding mode not supported" << std::endl;
    }

    float fpOut = (float)m_single;
    fpOut *= std::pow(2.0, e_single - manSizeFp32);
    if (fpOut < minValue) {
      fpOut = 0.0;
      s_single = 0;
    }
    std::memcpy(&inBits, &fpOut, sizeof(inBits));

    if (fpOut > maxValue) {
      if (gfFormatCfg.infAndNansEnabled() && gfCastCfg.isNanooModeEnabled()) {
        inBits = qNan;
        if ((pack || ((gfFormatCfg.getCalculationType() == FLOAT) &&
                      (gfCastCfg.getStorageType() != HALF))) &&
            (inVec[j] < 0)) {
          inBits |= s_single;
        }
      } else {
        fpOut = maxValue / (pack ? 1.0 : scale);
        std::memcpy(&inBits, &fpOut, sizeof(inBits));
        inBits |= s_single;
      }
    } else {
      if (!pack) {
        fpOut /= scale;
        std::memcpy(&inBits, &fpOut, sizeof(inBits));
      }
      inBits |= s_single;
    }

    int32_t outBits;
    if (pack) {
      int32_t m_single = (inBits & manMaskFp32) >> alignShr;
      int32_t e_single = ((inBits & expMaskFp32) >> manSizeFp32);
      int32_t s_single = (inBits & sgnMaskFp32) >> (31 - fpSize);

      outBits = 0;

      if (e_single == infAndNanExpFp32) {
        if (gfFormatCfg.getCalculationType() == FLOAT) {
          outBits = ((qnanFp32 >> alignShr) & manExpMask) | s_single;
        } else {
          outBits = ((qnanFp16 >> (manSizeFp16 - manSize)) & manExpMask);
        }
      } else if ((m_single != 0) || (e_single != 0)) {
        e_single -= expBiasFp32;
        if (e_single < minNormExp1) {
          if ((formatType == FormatType::MAX_NORM_ALIGN_GF8) &&
              (e_single == (minNormExp1 - 1))) {
            e_single = 1;
          } else {
            m_single |= (1 << manSize);
            m_single >>= (minNormExp1 - e_single);
            e_single = 0;
          }
        } else {
          e_single += expBias;
        }
        e_single = e_single << manSize;

        outBits = m_single | e_single | s_single;
      } else {
        if (gfFormatCfg.getCalculationType() == FLOAT) {
          outBits = s_single;
        }
      }

      pass &= ((outBits & fpMask) == ((int32_t)outVec[j] & fpMask));
    } else {
      std::memcpy(&outBits, &outVec[j], sizeof(outBits));
      pass &= (outBits == inBits);
    }
  }
  return pass;
}

template <typename T>
bool castGfloatToNativeCheck(T *inVec, float *outVec, unsigned sizeVec,
                             const GfloatCast::FormatConfig &gfFormatCfg,
                             Type storageType) {
  unsigned fpSize = 31;
  if ((storageType == HALF) || (storageType == SHORT)) {
    fpSize = 15;
  } else if (storageType == SIGNED_CHAR) {
    fpSize = 7;
  }
  unsigned manSize = fpSize - gfFormatCfg.getNumExponentBits();

  unsigned alignShr = manSizeFp32 - manSize;

  int32_t manMask = (1 << manSize) - 1;
  int32_t maxExp =
      (1 << gfFormatCfg.getNumExponentBits()) - gfFormatCfg.infAndNansEnabled();
  int32_t expMask = ((1 << gfFormatCfg.getNumExponentBits()) - 1) << manSize;
  int32_t sgnMask = 1 << (manSize + gfFormatCfg.getNumExponentBits());

  int32_t expBias = gfFormatCfg.getExponentBias();
  int32_t qNan = qnanFp32;

  if (gfFormatCfg.getCalculationType() == HALF) {
    float _qnan = halfToSingle(qnanFp16);
    std::memcpy(&qNan, &_qnan, sizeof(qNan));
  }

  int32_t minNormExp = 1 - expBias;

  bool pass = true;
  for (unsigned j = 0; j != sizeVec; ++j) {
    int32_t inBits = inVec[j];

    int32_t m_single = inBits & manMask;
    int32_t e_single = (inBits & expMask) >> manSize;
    int32_t s_single = (inBits & sgnMask) << (31 - fpSize);

    m_single <<= alignShr;

    inBits = 0;
    if ((e_single < maxExp) || gfFormatCfg.isBlockFloat()) {
      if (e_single == 0) {
        if (m_single != 0) {
          e_single = minNormExp;
          while (m_single < (1 << manSizeFp32)) {
            m_single <<= 1;
            --e_single;
          }
          e_single += expBiasFp32;
          e_single <<= manSizeFp32;
          m_single &= manMaskFp32;
          inBits = m_single | e_single | s_single;
        } else {
          auto gfFormatType = gfFormatCfg.getFormatType();
          if (gfFormatType == FormatType::ENABLE_DENORM_GF16) {
            inBits = s_single;
          }
        }
      } else {
        e_single -= expBias;
        e_single += expBiasFp32;
        e_single <<= manSizeFp32;
        inBits = m_single | e_single | s_single;
      }

      int32_t outBits;
      std::memcpy(&outBits, &outVec[j], sizeof(outBits));
      pass &= (outBits == inBits);
    }
  }
  return pass;
}

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  unsigned man;
  unsigned exp;
  int bias;
  bool enableDenorms;
  bool enableInfsAndNans;
  bool enableNanoo;
  std::string roundMode;
  std::string calcType;
  std::string storeType;
  unsigned numberSRBits;
  Type inType = FLOAT;
  unsigned inSize;
  DeviceType deviceType = DeviceType::Cpu;
  boost::optional<unsigned> tilesPerIPU;
  bool prng;
  unsigned seed;

  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
      ("help", "Produce help message")
      ("compile-only", "Stop after compilation; don't run the program")
      ("device-type",
       po::value<DeviceType>(&deviceType)->default_value(deviceType),
       deviceTypeHelp)
      ("tiles-per-ipu",
       po::value(&tilesPerIPU),
       "Number of tiles per IPU")
      ("prng", po::value<bool>(&prng)->default_value(false), "prng enable")
      ("seed", po::value<unsigned>(&seed)->default_value(12352345), "prng seed")
      ("profile", "Output profiling report")
      ("man", po::value<unsigned>(&man)->default_value(10),
       "Mantissa size of the Gfloat format")
      ("exp", po::value<unsigned>(&exp)->default_value(5),
       "Exponent size of the Gfloat format")
      ("bias", po::value<int>(&bias)->default_value(15),
       "Exponent bias of the Gfloat format")
      ("enable-denorms",
       po::value<bool>(&enableDenorms)->default_value(true),
       "Enable Denorms")
      ("enable-infs-and-nans",
       po::value<bool>(&enableInfsAndNans)->default_value(true),
       "Enable Infs and Nans")
      ("calc-type",
       po::value<std::string>(&calcType)->default_value("AUTO"),
       "Native type used for gfloat calculation: AUTO | FP32 | FP16")
      ("storage-type",
       po::value<std::string>(&storeType)->default_value("AUTO"),
       "Storage type used for gfloat format: AUTO | FP32 | FP16")
      ("enable-nanoo-mode",
       po::value<bool>(&enableNanoo)->default_value(true),
       "Propagate Nans, and generate qNan on overflow ")
      ("round-mode",
       po::value<std::string>(&roundMode)->default_value("RZ"),
       "Round mode")
      ("num-sr-bits",
       po::value<unsigned>(&numberSRBits)->default_value(23),
       "Maximum number of prng bits used for stochastic rounding")
      ("in-type", po::value<Type>(&inType)->default_value(poplar::FLOAT),
       "Type of the data")
      ("input-size",
       po::value<unsigned>(&inSize)->default_value(12),
       "Vector size");
  // clang-format on

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  auto dev = [&]() -> TestDevice {
    if (isIpuModel(deviceType)) {
      // When running on the IPU model we apply global exchange constraints,
      // which is why we create the device from the model here and not using
      // the normal createTestDevice factory function.
      IPUModel ipuModel(deviceTypeToIPUName(deviceType));
      if (tilesPerIPU)
        ipuModel.tilesPerIPU = *tilesPerIPU;
      addGlobalExchangeConstraints(ipuModel);
      setGlobalSyncLatency(ipuModel);
      return ipuModel.createDevice();
    } else {
      if (tilesPerIPU)
        return createTestDevice(deviceType, 1, *tilesPerIPU);
      else
        return createTestDeviceFullSize(deviceType);
    }
  }();

  poplar::Device::createCPUDevice();
  const auto &target = dev.getTarget();
  Graph graph(target);
  popfloat::experimental::addCodelets(graph);
  popops::addCodelets(graph);
  auto gfCastProg = Sequence();

  auto calculationType = convertStringToSpecType(calcType);
  auto rMode = convertStringToRoundType(roundMode, inType, numberSRBits);

  auto gfFormatCfg = GfloatCast::FormatConfig(
      man, exp, bias, enableDenorms, enableInfsAndNans, calculationType);

  // Create input tensor.
  Tensor input = graph.addVariable(inType, {inSize}, "input");
  mapTensorLinearly(graph, input);

  auto hInput = std::unique_ptr<float[]>(new float[inSize]);

  boost::multi_array<double, 1> hostInput(boost::extents[inSize]);

  std::mt19937 randomEngine;
  writeRandomValues(target, inType, hostInput, -5.0, +5.0, randomEngine);
  copy(target, hostInput, inType, hInput.get());

  auto flpCastOut = std::unique_ptr<float[]>(new float[inSize]);

  auto chrCastOut = std::unique_ptr<char[]>(new char[inSize]);
  auto shrCastOut = std::unique_ptr<short[]>(new short[inSize]);

  auto flpUnpackOut = std::unique_ptr<float[]>(new float[inSize]);

  // Create stream for input data
  auto inStreamV = graph.addHostToDeviceFIFO("InputVector", inType, inSize);

  gfCastProg = Sequence{Copy(inStreamV, input)};

  SpecType gfStorageType = convertStringToSpecType(storeType);

  auto roundCfg = GfloatCast::RoundConfig(rMode, numberSRBits,
                                          gfFormatCfg.getCalculationType());
  bool enableNanooMode = enableNanoo && enableInfsAndNans && (exp > 0);
  auto gfCast = GfloatCast(gfFormatCfg, roundCfg, enableNanooMode,
                           gfStorageType, calculationType);

  gfCast.createCastOpParamsTensor(graph, gfCastProg);

  auto gfCastOutput = gfCast.castNativeToGfloat(graph, input, gfCastProg);

  // Create stream for pack output
  auto castOutStream = graph.addDeviceToHostFIFO(
      "CastOutputStream", gfCast.getGFStorageType(), inSize);

  graph.createHostRead("castOutput", gfCastOutput);

  if (!gfCast.getStoreAsNative()) {
    auto unpackOutput =
        gfCast.castGfloatToNative(graph, gfCastOutput, gfCastProg);
    graph.createHostRead("unpackOut", unpackOutput);
  }

  Engine engine(
      graph, gfCastProg,
      OptionFlags{{"debug.allowOutOfMemory", "true"},
                  {"debug.outputAllSymbols", "true"},
                  {"debug.instrumentCompute", "true"},
                  {"debug.floatPointOpException", "false"},
                  {"debug.nanOverflowException", "false"},
                  {"prng.enableStochasticRounding", prng ? "true" : "false"},
                  {"prng.seed", std::to_string(seed)}});

  if (vm.count("compile-only"))
    return 0;

  engine.connectStream("InputVector", hInput.get());

  if (!gfCast.getStoreAsNative()) {
    if (gfFormatCfg.getStorageType() == poplar::SIGNED_CHAR) {
      engine.connectStream("CastOutputStream", chrCastOut.get());
    } else if (gfFormatCfg.getStorageType() == poplar::SHORT) {
      engine.connectStream("CastOutputStream", shrCastOut.get());
    } else {
      std::cout << "packOutType not valid" << std::endl;
    }
  }

  if (gfCast.getGFStorageType() == poplar::FLOAT) {
    dev.bind([&](const Device &d) {
      engine.load(d);
      engine.run();
      readAndConvertTensor<float, false>(
          graph.getTarget(), engine, "castOutput", flpCastOut.get(), inSize);
    });
  } else if (gfCast.getGFStorageType() == poplar::HALF) {
    dev.bind([&](const Device &d) {
      engine.load(d);
      engine.run();
      readAndConvertTensor<float, true>(graph.getTarget(), engine, "castOutput",
                                        flpCastOut.get(), inSize);
    });
  } else if (gfCast.getGFStorageType() == poplar::SHORT) {
    dev.bind([&](const Device &d) {
      engine.load(d);
      engine.run();
      readAndConvertTensor<short, false>(
          graph.getTarget(), engine, "castOutput", shrCastOut.get(), inSize);
      readAndConvertTensor<float, false>(graph.getTarget(), engine, "unpackOut",
                                         flpUnpackOut.get(), inSize);
    });
  } else if (gfCast.getGFStorageType() == poplar::SIGNED_CHAR) {
    if (gfCast.getCalculationType() == poplar::FLOAT) {
      dev.bind([&](const Device &d) {
        engine.load(d);
        engine.run();
        readAndConvertTensor<char, false>(
            graph.getTarget(), engine, "castOutput", chrCastOut.get(), inSize);
        readAndConvertTensor<float, false>(
            graph.getTarget(), engine, "unpackOut", flpUnpackOut.get(), inSize);
      });
    } else if (gfCast.getCalculationType() == poplar::HALF) {
      dev.bind([&](const Device &d) {
        engine.load(d);
        engine.run();
        readAndConvertTensor<char, false>(
            graph.getTarget(), engine, "castOutput", chrCastOut.get(), inSize);
        readAndConvertTensor<float, true>(
            graph.getTarget(), engine, "unpackOut", flpUnpackOut.get(), inSize);
      });
    }
  }

  if (vm.count("profile")) {
    auto reportOptions =
        OptionFlags{{"showExecutionSteps", "true"}, {"showVarStorage", "true"}};

    engine.printProfileSummary(std::cout, reportOptions);
  }

  bool pass = true;

  if (!gfCast.getStoreAsNative()) {
    if (gfCast.getGFStorageType() == poplar::SHORT) {
      auto nativeToGFConfig = gfCast.getNativeToGFConfig();
      pass = castNativeToGfloatCheck<short, true>(
          hInput.get(), shrCastOut.get(), inSize, gfFormatCfg,
          nativeToGFConfig);

      if (!pass) {
        std::cout << "castToGfloatCheck failed" << std::endl;
      }

      bool unpackCheck = castGfloatToNativeCheck<short>(
          shrCastOut.get(), flpUnpackOut.get(), inSize, gfFormatCfg,
          gfCast.getGFStorageType());

      pass &= unpackCheck;
      if (!unpackCheck) {
        std::cout << "castFromGfloatCheck failed" << std::endl;
      }
    } else if (gfCast.getGFStorageType() == poplar::SIGNED_CHAR) {
      auto nativeToGFConfig = gfCast.getNativeToGFConfig();
      pass = castNativeToGfloatCheck<char, true>(hInput.get(), chrCastOut.get(),
                                                 inSize, gfFormatCfg,
                                                 nativeToGFConfig);

      if (!pass) {
        std::cout << "castToGfloatCheck failed" << std::endl;
      }

      bool unpackCheck = castGfloatToNativeCheck<char>(
          chrCastOut.get(), flpUnpackOut.get(), inSize, gfFormatCfg,
          gfCast.getGFStorageType());

      pass &= unpackCheck;
      if (!unpackCheck) {
        std::cout << "castFromGfloatCheck failed" << std::endl;
      }
    } else {
      std::cout << "Cast output type (" << gfCast.getGFStorageType()
                << ") not valid" << std::endl;
    }
  } else {
    auto nativeToGFConfig = gfCast.getNativeToGFConfig();
    pass = castNativeToGfloatCheck<float, false>(
        hInput.get(), flpCastOut.get(), inSize, gfFormatCfg, nativeToGFConfig);

    if (!pass) {
      std::cout << "castToGfloatCheck failed" << std::endl;
    }
  }
  if (pass) {
    std::cout << "Test Passed\n";
  }
  return !pass;
}