/**
 * @file Optimizer.cpp
 * @brief Class implementing batch and incremental nonlinear equation solvers.
 * @author David Rosen
 * @author Michael Kaess
 * @version $Id: Optimizer.cpp 6371 2012-03-29 22:22:23Z kaess $
 *
 * Copyright (C) 2009-2013 Massachusetts Institute of Technology.
 * Michael Kaess, Hordur Johannsson, David Rosen,
 * Nicholas Carlevaris-Bianco and John. J. Leonard
 *
 * This file is part of iSAM.
 *
 * iSAM is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the
 * Free Software Foundation; either version 2.1 of the License, or (at
 * your option) any later version.
 *
 * iSAM is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
 * License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with iSAM.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <Eigen/Dense>

#include "isam/Optimizer.h"
#include "isam/OptimizationInterface.h"

using namespace std;
using namespace Eigen;
#include "iostream"
namespace isam {

/* Use Powell's Dog-Leg stopping criteria for all of the batch algorithms? */
// #define USE_PDL_STOPPING_CRITERIA

void Optimizer::permute_vector(const VectorXd& v, VectorXd& p,
    const int* permutation) {
  for (int i = 0; i < v.size(); i++) {
    p(permutation[i]) = v(i);
  }
}

VectorXd Optimizer::compute_gauss_newton_step(const SparseSystem& jacobian,
    SparseSystem* R, double lambda) {
  VectorXd delta_ordered;

//   std::cout<<"come here !!"<<jacobian<<std::endl;

  _cholesky->factorize(jacobian, &delta_ordered, lambda);
  if (R != NULL) {
    _cholesky->get_R(*R);
  }

  // delta has new ordering, need to return result with default ordering
  int nrows = delta_ordered.size();
  VectorXd delta(nrows);
  permute_vector(delta_ordered, delta, _cholesky->get_order());

  return delta;
}

VectorXd Optimizer::compute_dog_leg(double alpha, const VectorXd& h_sd,
    const VectorXd& h_gn, double delta, double& gain_ratio_denominator) {
  if (h_gn.norm() <= delta) {
    gain_ratio_denominator = current_SSE_at_linpoint;
    return h_gn;
  }

  double h_sd_norm = h_sd.norm();

  if ((alpha * h_sd_norm) >= delta) {
    gain_ratio_denominator = delta * (2 * alpha * h_sd_norm - delta)
        / (2 * alpha);
    return (delta / h_sd_norm) * h_sd;
  } else {
    // complicated case: calculate intersection of trust region with
    // line between Gauss-Newton and steepest descent solutions
    VectorXd a = alpha * h_sd;
    VectorXd b = h_gn;
    double c = a.dot(b - a);
    double b_a_norm2 = (b - a).squaredNorm();
    double a_norm2 = a.squaredNorm();
    double delta2 = delta * delta;
    double sqrt_term = sqrt(c * c + b_a_norm2 * (delta2 - a_norm2));
    double beta;
    if (c <= 0) {
      beta = (-c + sqrt_term) / b_a_norm2;
    } else {
      beta = (delta2 - a_norm2) / (c + sqrt_term);
    }

    gain_ratio_denominator = .5 * alpha * (1 - beta) * (1 - beta) * h_sd_norm
        * h_sd_norm + beta * (2 - beta) * current_SSE_at_linpoint;
    return (alpha * h_sd + beta * (h_gn - alpha * h_sd));
  }
}

void Optimizer::update_trust_radius(double rho, double hdl_norm) {
  if (rho < .25) {
    Delta /= 2.0;
  }
  if (rho > .75) {
    Delta = max(Delta, 3 * hdl_norm);
  }
}

void Optimizer::relinearize(const Properties& prop) {
  // We're going to relinearize about the current estimate.
  function_system.estimate_to_linpoint();

  // prepare factorization
  SparseSystem jac = function_system.jacobian();

  // factorization and new rhs based on new linearization point will be in _R
  VectorXd h_gn = compute_gauss_newton_step(jac, &function_system._R); // modifies _R

  if (prop.method == DOG_LEG) {
    // Compute the gradient and cache it.
    gradient = mul_SparseMatrixTrans_Vector(jac, jac.rhs());

    //Get the value of the sum-of-squared errors at the current linearization point.
    current_SSE_at_linpoint = jac.rhs().squaredNorm();

    // NB: alpha's denominator will be zero iff the gradient vector is zero
    // (since J is full-rank by hypothesis).  But the gradient is zero iff
    // we're already at the minimum, so we don't actually need to to any
    // updates; just set the estimate to be the current linearization point,
    // since we're already at the minimum.

    double alpha_denominator = (jac * gradient).squaredNorm();

    if (alpha_denominator > 0) {
      double alpha_numerator = gradient.squaredNorm();
      double alpha = alpha_numerator / alpha_denominator;

      // These values will be used to update the estimate of Delta
      double F_0, F_h;

      F_0 = current_SSE_at_linpoint;

      double rho_denominator, rho;

      VectorXd h_dl;

      do {
        // We repeat the computation of the dog-leg step, shrinking the
        // trust-region radius if necessary, until we generate a sufficiently
        // small region of trust that we accept the proposed step.

        // Compute dog-leg step.
        // NOTE: Here we use -h_gn because of the weird sign change in the exmap functions.
        h_dl = compute_dog_leg(alpha, -gradient, -h_gn, Delta, rho_denominator);

        // Update the estimate.
        // NOTE:  Here we use -h_dl because of the weird sign change in the exmap functions.
        function_system.apply_exmap(-h_dl);

        // Get the value of the sum-of-squared errors at the new estimate.
        F_h = function_system.weighted_errors(ESTIMATE).squaredNorm();

        // Compute gain ratio.
        rho = (F_0 - F_h) / (rho_denominator);

        update_trust_radius(rho, h_dl.norm());
      } while (rho < 0);

      // Cache last accepted dog-leg step
      last_accepted_hdl = h_dl;
    } else {
      function_system.linpoint_to_estimate();
      last_accepted_hdl = VectorXd::Zero(gradient.size());
    }

  } else {
    // For Gauss-Newton just apply the update directly.
    function_system.apply_exmap(h_gn);
  }
}

bool Optimizer::powells_dog_leg_update(double epsilon1, double epsilon3,
    SparseSystem& jacobian, VectorXd& f_x, VectorXd& grad) {
  jacobian = function_system.jacobian();
  f_x = function_system.weighted_errors(LINPOINT);
  grad = mul_SparseMatrixTrans_Vector(jacobian, f_x);
  return (f_x.lpNorm<Eigen::Infinity>() <= epsilon3)
      || (grad.lpNorm<Eigen::Infinity>() <= epsilon1);
}

void Optimizer::augment_sparse_linear_system(SparseSystem& W,
    const Properties& prop) {
  if (prop.method == DOG_LEG) {
    // We're using the incremental version of Powell's Dog-Leg, so we need
    // to form the updated gradient.
    const VectorXd& f_new = W.rhs();

    // Augment the running count for the sum-of-squared errors at the current
    // linearization point.
    current_SSE_at_linpoint += f_new.squaredNorm();

    // Allocate the new gradient vector
    VectorXd g_new(W.num_cols());

    // Compute W^T \cdot f_new
    VectorXd increment = mul_SparseMatrixTrans_Vector(W, f_new);

    // Set g_new = (g_old 0)^T + W^T f_new.
    g_new.head(gradient.size()) = gradient + increment.head(gradient.size());
    g_new.tail(W.num_cols() - gradient.size()) = increment.tail(
        W.num_cols() - gradient.size());

    // Cache the new gradient vector
    gradient = g_new;
  }

  // Apply Givens to QR factorize the newly augmented sparse system.
  for (int i = 0; i < W.num_rows(); i++) {
    SparseVector new_row = W.get_row(i);
    function_system._R.add_row_givens(new_row, W.rhs()(i));
  }
}

void Optimizer::update_estimate(const Properties& prop) {
  // Solve for the Gauss-Newton step.
  VectorXd h_gn_reordered = function_system._R.solve();

  // permute from R-ordering to J-ordering
  VectorXd h_gn(h_gn_reordered.size());
  permute_vector(h_gn_reordered, h_gn, function_system._R.r_to_a());

  if (prop.method == GAUSS_NEWTON) {
    function_system.apply_exmap(h_gn);
  } else { //method == DOG_LEG
    // Compute alpha.  Note that since the variable ordering of the factor
    // R differs from that of the original Jacobian J,
    // we must first rearrange the ordering of the elements in the gradient.
    VectorXd reordered_gradient(gradient.size());

    // Permute from J-ordering to R-ordering
    permute_vector(gradient, reordered_gradient, function_system._R.a_to_r());

    double alpha_denominator =
        (function_system._R * reordered_gradient).squaredNorm();

    if (alpha_denominator > 0) {
      double alpha = (gradient.squaredNorm()) / alpha_denominator;

      double rho_denominator;

      VectorXd h_dl = compute_dog_leg(alpha, -gradient, -h_gn, Delta,
          rho_denominator);
      function_system.apply_exmap(-h_dl);

      // Compute the gain ratio
      double rho = (current_SSE_at_linpoint
          - function_system.weighted_errors(ESTIMATE).squaredNorm())
          / rho_denominator;

      update_trust_radius(rho, h_dl.norm());

      if (rho < 0) {
        // The proposed update actually /increased/ the value of the
        // objective function; restore the last good estimate that we had
        VectorXd restore_step(gradient.size());
        restore_step.head(last_accepted_hdl.size()) = last_accepted_hdl;
        restore_step.tail(gradient.size() - last_accepted_hdl.size()).setZero();

        function_system.apply_exmap(-restore_step);
      } else {
        // The proposed update was accepted; cache the dog-leg step used
        // to produce it.
        last_accepted_hdl = h_dl;
      }
    }

    // NOTE:  The negatives prepended to "compute_dog_leg()" and "h_gn"
    // are due to the weird sign change in the exmap functions.
  }
}

void Optimizer::gauss_newton(const Properties& prop, int* num_iterations) {
  // Batch optimization
  int num_iter = 0;

  // Set the new linearization point to be the current estimate.
  function_system.estimate_to_linpoint();

  // Compute Jacobian about current estimate.
  SparseSystem jacobian = function_system.jacobian();

  // Get the current error residual vector
  VectorXd r = function_system.weighted_errors(LINPOINT);

#ifdef USE_PDL_STOPPING_CRITERIA
  // Compute the current gradient direction vector
  VectorXd g = mul_SparseMatrixTrans_Vector(jacobian, r);
#else
  double error = r.squaredNorm();
  double error_new;
  // We haven't computed a step yet, so this initialization is to ensure
  // that we never skip over the while loop as a result of failing
  // change-in-error check.
  double error_diff = prop.epsilon_rel * error + 1;
#endif

  // Compute Gauss-Newton step h_{gn} to get to the next estimated optimizing point.
  VectorXd delta = compute_gauss_newton_step(jacobian);
  while (
  // We ALWAYS use these criteria
  ((prop.max_iterations <= 0) || (num_iter < prop.max_iterations))
      && (delta.norm() > prop.epsilon2)

#ifdef USE_PDL_STOPPING_CRITERIA
      && (r.lpNorm<Eigen::Infinity>() > prop.epsilon3)
      && (g.lpNorm<Eigen::Infinity>() > prop.epsilon1)

#else // Custom stopping criteria for GN
      && (error > prop.epsilon_abs)
      && (fabs(error_diff) > prop.epsilon_rel * error)
#endif

  ) // end while conditional
  {
    num_iter++;
    // Apply the Gauss-Newton step h_{gn} to...
    function_system.apply_exmap(delta);
    // ...set the new linearization point to be the current estimate.
    function_system.estimate_to_linpoint();
    // Relinearize about the new current estimate.
    jacobian = function_system.jacobian();
    // Compute the error residual vector at the new estimate.
    r = function_system.weighted_errors(LINPOINT);

#ifdef USE_PDL_STOPPING_CRITERIA
    g = mul_SparseMatrixTrans_Vector(jacobian, r);
#else
    // Update the error difference in errors between the previous and
    // current estimates.
    error_new = r.squaredNorm();
    error_diff = error - error_new;
    error = error_new;  // Record the absolute error at the current estimate
#endif

    // Compute Gauss-Newton step h_{gn} to get to the next estimated
    // optimizing point.
    delta = compute_gauss_newton_step(jacobian);
    if (!prop.quiet) {
      cout << "Iteration " << num_iter << ": residual ";

#ifdef USE_PDL_STOPPING_CRITERIA
      cout << r.squaredNorm();
#else
      cout << error;
#endif
      cout << endl;
    }
  }  //end while

  if (num_iterations != NULL) {
    *num_iterations = num_iter;
  }
  _cholesky->get_R(function_system._R);
}

