/* ========================================================================= */
/* === umf4 ================================================================ */
/* ========================================================================= */

/* ------------------------------------------------------------------------- */
/* UMFPACK Version 4.1 (Apr. 30, 2003), Copyright (c) 2003 by Timothy A.     */
/* Davis.  All Rights Reserved.  See ../README for License.                  */
/* email: davis@cise.ufl.edu    CISE Department, Univ. of Florida.           */
/* web: http://www.cise.ufl.edu/research/sparse/umfpack                      */
/* ------------------------------------------------------------------------- */

/***********************************************************************/
/*         UMFPACK Copyright, License and Availability                 */
/***********************************************************************/
/*
 *
 * UMFPACK Version 4.1 (Apr. 30, 2003),  Copyright (c) 2003 by Timothy A.
 * Davis.  All Rights Reserved.
 *
 * UMFPACK License:
 *
 *   Your use or distribution of UMFPACK or any modified version of
 *   UMFPACK implies that you agree to this License.
 *
 *   THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
 *   EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
 *
 *   Permission is hereby granted to use or copy this program, provided
 *   that the Copyright, this License, and the Availability of the original
 *   version is retained on all copies.  User documentation of any code that
 *   uses UMFPACK or any modified version of UMFPACK code must cite the
 *   Copyright, this License, the Availability note, and "Used by permission."
 *   Permission to modify the code and to distribute modified code is granted,
 *   provided the Copyright, this License, and the Availability note are
 *   retained, and a notice that the code was modified is included.  This
 *   software was developed with support from the National Science Foundation,
 *   and is provided to you free of charge.
 *
 * Availability:
 *
 *   http://www.cise.ufl.edu/research/sparse/umfpack
 *
 */

/* Used by permission. */

/* modified by Kresimir Fresl, 2003              */

/* UMFPACK bindings
 * ublas::compressed_matrix<> or ublas::coordinate_matrix<>
 * ... to use coordinate_matrix<> define COORDINATE
 */

/* Demo program for UMFPACK v4.1.
 *
 * Reads in a triplet-form matrix and right-hand side vector.
 * Input file format:
 *   num_rows_of_A  num_cols_of_A
 *   num_nonzeros_in_A
 *   row col val
 *   row col val
 *   ...
 *   num_nonzeros_in_B
 *   idx val
 *   ...
 *
 * Then calls UMFPACK to analyze, factor, and solve the system.
 *
 * Syntax:
 *
 *	umf4		default "auto" strategy, 1-norm row scaling
 *	umf4 a		default "auto" strategy, 1-norm row scaling
 *	umf4 u		unsymmetric strategy, 1-norm row scaling
 *	umf4 s		symmetric strategy, 1-norm row scaling
 *	umf4 2		2-by-2 strategy, maxnorm row scaling
 *	umf4 A		default "auto" strategy, maxnorm row scaling
 *	umf4 U		unsymmetric strategy, maxnorm row scaling
 *	umf4 S		symmetric strategy, maxnorm row scaling
 *	umf4 T		2-by-2 strategy , maxnorm row scaling
 *      umf4 ?n         ? can be in [aus2AUST], n: no aggressive absorption
 */

#include <iostream>
#include <fstream>
#include <cstdlib>
#include <string>
#include <algorithm>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_sparse.hpp>
#include <boost/numeric/bindings/umfpack/umfpack.hpp>

using std::max;
using std::cout;
using std::cin;
using std::endl;
using std::string;
using std::ifstream;
using std::ofstream;
using std::exit;

namespace ublas = boost::numeric::ublas;
namespace umf = boost::numeric::bindings::umfpack;
namespace traits = boost::numeric::bindings::traits;

#ifndef COORDINATE
typedef ublas::compressed_matrix<double, ublas::column_major, 0,
  ublas::unbounded_array<int>, ublas::unbounded_array<double> > cm_t;
#else
typedef ublas::coordinate_matrix<double, ublas::column_major, 0,
  ublas::unbounded_array<int>, ublas::unbounded_array<double> > cm_t;
#endif
typedef ublas::vector<double> v_t;

// wait for 'y'

void wait4y() {
  char yes = 'n';
  while (1) {
    cout << "Continue (y/n) -> ";
    cin >> yes;
    if (yes == 'y') break;
    if (yes == 'n') exit (0);
  }
}

// resid: compute the relative residual, ||Ax-b||/||b||

template <typename M, typename V>
double resid (M const& m, V const& x, V const& b, V& r) {

  int const* Ap = traits::spmatrix_index1_storage (m);
  int const* Ai = traits::spmatrix_index2_storage (m);
  double const* Ax = traits::spmatrix_value_storage (m);

  double rnorm, bnorm, absr, absb;

  int n = traits::vector_size (r);

  r = prod (m, x);

#if 0
  if (transpose) {
    int i;
    for (int j = 0; j < n; j++)
      for (int p = Ap [j]; p < Ap [j+1]; p++) {
        i = Ai [p];
        r [j] += Ax [p] * x [i];
      }
  } else {
    int i;
    for (int j = 0; j < n; j++)
      for (int p = Ap [j]; p < Ap [j+1]; p++) {
        i = Ai [p];
        r [i] += Ax [p] * x [j];
      }
  }
#endif

  for (int i = 0; i < n; i++)
    r [i] -= b [i];

  rnorm = 0.;
  bnorm = 0.;
  for (int i = 0; i < n; i++) {
    if ((boost::math::isnan) (r [i])){
      rnorm = r [i];
      break;
    }
    absr = fabs (r [i]);
    rnorm = max (rnorm, absr);
  }
  for (int i = 0; i < n; i++) {
    if ((boost::math::isnan) (b [i])){
      bnorm = b [i];
      break;
    }
    absb = fabs (b [i]);
    bnorm = max (bnorm, absb);
  }
  if (bnorm == 0)
    bnorm = 1;
  return (rnorm / bnorm);
}

////////////////////////////////////////////////////////////////
// main program

int main (int argc, char **argv) {

  cout << "\n===========================================================\n"
       << "=== UMFPACK v4.1 ==========================================\n"
       << "===========================================================\n"
       << endl;

  // set controls

  umf::control_type<> Control;
  Control [UMFPACK_PRL] = 3;
  Control [UMFPACK_BLOCK_SIZE] = 32;

  if (argc > 1) {
    char *s = argv [1];
    // get the strategy
    if (s [0] == 'u')
      Control [UMFPACK_STRATEGY] = UMFPACK_STRATEGY_UNSYMMETRIC;
    else if (s [0] == 'a')
      Control [UMFPACK_STRATEGY] = UMFPACK_STRATEGY_AUTO;
    else if (s [0] == 's')
      Control [UMFPACK_STRATEGY] = UMFPACK_STRATEGY_SYMMETRIC;
    else if (s [0] == '2')
      Control [UMFPACK_STRATEGY] = UMFPACK_STRATEGY_2BY2;
    else if (s [0] == 'U') {
      Control [UMFPACK_STRATEGY] = UMFPACK_STRATEGY_UNSYMMETRIC;
      Control [UMFPACK_SCALE] = UMFPACK_SCALE_MAX;
    }
    else if (s [0] == 'A') {
      Control [UMFPACK_STRATEGY] = UMFPACK_STRATEGY_AUTO;
      Control [UMFPACK_SCALE] = UMFPACK_SCALE_MAX;
    }
    else if (s [0] == 'S') {
      Control [UMFPACK_STRATEGY] = UMFPACK_STRATEGY_SYMMETRIC;
      Control [UMFPACK_SCALE] = UMFPACK_SCALE_MAX;
    }
    else if (s [0] == 'T') {
      Control [UMFPACK_STRATEGY] = UMFPACK_STRATEGY_2BY2;
      Control [UMFPACK_SCALE] = UMFPACK_SCALE_MAX;
    }
    else
      printf ("unrecognized strategy: %s\n", argv [1]);

    if (s [1] == 'n')
      // no aggressive absorption
      Control [UMFPACK_AGGRESSIVE] = 0;
  }
  bool call_wait4y = true;
  string ifs_name;
  if (argc > 2) {
    call_wait4y = false;
    ifs_name = argv [2];
  }

  umf::report_control (Control);

  // open the matrix file
  if (ifs_name.empty()) {
    cout << "input file -> ";
    cin >> ifs_name;
  }
  ifstream f (ifs_name.c_str());
  if (!f) {
    cout << "unable to open file" << endl;
    exit (1);
  }

  // initialize matrix A
  cout << "reading A" << endl;
  int nrow, ncol, nz;
  f >> nrow >> ncol >> nz;
  cm_t A (nrow, ncol, nz);
  int r, c;
  for (int i = 0; i < nz; ++i) {
    f >> r >> c;
    double val;
    f >> val;
    A (r, c) = val;
  }
  Control[UMFPACK_PRL] = (nz > 20) ? 4 : 5;
  cout << "matrix A: ";
  umf::report_matrix (A, Control);

  if (call_wait4y) wait4y();

  // symbolic factorization
  umf::info_type<> Info;
  umf::symbolic_type<> Symbolic;

  int status = umf::symbolic (A, Symbolic, Control, Info);

  cout << "symbolic status:" << endl;
  umf::report_status (Control, status);
  if (status != UMFPACK_OK) {
    umf::report_info (Control, Info);
    cout << "umf::symbolic failed" << endl;
    exit (1);
  }
  umf::report_symbolic (Symbolic, Control);

  if (call_wait4y) wait4y();

  // numeric factorization
  umf::numeric_type<> Numeric;

  status = umf::numeric (A, Symbolic, Numeric, Control, Info);

  cout << "numeric status:" << endl;
  umf::report_status (Control, status);
  if (status < UMFPACK_OK) {
    umf::report_info (Control, Info);
    cout << "umf::numeric failed" << endl;
    exit (1);
  }
  umf::report_numeric (Numeric, Control);

  if (call_wait4y) wait4y();

  // solve Ax=b
  if (nrow == ncol && status == UMFPACK_OK) {

    // create right-hand side vector B
    v_t B (ncol), X (ncol), R (ncol);
    cout << "reading B" << endl;
    int m2, ii;
    f >> m2;
    for (int i = 0; i < m2; ++i) {
      f >> ii;
      f >> B[ii];
    }
    cout << "vector B: ";
    umf::report_vector (B, Control);

    status = umf::solve (A, X, B, Numeric, Control, Info);

    cout << "solve status:" << endl;
    umf::report_status (Control, status);
    umf::report_info (Control, Info);
    if (status < UMFPACK_OK) {
      cout << "umf::solve failed" << endl;
      exit (1);
    }
    cout << "solution vector X: ";
    umf::report_vector (X, Control);

    cout << "relative maxnorm of residual, ||Ax-b||/||b||: "
	 << resid (A, X, B, R) << endl;

  } else {

    cout << "system not solved" << endl;
    umf::report_info (Control, Info);

  }

  cout << endl;

}


// Copyright 2021 Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef STATE_ESTIMATION__NOISE_MODEL__UNIFORM_NOISE_HPP_
#define STATE_ESTIMATION__NOISE_MODEL__UNIFORM_NOISE_HPP_

#include <state_estimation/noise_model/noise_interface.hpp>
#include <state_estimation/visibility_control.hpp>

#include <Eigen/Core>

#include <algorithm>
#include <array>
#include <chrono>
#include <type_traits>
#include <vector>

namespace autoware
{
namespace common
{
namespace state_estimation
{

///
/// @brief      This class contains the logic for a uniform noise model.
///
/// @tparam     StateT  State vector type for which this noise model is created.
///
template<typename StateT>
class STATE_ESTIMATION_PUBLIC UniformNoise : public NoiseInterface<UniformNoise<StateT>>
{
  using Scalar = typename StateT::Scalar;
  using Matrix = typename StateT::Matrix;
  template<typename OtherScalarT>
  using VarianceVector = Eigen::Matrix<OtherScalarT, StateT::size(), 1>;

public:
  /// A convenience typedef for the state for which this noise model is to be used.
  using State = StateT;

  ///
  /// @brief      Create the noise model from an existing covariance matrix.
  ///
  /// @param[in]  covariance  The covariance
  ///
  explicit UniformNoise(const Matrix & covariance) noexcept
  : m_covariance{covariance} {}