void Optimizer::levenberg_marquardt(const Properties& prop,
    int* num_iterations) {
  int num_iter = 0;
  double lambda = prop.lm_lambda0;
  // Using linpoint as current estimate below.
  function_system.estimate_to_linpoint();

  // Get the current Jacobian at the linearization point.
  SparseSystem jacobian = function_system.jacobian();

  // Get the error residual vector at the current linearization point.
  VectorXd r = function_system.weighted_errors(LINPOINT);

  // Record the absolute sum-of-squares error (i.e., objective function value) here.
  double error = r.squaredNorm();

#ifdef USE_PDL_STOPPING_CRITERIA
  // Compute the gradient direction vector at the current linearization point
  VectorXd g = mul_SparseMatrixTrans_Vector(jacobian, r);
#endif

  double error_diff, error_new;

  // solve at J'J + lambda*diag(J'J)
  VectorXd delta = compute_gauss_newton_step(jacobian, &function_system._R,
      lambda);

  while (
  // We ALWAYS use these stopping criteria
  ((prop.max_iterations <= 0) || (num_iter < prop.max_iterations))
      && (delta.norm() > prop.epsilon2)

#ifdef USE_PDL_STOPPING_CRITERIA
      && (r.lpNorm<Eigen::Infinity>() > prop.epsilon3)
      && (g.lpNorm<Eigen::Infinity>() > prop.epsilon1)
#else
      && (error > prop.epsilon_abs)
#endif
  )  // end while conditional
  {
    num_iter++;

    // remember the last accepted linearization point
    function_system.linpoint_to_estimate();
    // Apply the delta vector DIRECTLY TO THE LINEARIZATION POINT!
    function_system.self_exmap(delta);
    error_new = function_system.weighted_errors(LINPOINT).squaredNorm();
    error_diff = error - error_new;
    // feedback
    if (!prop.quiet) {
      cout << "LM Iteration " << num_iter << ": (lambda=" << lambda << ") ";
      if (error_diff > 0.) {
        cout << "residual: " << error_new << endl;
      } else {
        cout << "rejected" << endl;
      }
    }
    // decide if acceptable
    if (error_diff > 0.) {

#ifndef USE_PDL_STOPPING_CRITERIA
      if (error_diff < prop.epsilon_rel * error) {
        break;
      }
#endif

      // Update lambda
      lambda /= prop.lm_lambda_factor;

      // Record the error at the newly-accepted estimate.
      error = error_new;

      // Relinearize around the newly-accepted estimate.
      jacobian = function_system.jacobian();

#ifdef USE_PDL_STOPPING_CRITERIA
      r = function_system.weighted_errors(LINPOINT);
      g = mul_SparseMatrixTrans_Vector(jacobian, r);
#endif
    } else {
      // reject new estimate
      lambda *= prop.lm_lambda_factor;
      // restore previous estimate
      function_system.estimate_to_linpoint();
    }

    // Compute the step for the next iteration.
    delta = compute_gauss_newton_step(jacobian, &function_system._R, lambda);

  } // end while

  if (num_iterations != NULL) {
    *num_iterations = num_iter;
  }
  // Copy current estimate contained in linpoint.
  function_system.linpoint_to_estimate();
}

void Optimizer::powells_dog_leg(int* num_iterations, double delta0,
    int max_iterations, double epsilon1, double epsilon2, double epsilon3) {
  // Batch optimization
  int num_iter = 0;
  // current estimate is used as new linearization point
  function_system.estimate_to_linpoint();

  double delta = delta0;
  SparseSystem jacobian(1, 1);
  VectorXd f_x;
  VectorXd grad;

  bool found = powells_dog_leg_update(epsilon1, epsilon3, jacobian, f_x, grad);

  double rho_denominator;

  while ((not found) && (max_iterations == 0 || num_iter < max_iterations)) {
    num_iter++;
    cout << "PDL Iteration " << num_iter << " residual: " << f_x.squaredNorm()
        << endl;
    // compute alpha
    double alpha = grad.squaredNorm() / (jacobian * grad).squaredNorm();
    // steepest descent
    VectorXd h_sd = -grad;
    // solve Gauss Newton
    VectorXd h_gn = compute_gauss_newton_step(jacobian, &function_system._R);
    // compute dog leg h_dl
    // x0 = x: remember (and return) linearization point of R
    function_system.linpoint_to_estimate();
    VectorXd h_dl = compute_dog_leg(alpha, h_sd, h_gn, delta, rho_denominator);
    // Evaluate new solution, update estimate and trust region.
    if (h_dl.norm() <= epsilon2) {
      found = true;
    } else {
      // new estimate
      // change linearization point directly (original LP saved in estimate)
      function_system.self_exmap(h_dl);
      // calculate gain ratio rho
      VectorXd f_x_new = function_system.weighted_errors(LINPOINT);
      double rho = (f_x.squaredNorm() - f_x_new.squaredNorm())
          / (rho_denominator);
      if (rho > 0) {
        // accept new estimate
        cout << "accepted" << endl;
        f_x = f_x_new;
        found = powells_dog_leg_update(epsilon1, epsilon3, jacobian, f_x, grad);
      } else {
        // reject new estimate, overwrite with last saved one
        cout << "rejected" << endl;
        function_system.estimate_to_linpoint();
      }
      if (rho > 0.75) {
        delta = max(delta, 3.0 * h_dl.norm());
      } else if (rho < 0.25) {
        delta *= 0.5;
        found = (delta <= epsilon2);
      }
    }
  }
  if (num_iterations) {
    *num_iterations = num_iter;
  }
  // Overwrite potentially rejected linearization point with last saved one
  // (could be identical if it was accepted in the last iteration).

  function_system.swap_estimates();

}

void Optimizer::batch_optimize(const Properties& prop, int* num_iterations) {

  const double delta0 = 1.0;

  switch (prop.method) {
  case GAUSS_NEWTON:
    gauss_newton(prop, num_iterations);
    break;
  case DOG_LEG:
    powells_dog_leg(num_iterations, delta0, prop.max_iterations, prop.epsilon1,
        prop.epsilon2, prop.epsilon3); // modifies x0,R
    break;
  case LEVENBERG_MARQUARDT:
    levenberg_marquardt(prop, num_iterations);
    break;
  }

}

}


/// @file  cmp.hpp
/// @brief Declarations for embedding methods using classic ordinal constraints

#pragma once
#ifndef ORDGEO_EMBED_CMP_HPP
#define ORDGEO_EMBED_CMP_HPP

#include <ordgeo/config.hpp>
#include <ordgeo/core/triplets.hpp>
#include <ordgeo/embed/embed.hpp>
#include <Eigen/Dense>
#include <vector>

namespace ORDGEO_NAMESPACE {
namespace embed {

/// Embeds a dataset using Soft Ordinal Embedding.
/// This method uses the margin parameter to set the scale. A default value of
/// 0.1 will be used if no margin is provided.
///
/// Citation: Y. Terada and U. von Luxburg, "Local ordinal embedding," presented
/// at the Proceedings of the 31st International Conference on Machine Learning,
/// 2014.
EmbedResult embedCmpWithSOE(std::vector<ORDGEO_NAMESPACE::core::CmpConstraint> cons,
	const Eigen::MatrixXd& X0, EmbedConfig config);

} // end namespace embed
} // end namespace ORDGEO_NAMESPACE
#endif /* ORDGEO_EMBED_CMP_HPP */


#include "knowledge_of_valid_opening.h"
#include <NTL/GF2X.h>
#include <utils/utils.h>
#include <hamming/jain_commitment/jain_commitment.h>

void hamming_metric::knowledge_of_valid_opening::generate_public_key(
        public_key_t * public_key,
        const private_key_t *private_key) {

    NTL::vec_GF2 r;
    utils::generate_random_binary_matrix(
            public_key->A,
            JAIN_K,
            JAIN_L + JAIN_V);

    utils::generate_random_binary_vector(
            r,
            JAIN_L);

    hamming_metric::commitment::generate_commitment(
            public_key->commitment,
            public_key->A,
            r,
            private_key->m,
            private_key->e);
}

void hamming_metric::knowledge_of_valid_opening::generate_private_key(
        private_key_t * private_key) {

    private_key->e.kill();
    private_key->m.kill();

    utils::generate_random_binary_vector(
            private_key->m,
            JAIN_V);

    utils::generate_vector_of_weight_w(
            private_key->e,
            JAIN_K,
            W);
}

void hamming_metric::knowledge_of_valid_opening::generate_random_values(
        random_values_t *random_values) {

    ::utils::generate_random_binary_vector(
            random_values->v,
            JAIN_L + JAIN_V);

    ::utils::generate_random_binary_vector(
            random_values->f,
            JAIN_K);
}

void hamming_metric::knowledge_of_valid_opening::generate_revealed_values(
        revealed_values_t *revealed_values) {

    utils::create_permutation_matrix(
            revealed_values->P,
            JAIN_K);
}

void hamming_metric::knowledge_of_valid_opening::generate_commitments_and_responses(
        responses_t *responses,
        commitments_t *commitments,
        const random_values_t *random_values,
        const revealed_values_t *revealed_values,
        const public_key_t *public_key,
        const private_key_t *private_key) {

    ::hamming_metric::knowledge_of_valid_opening::generate_commitment_and_response_0(
            commitments->c0,
            commitments->r0,
            responses->t0,
            random_values->v,
            public_key->A,
            random_values->f);

    ::hamming_metric::knowledge_of_valid_opening::generate_commitment_and_response_1(
            commitments->c1,
            commitments->r1,
            responses->t1,
            public_key->A,
            revealed_values->P,
            random_values->f);

    ::hamming_metric::knowledge_of_valid_opening::generate_commitment_and_response_2(
            commitments->c2,
            commitments->r2,
            responses->t2,
            public_key->A,
            revealed_values->P,
            random_values->f,
            private_key->e);
}

void hamming_metric::knowledge_of_valid_opening::encode(
        NTL::vec_GF2 &out,
        const NTL::vec_GF2 &v) {

    out = utils::encode_binary_vector(v, JAIN_V);
}

void hamming_metric::knowledge_of_valid_opening::commit(
        NTL::vec_GF2 &c,
        NTL::vec_GF2 &r,
        const NTL::vec_GF2 &v,
        const NTL::mat_GF2 &A) {

    utils::generate_random_binary_vector(
            r,
            JAIN_L);

    hamming_metric::commitment::generate_commitment(
            c,
            A,
            r,
            v);
}

void hamming_metric::knowledge_of_valid_opening::generate_response_0(
        NTL::vec_GF2 &t0,
        const NTL::vec_GF2 &n,
        const NTL::mat_GF2 &A,
        const NTL::vec_GF2 &f) {

    t0 = (A * n) + f;
}

void hamming_metric::knowledge_of_valid_opening::generate_commitment_and_response_0(
        NTL::vec_GF2 &c0,
        NTL::vec_GF2 &r0,
        NTL::vec_GF2 &t0,
        const NTL::vec_GF2 &n,
        const NTL::mat_GF2 &A,
        const NTL::vec_GF2 &f) {

    generate_response_0(
            t0,
            n,
            A,
            f);

    NTL::vec_GF2 encoded;
    encode(
            encoded,
            t0);

    commit(
            c0,
            r0,
            encoded,
            A);

}

void hamming_metric::knowledge_of_valid_opening::generate_response_1(
        NTL::vec_GF2 &t1,
        const NTL::mat_GF2 &P,
        const NTL::vec_GF2 &f) {

    t1 = P * f;
}

void hamming_metric::knowledge_of_valid_opening::generate_commitment_and_response_1(
        NTL::vec_GF2 &c1,
        NTL::vec_GF2 &r1,
        NTL::vec_GF2 &t1,
        const NTL::mat_GF2 &A,
        const NTL::mat_GF2 &P,
        const NTL::vec_GF2 &f) {

    generate_response_1(
            t1,
            P,
            f);

    NTL::vec_GF2 encoded;
    encode(
            encoded,
           t1);

    commit(
            c1,
           r1,
           encoded,
           A);
}

void hamming_metric::knowledge_of_valid_opening::generate_response_2(
        NTL::vec_GF2 &t2,
        const NTL::mat_GF2 &P,
        const NTL::vec_GF2 &f,
        const NTL::vec_GF2 &e) {

    t2 = P * (f + e);
}

void hamming_metric::knowledge_of_valid_opening::generate_commitment_and_response_2(
        NTL::vec_GF2 &c2,
        NTL::vec_GF2 &r2,
        NTL::vec_GF2 &t2,
        const NTL::mat_GF2 &A,
        const NTL::mat_GF2 &P,
        const NTL::vec_GF2 &f,
        const NTL::vec_GF2 &e) {

    generate_response_2(
            t2,
            P,
            f,
            e);

    NTL::vec_GF2 encoded;
    encode(
            encoded,
           t2);

    commit(
            c2,
           r2,
           encoded,
           A);
}

int hamming_metric::knowledge_of_valid_opening::verify_0(
        const NTL::vec_GF2 &c0,
        const NTL::vec_GF2 &r0,
        const NTL::vec_GF2 &t0,
        const NTL::vec_GF2 &c1,
        const NTL::vec_GF2 &r1,
        const NTL::vec_GF2 &t1,
        const NTL::mat_GF2 &P,
        const public_key_t *public_key) {

    auto _t0 = utils::encode_binary_vector(t0, JAIN_V);
    auto _t1 = utils::encode_binary_vector(t1, JAIN_V);

    if (hamming_metric::commitment::verify(
            c0,
            public_key->A,
            r0,
            _t0) != 0) {
        std::cout << "Knowledge of valid opening. Verification failed on ch = 0 and c0" << std::endl;
        return 1;
    }

    if (hamming_metric::commitment::verify(
            c1,
            public_key->A,
            r1,
            _t1) != 0) {
        std::cout << "Knowledge of valid opening. Verification failed on ch = 0 and c1" << std::endl;
        return 1;
    }

    auto sum = t0 + (NTL::inv(P) * t1);

    NTL::vec_GF2 result;
    if(utils::solve_equation(
            result,
            public_key->A,
            sum) != 0) {
        std::cout << "No solutions" << std::endl;
        return 1;
    }

    return 0;
}

int hamming_metric::knowledge_of_valid_opening::verify_1(
        const NTL::vec_GF2 &c0,
        const NTL::vec_GF2 &r0,
        const NTL::vec_GF2 &t0,
        const NTL::vec_GF2 &c2,
        const NTL::vec_GF2 &r2,
        const NTL::vec_GF2 &t2,
        const NTL::mat_GF2 &P,
        const public_key_t *public_key) {

    auto _t0 = utils::encode_binary_vector(t0, JAIN_V);
    auto _t2 = utils::encode_binary_vector(t2, JAIN_V);

    if (hamming_metric::commitment::verify(
            c0,
            public_key->A,
            r0,
            _t0) != 0) {
        std::cout << "Knowledge of valid opening. Verification failed on ch = 1 and c0" << std::endl;
        return 1;
    }

    if (hamming_metric::commitment::verify(
            c2,
            public_key->A,
            r2,
            _t2) != 0) {
        std::cout << "Knowledge of valid opening. Verification failed on ch = 1 and c2" << std::endl;
        return 1;
    }

    auto sum = t0 + public_key->commitment + (NTL::inv(P) * t2);

    NTL::vec_GF2 result;
    if(utils::solve_equation(
            result,
            public_key->A,
            sum) != 0) {
        std::cout << "No solutions" << std::endl;
        return 1;
    }

    return 0;
}

int hamming_metric::knowledge_of_valid_opening::verify_2(
        const NTL::vec_GF2 &c1,
        const NTL::vec_GF2 &r1,
        const NTL::vec_GF2 &t1,
        const NTL::vec_GF2 &c2,
        const NTL::vec_GF2 &r2,
        const NTL::vec_GF2 &t2,
        const public_key_t *public_key) {

    auto _t1 = utils::encode_binary_vector(t1, JAIN_V);
    auto _t2 = utils::encode_binary_vector(t2, JAIN_V);

    if (hamming_metric::commitment::verify(
            c1,
            public_key->A,
            r1,
            _t1) != 0) {
        std::cout << "Knowledge of valid opening. Verification failed on ch = 2 and c1" << std::endl;
        return 1;
    }

    if (hamming_metric::commitment::verify(
            c2,
            public_key->A,
            r2,
            _t2) != 0) {
        std::cout << "Knowledge of valid opening. Verification failed on ch = 2 and c2" << std::endl;
        return 1;
    }

    if (NTL::weight(t1 + t2) != W) {
        return 1;
    }

    return 0;
}


/**
 * @file testSummarization.cpp
 *
 * @brief Test ported from MastSLAM for a simple batch summarization technique
 *
 * @date May 7, 2013
 * @author Alex Cunningham
 */

#include <boost/assign/std/set.hpp>
#include <boost/assign/std/vector.hpp>

#include <CppUnitLite/TestHarness.h>

#include <gtsam/base/TestableAssertions.h>

#include <gtsam/geometry/Pose2.h>

#include <gtsam/nonlinear/LabeledSymbol.h>
#include <gtsam/nonlinear/summarization.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>

#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>

using namespace std;
using namespace boost::assign;
using namespace gtsam;

const double tol=1e-5;

typedef gtsam::PriorFactor<gtsam::Pose2> PosePrior;
typedef gtsam::BetweenFactor<gtsam::Pose2> PoseBetween;
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2> PosePointBearingRange;

gtsam::noiseModel::Base::shared_ptr model2 = noiseModel::Unit::Create(2);
gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3);

/* ************************************************************************* */
TEST( testSummarization, example_from_ddf1 ) {
  Key xA0 = LabeledSymbol('x', 'A', 0),
      xA1 = LabeledSymbol('x', 'A', 1),
      xA2 = LabeledSymbol('x', 'A', 2);
  Key lA3 = LabeledSymbol('l', 'A', 3), lA5 = LabeledSymbol('l', 'A', 5);

  SharedDiagonal diagmodel2 = noiseModel::Unit::Create(2);
  SharedDiagonal diagmodel4 = noiseModel::Unit::Create(4);

  Pose2 pose0;
  Pose2 pose1(1.0, 0.0, 0.0);
  Pose2 pose2(2.0, 0.0, 0.0);
  Point2 landmark3(3.0, 3.0);
  Point2 landmark5(5.0, 5.0);

  Values values;
  values.insert(xA0, pose0);
  values.insert(xA1, pose1);
  values.insert(xA2, pose2);
  values.insert(lA3, landmark3);
  values.insert(lA5, landmark5);

  // build from nonlinear graph/values
  NonlinearFactorGraph graph;
  graph.add(PosePrior(xA0, Pose2(), model3));
  graph.add(PoseBetween(xA0, xA1, pose0.between(pose1), model3));
  graph.add(PoseBetween(xA1, xA2, pose1.between(pose2), model3));
  graph.add(PosePointBearingRange(xA0, lA3, pose0.bearing(landmark3), pose0.range(landmark3), model2));
  graph.add(PosePointBearingRange(xA1, lA3, pose1.bearing(landmark3), pose1.range(landmark3), model2));
  graph.add(PosePointBearingRange(xA2, lA5, pose2.bearing(landmark5), pose2.range(landmark5), model2));

  KeySet saved_keys;
  saved_keys += lA3, lA5;

  {
    // Summarize to a linear system
    GaussianFactorGraph actLinGraph; Ordering actOrdering;
    SummarizationMode mode = PARTIAL_QR;
    boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);

    Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
    EXPECT(assert_equal(expSumOrdering, actOrdering));

    // Does not split out subfactors where possible
    GaussianFactorGraph expLinGraph;
    expLinGraph.add(
      expSumOrdering[lA3],
      Matrix_(4,2,
        0.595867,  0.605092,
        0.0, -0.406109,
        0.0,       0.0,
        0.0,       0.0),
      expSumOrdering[lA5],
      Matrix_(4,2,
        -0.125971, -0.160052,
        0.13586,  0.301096,
        0.268667,   0.31703,
        0.0, -0.131698),
      zero(4), diagmodel4);
    EXPECT(assert_equal(expLinGraph, actLinGraph, tol));

    // Summarize directly from a nonlinear graph to another nonlinear graph
    NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
    NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);

    EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
  }

  {
    // Summarize to a linear system using cholesky - compare to previous version
    GaussianFactorGraph actLinGraph; Ordering actOrdering;
    SummarizationMode mode = PARTIAL_CHOLESKY;
    boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);

    Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
    EXPECT(assert_equal(expSumOrdering, actOrdering));

    // Does not split out subfactors where possible
    GaussianFactorGraph expLinGraph;
    expLinGraph.add(HessianFactor(JacobianFactor(
        expSumOrdering[lA3],
        Matrix_(4,2,
          0.595867,  0.605092,
          0.0, -0.406109,
          0.0,       0.0,
          0.0,       0.0),
        expSumOrdering[lA5],
        Matrix_(4,2,
          -0.125971, -0.160052,
          0.13586,  0.301096,
          0.268667,   0.31703,
          0.0, -0.131698),
        zero(4), diagmodel4)));
    EXPECT(assert_equal(expLinGraph, actLinGraph, tol));

    // Summarize directly from a nonlinear graph to another nonlinear graph
    NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
    NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);

    EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
  }

  {
    // Summarize to a linear system with joint factor graph version
    GaussianFactorGraph actLinGraph; Ordering actOrdering;
    SummarizationMode mode = SEQUENTIAL_QR;
    boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);

    Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
    EXPECT(assert_equal(expSumOrdering, actOrdering));

    // Does not split out subfactors where possible
    GaussianFactorGraph expLinGraph;
    expLinGraph.add(
        expSumOrdering[lA3],
        Matrix_(2,2,
          0.595867, 0.605092,
          0.0, 0.406109),
        expSumOrdering[lA5],
        Matrix_(2,2,
          -0.125971, -0.160052,
          -0.13586, -0.301096),
        zero(2), diagmodel2);

    expLinGraph.add(
        expSumOrdering[lA5],
        Matrix_(2,2,
          0.268667,  0.31703,
          0.0, 0.131698),
        zero(2), diagmodel2);

    EXPECT(assert_equal(expLinGraph, actLinGraph, tol));

    // Summarize directly from a nonlinear graph to another nonlinear graph
    NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
    NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);

    EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
  }

  {
    // Summarize to a linear system with joint factor graph version
    GaussianFactorGraph actLinGraph; Ordering actOrdering;
    SummarizationMode mode = SEQUENTIAL_CHOLESKY;
    boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);

    Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
    EXPECT(assert_equal(expSumOrdering, actOrdering));

    // Does not split out subfactors where possible
    GaussianFactorGraph expLinGraph;
    expLinGraph.add(
        expSumOrdering[lA3],
        Matrix_(2,2,
          0.595867, 0.605092,
          0.0, 0.406109),
        expSumOrdering[lA5],
        Matrix_(2,2,
          -0.125971, -0.160052,
          -0.13586, -0.301096),
        zero(2), diagmodel2);

    expLinGraph.add(
        expSumOrdering[lA5],
        Matrix_(2,2,
          0.268667,  0.31703,
          0.0, 0.131698),
        zero(2), diagmodel2);

    EXPECT(assert_equal(expLinGraph, actLinGraph, tol));

    // Summarize directly from a nonlinear graph to another nonlinear graph
    NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
    NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);

    EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
  }
}

/* ************************************************************************* */
TEST( testSummarization, no_summarize_case ) {
  // Checks a corner case in which no variables are being eliminated
  gtsam::Key key = 7;
  gtsam::KeySet saved_keys; saved_keys.insert(key);
  NonlinearFactorGraph graph;
  graph.add(PosePrior(key, Pose2(1.0, 2.0, 0.3), model3));
  graph.add(PosePrior(key, Pose2(2.0, 3.0, 0.4), model3));
  Values values;
  values.insert(key, Pose2(0.0, 0.0, 0.1));

  SummarizationMode mode = SEQUENTIAL_CHOLESKY;
  GaussianFactorGraph actLinGraph; Ordering actOrdering;
  boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
  Ordering expOrdering; expOrdering += key;
  GaussianFactorGraph expLinGraph = *graph.linearize(values, expOrdering);
  EXPECT(assert_equal(expOrdering, actOrdering));
  EXPECT(assert_equal(expLinGraph, actLinGraph));
}

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


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// 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)
//=======================================================================
#include <boost/config.hpp>
#include <stdlib.h>
#include <iostream>
#include <stack>
#include <queue>
#include <boost/operators.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/property_map/property_map.hpp>