  ///
  /// @brief      Create the noise model from an array
  ///
  /// @param[in]  variances     The values for variances, i.e., values for σ (not squared)
  ///
  /// @tparam     OtherScalarT  Some scalar type.
  ///
  template<typename OtherScalarT>
  explicit UniformNoise(
    const std::array<OtherScalarT, State::size()> & variances) noexcept
  : m_covariance{
      Eigen::Map<const VarianceVector<OtherScalarT>>(variances.data())
      .template cast<Scalar>().array().square().matrix().asDiagonal()} {}

  ///
  /// @brief      Create the noise model from a vector
  ///
  /// @param[in]  variances     The values for variances, i.e., values for σ (not squared)
  ///
  /// @throws     std::runtime_error  if the vector is of a wrong size.
  ///
  /// @tparam     OtherScalarT  Some scalar type.
  ///
  template<typename OtherScalarT>
  explicit UniformNoise(const std::vector<OtherScalarT> & variances)
  {
    if (variances.size() != static_cast<std::size_t>(State::size())) {
      throw std::runtime_error(
              "There must be " + std::to_string(State::size()) +
              " variances for initializing the uniform noise model, but " +
              std::to_string(variances.size()) + " provided");
    }
    m_covariance =
      Eigen::Map<const VarianceVector<OtherScalarT>>(variances.data())
      .template cast<Scalar>().array().square().matrix().asDiagonal();
  }

  ///
  /// @brief      Create noise model from variances directly.
  ///
  /// @param[in]  variance    The first variance, i.e., value for σ (not squared)
  /// @param[in]  variances   The variances for all the other variables
  ///
  /// @tparam     VarianceTs  Types for other variances.
  ///
  template<typename ... VarianceTs>
  explicit UniformNoise(const Scalar variance, const VarianceTs ... variances)
  : UniformNoise{std::array<Scalar, State::size()> {variance, variances ...}}
  {
    static_assert(
      sizeof...(VarianceTs) + 1 == State::size(),
      "Wrong number of variances passed into the UniformNoise constructor");
  }

protected:
  /// Allow the interface to access the protected and private members to visually encapsulate the
  /// implementation.
  friend NoiseInterface<UniformNoise<StateT>>;

  ///
  /// @brief      A CRTP-called covariance getter.
  ///
  /// @return     A covariance of the noise process over a given time span.
  ///
  Matrix crtp_covariance(const std::chrono::nanoseconds & dt) const noexcept
  {
    return m_covariance * std::chrono::duration_cast<std::chrono::duration<Scalar>>(dt).count();
  }

private:
  /// Store the covariance matrix internally.
  Matrix m_covariance{};
};

}  // namespace state_estimation
}  // namespace common
}  // namespace autoware

#endif  // STATE_ESTIMATION__NOISE_MODEL__UNIFORM_NOISE_HPP_


#include <vector>

#include <boost/shared_ptr.hpp>
#include <gflags/gflags.h>
#include <glog/logging.h>

#include <cmath>
#include <fstream>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/ultinous/projective_matrix_layer.hpp"
#include "caffe/util/math_functions.hpp"

namespace caffe
{
namespace ultinous
{

template<typename Dtype>
void ProjectiveMatrixLayer<Dtype>::LayerSetUp(const vector<Blob<Dtype> *> &bottom,
                                              const vector<Blob<Dtype> *> &top)
{

  CHECK(bottom[0]->channels() == 9);

  m_base_scale = this->layer_param_.proj_matrix_param().base_scale();
  m_base_f = this->layer_param_.proj_matrix_param().base_f();
  m_base_tz = this->layer_param_.proj_matrix_param().base_tz();

  m_min_scale = this->layer_param_.proj_matrix_param().min_scale();
  m_max_scale = this->layer_param_.proj_matrix_param().max_scale();
  m_min_f = this->layer_param_.proj_matrix_param().min_f();
  m_max_f = this->layer_param_.proj_matrix_param().max_f();
  m_min_alpha = this->layer_param_.proj_matrix_param().min_alpha();
  m_max_alpha = this->layer_param_.proj_matrix_param().max_alpha();
  m_min_beta = this->layer_param_.proj_matrix_param().min_beta();
  m_max_beta = this->layer_param_.proj_matrix_param().max_beta();
  m_min_gamma = this->layer_param_.proj_matrix_param().min_gamma();
  m_max_gamma = this->layer_param_.proj_matrix_param().max_gamma();

  m_min_tx = this->layer_param_.proj_matrix_param().min_tx();
  m_max_tx = this->layer_param_.proj_matrix_param().max_tx();
  m_min_ty = this->layer_param_.proj_matrix_param().min_ty();
  m_max_ty = this->layer_param_.proj_matrix_param().max_ty();
  m_min_tz = this->layer_param_.proj_matrix_param().min_tz();
  m_max_tz = this->layer_param_.proj_matrix_param().max_tz();

  m_bias_scale = this->layer_param_.proj_matrix_param().bias_scale();
  m_bias_U = this->layer_param_.proj_matrix_param().bias_u();

  m_max_diff = this->layer_param_.proj_matrix_param().max_diff();
  m_boundary_violation_step = 0.1;
  m_iter = 0;

  if (this->blobs_.size() > 0)
  {
    LOG(INFO) << "Skipping parameter initialization";
  }
  else
  {
    this->blobs_.resize(1);
    vector<int> sz;
    sz.push_back(bottom[0]->channels());

    this->blobs_[0].reset(new Blob<Dtype>(sz));
    caffe_set(this->blobs_[0]->count(), Dtype(0),
              this->blobs_[0]->mutable_cpu_data());
  }

  std::vector<int> top_shape(2);
  top_shape[0] = bottom[0]->num();
  top_shape[1] = 9;
  top[0]->Reshape(top_shape);
}

template<typename Dtype>
void ProjectiveMatrixLayer<Dtype>::Reshape(const vector<Blob<Dtype> *> &bottom,
                                           const vector<Blob<Dtype> *> &top)
{
  std::vector<int> top_shape(2);
  top_shape[0] = bottom[0]->num();
  top_shape[1] = 9;
  top[0]->Reshape(top_shape);
}

template<typename Dtype>
void ProjectiveMatrixLayer<Dtype>::Forward_cpu(const vector<Blob<Dtype> *> &bottom,
                                               const vector<Blob<Dtype> *> &top)
{
  ++m_iter;
  if (this->phase_ == TRAIN && (m_iter % 100) == 0)
  {
    Dtype const *bias = this->blobs_[0]->cpu_data();
    std::cout << "Bias:";
    for (int c = 0; c < 9; ++c)
      std::cout << " " << bias[c];
    std::cout << std::endl;
  }

/**
  Original matrix:
  H = [ cos(gamma)*cos(beta)*S*f  (-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha))*S*f  U*f;
      sin(gamma)*cos(beta)*S*f  (cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha))*S*f   V*f;
    -sin(beta)*S             cos(beta)*sin(alpha)*S                                     W+f];
*/

/** Non-uniform scaling:
  Original matrix:
  H = [ cos(gamma)*cos(beta)*Sx*f  (-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha))*Sx*f  U*f*Sx;
      sin(gamma)*cos(beta)*Sy*f  (cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha))*Sy*f   V*f*Sy;
    -sin(beta)             cos(beta)*sin(alpha)                                     W+f];
*/

  for (int n = 0; n < bottom[0]->num(); ++n)
  {
    Dtype const *bottom_data = bottom[0]->cpu_data() + 9 * n;

    Dtype Sx = m_base_scale + bottom_data[0];
    Dtype Sy = m_base_scale + bottom_data[1];
    Dtype f = m_base_f + bottom_data[2];
    Dtype alpha = bottom_data[3];
    Dtype beta = bottom_data[4];
    Dtype gamma = bottom_data[5];
    Dtype U = bottom_data[6];
    Dtype V = bottom_data[7];
    Dtype W = m_base_tz + bottom_data[8];

    Dtype const *bias = this->blobs_[0]->cpu_data();
    if (m_bias_scale)
    {
      Sx += bias[0];
      Sy += bias[1];
    }
    if (m_bias_U)
    {
      U += bias[6];
    }

    Dtype *top_data = top[0]->mutable_cpu_data() + 9 * n;
    top_data[0] = cos(gamma)*cos(beta)*Sx*f;
    top_data[1] = (-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha))*Sx*f;
    top_data[2] = U*f*Sx;

    top_data[3] = sin(gamma)*cos(beta)*Sy*f;
    top_data[4] = (cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha))*Sy*f;
    top_data[5] = V*f*Sy;

    top_data[6] = -sin(beta);
    top_data[7] = cos(beta)*sin(alpha);
    top_data[8] = W+f;
  }

}

template<typename Dtype>
void ProjectiveMatrixLayer<Dtype>::Backward_cpu(const vector<Blob<Dtype> *> &top,
                                                const vector<bool> &propagate_down,
                                                const vector<Blob<Dtype> *> &bottom)
{

  if (!propagate_down[0]) return;

  for (int n = 0; n < bottom[0]->num(); ++n)
  {
    Dtype const *bottom_data = bottom[0]->cpu_data() + 9 * n;

    Dtype Sx = m_base_scale + bottom_data[0];
    Dtype Sy = m_base_scale + bottom_data[1];
    Dtype f = m_base_f + bottom_data[2];
    Dtype alpha = bottom_data[3];
    Dtype beta = bottom_data[4];
    Dtype gamma = bottom_data[5];
    Dtype U = bottom_data[6];
    Dtype V = bottom_data[7];
    Dtype W = m_base_tz + bottom_data[8];

    Dtype const *bias = this->blobs_[0]->cpu_data();
    if (m_bias_scale)
    {
      Sx += bias[0];
      Sy += bias[1];
    }
    if (m_bias_U)
    {
      U += bias[6];
    }

    Dtype *bottom_diff = bottom[0]->mutable_cpu_diff() + 9 * n;
    Dtype const *top_diff = top[0]->cpu_diff() + 9 * n;

    if (Sx < m_min_scale)
      bottom_diff[0] = (Sx - m_min_scale) * m_boundary_violation_step;
    else if (Sx > m_max_scale)
      bottom_diff[0] = (Sx - m_max_scale) * m_boundary_violation_step;
    else
      bottom_diff[0] = top_diff[0] * (cos(gamma)*cos(beta)*f)
                       +top_diff[1] * ( (-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha))*f)
                      + top_diff[2] * U*f;

    if (Sy < m_min_scale)
      bottom_diff[1] = (Sy - m_min_scale) * m_boundary_violation_step;
    else if (Sy > m_max_scale)
      bottom_diff[1] = (Sy - m_max_scale) * m_boundary_violation_step;
    else
      bottom_diff[1] = top_diff[3] * ( sin(gamma)*cos(beta)*f )
                       +top_diff[4] * (  (cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha))*f )
                      + top_diff[5] * V*f;

    if (f < m_min_f)
      bottom_diff[2] = (f - m_min_f) * m_boundary_violation_step;
    else if (f > m_max_f)
      bottom_diff[2] = (f - m_max_f) * m_boundary_violation_step;
    else
      bottom_diff[2] =  top_diff[0] * (cos(gamma)*cos(beta)*Sx)
                        +top_diff[1] * ( (-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha))*Sx)
                         +top_diff[2] * ( U * Sx )
                        +top_diff[3] * ( sin(gamma)*cos(beta)*Sy )
                        +top_diff[4] * (  (cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha))*Sy )
                        +top_diff[5] * ( V * Sy)
                        +top_diff[8] * ( 1 );

    if (alpha < m_min_alpha)
      bottom_diff[3] = (alpha - m_min_alpha) * m_boundary_violation_step;
    else if (alpha > m_max_alpha)
      bottom_diff[3] = (alpha - m_max_alpha) * m_boundary_violation_step;
    else
      bottom_diff[3] = top_diff[1] * ( (-sin(gamma)*-sin(alpha)+cos(gamma)*sin(beta)*cos(alpha))*Sx*f )
                      + top_diff[4] * ((cos(gamma)*-sin(alpha)+sin(gamma)*sin(beta)*cos(alpha))*Sy*f)
                      + top_diff[7] * (cos(beta)*cos(alpha));

    if (beta < m_min_beta)
      bottom_diff[4] = (beta - m_min_beta) * m_boundary_violation_step;
    else if (beta > m_max_beta)
      bottom_diff[4] = (beta - m_max_beta) * m_boundary_violation_step;
    else
      bottom_diff[4] = top_diff[0] * (cos(gamma)*-sin(beta)*Sx*f)
                  + top_diff[1] * ( (-sin(gamma)*cos(alpha)+cos(gamma)*cos(beta)*sin(alpha))*Sx*f)
              + top_diff[3] * (sin(gamma)*-sin(beta)*Sy*f)
            + top_diff[4] * ( (cos(gamma)*cos(alpha)+sin(gamma)*cos(beta)*sin(alpha))*Sy*f)
            + top_diff[6] * (-cos(beta))
            + top_diff[7] * (-sin(beta)*sin(alpha));

    if (gamma < m_min_gamma)
      bottom_diff[5] = (gamma - m_min_gamma) * m_boundary_violation_step;
    else if (gamma > m_max_gamma)
      bottom_diff[5] = (gamma - m_max_gamma) * m_boundary_violation_step;
    else
      bottom_diff[5] = top_diff[0] * (-sin(gamma)*cos(beta)*Sx*f)
              + top_diff[1] * ((-cos(gamma)*cos(alpha)+-sin(gamma)*sin(beta)*sin(alpha))*Sx*f)
                + top_diff[3] * (cos(gamma)*cos(beta)*Sy*f)
                  + top_diff[4] * ((-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha))*Sy*f);

    if (U < m_min_tx)
      bottom_diff[6] = (U - m_min_tx) * m_boundary_violation_step;
    else if (U > m_max_tx)
      bottom_diff[6] = (U - m_max_tx) * m_boundary_violation_step;
    else
      bottom_diff[6] = top_diff[2]*f*Sx;

    if (V < m_min_ty)
      bottom_diff[7] = (V - m_min_ty) * m_boundary_violation_step;
    else if (V > m_max_ty)
      bottom_diff[7] = (V - m_max_ty) * m_boundary_violation_step;
    else
      bottom_diff[7] = top_diff[5]*f*Sy;

    if (W < m_min_tz)
      bottom_diff[8] = (W - m_min_tz) * m_boundary_violation_step;
    else if (W > m_max_tz)
      bottom_diff[8] = (W - m_max_tz) * m_boundary_violation_step;
    else
      bottom_diff[8] = top_diff[8] * (1);

    if (m_max_diff != 0)
      for (int i = 0; i < 9; i++)
        bottom_diff[i] = std::max(-m_max_diff, std::min(m_max_diff, bottom_diff[i]));

    // Bias - only on specific params
    Dtype *bias_diff = this->blobs_[0]->mutable_cpu_diff();
    if (m_bias_scale)
    {
      bias_diff[0] += bottom_diff[0]; // scaleX
      bias_diff[1] += bottom_diff[1]; // scaleY
    }
    if (m_bias_U)
    {
      bias_diff[6] += bottom_diff[6]; // // U
    }

  }
}