using namespace boost;

typedef
std::pair < int, int >
  Position;
Position
  knight_jumps[8] = {
    Position(2, -1),
    Position(1, -2),
  Position(-1, -2),
    Position(-2, -1),
    Position(-2, 1),
  Position(-1, 2),
    Position(1, 2),
  Position(2, 1)
};

Position
operator + (const Position & p1, const Position & p2)
{
  return Position(p1.first + p2.first, p1.second + p2.second);
}

struct knights_tour_graph;
struct knight_adjacency_iterator:
  public
  boost::forward_iterator_helper <
  knight_adjacency_iterator,
  Position,
  std::ptrdiff_t,
  Position *,
  Position >
{
  knight_adjacency_iterator()
  {
  }
  knight_adjacency_iterator(int ii, Position p, const knights_tour_graph & g)
    :
  m_pos(p),
  m_g(&g),
  m_i(ii)
  {
    valid_position();
  }
  Position operator *() const
  {
    return
      m_pos +
      knight_jumps[m_i];
  }
  void
  operator++ ()
  {
    ++m_i;
    valid_position();
  }
  bool
    operator == (const knight_adjacency_iterator & x) const {
      return
      m_i ==
      x.
      m_i;
  }
protected:
  void
  valid_position();
  Position
    m_pos;
  const knights_tour_graph *
    m_g;
  int
    m_i;
};

struct knights_tour_graph
{
  typedef Position
    vertex_descriptor;
  typedef
    std::pair <
    vertex_descriptor,
    vertex_descriptor >
    edge_descriptor;
  typedef knight_adjacency_iterator
    adjacency_iterator;
  typedef void
    out_edge_iterator;
  typedef void
    in_edge_iterator;
  typedef void
    edge_iterator;
  typedef void
    vertex_iterator;
  typedef int
    degree_size_type;
  typedef int
    vertices_size_type;
  typedef int
    edges_size_type;
  typedef directed_tag
    directed_category;
  typedef disallow_parallel_edge_tag
    edge_parallel_category;
  typedef adjacency_graph_tag
    traversal_category;
  knights_tour_graph(int n):
  m_board_size(n)
  {
  }
  int
    m_board_size;
};
int
num_vertices(const knights_tour_graph & g)
{
  return g.m_board_size * g.m_board_size;
}

void
knight_adjacency_iterator::valid_position()
{
  Position new_pos = m_pos + knight_jumps[m_i];
  while (m_i < 8 && (new_pos.first < 0 || new_pos.second < 0
                     || new_pos.first >= m_g->m_board_size
                     || new_pos.second >= m_g->m_board_size)) {
    ++m_i;
    new_pos = m_pos + knight_jumps[m_i];
  }
}

std::pair < knights_tour_graph::adjacency_iterator,
  knights_tour_graph::adjacency_iterator >
adjacent_vertices(knights_tour_graph::vertex_descriptor v,
                  const knights_tour_graph & g)
{
  typedef knights_tour_graph::adjacency_iterator Iter;
  return std::make_pair(Iter(0, v, g), Iter(8, v, g));
}

struct compare_first
{
  template < typename P > bool operator() (const P & x, const P & y)
  {
    return x.first < y.first;
  }
};

template < typename Graph, typename TimePropertyMap >
  bool backtracking_search(Graph & g,
                           typename graph_traits <
                           Graph >::vertex_descriptor src,
                           TimePropertyMap time_map)
{
  typedef typename graph_traits < Graph >::vertex_descriptor Vertex;
  typedef std::pair < int, Vertex > P;
  std::stack < P > S;
  int time_stamp = 0;

  S.push(std::make_pair(time_stamp, src));
  while (!S.empty()) {
    Vertex x;
    boost::tie(time_stamp, x) = S.top();
    put(time_map, x, time_stamp);
    // all vertices have been visited, success!
    if (time_stamp == num_vertices(g) - 1)
      return true;

    bool deadend = true;
    typename graph_traits < Graph >::adjacency_iterator i, end;
    for (boost::tie(i, end) = adjacent_vertices(x, g); i != end; ++i)
      if (get(time_map, *i) == -1) {
        S.push(std::make_pair(time_stamp + 1, *i));
        deadend = false;
      }

    if (deadend) {
      put(time_map, x, -1);
      S.pop();
      boost::tie(time_stamp, x) = S.top();
      while (get(time_map, x) != -1) {  // unwind stack to last unexplored vertex
        put(time_map, x, -1);
        S.pop();
        boost::tie(time_stamp, x) = S.top();
      }
    }

  }                             // while (!S.empty())
  return false;
}

template < typename Vertex, typename Graph, typename TimePropertyMap > int
number_of_successors(Vertex x, Graph & g, TimePropertyMap time_map)
{
  int s_x = 0;
  typename graph_traits < Graph >::adjacency_iterator i, end;
  for (boost::tie(i, end) = adjacent_vertices(x, g); i != end; ++i)
    if (get(time_map, *i) == -1)
      ++s_x;
  return s_x;
}

template < typename Graph, typename TimePropertyMap >
  bool warnsdorff(Graph & g,
                  typename graph_traits < Graph >::vertex_descriptor src,
                  TimePropertyMap time_map)
{
  typedef typename graph_traits < Graph >::vertex_descriptor Vertex;
  typedef std::pair < int, Vertex > P;
  std::stack < P > S;
  int time_stamp = 0;

  S.push(std::make_pair(time_stamp, src));
  while (!S.empty()) {
    Vertex x;
    boost::tie(time_stamp, x) = S.top();
    put(time_map, x, time_stamp);
    // all vertices have been visited, success!
    if (time_stamp == num_vertices(g) - 1)
      return true;

    // Put adjacent vertices into a local priority queue
    std::priority_queue < P, std::vector < P >, compare_first > Q;
    typename graph_traits < Graph >::adjacency_iterator i, end;
    int num_succ;
    for (boost::tie(i, end) = adjacent_vertices(x, g); i != end; ++i)
      if (get(time_map, *i) == -1) {
        num_succ = number_of_successors(*i, g, time_map);
        Q.push(std::make_pair(num_succ, *i));
      }
    bool deadend = Q.empty();
    // move vertices from local priority queue to the stack
    for (; !Q.empty(); Q.pop()) {
      boost::tie(num_succ, x) = Q.top();
      S.push(std::make_pair(time_stamp + 1, x));
    }
    if (deadend) {
      put(time_map, x, -1);
      S.pop();
      boost::tie(time_stamp, x) = S.top();
      while (get(time_map, x) != -1) {  // unwind stack to last unexplored vertex
        put(time_map, x, -1);
        S.pop();
        boost::tie(time_stamp, x) = S.top();
      }
    }

  }                             // while (!S.empty())
  return false;
}

struct board_map
{
  typedef int value_type;
  typedef Position key_type;
  typedef read_write_property_map_tag category;
    board_map(int *b, int n):m_board(b), m_size(n)
  {
  }
  friend int get(const board_map & ba, Position p);
  friend void put(const board_map & ba, Position p, int v);
  friend std::ostream & operator << (std::ostream & os, const board_map & ba);
private:
  int *m_board;
  int m_size;
};

int
get(const board_map & ba, Position p)
{
  return ba.m_board[p.first * ba.m_size + p.second];
}

void
put(const board_map & ba, Position p, int v)
{
  ba.m_board[p.first * ba.m_size + p.second] = v;
}

std::ostream & operator << (std::ostream & os, const board_map & ba) {
  for (int i = 0; i < ba.m_size; ++i) {
    for (int j = 0; j < ba.m_size; ++j)
      os << get(ba, Position(i, j)) << "\t";
    os << std::endl;
  }
  return os;
}

int
main(int argc, char *argv[])
{
  int
    N;
  if (argc == 2)
    N = atoi(argv[1]);
  else
    N = 8;

  knights_tour_graph
  g(N);
  int *
    board =
    new int[num_vertices(g)];
  board_map
  chessboard(board, N);
  for (int i = 0; i < N; ++i)
    for (int j = 0; j < N; ++j)
      put(chessboard, Position(i, j), -1);

  bool
    ret =
    warnsdorff(g, Position(0, 0), chessboard);

  if (ret)
    for (int i = 0; i < N; ++i) {
      for (int j = 0; j < N; ++j)
        std::cout << get(chessboard, Position(i, j)) << "\t";
      std::cout << std::endl;
  } else
    std::cout << "method failed" << std::endl;
  return 0;
}


//
//  Copyright (c) 2018-2020, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019-2020, Amit Singh, amitsingh19975@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/numeric/ublas/tensor.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include "utility.hpp"

BOOST_AUTO_TEST_SUITE(test_tensor_static_comparison)

using double_extended = boost::multiprecision::cpp_bin_float_double_extended;

using test_types = zip<int,float,double_extended>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

struct fixture {

    template<size_t... N>
    using extents_type = boost::numeric::ublas::extents<N...>;

    fixture()= default;