#ifdef CPU_ONLY
//STUB_GPU(ProjectiveMatrixLayer);
#endif

INSTANTIATE_CLASS(ProjectiveMatrixLayer);

REGISTER_LAYER_CLASS(ProjectiveMatrix);

}  // namespace ultinous
}  // namespace caffe


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//! OPENOR_INTERFACE_FILE(openOR_core)
//****************************************************************************
//!
//! @file
//! @ingroup openOR_core

#ifndef openOR_core_Math_ublasvector_hpp
#define openOR_core_Math_ublasvector_hpp

#if defined(_MSC_VER)
#  pragma warning(disable:4172)
#endif

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#if defined(_MSC_VER)
#  pragma warning(default:4172)
#endif

#include <openOR/Math/traits.hpp>
#include <openOR/Math/vectorconcept.hpp>
#include <openOR/Math/vector_types.hpp>

namespace openOR {
   namespace Math {

      //----------------------------------------------------------------------------
      // Adapting the corresponding traits
      //----------------------------------------------------------------------------

      template<typename VectorType>
      struct UBLASVectorAccess {
         typedef typename VectorType::reference AccessType;
         typedef typename VectorType::const_reference ConstAccessType;

         template <int I, typename PP> static
            typename if_const<PP, ConstAccessType, AccessType>::
         type get(PP& p) {
            assert(I < p.size());
            return p(I);
         }
      };

      template<typename Value, std::size_t d>
      struct VectorTraits<boost::numeric::ublas::bounded_vector<Value, d> > {
         typedef boost::numeric::ublas::bounded_vector<Value, d> VectorType;
         typedef boost::numeric::ublas::bounded_vector<typename ScalarTraits<Value>::RealType, d>  RealVectorType;
         typedef Value ValueType;
         typedef boost::numeric::ublas::bounded_vector<Value, 3> Vector3Type;
         typedef boost::numeric::ublas::bounded_vector<Value, 2> Vector2Type;
         typedef boost::mpl::int_<d> Dimension;
         typedef UBLASVectorAccess<VectorType> Access;
         typedef boost::mpl::true_ IsVector;

         static VectorType ZEROS;
      };

      template<typename Value, std::size_t d>
      typename VectorTraits< boost::numeric::ublas::bounded_vector<Value, d> >::VectorType
      VectorTraits< boost::numeric::ublas::bounded_vector<Value, d> >::ZEROS = boost::numeric::ublas::zero_vector<Value>(d);

      template<typename E>
      struct UBLASVectorExpressionTraits {
         typedef E VectorType;
         typedef typename E::value_type ValueType;
         typedef boost::numeric::ublas::bounded_vector<typename E::value_type, 3> Vector3Type;
         typedef boost::numeric::ublas::bounded_vector<typename E::value_type, 2> Vector2Type;
         typedef boost::mpl::int_<0> Dimension;
         typedef UBLASVectorAccess<E> Access;
         typedef boost::mpl::true_ IsVector;
      };

      template<class E>
      struct VectorTraits<boost::numeric::ublas::vector_expression<E> > : VectorTraits<E> {};

      template<class C>
      struct VectorTraits<boost::numeric::ublas::vector_container<C> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector_container<C> > {};

      template<class T, class A>
      struct VectorTraits<boost::numeric::ublas::vector<T, A> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector<T, A> > {};

      template<class E>
      struct VectorTraits<boost::numeric::ublas::vector_reference<E> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector_reference<E> > {};

      template<typename E, typename F>
      struct VectorTraits<boost::numeric::ublas::vector_unary<E, F> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector_unary<E, F> > {};

      template<typename E1, typename E2, typename F>
      struct VectorTraits<boost::numeric::ublas::vector_binary<E1, E2, F> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector_binary<E1, E2, F> > {};

      template<typename E1, typename E2, typename F>
      struct VectorTraits<boost::numeric::ublas::vector_binary_scalar1<E1, E2, F> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector_binary_scalar1<E1, E2, F> > {};

      template<typename E1, typename E2, typename F>
      struct VectorTraits<boost::numeric::ublas::vector_binary_scalar2<E1, E2, F> > : UBLASVectorExpressionTraits<boost::numeric::ublas::vector_binary_scalar2<E1, E2, F> > {};

      //----------------------------------------------------------------------------
      //! @brief
      //! @ingroup openOR_core
      //----------------------------------------------------------------------------
      template <class E>
         inline typename boost::numeric::ublas::vector_scalar_unary_traits<E, boost::numeric::ublas::vector_norm_2<E> >::
      result_type norm(const boost::numeric::ublas::vector_expression<E>& e) {
         return boost::numeric::ublas::norm_2(e);
      }

      //----------------------------------------------------------------------------
      //! @brief
      //! @ingroup openOR_core
      //----------------------------------------------------------------------------
      template <class E1, class E2>
         inline typename boost::numeric::ublas::vector_scalar_binary_traits < E1, E2,
                  boost::numeric::ublas::vector_inner_prod < E1, E2,
                  typename boost::numeric::ublas::promote_traits < typename E1::value_type,
                  typename E2::value_type >::promote_type > >::
      result_type dot(  const boost::numeric::ublas::vector_expression<E1> &e1,
                        const boost::numeric::ublas::vector_expression<E2> &e2)
      {
         return boost::numeric::ublas::inner_prod(e1, e2);
      }

   }
}

//namespace boost {
//   namespace numeric {
//      namespace ublas {
//
//         template<class E1, class E2>
//         BOOST_UBLAS_INLINE
//            typename vector_scalar_binary_traits < E1, E2, vector_inner_prod < E1, E2,
//                        typename promote_traits < typename E1::value_type,
//                        typename E2::value_type >::promote_type > >::
//         result_type operator% ( const vector_expression<E1> &e1,
//                                 const vector_expression<E2> &e2)
//         {
//            typedef typename vector_scalar_binary_traits < E1, E2, vector_inner_prod < E1, E2,
//                                 typename promote_traits < typename E1::value_type,
//                                 typename E2::value_type >::promote_type > >::expression_type expression_type;
//            return expression_type(e1(), e2());
//         }
//
//      }
//   }
//}

#endif


//
// Created by Mamba on 2020/2/18.
//
// #define R_BUILD
// #define TEST

#ifdef R_BUILD
#include <Rcpp.h>
#include <RcppEigen.h>
using namespace Rcpp;
// [[Rcpp::depends(RcppEigen)]]
#else

#include <Eigen/Eigen>
#include "List.h"

#endif

#include <iostream>
#include "Data.h"
#include "Algorithm.h"
#include "Metric.h"
#include "abess.h"

using namespace Eigen;
using namespace std;

// int sign(double a)
// {
//   if (a > 0)
//   {
//     return 1;
//   }
//   else if (a < 0)
//   {
//     return -1;
//   }
//   else
//   {
//     return 0;
//   }
// }

// double det(double a[], double b[])
// {
//   return a[0] * b[1] - a[1] * b[0];
// }

// calculate the intersection of two lines
// if parallal, need_flag = false.
// void line_intersection(double line1[2][2], double line2[2][2], double intersection[], bool &need_flag)
// {
//   double xdiff[2], ydiff[2], d[2];
//   double div;

//   xdiff[0] = line1[0][0] - line1[1][0];
//   xdiff[1] = line2[0][0] - line2[1][0];
//   ydiff[0] = line1[0][1] - line1[1][1];
//   ydiff[1] = line2[0][1] - line2[1][1];

//   div = det(xdiff, ydiff);
//   if (div == 0)
//   {
//     need_flag = false;
//     return;
//   }
//   else
//   {
//     d[0] = det(line1[0], line1[1]);
//     d[1] = det(line2[0], line2[1]);

//     intersection[0] = det(d, xdiff) / div;
//     intersection[1] = det(d, ydiff) / div;
//     need_flag = true;
//     return;
//   }
// }

// // boundary: s=smin, s=max, lambda=lambda_min, lambda_max
// // line: crosses p and is parallal to u
// // calculate the intersections between boundary and line
// void cal_intersections(double p[], double u[], int s_min, int s_max, double lambda_min, double lambda_max, double a[], double b[])
// {
//   double line0[2][2], line_set[4][2][2], intersections[4][2];
//   bool need_flag[4];
//   int i, j;

//   line0[0][0] = double(p[0]);
//   line0[0][1] = double(p[1]);
//   line0[1][0] = double(p[0] + u[0]);
//   line0[1][1] = double(p[1] + u[1]);

//   line_set[0][0][0] = double(s_min);
//   line_set[0][0][1] = double(lambda_min);
//   line_set[0][1][0] = double(s_min);
//   line_set[0][1][1] = double(lambda_max);

//   line_set[1][0][0] = double(s_max);
//   line_set[1][0][1] = double(lambda_min);
//   line_set[1][1][0] = double(s_max);
//   line_set[1][1][1] = double(lambda_max);

//   line_set[2][0][0] = double(s_min);
//   line_set[2][0][1] = double(lambda_min);
//   line_set[2][1][0] = double(s_max);
//   line_set[2][1][1] = double(lambda_min);

//   line_set[3][0][0] = double(s_min);
//   line_set[3][0][1] = double(lambda_max);
//   line_set[3][1][0] = double(s_max);
//   line_set[3][1][1] = double(lambda_max);

//   for (i = 0; i < 4; i++)
//   {
//     line_intersection(line0, line_set[i], intersections[i], need_flag[i]);
//   }

//   // delete intersections beyond boundary
//   for (i = 0; i < 4; i++)
//   {
//     if (need_flag[i])
//     {
//       if ((intersections[i][0] < s_min - 0.0001) | (intersections[i][0] > s_max + 0.0001) | (intersections[i][1] < lambda_min - 0.001) | (intersections[i][1] > lambda_max + 0.001))
//       {
//         need_flag[i] = false;
//       }
//     }
//   }