    std::tuple<
        extents_type<1,1>,   // 1
        extents_type<2,3>,   // 2
        extents_type<4,1,3>,  // 3
        extents_type<4,2,3>,  // 4
        extents_type<4,2,3,5>   // 5
    > extents;
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_comparison, value,  test_types, fixture)
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

    auto check = [](auto const& /*unused*/, auto& e)
    {
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type, extents_type, layout_type>;
        auto t  = tensor_type ();
        auto t2 = tensor_type ();
        auto v  = value_type  {};

        std::iota(t.begin(), t.end(), v);
        std::iota(t2.begin(), t2.end(), v+2);

        BOOST_CHECK( t == t  );
        BOOST_CHECK( t != t2 );

        if(t.empty())
            return;

        BOOST_CHECK(!(t < t));
        BOOST_CHECK(!(t > t));
        BOOST_CHECK( t < t2 );
        BOOST_CHECK( t2 > t );
        BOOST_CHECK( t <= t );
        BOOST_CHECK( t >= t );
        BOOST_CHECK( t <= t2 );
        BOOST_CHECK( t2 >= t );
        BOOST_CHECK( t2 >= t2 );
        BOOST_CHECK( t2 >= t );
    };

    for_each_in_tuple(extents,check);

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_comparison_with_tensor_expressions, value,  test_types, fixture)
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

    auto check = [](auto const& /*unused*/, auto& e)
    {
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type, extents_type, layout_type>;

        auto t  = tensor_type ();
        auto t2 = tensor_type ();
        auto v  = value_type  {};

        std::iota(t.begin(), t.end(), v);
        std::iota(t2.begin(), t2.end(), v+2);

        BOOST_CHECK( t == t  );
        BOOST_CHECK( t != t2 );

        if(t.empty())
            return;

        BOOST_CHECK( !(t < t) );
        BOOST_CHECK( !(t > t) );
        BOOST_CHECK( t < (t2+t) );
        BOOST_CHECK( (t2+t) > t );
        BOOST_CHECK( t <= (t+t) );
        BOOST_CHECK( (t+t2) >= t );
        BOOST_CHECK( (t2+t2+2) >= t);
        BOOST_CHECK( 2*t2 > t );
        BOOST_CHECK( t < 2*t2 );
        BOOST_CHECK( 2*t2 > t);
        BOOST_CHECK( 2*t2 >= t2 );
        BOOST_CHECK( t2 <= 2*t2);
        BOOST_CHECK( 3*t2 >= t );

    };

    for_each_in_tuple(extents,check);

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_comparison_with_scalar, value,  test_types, fixture)
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

    for_each_in_tuple(extents,[](auto const& /*unused*/, auto& e)
    {
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type, extents_type, layout_type>;

        BOOST_CHECK( tensor_type(value_type{2}) == tensor_type(value_type{2})  );
        BOOST_CHECK( tensor_type(value_type{2}) != tensor_type(value_type{1})  );

        if(ublas::empty(e))
            return;

        BOOST_CHECK( !(tensor_type(2) <  2) );
        BOOST_CHECK( !(tensor_type(2) >  2) );
        BOOST_CHECK(  (tensor_type(2) >= 2) );
        BOOST_CHECK(  (tensor_type(2) <= 2) );
        BOOST_CHECK(  (tensor_type(2) == 2) );
        BOOST_CHECK(  (tensor_type(2) != 3) );

        BOOST_CHECK( !(2 >  tensor_type(2)) );
        BOOST_CHECK( !(2 <  tensor_type(2)) );
        BOOST_CHECK(  (2 <= tensor_type(2)) );
        BOOST_CHECK(  (2 >= tensor_type(2)) );
        BOOST_CHECK(  (2 == tensor_type(2)) );
        BOOST_CHECK(  (3 != tensor_type(2)) );

        BOOST_CHECK( !( tensor_type(2)+3 <  5) );
        BOOST_CHECK( !( tensor_type(2)+3 >  5) );
        BOOST_CHECK(  ( tensor_type(2)+3 >= 5) );
        BOOST_CHECK(  ( tensor_type(2)+3 <= 5) );
        BOOST_CHECK(  ( tensor_type(2)+3 == 5) );
        BOOST_CHECK(  ( tensor_type(2)+3 != 6) );

        BOOST_CHECK( !( 5 >  tensor_type(2)+3) );
        BOOST_CHECK( !( 5 <  tensor_type(2)+3) );
        BOOST_CHECK(  ( 5 >= tensor_type(2)+3) );
        BOOST_CHECK(  ( 5 <= tensor_type(2)+3) );
        BOOST_CHECK(  ( 5 == tensor_type(2)+3) );
        BOOST_CHECK(  ( 6 != tensor_type(2)+3) );

        BOOST_CHECK( !( tensor_type(2)+tensor_type(3) <  5) );
        BOOST_CHECK( !( tensor_type(2)+tensor_type(3) >  5) );
        BOOST_CHECK(  ( tensor_type(2)+tensor_type(3) >= 5) );
        BOOST_CHECK(  ( tensor_type(2)+tensor_type(3) <= 5) );
        BOOST_CHECK(  ( tensor_type(2)+tensor_type(3) == 5) );
        BOOST_CHECK(  ( tensor_type(2)+tensor_type(3) != 6) );

        BOOST_CHECK( !( 5 >  tensor_type(2)+tensor_type(3)) );
        BOOST_CHECK( !( 5 <  tensor_type(2)+tensor_type(3)) );
        BOOST_CHECK(  ( 5 >= tensor_type(2)+tensor_type(3)) );
        BOOST_CHECK(  ( 5 <= tensor_type(2)+tensor_type(3)) );
        BOOST_CHECK(  ( 5 == tensor_type(2)+tensor_type(3)) );
        BOOST_CHECK(  ( 6 != tensor_type(2)+tensor_type(3)) );

    });

}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/combinatorial/include/functions/cnp.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( cnp_real__2_0,  NT2_REAL_TYPES)
{
  using nt2::cnp;
  using nt2::tag::cnp_;
  typedef typename nt2::meta::call<cnp_(T,T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(cnp(nt2::Inf<T>(), nt2::Inf<T>()), nt2::Nan<T>(), 0);
  NT2_TEST_ULP_EQUAL(cnp(nt2::Nan<T>(), nt2::Nan<T>()), nt2::Nan<T>(), 0);
#endif
  NT2_TEST_ULP_EQUAL(cnp(T(10),T(1)), T(10), 0);
  NT2_TEST_ULP_EQUAL(cnp(T(10),T(2)), T(45), 0);
  NT2_TEST_ULP_EQUAL(cnp(T(10),T(8)), T(45), 0);
  NT2_TEST_ULP_EQUAL(cnp(T(2),T(1)), T(2), 0);
  NT2_TEST_ULP_EQUAL(cnp(T(2),T(2)), T(1), 0);
  NT2_TEST_ULP_EQUAL(cnp(nt2::One<T>(), nt2::One<T>()), nt2::One<T>(), 0);
  NT2_TEST_ULP_EQUAL(cnp(nt2::Zero<T>(), nt2::Zero<T>()), nt2::One<T>(), 0);
}

NT2_TEST_CASE_TPL ( cnp_unsigned_int__2_0,  NT2_UNSIGNED_TYPES)
{
  using nt2::cnp;
  using nt2::tag::cnp_;
  typedef typename nt2::meta::call<cnp_(T,T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_ULP_EQUAL(cnp(nt2::One<T>(), nt2::One<T>()), nt2::One<T>(), 0);
  NT2_TEST_ULP_EQUAL(cnp(nt2::Zero<T>(), nt2::Zero<T>()), nt2::One<T>(), 0);
}

NT2_TEST_CASE_TPL ( cnp_signed_int__2_0,  NT2_INTEGRAL_SIGNED_TYPES)
{
  using nt2::cnp;
  using nt2::tag::cnp_;
  typedef typename nt2::meta::call<cnp_(T,T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_ULP_EQUAL(cnp(nt2::One<T>(), nt2::One<T>()), nt2::One<T>(), 0);
  NT2_TEST_ULP_EQUAL(cnp(nt2::Zero<T>(), nt2::Zero<T>()), nt2::One<T>(), 0);
}


#ifndef SPARSE_LINEAR_SOLVERS_HPP
#define SPARSE_LINEAR_SOLVERS_HPP

#include "SparseMatrix.hpp"
#include "Utils.hpp"

#ifdef USEMKL
#include "MklLayer.hpp"
#include "mkl.h"
#endif

#include <Eigen/Sparse>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <iterator>
#include <map>
#include <memory>
#include <unordered_map>
#include <vector>

namespace cask {
  namespace sparse_linear_solvers {

    class Solver {
      public:
        virtual void analyze(
            const Eigen::SparseMatrix<double> &A) {
        }

        virtual void preprocess(
            const Eigen::SparseMatrix<double> &A) {
  }

        virtual Eigen::VectorXd solve(
            const Eigen::SparseMatrix<double>& A,
            const Eigen::VectorXd& b) = 0;
    };

    class EigenSolver: public Solver {
      public:
        virtual Eigen::VectorXd solve(
            const Eigen::SparseMatrix<double>& A,
            const Eigen::VectorXd& b);
    };

    class DfeCgSolver: public Solver {
      public:
        virtual Eigen::VectorXd solve(
            const Eigen::SparseMatrix<double>& A,
            const Eigen::VectorXd& b);
    };

    class DfeBiCgSolver: public Solver {
      public:
        virtual Eigen::VectorXd solve(
            const Eigen::SparseMatrix<double>& A,
            const Eigen::VectorXd& b);
    };

// Equivalent to un-precontitioned CG
class IdentityPreconditioner {
 public:
  IdentityPreconditioner(const CsrMatrix& a) {
      // nothing to do, but maintain a consistent interface
  }

  virtual std::vector<double> apply(const std::vector<double>& x) {
      return x;
  }
};

#ifdef USEMKL

class ILUPreconditioner {
 public:
  DokMatrix pc;
  CsrMatrix l, u; // lower and upper factors stored independently

  // cached values, row and col ptrs; the latter two are 1 based indexed, as required by unitrrsolve
  std::vector<double> Lvalues, Uvalues;
  std::vector<int> Lrow_ptr, Lcol_ind, Urow_ptr, Ucol_ind;
  std::vector<double> res;

  // pre - a is a symmetric matrix
  ILUPreconditioner(const CsrMatrix &a) {
      if (!a.isSymmetric())
          throw std::invalid_argument("ILUPreconditioner only supports symmetric CSR matrices");
      pc = a.toDok();
      for (int i = 1; i < a.n; i++) {
          if (pc.dok.count(i) == 0)
              continue;

          for (auto&p : pc.dok[i]) {
            int k = p.first;
            if (k >= i)
                break;
            if (!pc.isNnz(k, k))
              continue;
            pc.dok[i][k] = pc.dok[i][k] / pc.dok[k][k];
            double beta = pc.dok[i][k];

            for (auto&p : pc.dok[i]) {
              int j = p.first;
              if (j < k + 1)
                continue;

              if (pc.isNnz(k, j)) {
                pc.dok[i][j] = pc.dok[i][j] - pc.dok[k][j] * beta;
              }
            }
          }

          // simplified original version of the inner loops
          // for (int k = 0; k < i; k++) {
          //     // update pivot - a[i,k] = a[i, k] / a[k, k]
          //     if (pc.isNnz(i, k) && pc.isNnz(k, k)) {
          //         pc.get(i, k) = pc.get(i, k) / pc.get(k, k);
          //         double beta = pc.get(i, k);
          //         for (int j = k + 1; j < pc.n; j++) {
          //             // update row - a[i, j] -= a[k, j] * a[i, k]
          //             if (pc.isNnz(i, j) && pc.isNnz(k, j)) {
          //                 pc.get(i, j) = pc.get(i, j) - pc.get(k, j) * beta;
          //             }
          //         }
          //     }
          // }
      }

      l = CsrMatrix{pc.getLowerTriangular()};
      Lvalues = l.values;
      Lrow_ptr = l.getRowPtrWithOneBasedIndex();
      Lcol_ind = l.getColIndWithOneBasedIndex();
      u = CsrMatrix{pc.getUpperTriangular()};
      Uvalues = u.values;
      Urow_ptr = u.getRowPtrWithOneBasedIndex();
      Ucol_ind = u.getColIndWithOneBasedIndex();
      res = std::vector<double>(l.n);
  }

  virtual std::vector<double> apply(const std::vector<double>& x) {
      // solve z = M^-1 r <==> Mz = r <==> LUz = r
      // solve: Ly = r
      cask::mkl::unittrsolve(Lvalues.data(), Lrow_ptr.data(), Lcol_ind.data(), x, res.data(), true);
      // then solve Uz = y
      auto y = res;
      cask::mkl::unittrsolve(Uvalues.data(), Urow_ptr.data(), Ucol_ind.data(), y, res.data(), false);
      return res;
  }

  void pretty_print() {
      pc.pretty_print();
  }
};

/**
 *  Standard preconditioned CG, (Saad et al)
 *  https://en.wikipedia.org/wiki/Conjugate_gradient_method
 */
template<typename T=double, typename Precon=IdentityPreconditioner>
bool pcg(const CsrMatrix& a, double *rhs, double *x, int &iterations, bool verbose = false, cask::utils::Timer* t = nullptr) {
    // configuration (TODO Should be exposed through params)
    char tr = 'l';
    int maxiters = 2000;
    double tol = 1E-5;
    if (t)
      t->tic("cg:setup");
    Precon precon{a};

    int n = a.n;
    auto values  = a.values;
    auto row_ptr = a.getRowPtrWithOneBasedIndex();
    auto col_ind = a.getColIndWithOneBasedIndex();
    assert(row_ptr[0] == 1 && "Expecting one based indexing for use with mkl_?csrsymv");

    std::vector<double> r(n);             // residual
    std::vector<double> b(rhs, rhs + n);  // rhs
    std::vector<double> p(n);             //
    std::vector<double> z(n);             //
    if (t)
      t->toc("cg:setup");

    if (t)
      t->tic("cg:solve");

    //  r = b - A * x
    mkl_dcsrsymv(&tr, &n, values.data(), row_ptr.data(), col_ind.data(), &x[0], &r[0]);
    cblas_daxpby(n, 1.0, &b[0], 1, -1.0, &r[0], 1);

    // z = M^-1 * r
    z = precon.apply(r);

    p = z;

    // rsold = r * z
    double rsold = cblas_ddot(n, &r[0], 1, &z[0], 1);

    for (int i = 0; i < maxiters; i++) {
        if (verbose) {
            std::cout << " rsold " << rsold << "iteration " << iterations << "\n";
        }
        std::vector<double> Ap(n);
        // Ap = A * p
        mkl_dcsrsymv(&tr, &n, values.data(), row_ptr.data(), col_ind.data(), &p[0], &Ap[0]);
        // alpha = rsold / (p * Ap)
        double alpha = rsold / cblas_ddot(n, &p[0], 1, &Ap[0], 1);
        // x = x + alpha * p
        cblas_daxpy(n, alpha, &p[0], 1, &x[0], 1);
        // r = r - alpha * Ap
        cblas_daxpby(n, -alpha, &Ap[0], 1, 1.0, &r[0], 1);

        // z = M^-1 * r
        z = precon.apply(r);

        // rsnew = r * z
        double rsnew = cblas_ddot(n, &r[0], 1, &z[0], 1);

        if (rsnew <= tol * tol) {
            // std::cout << "Found solution" << std::endl;
            // print_array("x", &x[0], n);
          if (t)
            t->toc("cg:solve");
          return true;
        }

        // p = r + (rsnew/rsold) * p
        cblas_daxpby(n, 1, &z[0], 1, rsnew / rsold, &p[0], 1);
        rsold = rsnew;
        iterations = i;
    }

    mkl_free_buffers ();

    if (t)
      t->toc("cg:solve");
    return false;
}
#endif

  }
}

#endif /* end of include guard: SPARSE_LINEAR_SOLVERS_HPP */


#include <stan/math/prim/mat.hpp>
#include <stan/math/prim/scal.hpp>
#include <test/unit/math/prim/prob/vector_rng_test_helper.hpp>
#include <test/unit/math/prim/prob/VectorIntRNGTestRig.hpp>
#include <test/unit/math/prim/prob/util.hpp>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <vector>

class BernoulliTestRig : public VectorIntRNGTestRig {
 public:
  BernoulliTestRig()
      : VectorIntRNGTestRig(10000, 10, {0, 1}, {0.0, 0.1, 0.2, 0.7, 1.0},
                            {0, 1}, {-2.0, -0.5, 1.1, 2.0}, {-2, -1, 2}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& theta, const T2&, const T3&,
                        T_rng& rng) const {
    return stan::math::bernoulli_rng(theta, rng);
  }

  template <typename T1>
  double pmf(int y, T1 theta, double, double) const {
    return std::exp(stan::math::bernoulli_lpmf(y, theta));
  }
};

TEST(ProbDistributionsBernoulli, errorCheck) {
  check_dist_throws_all_types(BernoulliTestRig());
}

TEST(ProbDistributionsBernoulli, distributionCheck) {
  check_counts_real(BernoulliTestRig());
}

TEST(ProbDistributionsBernoulli, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::bernoulli_rng(0.6, rng));

  EXPECT_THROW(stan::math::bernoulli_rng(1.6, rng), std::domain_error);
  EXPECT_THROW(stan::math::bernoulli_rng(-0.6, rng), std::domain_error);
  EXPECT_THROW(stan::math::bernoulli_rng(stan::math::positive_infinity(), rng),
               std::domain_error);
}

TEST(ProbDistributionsBernoulli, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 10000;

  std::vector<double> expected;
  expected.push_back(N * (1 - 0.4));
  expected.push_back(N * 0.4);

  std::vector<int> counts(2);
  for (int i = 0; i < N; ++i) {
    ++counts[stan::math::bernoulli_rng(0.4, rng)];
  }

  assert_chi_squared(counts, expected, 1e-6);
}


/**
 * @file perceptron_test.cpp
 * @author Udit Saxena
 *
 * Tests for perceptron.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/perceptron/perceptron.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace arma;
using namespace mlpack::perceptron;
using namespace mlpack::distribution;

BOOST_AUTO_TEST_SUITE(PerceptronTest);

/**
 * This test tests whether the perceptron converges for the AND gate classifier.
 */
BOOST_AUTO_TEST_CASE(And)
{
  mat trainData;
  trainData << 0 << 1 << 1 << 0 << endr
            << 1 << 0 << 1 << 0 << endr;
  Mat<size_t> labels;
  labels << 0 << 0 << 1 << 0;

  Perceptron<> p(trainData, labels.row(0), 2, 1000);

  mat testData;
  testData << 0 << 1 << 1 << 0 << endr
           << 1 << 0 << 1 << 0 << endr;
  Row<size_t> predictedLabels(testData.n_cols);
  p.Classify(testData, predictedLabels);

  BOOST_CHECK_EQUAL(predictedLabels(0, 0), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 1), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 2), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 3), 0);
}