//   // delecte repetitive intersections
//   for (i = 0; i < 4; i++)
//   {
//     if (need_flag[i])
//     {
//       for (j = i + 1; j < 4; j++)
//       {
//         if (need_flag[j])
//         {
//           if (abs(intersections[i][0] - intersections[j][0]) < 0.0001 && abs(intersections[i][1] - intersections[j][1]) < 0.0001)
//           {
//             need_flag[j] = false;
//           }
//         }
//       }
//     }
//   }

//   j = 0;
//   for (i = 0; i < 4; i++)
//   {
//     if (need_flag[i])
//     {
//       if (j == 2)
//       {
//         j += 1;
//       }
//       if (j == 1)
//       {
//         b[0] = intersections[i][0];
//         b[1] = intersections[i][1];
//         j += 1;
//       }
//       if (j == 0)
//       {
//         a[0] = intersections[i][0];
//         a[1] = intersections[i][1];
//         j += 1;
//       }
//     }
//   }

//   if (j != 2)
//   {
// #ifdef R_BUILD
//     Rcpp::Rcout << "---------------------------" << endl;
//     Rcpp::Rcout << "j: " << j << endl;
//     Rcpp::Rcout << "inetrsection numbers wrong" << j << endl;
//     Rcpp::Rcout << "p" << p[0] << "," << p[1] << endl;
//     Rcpp::Rcout << "u" << u[0] << "," << u[1] << endl;
//     Rcpp::Rcout << "s_min" << s_min << endl;
//     Rcpp::Rcout << "s_max" << s_max << endl;
//     Rcpp::Rcout << "lambda_min" << lambda_min << endl;
//     Rcpp::Rcout << "lambda_max" << lambda_max << endl;
//     Rcpp::Rcout << "intersections[0]" << intersections[0][0] << "," << intersections[0][1] << endl;
//     Rcpp::Rcout << "intersections[1]" << intersections[1][0] << "," << intersections[1][1] << endl;
//     Rcpp::Rcout << "intersections[2]" << intersections[2][0] << "," << intersections[2][1] << endl;
//     Rcpp::Rcout << "intersections[3]" << intersections[3][0] << "," << intersections[3][1] << endl;
//     Rcpp::Rcout << "need_flag[0]" << need_flag[0] << endl;
//     Rcpp::Rcout << "need_flag[1]" << need_flag[1] << endl;
//     Rcpp::Rcout << "need_flag[2]" << need_flag[2] << endl;
//     Rcpp::Rcout << "need_flag[3]" << need_flag[3] << endl;
// #else
//     cout << "---------------------------" << endl;
//     cout << "j: " << j << endl;
//     cout << "inetrsection numbers wrong" << j << endl;
//     cout << "p" << p[0] << "," << p[1] << endl;
//     cout << "u" << u[0] << "," << u[1] << endl;
//     cout << "s_min" << s_min << endl;
//     cout << "s_max" << s_max << endl;
//     cout << "lambda_min" << lambda_min << endl;
//     cout << "lambda_max" << lambda_max << endl;
//     cout << "intersections[0]" << intersections[0][0] << "," << intersections[0][1] << endl;
//     cout << "intersections[1]" << intersections[1][0] << "," << intersections[1][1] << endl;
//     cout << "intersections[2]" << intersections[2][0] << "," << intersections[2][1] << endl;
//     cout << "intersections[3]" << intersections[3][0] << "," << intersections[3][1] << endl;
//     cout << "need_flag[0]" << need_flag[0] << endl;
//     cout << "need_flag[1]" << need_flag[1] << endl;
//     cout << "need_flag[2]" << need_flag[2] << endl;
//     cout << "need_flag[3]" << need_flag[3] << endl;
// #endif
//   }

//   return;
// }

// void golden_section_search(Data &data, Algorithm *algorithm, Metric *metric, double p[], double u[], int s_min, int s_max, double log_lambda_min, double log_lambda_max, double best_arg[],
//                            Eigen::VectorXd &beta1, double &coef01, double &train_loss1, double &ic1, Eigen::MatrixXd &ic_sequence)
// {
//     int n = data.get_n();
//     Eigen::VectorXi full_mask(n);
//     for (int i = 0; i < n; i++)
//     {
//         full_mask(i) = int(i);
//     }
//     vector<Eigen::MatrixXd> full_group_XTX = group_XTX(data.x, data.g_index, data.g_size, data.n, data.p, data.g_num, algorithm->model_type);

//     Eigen::VectorXd beta_init = Eigen::VectorXd::Zero(data.get_p());
//     Eigen::VectorXd beta_temp1 = Eigen::VectorXd::Zero(data.get_p());
//     Eigen::VectorXd beta_temp2 = Eigen::VectorXd::Zero(data.get_p());
//     double train_loss_temp1, train_loss_temp2;
//     double coef0_temp1, coef0_temp2;
//     double coef0_init = 0.0;
//     int ic_row, ic_col;
//     double d_lambda = (log_lambda_max - log_lambda_min) / 99;

//     double invphi, invphi2, closs, dloss;
//     double a[2], b[2], c[2], d[2], h[2];

//     // stop condiction
//     double s_tol = 2;
//     double log_lambda_tol = (log_lambda_max - log_lambda_min) / 200;

//     invphi = (pow(5, 0.5) - 1.0) / 2.0;
//     invphi2 = (3.0 - pow(5, 0.5)) / 2.0;
//     cal_intersections(p, u, s_min, s_max, log_lambda_min, log_lambda_max, a, b);

//     h[0] = b[0] - a[0];
//     h[1] = b[1] - a[1];

//     c[0] = a[0] + invphi2 * h[0];
//     c[1] = a[1] + invphi2 * h[1];
//     d[0] = a[0] + invphi * h[0];
//     d[1] = a[1] + invphi * h[1];

//     if (h[0] > 0.0001)
//     {
//         c[0] = int(c[0]);
//         d[0] = ceil(d[0]);
//     }
//     else if (h[0] < -0.0001)
//     {
//         c[0] = ceil(c[0]);
//         d[0] = int(d[0]);
//     }
//     else
//     {
//         c[0] = round(c[0]);
//         d[0] = round(d[0]);
//     }

//     algorithm->update_train_mask(full_mask);
//     algorithm->update_sparsity_level(int(c[0]));
//     algorithm->update_lambda_level(exp(c[1]));
//     algorithm->update_beta_init(beta_init);
//     algorithm->update_coef0_init(coef0_init);
//     algorithm->update_group_XTX(full_group_XTX);
//     algorithm->fit();
//     if (algorithm->warm_start)
//     {
//         beta_init = algorithm->get_beta();
//         coef0_init = algorithm->get_coef0();
//     }
//     closs = metric->ic(algorithm, data);
//     coef0_temp1 = algorithm->get_coef0();
//     beta_temp1 = algorithm->get_beta();
//     train_loss_temp1 = metric->train_loss(algorithm, data);

//     ic_row = int(c[0]);
//     ic_col = floor((c[1] - log_lambda_min) / d_lambda);
//     if (std::isnan(ic_sequence(ic_row, ic_col)))
//     {
//         ic_sequence(ic_row, ic_col) = closs;
//     }
//     else
//     {
//         ic_sequence(ic_row, ic_col) = (ic_sequence(ic_row, ic_col) > closs) ? closs : ic_sequence(ic_row, ic_col);
//     }

//     algorithm->update_train_mask(full_mask);
//     algorithm->update_sparsity_level(int(d[0]));
//     algorithm->update_lambda_level(exp(d[1]));
//     algorithm->update_beta_init(beta_init);
//     algorithm->update_coef0_init(coef0_init);
//     algorithm->update_group_XTX(full_group_XTX);
//     algorithm->fit();
//     if (algorithm->warm_start)
//     {
//         beta_init = algorithm->get_beta();
//         coef0_init = algorithm->get_coef0();
//     }

//     dloss = metric->ic(algorithm, data);
//     coef0_temp2 = algorithm->get_coef0();
//     beta_temp2 = algorithm->get_beta();
//     train_loss_temp2 = metric->train_loss(algorithm, data);
//     ic_row = int(d[0]);
//     ic_col = floor((d[1] - log_lambda_min) / d_lambda);
//     if (std::isnan(ic_sequence(ic_row, ic_col)))
//     {
//         ic_sequence(ic_row, ic_col) = dloss;
//     }
//     else
//     {
//         ic_sequence(ic_row, ic_col) = (ic_sequence(ic_row, ic_col) > dloss) ? dloss : ic_sequence(ic_row, ic_col);
//     }

//     if (abs((invphi2 - invphi) * h[0]) <= s_tol && abs((invphi2 - invphi) * h[1]) < log_lambda_tol)
//     {
//         double min_loss;
//         double tmp_loss;
//         if (closs < dloss)
//         {
//             best_arg[0] = c[0];
//             best_arg[1] = c[1];
//             min_loss = closs;

//             beta1 = beta_temp1;
//             coef01 = coef0_temp1;
//             ic1 = closs;
//             train_loss1 = train_loss_temp1;
//         }
//         else
//         {
//             best_arg[0] = d[0];
//             best_arg[1] = d[1];
//             min_loss = dloss;

//             beta1 = beta_temp2;
//             coef01 = coef0_temp2;
//             ic1 = dloss;
//             train_loss1 = train_loss_temp2;
//         }
//         for (int i = 1; i < abs((invphi2 - invphi) * h[0]); i++)
//         {
//             algorithm->update_train_mask(full_mask);
//             algorithm->update_sparsity_level(int(c[0] + sign(h[0]) * i));
//             algorithm->update_lambda_level(exp(c[1]));
//             algorithm->update_beta_init(beta_init);
//             algorithm->update_coef0_init(coef0_init);
//             algorithm->update_group_XTX(full_group_XTX);
//             algorithm->fit();
//             if (algorithm->warm_start)
//             {
//                 beta_init = algorithm->get_beta();
//                 coef0_init = algorithm->get_coef0();
//             }

//             tmp_loss = metric->ic(algorithm, data);
//             ic_row = int(c[0]);
//             ic_col = floor((c[1] - log_lambda_min) / d_lambda);
//             if (std::isnan(ic_sequence(ic_row, ic_col)))
//             {
//                 ic_sequence(ic_row, ic_col) = tmp_loss;
//             }
//             else
//             {
//                 ic_sequence(ic_row, ic_col) = (ic_sequence(ic_row, ic_col) > tmp_loss) ? tmp_loss : ic_sequence(ic_row, ic_col);
//             }
//             if (tmp_loss < min_loss)
//             {
//                 best_arg[0] = c[0] + sign(h[0]) * i;
//                 best_arg[1] = c[1];
//                 min_loss = tmp_loss;

//                 beta1 = algorithm->get_beta();
//                 coef01 = algorithm->get_coef0();
//                 train_loss1 = metric->train_loss(algorithm, data);
//                 ic1 = min_loss;
//             }
//         }
//         return;
//     }
//     int tt = 0;
//     while (tt < 100)
//     {
//         tt++;
//         if (closs < dloss)
//         {
//             b[0] = d[0];
//             b[1] = d[1];
//             d[0] = c[0];
//             d[1] = c[1];
//             dloss = closs;
//             h[0] = b[0] - a[0];
//             h[1] = b[1] - a[1];

//             c[0] = a[0] + invphi2 * h[0];
//             c[1] = a[1] + invphi2 * h[1];
//             if (h[0] > 0.0001)
//             {
//                 c[0] = int(c[0]);
//             }
//             else if (h[0] < -0.0001)
//             {
//                 c[0] = ceil(c[0]);
//             }
//             else
//             {
//                 c[0] = round(c[0]);
//             }

//             algorithm->update_train_mask(full_mask);
//             algorithm->update_sparsity_level(int(c[0]));
//             algorithm->update_lambda_level(exp(c[1]));
//             algorithm->update_beta_init(beta_init);
//             algorithm->update_coef0_init(coef0_init);
//             algorithm->update_group_XTX(full_group_XTX);
//             algorithm->fit();
//             if (algorithm->warm_start)
//             {
//                 beta_init = algorithm->get_beta();
//                 coef0_init = algorithm->get_coef0();
//             }
//             closs = metric->ic(algorithm, data);
//             coef0_temp1 = algorithm->get_coef0();
//             beta_temp1 = algorithm->get_beta();
//             train_loss_temp1 = metric->train_loss(algorithm, data);
//             ic_row = int(c[0]);
//             ic_col = floor((c[1] - log_lambda_min) / d_lambda);
//             if (std::isnan(ic_sequence(ic_row, ic_col)))
//             {
//                 ic_sequence(ic_row, ic_col) = closs;
//             }
//             else
//             {
//                 ic_sequence(ic_row, ic_col) = (ic_sequence(ic_row, ic_col) > closs) ? closs : ic_sequence(ic_row, ic_col);
//             }
//         }

//         else
//         {
//             a[0] = c[0];
//             a[1] = c[1];
//             c[0] = d[0];
//             c[1] = d[1];
//             closs = dloss;
//             h[0] = b[0] - a[0];
//             h[1] = b[1] - a[1];

//             d[0] = a[0] + invphi * h[0];
//             d[1] = a[1] + invphi * h[1];

//             if (h[0] > 0.0001)
//             {
//                 d[0] = ceil(d[0]);
//             }
//             else if (h[0] < -0.0001)
//             {
//                 d[0] = int(d[0]);
//             }
//             else
//             {
//                 d[0] = round(d[0]);
//             }

//             algorithm->update_train_mask(full_mask);
//             algorithm->update_sparsity_level(int(d[0]));
//             algorithm->update_lambda_level(exp(d[1]));
//             algorithm->update_beta_init(beta_init);
//             algorithm->update_coef0_init(coef0_init);
//             algorithm->update_group_XTX(full_group_XTX);
//             algorithm->fit();
//             if (algorithm->warm_start)
//             {
//                 beta_init = algorithm->get_beta();
//                 coef0_init = algorithm->get_coef0();
//             }

//             dloss = metric->ic(algorithm, data);
//             coef0_temp2 = algorithm->get_coef0();
//             beta_temp2 = algorithm->get_beta();
//             train_loss_temp2 = metric->train_loss(algorithm, data);
//             ic_row = int(d[0]);
//             ic_col = floor((d[1] - log_lambda_min) / d_lambda);
//             if (std::isnan(ic_sequence(ic_row, ic_col)))
//             {
//                 ic_sequence(ic_row, ic_col) = dloss;
//             }
//             else
//             {
//                 ic_sequence(ic_row, ic_col) = (ic_sequence(ic_row, ic_col) > dloss) ? dloss : ic_sequence(ic_row, ic_col);
//             }
//         }

//         if ((abs((invphi2 - invphi) * h[0]) <= s_tol && abs((invphi2 - invphi) * h[1]) < log_lambda_tol) || tt == 50)
//         {
//             double min_loss;
//             double tmp_loss;
//             if (closs < dloss)
//             {
//                 best_arg[0] = c[0];
//                 best_arg[1] = c[1];
//                 min_loss = closs;

//                 beta1 = beta_temp1;
//                 coef01 = coef0_temp1;
//                 ic1 = closs;
//                 train_loss1 = train_loss_temp1;
//             }
//             else
//             {
//                 best_arg[0] = d[0];
//                 best_arg[1] = d[1];
//                 min_loss = dloss;

//                 beta1 = beta_temp2;
//                 coef01 = coef0_temp2;
//                 ic1 = dloss;
//                 train_loss1 = train_loss_temp2;
//             }
//             for (int i = 1; i < abs((invphi2 - invphi) * h[0]); i++)
//             {
//                 algorithm->update_train_mask(full_mask);
//                 algorithm->update_sparsity_level(int(c[0] + sign(h[0]) * i));
//                 algorithm->update_lambda_level(exp(c[1]));
//                 algorithm->update_beta_init(beta_init);
//                 algorithm->update_coef0_init(coef0_init);
//                 algorithm->update_group_XTX(full_group_XTX);
//                 algorithm->fit();
//                 if (algorithm->warm_start)
//                 {
//                     beta_init = algorithm->get_beta();
//                     coef0_init = algorithm->get_coef0();
//                 }
//                 tmp_loss = metric->ic(algorithm, data);

//                 ic_row = int(c[0]);
//                 ic_col = floor((c[1] - log_lambda_min) / d_lambda);
//                 if (std::isnan(ic_sequence(ic_row, ic_col)))
//                 {
//                     ic_sequence(ic_row, ic_col) = tmp_loss;
//                 }
//                 else
//                 {
//                     ic_sequence(ic_row, ic_col) = (ic_sequence(ic_row, ic_col) > tmp_loss) ? tmp_loss : ic_sequence(ic_row, ic_col);
//                 }
//                 if (tmp_loss < min_loss)
//                 {
//                     best_arg[0] = c[0] + sign(h[0]) * i;
//                     best_arg[1] = c[1];
//                     min_loss = tmp_loss;

//                     beta1 = algorithm->get_beta();
//                     coef01 = algorithm->get_coef0();
//                     train_loss1 = metric->train_loss(algorithm, data);
//                     ic1 = min_loss;
//                 }
//             }
//             return;
//         }
//     }
// }

// int GDC(int a, int b)
// {
//   int Max, Min;
//   Max = a > b ? a : b;
//   if (a == Max)
//     Min = b;
//   else
//     Min = a;
//   int z = Min;
//   while (Max % Min != 0)
//   {
//     z = Max % Min;
//     Max = Min;
//     Min = z;
//   }
//   return z;
// }

// void seq_search(Data &data, Algorithm *algorithm, Metric *metric, double p[], double u[], int s_min, int s_max, double log_lambda_min, double log_lambda_max, double best_arg[],
//                 Eigen::VectorXd &beta1, double &coef01, double &train_loss1, double &ic1, int nlambda, Eigen::MatrixXd &ic_sequence)
// {
//     vector<Eigen::MatrixXd> full_group_XTX = group_XTX(data.x, data.g_index, data.g_size, data.n, data.p, data.g_num, algorithm->model_type);

//     int n = data.get_n();
//     Eigen::VectorXi full_mask(n);
//     for (int i = 0; i < n; i++)
//     {
//         full_mask(i) = int(i);
//     }
//     Eigen::VectorXd beta_init = Eigen::VectorXd::Zero(data.get_p());
//     double coef0_init = 0.0;
//     Eigen::VectorXd beta_warm(data.get_p());
//     double coef0_warm = 0.0;

//     double d_lambda = (log_lambda_max - log_lambda_min) / (nlambda - 1);
//     int k_lambda = abs(round(u[1] / d_lambda));
//     cout << "d_lambda: " << d_lambda << ", k_lambda: " << k_lambda << ", u[1]: " << u[1] << ", u[0]: " << u[0] << endl;
//     if (abs(u[0]) != 1 && k_lambda != 1)
//     {
//         if (k_lambda == 0 && u[0] != 0)
//         {
//             u[0] = u[0] / abs(u[0]);
//         }
//         else if (u[0] == 0 && k_lambda != 0)
//         {
//             u[1] = u[1] / k_lambda;
//         }
//         else
//         {
//             int gdc;
//             gdc = GDC(k_lambda, abs(int(u[0])));
//             if (gdc)
//             {
//                 u[0] = round(u[0] / gdc);
//                 u[1] = u[1] / gdc;
//             }
//         }
//     }
//     cout << "u[0]: " << u[0] << ", u[1]: " << u[1] << endl;

//     int i = 0;
//     int j = 0;
//     int ic_row, ic_col;
//     Eigen::MatrixXd beta_all_1 = Eigen::MatrixXd::Zero(data.get_p(), (s_max - s_min + 1) * nlambda);
//     Eigen::MatrixXd beta_all_2 = Eigen::MatrixXd::Zero(data.get_p(), (s_max - s_min + 1) * nlambda);
//     Eigen::VectorXd coef0_all_1 = Eigen::VectorXd::Zero((s_max - s_min + 1) * nlambda);
//     Eigen::VectorXd coef0_all_2 = Eigen::VectorXd::Zero((s_max - s_min + 1) * nlambda);
//     Eigen::VectorXd train_loss_1 = Eigen::VectorXd::Zero((s_max - s_min + 1) * nlambda);
//     Eigen::VectorXd train_loss_2 = Eigen::VectorXd::Zero((s_max - s_min + 1) * nlambda);
//     Eigen::VectorXd ic_sequence_1 = Eigen::VectorXd::Zero((s_max - s_min + 1) * nlambda);
//     Eigen::VectorXd ic_sequence_2 = Eigen::VectorXd::Zero((s_max - s_min + 1) * nlambda);
//     beta_warm.setZero();

//     algorithm->update_train_mask(full_mask);
//     algorithm->update_sparsity_level(p[0] + i * u[0]);
//     algorithm->update_lambda_level(exp(p[1] + i * u[1]));
//     algorithm->update_beta_init(beta_init);
//     algorithm->update_coef0_init(coef0_init);
//     algorithm->update_group_XTX(full_group_XTX);

//     algorithm->fit();
//     if (algorithm->warm_start)
//     {
//         beta_init = algorithm->get_beta();
//         coef0_init = algorithm->get_coef0();
//     }
//     ic_sequence_1(i) = metric->ic(algorithm, data);
//     beta_all_1.col(i) = algorithm->get_beta();
//     coef0_all_1(i) = algorithm->get_coef0();
//     train_loss_1(i) = metric->train_loss(algorithm, data);

//     ic_sequence_2(j) = ic_sequence_1(i);
//     beta_all_2.col(j) = beta_all_1.col(i);
//     coef0_all_2(j) = coef0_all_1(i);
//     train_loss_2(j) = train_loss_1(i);

//     ic_row = p[0] + i * u[0] - s_min;
//     ic_col = (p[1] + i * u[1] - log_lambda_min) / d_lambda;
//     if (std::isnan(ic_sequence(ic_row, ic_col)))
//     {
//         ic_sequence(ic_row, ic_col) = ic_sequence_1(i);
//     }
//     else
//     {
//         ic_sequence(ic_row, ic_col) = ic_sequence(ic_row, ic_col) > ic_sequence_1(i) ? ic_sequence_1(i) : ic_sequence(ic_row, ic_col);
//     }
//     cout << "i: " << i << ", ic_sequence(" << ic_row << "," << ic_col << "): " << ic_sequence(ic_row, ic_col) << endl;

//     i++;
//     j++;
//     beta_warm = beta_init;
//     coef0_warm = coef0_init;
//     cout << "s_max: " << s_max << ", s_min: " << s_min << ", log_lambda_max + d_lambda * 1e-4:" << log_lambda_max + d_lambda * 1e-4 << ", log_lambda_min - d_lambda * 1e-4: " << log_lambda_min - d_lambda * 1e-4 << endl;
//     while ((p[0] + i * u[0] <= s_max) && (p[1] + i * u[1] <= log_lambda_max + d_lambda * 1e-4) && (p[0] + i * u[0] >= s_min) && (p[1] + i * u[1] >= log_lambda_min - d_lambda * 1e-4))
//     {
//         cout << "i:" << i << ", p[0]: " << p[0] << ", u[0]: " << u[0] << ", p[0] + i * u[0]: " << p[0] + i * u[0] << ", p[1]" << p[1] << ", u[1]: " << u[1] << ", p[1] + i * u[1] : " << p[1] + i * u[1] << endl;
//         algorithm->update_train_mask(full_mask);
//         algorithm->update_sparsity_level(p[0] + i * u[0]);
//         algorithm->update_lambda_level(exp(p[1] + i * u[1]));
//         algorithm->update_beta_init(beta_init);
//         algorithm->update_coef0_init(coef0_init);
//         algorithm->update_group_XTX(full_group_XTX);

//         algorithm->fit();
//         if (algorithm->warm_start)
//         {
//             beta_init = algorithm->get_beta();
//             coef0_init = algorithm->get_coef0();
//         }
//         ic_sequence_1(i) = metric->ic(algorithm, data);
//         beta_all_1.col(i) = algorithm->get_beta();
//         coef0_all_1(i) = algorithm->get_coef0();
//         train_loss_1(i) = metric->train_loss(algorithm, data);
//         ic_row = p[0] + i * u[0] - s_min;
//         ic_col = round((p[1] + i * u[1] - log_lambda_min) / d_lambda);
//         if (std::isnan(ic_sequence(ic_row, ic_col)))
//         {
//             ic_sequence(ic_row, ic_col) = ic_sequence_1(i);
//         }
//         else
//         {
//             ic_sequence(ic_row, ic_col) = ic_sequence(ic_row, ic_col) > ic_sequence_1(i) ? ic_sequence_1(i) : ic_sequence(ic_row, ic_col);
//         }
//         cout << "i: " << i << ", ic_sequence(" << ic_row << "," << ic_col << "): " << ic_sequence(ic_row, ic_col) << ", ic_sequence_1(i): " << ic_sequence_1(i) << endl;
//         i++;
//     }
//     cout << "out: i:" << i << ", p[0] - i * u[0]: " << p[0] - i * u[0] << ", p[1] - i * u[1] : " << p[1] - i * u[1] << endl;