/**
 * This test tests whether the perceptron converges for the OR gate classifier.
 */
BOOST_AUTO_TEST_CASE(Or)
{
  mat trainData;
  trainData << 0 << 1 << 1 << 0 << endr
            << 1 << 0 << 1 << 0 << endr;

  Mat<size_t> labels;
  labels << 1 << 1 << 1 << 0;

  Perceptron<> p(trainData, labels.row(0), 2, 1000);

  mat testData;
  testData << 0 << 1 << 1 << 0 << endr
           << 1 << 0 << 1 << 0 << endr;
  Row<size_t> predictedLabels(testData.n_cols);
  p.Classify(testData, predictedLabels);

  BOOST_CHECK_EQUAL(predictedLabels(0, 0), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 1), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 2), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 3), 0);
}

/**
 * This tests the convergence on a set of linearly separable data with 3
 * classes.
 */
BOOST_AUTO_TEST_CASE(Random3)
{
  mat trainData;
  trainData << 0 << 1 << 1 << 4 << 5 << 4 << 1 << 2 << 1 << endr
            << 1 << 0 << 1 << 1 << 1 << 2 << 4 << 5 << 4 << endr;

  Mat<size_t> labels;
  labels << 0 << 0 << 0 << 1 << 1 << 1 << 2 << 2 << 2;

  Perceptron<> p(trainData, labels.row(0), 3, 1000);

  mat testData;
  testData << 0 << 1 << 1 << endr
           << 1 << 0 << 1 << endr;
  Row<size_t> predictedLabels(testData.n_cols);
  p.Classify(testData, predictedLabels);

  for (size_t i = 0; i < predictedLabels.n_cols; i++)
    BOOST_CHECK_EQUAL(predictedLabels(0, i), 0);

}

/**
 * This tests the convergence of the perceptron on a dataset which has only TWO
 * points which belong to different classes.
 */
BOOST_AUTO_TEST_CASE(TwoPoints)
{
  mat trainData;
  trainData << 0 << 1 << endr
            << 1 << 0 << endr;

  Mat<size_t> labels;
  labels << 0 << 1;

  Perceptron<> p(trainData, labels.row(0), 2, 1000);

  mat testData;
  testData << 0 << 1 << endr
           << 1 << 0 << endr;
  Row<size_t> predictedLabels(testData.n_cols);
  p.Classify(testData, predictedLabels);

  BOOST_CHECK_EQUAL(predictedLabels(0, 0), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 1), 1);
}

/**
 * This tests the convergence of the perceptron on a dataset which has a
 * non-linearly separable dataset.
 */
BOOST_AUTO_TEST_CASE(NonLinearlySeparableDataset)
{
  mat trainData;
  trainData << 1 << 2 << 3 << 4 << 5 << 6 << 7 << 8
            << 1 << 2 << 3 << 4 << 5 << 6 << 7 << 8 << endr
            << 1 << 1 << 1 << 1 << 1 << 1 << 1 << 1
            << 2 << 2 << 2 << 2 << 2 << 2 << 2 << 2 << endr;

  Mat<size_t> labels;
  labels << 0 << 0 << 0 << 1 << 0 << 1 << 1 << 1
         << 0 << 0 << 0 << 1 << 0 << 1 << 1 << 1;

  Perceptron<> p(trainData, labels.row(0), 2, 1000);

  mat testData;
  testData << 3 << 4   << 5   << 6   << endr
           << 3 << 2.3 << 1.7 << 1.5 << endr;
  Row<size_t> predictedLabels(testData.n_cols);
  p.Classify(testData, predictedLabels);

  BOOST_CHECK_EQUAL(predictedLabels(0, 0), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 1), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 2), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 3), 1);
}

BOOST_AUTO_TEST_CASE(SecondaryConstructor)
{
  mat trainData;
  trainData << 1 << 2 << 3 << 4 << 5 << 6 << 7 << 8
            << 1 << 2 << 3 << 4 << 5 << 6 << 7 << 8 << endr
            << 1 << 1 << 1 << 1 << 1 << 1 << 1 << 1
            << 2 << 2 << 2 << 2 << 2 << 2 << 2 << 2 << endr;

  Mat<size_t> labels;
  labels << 0 << 0 << 0 << 1 << 0 << 1 << 1 << 1
         << 0 << 0 << 0 << 1 << 0 << 1 << 1 << 1;

  Perceptron<> p1(trainData, labels.row(0), 2, 1000);

  Perceptron<> p2(p1);
}

BOOST_AUTO_TEST_SUITE_END();


// Bring in gtest
#include <gtest/gtest.h>
#include <boost/cstdint.hpp>

// Helpful functions from libsm
#include <sm/eigen/gtest.hpp>
#include <sm/kinematics/quaternion_algebra.hpp>
#include <sm/kinematics/UncertainTransformation.hpp>

TEST(UncertainTransformationTestSuite, testConstructor)
{
  using namespace sm::kinematics;

  UncertainTransformation T_a_b;
  Eigen::Matrix4d T;
  T.setIdentity();

  UncertainTransformation::covariance_t U;
  U.setZero();

  sm::eigen::assertNear(T, T_a_b.T(), 1e-10, SM_SOURCE_FILE_POS, "Checking for the default constructor creating identity");
  sm::eigen::assertNear(U, T_a_b.U(), 1e-10, SM_SOURCE_FILE_POS, "Checking for the default constructor creating zero uncertainty");

}

TEST(UncertainTransformationTestSuite, testTV4Multiplication)
{
  using namespace sm::kinematics;

  for(int i = 0; i < 100; i++)
    {
      UncertainTransformation T_a_b;
      T_a_b.setRandom();
      Eigen::Vector4d v_b;
      v_b.setRandom();
      v_b *= 100.0;

      Eigen::Vector4d v_a = T_a_b * v_b;
      Eigen::Vector4d v_a_prime = T_a_b.T() * v_b;
      sm::eigen::assertNear(v_a, v_a_prime, 1e-10, SM_SOURCE_FILE_POS, "Checking for composition equal to matrix multiplication");
    }

}

TEST(UncertainTransformationTestSuite, testTVhMultiplication)
{
  using namespace sm::kinematics;

  for(int i = 0; i < 100; i++)
    {
      UncertainTransformation T_a_b;
      T_a_b.setRandom();
      Eigen::Vector3d v_b;
      v_b.setRandom();
      v_b *= 100.0;
      HomogeneousPoint V_b(v_b);

      Eigen::Vector3d v_a = T_a_b * v_b;
      HomogeneousPoint V_a = T_a_b * V_b;
      sm::eigen::assertNear(V_a.toEuclidean(), v_a, 1e-10, SM_SOURCE_FILE_POS, "Checking for composition equal to matrix multiplication");
    }

}

TEST(UncertainTransformationTestSuite, testTVMultiplication)
{
  using namespace sm::kinematics;

  for(int i = 0; i < 100; i++)
    {
      UncertainTransformation T_a_b;
      T_a_b.setRandom();
      Eigen::Vector3d v_b;
      v_b.setRandom();
      v_b *= 100.0;

      Eigen::Vector3d v_a = T_a_b * v_b;
      Eigen::Vector3d v_a_prime = T_a_b.C() * v_b + T_a_b.t();
      sm::eigen::assertNear(v_a, v_a_prime, 1e-10, SM_SOURCE_FILE_POS, "Checking for composition equal to matrix multiplication");
    }

}

TEST(UncertainTransformationTestSuite, testTTMultiplication)
{
  try {
    using namespace sm::kinematics;

    for(int i = 0; i < 100; i++)
      {
	UncertainTransformation T_a_b;
	T_a_b.setRandom();

	UncertainTransformation T_b_c;
	T_b_c.setRandom();

	UncertainTransformation T_a_c = T_a_b * T_b_c;
	Eigen::Matrix4d T_a_c_prime = T_a_b.T() * T_b_c.T();
	sm::eigen::assertNear(T_a_c.T(), T_a_c_prime, 1e-10, SM_SOURCE_FILE_POS, "Checking for composition equal to matrix multiplication");
      }

  } catch(const std::exception & e)
    {
      FAIL() << e.what();
    }

}

TEST(UncertainTransformationTestSuite, testInvert)
{
  using namespace sm::kinematics;

  for(int i = 0; i < 100; i++)
    {
      UncertainTransformation T_a_b;
      T_a_b.setRandom();

      UncertainTransformation T_b_a = T_a_b.inverse();
      UncertainTransformation T_a_b_prime = T_b_a.inverse();

      sm::eigen::assertNear(T_a_b_prime.T(), T_a_b.T(), 1e-10, SM_SOURCE_FILE_POS, "Checking for identity");

    }
}

TEST(UncertainTransformationTestSuite, testInvertProducesIdentity)
{
  using namespace sm::kinematics;

  for(int i = 0; i < 100; i++)
    {
      UncertainTransformation T_a_b;
      T_a_b.setRandom();
      UncertainTransformation T_b_a = T_a_b.inverse();
      UncertainTransformation Eye1 = T_a_b * T_b_a;
      UncertainTransformation Eye2 = T_b_a * T_a_b;

      sm::eigen::assertNear(Eye1.T(), Eigen::Matrix4d::Identity(), 1e-10, SM_SOURCE_FILE_POS, "Checking for identity");
      sm::eigen::assertNear(Eye2.T(), Eigen::Matrix4d::Identity(), 1e-10, SM_SOURCE_FILE_POS, "Checking for identity");
    }

}