//     beta_init = beta_warm;
//     coef0_init = coef0_warm;

//     while ((p[0] - j * u[0] <= s_max) && (p[1] - j * u[1] <= log_lambda_max + d_lambda * 1e-4) && (p[0] - j * u[0] >= s_min) && (p[1] - j * u[1] >= log_lambda_min - d_lambda * 1e-4))
//     {
//         cout << "j:" << j << ", p[0]: " << p[0] << ", u[0]: " << u[0] << ", p[0] - j * u[0]: " << p[0] - j * u[0] << ", p[1]" << p[1] << ", u[1]: " << u[1] << ", p[1] - j * u[1]" << p[1] - j * u[1] << endl;
//         algorithm->update_train_mask(full_mask);
//         algorithm->update_sparsity_level(p[0] - j * u[0]);
//         algorithm->update_lambda_level(exp(p[1] - j * u[1]));
//         algorithm->update_beta_init(beta_init);
//         algorithm->update_coef0_init(coef0_init);
//         algorithm->update_group_XTX(full_group_XTX);

//         algorithm->fit();
//         if (algorithm->warm_start)
//         {
//             beta_init = algorithm->get_beta();
//             coef0_init = algorithm->get_coef0();
//         }
//         ic_sequence_2(j) = metric->ic(algorithm, data);
//         beta_all_2.col(j) = algorithm->get_beta();
//         coef0_all_2(j) = algorithm->get_coef0();
//         train_loss_2(j) = metric->train_loss(algorithm, data);
//         ic_row = p[0] - j * u[0] - s_min;
//         ic_col = round((p[1] - j * u[1] - log_lambda_min) / d_lambda);
//         if (std::isnan(ic_sequence(ic_row, ic_col)))
//         {
//             ic_sequence(ic_row, ic_col) = ic_sequence_2(j);
//         }
//         else
//         {
//             ic_sequence(ic_row, ic_col) = ic_sequence(ic_row, ic_col) > ic_sequence_2(j) ? ic_sequence_2(j) : ic_sequence(ic_row, ic_col);
//         }
//         cout << "j: " << j << ", ic_sequence(" << ic_row << "," << ic_col << "): " << ic_sequence(ic_row, ic_col) << "ic_sequence_2(j): " << ic_sequence_2(j) << endl;

//         j++;
//     }
//     cout << "out: j:" << j << ", p[0] - j * u[0]: " << p[0] - j * u[0] << ", p[1] - j * u[1] : " << p[1] - j * u[1] << endl;
//     int minPosition_1, minPosition_2;
//     ic_sequence_1 = ic_sequence_1.head(i).eval();
//     ic_sequence_2 = ic_sequence_2.head(j).eval();
//     ic_sequence_1.minCoeff(&minPosition_1);
//     ic_sequence_2.minCoeff(&minPosition_2);
//     cout << "minPosition_1: " << minPosition_1 << ", minPosition_2: " << minPosition_2 << ",  ic_sequence_1.minCoeff(&minPosition_1): " << ic_sequence_1(minPosition_1) << ", ic_sequence_2.minCoeff(&minPosition_2): " << ic_sequence_2(minPosition_2) << endl;
//     cout << "ic_sequence_2(minPosition_2): " << ic_sequence_2(minPosition_2) << ", ic_sequence_2(minPosition_2+1): " << ic_sequence_2(minPosition_2 + 1) << ", ic_sequence_2(minPosition_2+1)< ic_sequence_2(minPosition_2): " << (ic_sequence_2(minPosition_2 + 1) < ic_sequence_2(minPosition_2)) << endl;
//     int minPosition;
//     if (ic_sequence_1(minPosition_1) < ic_sequence_2(minPosition_2))
//     {
//         minPosition = minPosition_1;
//         ic1 = ic_sequence_1(minPosition);
//         train_loss1 = train_loss_1(minPosition);
//         beta1 = beta_all_1.col(minPosition);
//         coef01 = coef0_all_1(minPosition);
//     }
//     else
//     {
//         minPosition = -minPosition_2;
//         ic1 = ic_sequence_2(minPosition_2);
//         train_loss1 = train_loss_2(minPosition_2);
//         beta1 = beta_all_2.col(minPosition_2);
//         coef01 = coef0_all_2(minPosition_2);
//     }
//     best_arg[0] = p[0] + (minPosition)*u[0];
//     best_arg[1] = p[1] + (minPosition)*u[1];
//     cout << "minPosition :" << minPosition << ",  best_arg[0]:" << best_arg[0] << " best_arg[1]: " << best_arg[1] << ", p[1] + (minPosition)*u[1]= " << p[1] << "+" << minPosition << "* " << u[1] << "= " << p[1] + (minPosition)*u[1] << ", (minPosition)*u[1]: " << (minPosition)*u[1] << endl;
//     return;
// }

// List pgs_path(Data &data, Algorithm *algorithm, Metric *metric, int s_min, int s_max, double log_lambda_min, double log_lambda_max, int powell_path, int nlambda)
// {
//     int n = data.get_n();
//     Eigen::VectorXi full_mask(n);
//     vector<Eigen::MatrixXd> full_group_XTX = group_XTX(data.x, data.g_index, data.g_size, data.n, data.p, data.g_num, algorithm->model_type);
//     for (int i = 0; i < n; i++)
//     {
//         full_mask(i) = i;
//     }
//     Eigen::MatrixXd beta_all = Eigen::MatrixXd::Zero(data.get_p(), 100);
//     Eigen::VectorXd coef0_all = Eigen::VectorXd::Zero(100);
//     Eigen::VectorXd train_loss_all = Eigen::VectorXd::Zero(100);
//     Eigen::VectorXd ic_all = Eigen::VectorXd::Zero(100);
//     Eigen::VectorXd lambda_chosen = Eigen::VectorXd::Zero(100);

//     Eigen::VectorXd beta_temp = Eigen::VectorXd::Zero(data.get_p());
//     double coef0_temp;
//     double train_loss_temp;
//     double ic_temp;
//     //double lambda_temp;

//     if (powell_path == 1)
//         nlambda = 100;
//     Eigen::MatrixXd ic_sequence = NAN * Eigen::MatrixXd::Ones(s_max - s_min + 1, nlambda);

//     double P[3][3], U[2][2];
//     int i;

//     P[0][0] = double(s_min);
//     P[0][1] = log_lambda_min;

//     U[1][0] = 1.; //search directions
//     U[1][1] = 0.;
//     U[0][0] = 0.;
//     U[0][1] = (log_lambda_max - log_lambda_min) / (nlambda - 1);

//     int ttt = 0;
//     if (powell_path == 1)
//         golden_section_search(data, algorithm, metric, P[0], U[1], s_min, s_max, log_lambda_min, log_lambda_max, P[0], beta_temp, coef0_temp, train_loss_temp, ic_temp, ic_sequence);
//     else
//         seq_search(data, algorithm, metric, P[0], U[1], s_min, s_max, log_lambda_min, log_lambda_max, P[0], beta_temp, coef0_temp, train_loss_temp, ic_temp, nlambda, ic_sequence);
//     beta_all.col(ttt) = beta_temp;
//     coef0_all(ttt) = coef0_temp;
//     train_loss_all(ttt) = train_loss_temp;
//     ic_all(ttt) = ic_temp;
//     lambda_chosen(ttt) = exp(P[0][1]);

//     while (ttt < 11)
//     {
//         ttt++;
//         for (i = 0; i < 2; i++)
//         {
//             if (powell_path == 1)
//                 golden_section_search(data, algorithm, metric, P[i], U[i], s_min, s_max, log_lambda_min, log_lambda_max, P[i + 1], beta_temp, coef0_temp, train_loss_temp, ic_temp, ic_sequence);
//             else
//                 seq_search(data, algorithm, metric, P[i], U[i], s_min, s_max, log_lambda_min, log_lambda_max, P[i + 1], beta_temp, coef0_temp, train_loss_temp, ic_temp, nlambda, ic_sequence);
//             beta_all.col(ttt) = beta_temp;
//             coef0_all(ttt) = coef0_temp;
//             train_loss_all(ttt) = train_loss_temp;
//             ic_all(ttt) = ic_temp;
//             lambda_chosen(ttt) = exp(P[i + 1][1]);
//             ttt++;
//         }
//         U[0][0] = U[1][0];
//         U[0][1] = U[1][1];
//         U[1][0] = P[2][0] - P[0][0];
//         U[1][1] = P[2][1] - P[0][1];
//         if ((!(abs(U[1][0]) <= 0.0001 && abs(U[1][1]) <= 0.0001)) && ttt < 11)
//         {
//             if (powell_path == 1)
//                 golden_section_search(data, algorithm, metric, P[0], U[1], s_min, s_max, log_lambda_min, log_lambda_max, P[0], beta_temp, coef0_temp, train_loss_temp, ic_temp, ic_sequence);
//             else
//                 seq_search(data, algorithm, metric, P[0], U[1], s_min, s_max, log_lambda_min, log_lambda_max, P[0], beta_temp, coef0_temp, train_loss_temp, ic_temp, nlambda, ic_sequence);
//             beta_all.col(ttt) = beta_temp;
//             coef0_all(ttt) = coef0_temp;
//             train_loss_all(ttt) = train_loss_temp;
//             ic_all(ttt) = ic_temp;
//             lambda_chosen(ttt) = exp(P[0][1]);
//         }
//         else
//         {
//             // P[0] is the best parameter.
//             algorithm->update_train_mask(full_mask);
//             algorithm->update_sparsity_level(int(P[0][0]));
//             algorithm->update_lambda_level(exp(P[0][1]));
//             algorithm->update_group_XTX(full_group_XTX);
//             algorithm->fit();

//             Eigen::VectorXd best_beta = algorithm->get_beta();
//             double best_coef0 = algorithm->get_coef0();
//             double best_train_loss = metric->train_loss(algorithm, data);
//             double best_ic = metric->ic(algorithm, data);

//             beta_all.col(ttt) = best_beta;
//             coef0_all(ttt) = best_coef0;
//             train_loss_all(ttt) = best_train_loss;
//             ic_all(ttt) = best_ic;
//             lambda_chosen(ttt) = exp(P[0][1]);

//             ttt++;
//             beta_all = beta_all.leftCols(ttt).eval();
//             coef0_all = coef0_all.head(ttt).eval();
//             train_loss_all = train_loss_all.head(ttt).eval();
//             ic_all = ic_all.head(ttt).eval();
//             lambda_chosen = lambda_chosen.head(ttt).eval();

//             if (data.is_normal)
//             {
//                 if (algorithm->model_type == 1)
//                 {
//                     for (int k = 0; k < ttt; k++)
//                     {
//                         beta_all.col(k) = sqrt(double(n)) * beta_all.col(k).cwiseQuotient(data.x_norm);
//                         coef0_all(k) = data.y_mean - beta_all.col(k).dot(data.x_mean);
//                     }
//                 }

//                 else if (data.data_type == 2)
//                 {
//                     for (int k = 0; k < ttt; k++)
//                     {
//                         beta_all.col(k) = sqrt(double(n)) * beta_all.col(k).cwiseQuotient(data.x_norm);
//                         coef0_all(k) = coef0_all(k) - beta_all.col(k).dot(data.x_mean);
//                     }
//                 }
//                 else
//                 {
//                     for (int k = 0; k < ttt; k++)
//                     {
//                         beta_all.col(k) = sqrt(double(n)) * beta_all.col(k).cwiseQuotient(data.x_norm);
//                     }
//                 }
//             }
//             int min_ic_index = 0;
//             double min_ic = ic_all.minCoeff(&min_ic_index);
//             if (min_ic == ic_all(ttt - 1))
//             {
//                 min_ic_index = ttt - 1;
//             }
// #ifdef R_BUILD
//             return List::create(Named("beta") = beta_all.col(min_ic_index).eval(), Named("coef0") = coef0_all(min_ic_index),
//                                 Named("train_loss") = train_loss_all(min_ic_index), Named("ic") = ic_all(min_ic_index),
//                                 Named("lambda") = lambda_chosen(min_ic_index),
//                                 Named("beta_all") = beta_all, Named("coef0_all") = coef0_all,
//                                 Named("train_loss_all") = train_loss_all,
//                                 Named("ic_all") = ic_all,
//                                 Named("lambda_all") = lambda_chosen,
//                                 Named("ic_mat") = ic_sequence);
// #else
//             List mylist;
//             mylist.add("beta", beta_all.col(min_ic_index).eval());
//             mylist.add("coef0", coef0_all(min_ic_index));
//             mylist.add("train_loss", train_loss_all(min_ic_index));
//             mylist.add("ic", ic_all(min_ic_index));
//             mylist.add("lambda", lambda_chosen(min_ic_index));
//             return mylist;
// #endif
//         }
//     }
// #ifdef R_BUILD
//     Rcpp::Rcout << "powell end wrong" << endl;
// #else
//     cout << "powell end wrong" << endl;
// #endif
// #ifdef R_BUILD
//     return List::create(Named("beta") = 0);
// #else
//     List mylist;
//     return mylist;
// #endif
// }


/*
 * Copyright (c) 2011-2021, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef DART_OPTIMIZATION_GENERICMULTIOBJECTIVEPROBLEM_HPP_
#define DART_OPTIMIZATION_GENERICMULTIOBJECTIVEPROBLEM_HPP_

#include <cstddef>
#include <vector>

#include <Eigen/Dense>

#include "dart/optimization/Function.hpp"
#include "dart/optimization/MultiObjectiveProblem.hpp"

namespace dart {
namespace optimization {

class GenericMultiObjectiveProblem : public MultiObjectiveProblem
{
public:
  /// Constructor
  explicit GenericMultiObjectiveProblem(
      std::size_t dim, std::size_t integerDim = 0u);

  /// Destructor
  ~GenericMultiObjectiveProblem() override = default;

  /// \{ \name Objectives

  // Documentation inherited.
  std::size_t getObjectiveDimension() const override;

  /// Sets objective functions to be minimized.
  void setObjectiveFunctions(const std::vector<FunctionPtr>& objectives);

  /// Adds a minimum objective function
  void addObjectiveFunction(FunctionPtr objective);

  /// Returns the number objective functions.
  std::size_t getNumObjectiveFunctions() const;

  /// Returns objective functions
  const std::vector<FunctionPtr>& getObjectiveFunctions() const;

  /// Removes an objective function
  void removeObjectiveFunction(FunctionPtr function);

  /// Removes all objective functions
  void removeAllObjectiveFunctions();

  /// \}

  /// \{ \name Equality Constraints

  // Documentation inherited.
  std::size_t getEqConstraintDimension() const override;

  /// Adds equality constraint
  void addEqConstraintFunction(FunctionPtr eqConst);

  /// Returns number of equality constraints
  std::size_t getNumEqualityConstraintFunctions() const;

  /// Returns equality constraint
  FunctionPtr getEqConstraintFunction(std::size_t index) const;

  /// Removes equality constraint
  void removeEqConstraintFunction(FunctionPtr eqConst);

  /// Removes all equality constraints
  void removeAllEqConstraintFunctions();

  /// \}

  /// \{ \name Inequality Constraints

  // Documentation inherited.
  std::size_t getIneqConstraintDimension() const override;

  /// Adds inequality constraint. Inequality constraints must evaluate
  /// to LESS THAN or equal to zero (within some tolerance) to be satisfied.
  void addIneqConstraintFunction(FunctionPtr ineqConst);

  /// Returns number of inequality constraints
  std::size_t getNumIneqConstraintFunctions() const;

  /// Returns inequality constraint
  FunctionPtr getIneqConstraintFunction(std::size_t index) const;

  /// Removes inequality constraint
  void removeIneqConstraintFunction(FunctionPtr ineqConst);

  /// Removes all inequality constraints
  void removeAllIneqConstraintFunctions();

  /// \}

  /// \{ \name Evaluations

  /// Evaluates objectives
  Eigen::VectorXd evaluateObjectives(const Eigen::VectorXd& x) const override;

  /// Evaluates equality constraints
  Eigen::VectorXd evaluateEqConstraints(
      const Eigen::VectorXd& x) const override;

  /// Evaluates inequality constraints
  Eigen::VectorXd evaluateIneqConstraints(
      const Eigen::VectorXd& x) const override;

  /// Return dimension of fitness
  std::size_t getFitnessDimension() const;

  /// Evaluates fitness, which is [objectives, equality constraints, inequality
  /// constraints].
  Eigen::VectorXd evaluateFitness(const Eigen::VectorXd& x) const;

  /// \}

protected:
  /// Objective functions
  std::vector<FunctionPtr> mObjectiveFunctions;

  /// Equality constraint functions
  std::vector<FunctionPtr> mEqConstraintFunctions;

  /// Inequality constraint functions
  std::vector<FunctionPtr> mIneqConstraintFunctions;

private:
  /// Cache for objective dimension
  std::size_t mObjectiveDimension;

  /// Cache for equality constraint dimension
  std::size_t mEqConstraintDimension;

  /// Cache for inequality constraint dimension
  std::size_t mIneqConstraintDimension;
};

} // namespace optimization
} // namespace dart

#endif // DART_OPTIMIZATION_GENERICMULTIOBJECTIVEPROBLEM_HPP_


/* transfer_function.cc
   Jeremy Barnes, 4 November 2009
   Copyright (c) 2009 Jeremy Barnes.  All rights reserved.

   Transfer function implementation.
*/

#include "transfer_function.h"
#include <cmath>
#include "jml/db/persistent.h"
#include "jml/boosting/registry.h"
#include "jml/utils/smart_ptr_utils.h"

#include <boost/static_assert.hpp>

using namespace ML::DB;
using namespace std;

namespace ML {

/*****************************************************************************/
/* RANGE_TYPE                                                                */
/*****************************************************************************/

/*****************************************************************************/
/* RANGE                                                                     */
/*****************************************************************************/

/*****************************************************************************/
/* TRANSFER_FUNCTION                                                         */
/*****************************************************************************/

void
Transfer_Function::
poly_serialize(DB::Store_Writer & store) const
{
    Registry<Transfer_Function>::singleton().serialize(store, this);
}

std::shared_ptr<Transfer_Function>
Transfer_Function::
poly_reconstitute(DB::Store_Reader & store)
{
    return Registry<Transfer_Function>::singleton().reconstitute(store);
}

distribution<float>
Transfer_Function::
transfer(const distribution<float> & activation) const
{
    int no = activation.size();
    distribution<float> output(no);
    transfer(&activation[0], &output[0], no);
    return output;
}

distribution<double>
Transfer_Function::
transfer(const distribution<double> & activation) const
{
    int no = activation.size();
    distribution<double> output(no);
    transfer(&activation[0], &output[0], no);
    return output;
}

distribution<float>
Transfer_Function::
derivative(const distribution<float> & outputs) const
{
    int no = outputs.size();
    distribution<float> result(no);
    derivative(&outputs[0], &result[0], no);
    return result;
}

distribution<double>
Transfer_Function::
derivative(const distribution<double> & outputs) const
{
    int no = outputs.size();
    distribution<double> result(no);
    derivative(&outputs[0], &result[0], no);
    return result;
}

distribution<float>
Transfer_Function::
second_derivative(const distribution<float> & outputs) const
{
    int no = outputs.size();
    distribution<float> result(no);
    second_derivative(&outputs[0], &result[0], no);
    return result;
}

distribution<double>
Transfer_Function::
second_derivative(const distribution<double> & outputs) const
{
    int no = outputs.size();
    distribution<double> result(no);
    second_derivative(&outputs[0], &result[0], no);
    return result;
}

/*****************************************************************************/
/* STANDARD_TRANSFER_FUNCTION                                                */
/*****************************************************************************/

Standard_Transfer_Function::
Standard_Transfer_Function(Transfer_Function_Type transfer_function)
    : transfer_function(transfer_function)
{
}

std::string
Standard_Transfer_Function::
print() const
{
    return ML::print(transfer_function);
}

Range
Standard_Transfer_Function::
range() const
{
    static Range ranges[5] = {
        { -INFINITY, INFINITY, 0.0, false, false, RT_PM_INF },  /* TF_LOGSIG */
        { -1.0,      1.0,      0.0, true,  true,  RT_PM_ONE },  /* TF_TANH */
        { -1.7159,   1.7159,   0.0, true,  true,  RT_OTHER  },  /* TF_TANHS */
        { -INFINITY, INFINITY, 0.0, false, false, RT_PM_INF }, /* TF_IDENTITY */
        { 0.0,       1.0,      0.5, true,  true,  RT_PROB   }  /* TF_SOFTMAX */};

    BOOST_STATIC_ASSERT(TF_LOGSIG   == 0);
    BOOST_STATIC_ASSERT(TF_TANH     == 1);
    BOOST_STATIC_ASSERT(TF_TANHS    == 2);
    BOOST_STATIC_ASSERT(TF_IDENTITY == 3);
    BOOST_STATIC_ASSERT(TF_SOFTMAX  == 4);

    if (transfer_function <= TF_SOFTMAX)
        return ranges[transfer_function];

    throw Exception("Standard_Transfer_Function::range(): non-standard");
}

std::pair<float, float>
Standard_Transfer_Function::
targets(float maximum) const
{
    switch (transfer_function) {
    case TF_TANH:
    case TF_IDENTITY: return std::make_pair(-maximum, maximum);
    case TF_TANHS: return make_pair(-1.0, 1.0);
    case TF_SOFTMAX:
    case TF_LOGSIG: return std::make_pair(0.0f, maximum);
    default:
        throw Exception("Layer::targets(): invalid transfer_function");
    }
}

void
Standard_Transfer_Function::
serialize(DB::Store_Writer & store) const
{
    store << (char)0 // version
          << transfer_function;
}

void
Standard_Transfer_Function::
reconstitute(DB::Store_Reader & store)
{
    char version;
    store >> version;
    if (version != 0)
        throw Exception("Standard_Transfer_Function::reconstitute(): "
                        "unknown version");
    store >> transfer_function;
}

std::string
Standard_Transfer_Function::
class_id() const
{
    return "Standard";
}

bool
Standard_Transfer_Function::
equal(const Transfer_Function & other) const
{
    const Standard_Transfer_Function * other_cast
        = dynamic_cast<const Standard_Transfer_Function *>(&other);
    if (!other_cast)
        return false;  // not a standard transfer function...

    return transfer_function == other_cast->transfer_function;
}

template<typename FloatIn>
void
Standard_Transfer_Function::
transfer(const FloatIn * activation, FloatIn * outputs, int nvals,
         Transfer_Function_Type transfer_function)
{
    switch (transfer_function) {
    case TF_IDENTITY:
        std::copy(activation, activation + nvals, outputs);
        return;

    case TF_LOGSIG:
        for (unsigned i = 0;  i < nvals;  ++i) {
            // See https://bugzilla.redhat.com/show_bug.cgi?id=521190
            // for why we use double version of exp
            outputs[i] = 1.0 / (1.0 + exp((double)-activation[i]));
        }
        break;

    case TF_TANH:
        for (unsigned i = 0;  i < nvals;  ++i)
            outputs[i] = tanh(activation[i]);
        break;

    case TF_TANHS:
        for (unsigned i = 0;  i < nvals;  ++i)
            outputs[i] = 1.7159 * tanh(0.66666666666666666666 * activation[i]);
        break;

    case TF_SOFTMAX: {
        double total = 0.0;

        for (unsigned i = 0;  i < nvals;  ++i) {
            // See https://bugzilla.redhat.com/show_bug.cgi?id=521190
            // for why we use double version of exp
            total += (outputs[i] = exp((double)activation[i]));
        }

        double factor = 1.0 / total;

        for (unsigned i = 0;  i < nvals;  ++i)
            outputs[i] *= factor;

        break;
    }

    default:
        throw Exception("Standard_Transfer_Function::transfer(): invalid transfer_function");
    }
}

void
Standard_Transfer_Function::
transfer(const float * activation, float * outputs, size_t n) const
{
    transfer(activation, outputs, n, transfer_function);
}

void
Standard_Transfer_Function::
transfer(const double * activation, double * outputs, size_t n) const
{
    transfer(activation, outputs, n, transfer_function);
}

template<typename FloatIn>
void
Standard_Transfer_Function::
derivative(const FloatIn * outputs, FloatIn * deriv, int nvals,
           Transfer_Function_Type transfer_function)
{
    switch (transfer_function) {

    case TF_IDENTITY:
        std::fill(deriv, deriv + nvals, 1.0);
        break;

    case TF_LOGSIG:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = outputs[i] * (1.0 - outputs[i]);
        break;

    case TF_TANH:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = 1.0 - (outputs[i] * outputs[i]);
        break;

    case TF_TANHS:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = 1.7159 * 0.6666666666666
                * (1.0 - (outputs[i] * outputs[i]));
        break;

    case TF_SOFTMAX:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = 1.0 / outputs[i];
        break;

    default:
        throw Exception("Standard_Transfer_Function::transfer(): invalid transfer_function");
    }
}

void
Standard_Transfer_Function::
derivative(const float * outputs, float * derivatives, size_t n) const
{
    derivative(outputs, derivatives, n, transfer_function);
}

void
Standard_Transfer_Function::
derivative(const double * outputs, double * derivatives, size_t n) const
{
    derivative(outputs, derivatives, n, transfer_function);
}

template<typename FloatIn>
void
Standard_Transfer_Function::
second_derivative(const FloatIn * outputs, FloatIn * deriv, int nvals,
                  Transfer_Function_Type transfer_function)
{
    switch (transfer_function) {

    case TF_IDENTITY:
        std::fill(deriv, deriv + nvals, 0.0);
        break;

    case TF_TANH:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = -2.0 * outputs[i] * (1.0 - (outputs[i] * outputs[i]));
        break;

    case TF_TANHS:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = 1.7159 * 0.6666666666666 * 0.6666666666666
                * -2.0 * outputs[i] * (1.0 - (outputs[i] * outputs[i]));
        break;

    case TF_LOGSIG:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = outputs[i] * (1 - outputs[i]) * (1 - 2 * outputs[i]);
        break;

#if 0
    case TF_SOFTMAX:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = ...;
        break;
#endif

    default:
        throw Exception("Standard_Transfer_Function::transfer(): second derivative not implemented "
                        "for this transfer_function "
                        + ML::print(transfer_function));
    }
}

void
Standard_Transfer_Function::
second_derivative(const float * outputs, float * second_derivatives,
                  size_t n) const
{
    second_derivative(outputs, second_derivatives, n, transfer_function);
}

void
Standard_Transfer_Function::
second_derivative(const double * outputs, double * second_derivatives,
                  size_t n) const
{
    second_derivative(outputs, second_derivatives, n, transfer_function);
}

namespace {

Register_Factory<Transfer_Function, Standard_Transfer_Function>
    STF_REGISTER("Standard");

} // file scope

/*****************************************************************************/
/* FACTORY                                                                   */
/*****************************************************************************/

std::shared_ptr<Transfer_Function>
create_transfer_function(const Transfer_Function_Type & function)
{
    return make_sp(new Standard_Transfer_Function(function));
}

std::shared_ptr<Transfer_Function>
create_transfer_function(const std::string & name)
{
    throw Exception("create_transfer_function(name): not implemented");
}

} // namespace ML