TEST(UncertainTransformationTestSuite, testInverse)
{
 using namespace sm::kinematics;
  UncertainTransformation TT_01;
  TT_01.setRandom();
  UncertainTransformation TT_10 = TT_01.inverse();
  UncertainTransformation TTT_01 = TT_10.inverse();

  Eigen::Matrix4d r1_T_01 = TT_01.T();
  Eigen::Matrix4d r2_T_01 = TTT_01.T();

  sm::eigen::assertNear(r1_T_01, r2_T_01, 1e-10, SM_SOURCE_FILE_POS, "Testing that composition and inverse derive the same answer");

  UncertainTransformation::covariance_t r1_U_01 = TT_01.U();
  UncertainTransformation::covariance_t r2_U_01 = TTT_01.U();
  sm::eigen::assertNear(r1_U_01, r2_U_01, 1e-10, SM_SOURCE_FILE_POS, "Testing that composition and inverse derive the same answer");

}

TEST(UncertainTransformationTestSuite, testComposition2)
{

 using namespace sm::kinematics;
  UncertainTransformation TT_01;
  TT_01.setRandom();
  UncertainTransformation TT_12;
  TT_12.setRandom();
  UncertainTransformation TT_02 = TT_01 * TT_12;

  UncertainTransformation TT_10 = TT_01.inverse();
  UncertainTransformation TT_21 = TT_12.inverse();
  UncertainTransformation TT_20 = TT_21 * TT_10;

  UncertainTransformation TT_20direct = TT_02.inverse();

  Eigen::Matrix4d r1_T_20 = TT_20.T();
  Eigen::Matrix4d r2_T_20 = TT_20direct.T();

  sm::eigen::assertNear(r1_T_20, r2_T_20, 1e-10, SM_SOURCE_FILE_POS, "Testing that composition and inverse derive the same answer");

  UncertainTransformation::covariance_t r1_U_20 = TT_20.U();
  UncertainTransformation::covariance_t r2_U_20 = TT_20direct.U();
  sm::eigen::assertNear(r1_U_20, r2_U_20, 1e-8, SM_SOURCE_FILE_POS, "Testing that composition and inverse derive the same answer");

}

TEST(UncertainTransformationTestSuite, testTVMultiplication2)
{
  using namespace sm::kinematics;

  for(int i = 0; i < 10; i++)
    {
      UncertainTransformation T_a_b;
      T_a_b.setRandom();

      UncertainHomogeneousPoint v_b;
      v_b.setRandom();

      UncertainHomogeneousPoint v_a = T_a_b * v_b;
      Eigen::Vector3d v_a_prime = T_a_b.C() * v_b.toEuclidean() + T_a_b.t();
      sm::eigen::assertNear(v_a.toEuclidean(), v_a_prime, 1e-10, SM_SOURCE_FILE_POS, "Checking for composition equal to matrix multiplication");
    }

}

// lestefan: added this
TEST(UncertainTransformationTestSuite, testUOplus)
{
  using namespace sm::kinematics;

  for(int i = 0; i < 10; i++)
    {
      UncertainTransformation T_a_b_1;
      T_a_b_1.setRandom();

      UncertainTransformation::covariance_t UOplus = T_a_b_1.UOplus();

      UncertainTransformation T_a_b_2=T_a_b_1;
      T_a_b_2.setUOplus(UOplus);
      sm::eigen::assertNear(T_a_b_1.U(), T_a_b_2.U(), 1e-10, SM_SOURCE_FILE_POS, "Checking for getting and setting OPlus-type uncertainties");
    }

}


#include <NTL/mat_ZZ_pE.h>
#include <NTL/BasicThreadPool.h>

NTL_START_IMPL

//===================================================

#define PAR_THRESH (40000.0)

static double
ZZ_pE_SizeInWords()
{
   return double(deg(ZZ_pE::modulus()))*double(ZZ_p::ModulusSize());
}

static
void mul_aux(Mat<ZZ_pE>& X, const Mat<ZZ_pE>& A, const Mat<ZZ_pE>& B)
{
   long n = A.NumRows();
   long l = A.NumCols();
   long m = B.NumCols();

   if (l != B.NumRows())
      LogicError("matrix mul: dimension mismatch");

   X.SetDims(n, m);

   ZZ_pContext ZZ_p_context;
   ZZ_p_context.save();
   ZZ_pEContext ZZ_pE_context;
   ZZ_pE_context.save();
   double sz = ZZ_pE_SizeInWords();

   bool seq = (double(n)*double(l)*double(m)*sz*sz < PAR_THRESH);

   NTL_GEXEC_RANGE(seq, m, first, last)
   NTL_IMPORT(n)
   NTL_IMPORT(l)
   NTL_IMPORT(m)

   ZZ_p_context.restore();
   ZZ_pE_context.restore();

   long i, j, k;
   ZZ_pX acc, tmp;

   Vec<ZZ_pE> B_col;
   B_col.SetLength(l);

   for (j = first; j < last; j++) {
      for (k = 0; k < l; k++) B_col[k] = B[k][j];

      for (i = 0; i < n; i++) {
         clear(acc);
         for (k = 0; k < l; k++) {
            mul(tmp, rep(A[i][k]), rep(B_col[k]));
            add(acc, acc, tmp);
         }
         conv(X[i][j], acc);
      }
   }

   NTL_GEXEC_RANGE_END
}

void mul(mat_ZZ_pE& X, const mat_ZZ_pE& A, const mat_ZZ_pE& B)
{
   if (&X == &A || &X == &B) {
      mat_ZZ_pE tmp;
      mul_aux(tmp, A, B);
      X = tmp;
   }
   else
      mul_aux(X, A, B);
}

void inv(ZZ_pE& d, Mat<ZZ_pE>& X, const Mat<ZZ_pE>& A)
{
   long n = A.NumRows();

   if (A.NumCols() != n)
      LogicError("inv: nonsquare matrix");

   if (n == 0) {
      set(d);
      X.SetDims(0, 0);
      return;
   }

   const ZZ_pXModulus& G = ZZ_pE::modulus();

   ZZ_pX t1, t2;
   ZZ_pX pivot;
   ZZ_pX pivot_inv;

   Vec< Vec<ZZ_pX> > M;
   // scratch space

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetLength(n);
      for (long j = 0; j < n; j++) {
         M[i][j].SetMaxLength(2*deg(G)-1);
         M[i][j] = rep(A[i][j]);
      }
   }

   ZZ_pX det;
   det = 1;

   Vec<long> P;
   P.SetLength(n);
   for (long k = 0; k < n; k++) P[k] = k;
   // records swap operations

   ZZ_pContext ZZ_p_context;
   ZZ_p_context.save();
   double sz = ZZ_pE_SizeInWords();

   bool seq = double(n)*double(n)*sz*sz < PAR_THRESH;

   bool pivoting = false;

   for (long k = 0; k < n; k++) {

      long pos = -1;

      for (long i = k; i < n; i++) {
         rem(pivot, M[i][k], G);
         if (pivot != 0) {
            InvMod(pivot_inv, pivot, G);
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
            P[k] = pos;
            pivoting = true;
         }

         MulMod(det, det, pivot, G);

         {
            // multiply row k by pivot_inv
            ZZ_pX *y = &M[k][0];
            for (long j = 0; j < n; j++) {
               rem(t2, y[j], G);
               MulMod(y[j], t2, pivot_inv, G);
            }
            y[k] = pivot_inv;
         }

         NTL_GEXEC_RANGE(seq, n, first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         ZZ_p_context.restore();

         ZZ_pX *y = &M[k][0];
         ZZ_pX t1, t2;

         for (long i = first; i < last; i++) {
            if (i == k) continue; // skip row k

            ZZ_pX *x = &M[i][0];
            rem(t1, x[k], G);
            negate(t1, t1);
            x[k] = 0;
            if (t1 == 0) continue;

            // add t1 * row k to row i
            for (long j = 0; j < n; j++) {
               mul(t2, y[j], t1);
               add(x[j], x[j], t2);
            }
         }
         NTL_GEXEC_RANGE_END
      }
      else {
         clear(d);
         return;
      }
   }

   if (pivoting) {
      // pivot colums, using reverse swap sequence

      for (long i = 0; i < n; i++) {
         ZZ_pX *x = &M[i][0];

         for (long k = n-1; k >= 0; k--) {
            long pos = P[k];
            if (pos != k) swap(x[pos], x[k]);
         }
      }
   }

   X.SetDims(n, n);
   for (long i = 0; i < n; i++)
      for (long j = 0; j < n; j++)
         conv(X[i][j], M[i][j]);

   conv(d, det);
}

static
void solve_impl(ZZ_pE& d, Vec<ZZ_pE>& X,
                const Mat<ZZ_pE>& A, const Vec<ZZ_pE>& b, bool trans)

{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("solve: nonsquare matrix");

   if (b.length() != n)
      LogicError("solve: dimension mismatch");

   if (n == 0) {
      set(d);
      X.SetLength(0);
      return;
   }

   ZZ_pX t1, t2;

   const ZZ_pXModulus& G = ZZ_pE::modulus();

   Vec< Vec<ZZ_pX> > M;

   M.SetLength(n);

   for (long i = 0; i < n; i++) {
      M[i].SetLength(n+1);
      for (long j = 0; j < n; j++) M[i][j].SetMaxLength(2*deg(G)-1);

      if (trans)
         for (long j = 0; j < n; j++) M[i][j] = rep(A[j][i]);
      else
         for (long j = 0; j < n; j++) M[i][j] = rep(A[i][j]);

      M[i][n] = rep(b[i]);
   }

   ZZ_pX det;
   set(det);

   ZZ_pContext ZZ_p_context;
   ZZ_p_context.save();
   double sz = ZZ_pE_SizeInWords();

   for (long k = 0; k < n; k++) {
      long pos = -1;
      for (long i = k; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], G);

         // make M[k, k] == -1 mod G, and make row k reduced

         InvMod(t1, M[k][k], G);
         negate(t1, t1);
         for (long j = k+1; j <= n; j++) {
            rem(t2, M[k][j], G);
            MulMod(M[k][j], t2, t1, G);
         }

         bool seq =
            double(n-(k+1))*(n-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(k+1), first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         ZZ_p_context.restore();

         ZZ_pX t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + k+1;

            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            ZZ_pX *x = M[i].elts() + (k+1);
            ZZ_pX *y = M[k].elts() + (k+1);

            for (long j = k+1; j <= n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }

         NTL_GEXEC_RANGE_END

      }
      else {
         clear(d);
         return;
      }
   }

   X.SetLength(n);
   for (long i = n-1; i >= 0; i--) {
      clear(t1);
      for (long j = i+1; j < n; j++) {
         mul(t2, rep(X[j]), M[i][j]);
         add(t1, t1, t2);
      }
      sub(t1, t1, M[i][n]);
      conv(X[i], t1);
   }

   conv(d, det);
}

void solve(ZZ_pE& d, Vec<ZZ_pE>& x,
               const Mat<ZZ_pE>& A, const Vec<ZZ_pE>& b)
{
   solve_impl(d, x, A, b, true);
}

void solve(ZZ_pE& d, const Mat<ZZ_pE>& A,
               Vec<ZZ_pE>& x, const Vec<ZZ_pE>& b)
{
   solve_impl(d, x, A, b, false);
}

long gauss(Mat<ZZ_pE>& M_in, long w)
{
   ZZ_pX t1, t2;
   ZZ_pX piv;

   long n = M_in.NumRows();
   long m = M_in.NumCols();

   if (w < 0 || w > m)
      LogicError("gauss: bad args");

   const ZZ_pXModulus& G = ZZ_pE::modulus();

   Vec< Vec<ZZ_pX> > M;

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetLength(m);
      for (long j = 0; j < m; j++) {
         M[i][j].SetLength(2*deg(G)-1);
         M[i][j] = rep(M_in[i][j]);
      }
   }

   ZZ_pContext ZZ_p_context;
   ZZ_p_context.save();
   double sz = ZZ_pE_SizeInWords();

   long l = 0;
   for (long k = 0; k < w && l < n; k++) {

      long pos = -1;
      for (long i = l; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         swap(M[pos], M[l]);

         InvMod(piv, M[l][k], G);
         negate(piv, piv);

         for (long j = k+1; j < m; j++) {
            rem(M[l][j], M[l][j], G);
         }

         bool seq =
            double(n-(l+1))*double(m-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(l+1), first, last)
         NTL_IMPORT(m)
         NTL_IMPORT(k)
         NTL_IMPORT(l)

         ZZ_p_context.restore();

         ZZ_pX t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + l+1;

            // M[i] = M[i] + M[l]*M[i,k]*piv

            MulMod(t1, M[i][k], piv, G);

            clear(M[i][k]);

            ZZ_pX *x = M[i].elts() + (k+1);
            ZZ_pX *y = M[l].elts() + (k+1);

            for (long j = k+1; j < m; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(t2, t2, *x);
               *x = t2;
            }
         }

         NTL_GEXEC_RANGE_END

         l++;
      }
   }

   for (long i = 0; i < n; i++)
      for (long j = 0; j < m; j++)
         conv(M_in[i][j], M[i][j]);

   return l;
}