/* Copyright (C) 2012,2013 IBM Corp.
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */
#include "matrix.h"
#include <NTL/ZZ.h>
#include <NTL/lzz_pXFactoring.h>
#include <cassert>
#include <cstdio>

template<class type>
class SingleBlockMatrix : public  PlaintextBlockMatrixInterface<type> {
public:
  PA_INJECT(type)

private:
  const EncryptedArray& ea;

  Mat<R> data;

public:
  SingleBlockMatrix(const EncryptedArray& _ea, const vector<ZZX>& vec) : ea(_ea) {
    long d = ea.getDegree();

    RBak bak; bak.save(); ea.getContext().alMod.restoreContext();

    data.SetDims(d, d);
    for (long i = 0; i < d; i++)
      for (long j = 0; j < d; j++)
         conv(data[i][j], coeff(vec[i], j));
  }

  virtual const EncryptedArray& getEA() const {
    return ea;
  }

  virtual bool get(Mat<R>& out, long i, long j) const {
    assert(i >= 0 && i < ea.size());
    assert(j >= 0 && j < ea.size());
    if (i != j) return true;
    out = data;
    return false;
  }

};

PlaintextBlockMatrixBaseInterface *buildSingleBlockMatrix(const EncryptedArray& ea,
                                                          const vector<ZZX>& vec)
{
  switch (ea.getTag()) {
    case PA_GF2_tag: {
      return new SingleBlockMatrix<PA_GF2>(ea, vec);
    }

    case PA_zz_p_tag: {
      return new SingleBlockMatrix<PA_zz_p>(ea, vec);
    }

    default: return 0;
  }
}

template<class type>
class MultiBlockMatrix : public  PlaintextBlockMatrixInterface<type> {
public:
  PA_INJECT(type)

private:
  const EncryptedArray& ea;

  Vec< Mat<R> > data;

public:
  MultiBlockMatrix(const EncryptedArray& _ea, const vector< vector<ZZX> >& vec) : ea(_ea) {
    long n = ea.size();
    long d = ea.getDegree();

    RBak bak; bak.save(); ea.getContext().alMod.restoreContext();

    data.SetLength(n);
    for (long k = 0; k < n; k++) {
      data[k].SetDims(d, d);
      for (long i = 0; i < d; i++)
        for (long j = 0; j < d; j++)
           conv(data[k][i][j], coeff(vec[k][i], j));
    }
  }

  virtual const EncryptedArray& getEA() const {
    return ea;
  }

  virtual bool get(Mat<R>& out, long i, long j) const {
    assert(i >= 0 && i < ea.size());
    assert(j >= 0 && j < ea.size());
    if (i != j) return true;
    out = data[i];
    return false;
  }
};

PlaintextBlockMatrixBaseInterface *buildMultiBlockMatrix(const EncryptedArray& ea,
                                                          const vector< vector<ZZX> >& vec)
{
  switch (ea.getTag()) {
    case PA_GF2_tag: {
      return new MultiBlockMatrix<PA_GF2>(ea, vec);
    }

    case PA_zz_p_tag: {
      return new MultiBlockMatrix<PA_zz_p>(ea, vec);
    }

    default: return 0;
  }
}

void  TestIt(long m, long p, long r, long d)
{
  cout << "\n\n******** TestIt" << (isDryRun()? "(dry run):" : ":")
       << " m=" << m
       << ", p=" << p
       << ", r=" << r
       << ", d=" << d
       << endl;

  FHEcontext context(m, p, r);
  buildModChain(context, /*L=*/10, /*c=*/2);
  context.zMStar.printout();
  cout << endl;

  cout << "generating keys and key-switching matrices... " << std::flush;
  FHESecKey secretKey(context);
  const FHEPubKey& publicKey = secretKey;
  secretKey.GenSecKey(/*w=*/64);// A Hamming-weight-w secret key
  addSome1DMatrices(secretKey); // compute key-switching matrices that we need
  addFrbMatrices(secretKey); // compute key-switching matrices that we need
  cout << "done\n";

  ZZX G;
  if (d == 0) {
    G = context.alMod.getFactorsOverZZ()[0];
    d = deg(G);
  }
  else
    G = makeIrredPoly(p, d);
  cout << "G = " << G << "\n";
  cout << "computing masks and tables for rotation... " << std::flush;
  EncryptedArray ea(context, G);
  cout << "done\n";

  long nslots = ea.size();

  PlaintextArray p0(ea);
  PlaintextArray pp0(ea);

  Ctxt c0(publicKey), cc0(publicKey);

  cout << "\nTest #1: Apply the same linear transformation to all slots\n";
  {
  vector<ZZX> LM(d); // LM selects even coefficients
  for (long j = 0; j < d; j++)
    if (j % 2 == 0) LM[j] = ZZX(j, 1);

  // "building" the linearized-polynomial coefficients
  vector<ZZX> C;
  ea.buildLinPolyCoeffs(C, LM);

  p0.random();
  ea.encrypt(c0, publicKey, p0);
  ea.encrypt(cc0, publicKey, p0);

  applyLinPoly1(ea, c0, C);
  ea.decrypt(c0, secretKey, p0);

  shared_ptr<PlaintextBlockMatrixBaseInterface> mat(buildSingleBlockMatrix(ea, LM));
  free_mat_mul(ea, cc0, *mat);
  ea.decrypt(cc0, secretKey, pp0);

  if (pp0.equals(p0))
    cout << "GOOD\n";
  else
    cout << "BAD\n";
  }

  cout << "\nTest #2: Apply different transformations to the different slots\n";
  {
  vector< vector<ZZX> > LM(nslots);
  // LM[i] rotates the coefficients in the i'th slot by (i % d)
  for (long i = 0; i < nslots; i++) {
    LM[i].resize(d);
    for (long j = 0; j < d; j++)  {
      long jj = (i+j) % d;
      LM[i][j] = ZZX(jj, 1);
    }
  }

  // "building" the linearized-polynomial coefficients
  vector< vector<ZZX> > C(nslots);
  for (long i = 0; i < nslots; i++)
    ea.buildLinPolyCoeffs(C[i], LM[i]);

  p0.random();
  ea.encrypt(c0, publicKey, p0);
  ea.encrypt(cc0, publicKey, p0);

  applyLinPolyMany(ea, c0, C); // apply the linearized polynomials
  ea.decrypt(c0, secretKey, p0);

  shared_ptr<PlaintextBlockMatrixBaseInterface> mat(buildMultiBlockMatrix(ea, LM));
  free_mat_mul(ea, cc0, *mat);
  ea.decrypt(cc0, secretKey, pp0);

  if (pp0.equals(p0))
    cout << "GOOD\n";
  else
    cout << "BAD\n";
  }

  cout << "\nTest #3: Testing low-level (cached) implementation\n";
  {
  vector< vector<ZZX> > LM(nslots);
  // LM[i] adds coefficients (i % d) and (i+1 % d) in the i'th slot
  for (long i = 0; i < nslots; i++) {
    LM[i].resize(d);
    for (long j = 0; j < d; j++)  {
      if ( j == (i % d) || j == ((i+1)%d) )
	LM[i][j] = conv<ZZX>(1L);
    }
  }

  // "building" the linearized-polynomial coefficients
  vector< vector<ZZX> > C(nslots);
  for (long i = 0; i < nslots; i++)
    ea.buildLinPolyCoeffs(C[i], LM[i]);

  // "encoding" the linearized-polynomial coefficients
  vector<ZZX> encodedC(d);
  for (long j = 0; j < d; j++) {
    vector<ZZX> v(nslots);
    for (long i = 0; i < nslots; i++) v[i] = C[i][j];
    ea.encode(encodedC[j], v);
  }

  p0.random();
  ea.encrypt(c0, publicKey, p0);
  ea.encrypt(cc0, publicKey, p0);

  applyLinPolyLL(c0, encodedC, ea.getDegree()); // apply linearized polynomials
  ea.decrypt(c0, secretKey, p0);

  shared_ptr<PlaintextBlockMatrixBaseInterface> mat(buildMultiBlockMatrix(ea, LM));
  free_mat_mul(ea, cc0, *mat);
  ea.decrypt(cc0, secretKey, pp0);

  if (pp0.equals(p0))
    cout << "GOOD\n";
  else
    cout << "BAD\n";
  }
}

int main(int argc, char *argv[])
{
  ArgMapping amap;

  bool dry = false;
  amap.arg("dry", dry, "dry=1 for a dry-run");

  long m=91;
  amap.arg("m", m, "use specified value as modulus");

  long p=2;
  amap.arg("p", p, "plaintext base");

  long r=1;
  amap.arg("r", r,  "lifting");

  long d=0;
  amap.arg("d", d, "degree of the field extension");
  amap.note("d == 0 => factors[0] defines extension");

  amap.parse(argc, argv);

  long repeat = 2;
  setTimersOn();
  setDryRun(dry);
  for (long repeat_cnt = 0; repeat_cnt < repeat; repeat_cnt++) {
    TestIt(m, p, r, d);
  }

}