long gauss(Mat<ZZ_pE>& M)
{
   return gauss(M, M.NumCols());
}

void image(Mat<ZZ_pE>& X, const Mat<ZZ_pE>& A)
{
   Mat<ZZ_pE> M;
   M = A;
   long r = gauss(M);
   M.SetDims(r, M.NumCols());
   X = M;
}

void kernel(Mat<ZZ_pE>& X, const Mat<ZZ_pE>& A)
{
   long m = A.NumRows();
   long n = A.NumCols();

   const ZZ_pXModulus& G = ZZ_pE::modulus();

   Mat<ZZ_pE> M;

   transpose(M, A);
   long r = gauss(M);

   if (r == 0) {
      ident(X, m);
      return;
   }

   X.SetDims(m-r, m);

   if (m-r == 0 || m == 0) return;

   Vec<long> D;
   D.SetLength(m);
   for (long j = 0; j < m; j++) D[j] = -1;

   Vec<ZZ_pE> inverses;
   inverses.SetLength(m);

   for (long i = 0, j = -1; i < r; i++) {
      do {
         j++;
      } while (IsZero(M[i][j]));

      D[j] = i;
      inv(inverses[j], M[i][j]);
   }

   ZZ_pEContext ZZ_pE_context;
   ZZ_pE_context.save();
   ZZ_pContext ZZ_p_context;
   ZZ_p_context.save();
   double sz = ZZ_pE_SizeInWords();

   bool seq =
      double(m-r)*double(r)*double(r)*sz*sz < PAR_THRESH;

   NTL_GEXEC_RANGE(seq, m-r, first, last)
   NTL_IMPORT(m)
   NTL_IMPORT(r)

   ZZ_p_context.restore();
   ZZ_pE_context.restore();

   ZZ_pX t1, t2;
   ZZ_pE T3;

   for (long k = first; k < last; k++) {
      Vec<ZZ_pE>& v = X[k];
      long pos = 0;
      for (long j = m-1; j >= 0; j--) {
         if (D[j] == -1) {
            if (pos == k)
               set(v[j]);
            else
               clear(v[j]);
            pos++;
         }
         else {
            long i = D[j];

            clear(t1);

            for (long s = j+1; s < m; s++) {
               mul(t2, rep(v[s]), rep(M[i][s]));
               add(t1, t1, t2);
            }

            conv(T3, t1);
            mul(T3, T3, inverses[j]);
            negate(v[j], T3);
         }
      }
   }

   NTL_GEXEC_RANGE_END
}

void determinant(ZZ_pE& d, const Mat<ZZ_pE>& M_in)
{
   ZZ_pX t1, t2;

   const ZZ_pXModulus& G = ZZ_pE::modulus();

   long n = M_in.NumRows();

   if (M_in.NumCols() != n)
      LogicError("determinant: nonsquare matrix");

   if (n == 0) {
      set(d);
      return;
   }

   Vec< Vec<ZZ_pX> > M;

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetLength(n);
      for (long j = 0; j < n; j++) {
         M[i][j].SetMaxLength(2*deg(G)-1);
         M[i][j] = rep(M_in[i][j]);
      }
   }

   ZZ_pX det;
   set(det);

   ZZ_pContext ZZ_p_context;
   ZZ_p_context.save();
   double sz = ZZ_pE_SizeInWords();

   for (long k = 0; k < n; k++) {
      long pos = -1;
      for (long i = k; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1))
            pos = i;
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], G);

         // make M[k, k] == -1 mod G, and make row k reduced

         InvMod(t1, M[k][k], G);
         negate(t1, t1);
         for (long j = k+1; j < n; j++) {
            rem(t2, M[k][j], G);
            MulMod(M[k][j], t2, t1, G);
         }

         bool seq =
            double(n-(k+1))*(n-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(k+1), first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         ZZ_p_context.restore();

         ZZ_pX t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + k+1;

            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            ZZ_pX *x = M[i].elts() + (k+1);
            ZZ_pX *y = M[k].elts() + (k+1);

            for (long j = k+1; j < n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }

         NTL_GEXEC_RANGE_END

      }
      else {
         clear(d);
         return;
      }
   }

   conv(d, det);
}

//===================================================

void add(mat_ZZ_pE& X, const mat_ZZ_pE& A, const mat_ZZ_pE& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix add: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         add(X(i,j), A(i,j), B(i,j));
}

void sub(mat_ZZ_pE& X, const mat_ZZ_pE& A, const mat_ZZ_pE& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix sub: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         sub(X(i,j), A(i,j), B(i,j));
}

void negate(mat_ZZ_pE& X, const mat_ZZ_pE& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         negate(X(i,j), A(i,j));
}

static
void mul_aux(vec_ZZ_pE& x, const mat_ZZ_pE& A, const vec_ZZ_pE& b)
{
   long n = A.NumRows();
   long l = A.NumCols();

   if (l != b.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(n);

   long i, k;
   ZZ_pX acc, tmp;

   for (i = 1; i <= n; i++) {
      clear(acc);
      for (k = 1; k <= l; k++) {
         mul(tmp, rep(A(i,k)), rep(b(k)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_ZZ_pE& x, const mat_ZZ_pE& A, const vec_ZZ_pE& b)
{
   if (&b == &x || A.alias(x)) {
      vec_ZZ_pE tmp;
      mul_aux(tmp, A, b);
      x = tmp;
   }
   else
      mul_aux(x, A, b);
}

static
void mul_aux(vec_ZZ_pE& x, const vec_ZZ_pE& a, const mat_ZZ_pE& B)
{
   long n = B.NumRows();
   long l = B.NumCols();

   if (n != a.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(l);

   long i, k;
   ZZ_pX acc, tmp;

   for (i = 1; i <= l; i++) {
      clear(acc);
      for (k = 1; k <= n; k++) {
         mul(tmp, rep(a(k)), rep(B(k,i)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_ZZ_pE& x, const vec_ZZ_pE& a, const mat_ZZ_pE& B)
{
   if (&a == &x) {
      vec_ZZ_pE tmp;
      mul_aux(tmp, a, B);
      x = tmp;
   }
   else
      mul_aux(x, a, B);

}

void ident(mat_ZZ_pE& X, long n)
{
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            set(X(i, j));
         else
            clear(X(i, j));
}

long IsIdent(const mat_ZZ_pE& A, long n)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (!IsOne(A(i, j))) return 0;
         }

   return 1;
}

void transpose(mat_ZZ_pE& X, const mat_ZZ_pE& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   long i, j;

   if (&X == & A) {
      if (n == m)
         for (i = 1; i <= n; i++)
            for (j = i+1; j <= n; j++)
               swap(X(i, j), X(j, i));
      else {
         mat_ZZ_pE tmp;
         tmp.SetDims(m, n);
         for (i = 1; i <= n; i++)
            for (j = 1; j <= m; j++)
               tmp(j, i) = A(i, j);
         X.kill();
         X = tmp;
      }
   }
   else {
      X.SetDims(m, n);
      for (i = 1; i <= n; i++)
         for (j = 1; j <= m; j++)
            X(j, i) = A(i, j);
   }
}

void mul(mat_ZZ_pE& X, const mat_ZZ_pE& A, const ZZ_pE& b_in)
{
   ZZ_pE b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_ZZ_pE& X, const mat_ZZ_pE& A, const ZZ_p& b_in)
{
   NTL_ZZ_pRegister(b);
   b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_ZZ_pE& X, const mat_ZZ_pE& A, long b_in)
{
   NTL_ZZ_pRegister(b);
   b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void diag(mat_ZZ_pE& X, long n, const ZZ_pE& d_in)
{
   ZZ_pE d = d_in;
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            X(i, j) = d;
         else
            clear(X(i, j));
}

long IsDiag(const mat_ZZ_pE& A, long n, const ZZ_pE& d)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (A(i, j) != d) return 0;
         }

   return 1;
}

long IsZero(const mat_ZZ_pE& a)
{
   long n = a.NumRows();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

void clear(mat_ZZ_pE& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

mat_ZZ_pE operator+(const mat_ZZ_pE& a, const mat_ZZ_pE& b)
{
   mat_ZZ_pE res;
   add(res, a, b);
   NTL_OPT_RETURN(mat_ZZ_pE, res);
}

mat_ZZ_pE operator*(const mat_ZZ_pE& a, const mat_ZZ_pE& b)
{
   mat_ZZ_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(mat_ZZ_pE, res);
}

mat_ZZ_pE operator-(const mat_ZZ_pE& a, const mat_ZZ_pE& b)
{
   mat_ZZ_pE res;
   sub(res, a, b);
   NTL_OPT_RETURN(mat_ZZ_pE, res);
}

mat_ZZ_pE operator-(const mat_ZZ_pE& a)
{
   mat_ZZ_pE res;
   negate(res, a);
   NTL_OPT_RETURN(mat_ZZ_pE, res);
}

vec_ZZ_pE operator*(const mat_ZZ_pE& a, const vec_ZZ_pE& b)
{
   vec_ZZ_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_ZZ_pE, res);
}

vec_ZZ_pE operator*(const vec_ZZ_pE& a, const mat_ZZ_pE& b)
{
   vec_ZZ_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_ZZ_pE, res);
}

void inv(mat_ZZ_pE& X, const mat_ZZ_pE& A)
{
   ZZ_pE d;
   inv(d, X, A);
   if (d == 0) ArithmeticError("inv: non-invertible matrix");
}

void power(mat_ZZ_pE& X, const mat_ZZ_pE& A, const ZZ& e)
{
   if (A.NumRows() != A.NumCols()) LogicError("power: non-square matrix");

   if (e == 0) {
      ident(X, A.NumRows());
      return;
   }

   mat_ZZ_pE T1, T2;
   long i, k;

   k = NumBits(e);
   T1 = A;

   for (i = k-2; i >= 0; i--) {
      sqr(T2, T1);
      if (bit(e, i))
         mul(T1, T2, A);
      else
         T1 = T2;
   }

   if (e < 0)
      inv(X, T1);
   else
      X = T1;
}

void random(mat_ZZ_pE& x, long n, long m)
{
   x.SetDims(n, m);
   for (long i = 0; i < n; i++) random(x[i], m);
}

NTL_END_IMPL


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_VECTOR_UNIT_VECTOR_INCLUDE
#define MTL_VECTOR_UNIT_VECTOR_INCLUDE

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/vector/parameter.hpp>

namespace mtl {

namespace traits {

    /// Type of unit_vector; will be changed later to a proxy for the sake of efficiency
    template  <typename Value= double>
    struct unit_vector
    {
	typedef mtl::vector::dense_vector<Value, mtl::vector::parameters<> >  type;
    };
}

namespace vector {

    /// Return k-th unit vector of size n
    /** The result is a dense column vector. In the future this will
	be replaced by a proxy for the sake of efficiency.
	If you use unit_vector in an expression you will not encounter
	this change. If you define a variable you should use
	traits::unit_vector, e.g.:
	\code
	typename mtl::traits::unit_vector<float>::type  e_k(mtl::vector::unit_vector(k, n));
	\endcode
    **/
    template <typename Value>
    typename traits::unit_vector<Value>::type
    inline unit_vector(std::size_t k, std::size_t n)
    {
	using ::math::zero; using ::math::one;
	dense_vector<Value> v(n, zero(Value()));
	v[k]= one(Value());
	return v;
    }

    /// Unit vector of type double
    traits::unit_vector<double>::type
    inline unit_vector(std::size_t k, std::size_t n)
    {
	return unit_vector<double>(k, n);
    }

} // namespace mtl::vector

} // namespace mtl

#endif // MTL_VECTOR_UNIT_VECTOR_INCLUDE