/* Copyright (C) 2017 IBM Corp.
 *  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.
 */
/*****************************************************************************
 * DGaussSampler.cpp - Sampling from non-spherical discrete Gaussian.
 *   A vector x is sampled one entry at a time, each entry sampled from
 *   the marginal distribution, conditioned on all previous entries.
 ******************************************************************************/
#include <NTL/ZZ.h>
#include <NTL/mat_lzz_p.h>
#include <limits>
#include <NTL/BasicThreadPool.h>
#include <NTL/ZZ.h>
#include <NTL/FFT.h>
#include <NTL/SmartPtr.h>

NTL_CLIENT
#include "mat_l.h"
#include "vec_l.h"
#include "DGaussSampler.h"
#include "Gaussian1Dsampler.h"
#include "utils/tools.h"

#ifdef NTL_HAVE_AVX
#warning "HAVE_AVX"

#include <immintrin.h>

#ifdef NTL_HAVE_FMA
#define MUL_ADD(a, b, c) a = _mm256_fmadd_pd(b, c, a)
#else
#define MUL_ADD(a, b, c) a = _mm256_add_pd(a, _mm256_mul_pd(b, c))
#endif
#endif

// Choose a small dimension-n integer vector
void setSmall(vec_l& u, long n, double sigma)
{
    FHE_TIMER_START;
    u.SetLength(n);
    NTL::Pair<bool,long> extra(false,0);

    // Choose a small noise vector
    Gaussian1Dsampler gSampler(sigma);
    for (long i=0; i<n; i++)
      u[i] = gSampler.getSample(/*mu=*/0.0, extra);
}

// Choose a small n-by-m integer matrix
void setSmall(mat_l& E, long n, long m, double sigma)
{
    FHE_TIMER_START;
    E.SetDims(n, m); // the noise n-by-m matrix E

    // Choose a small noise matrix
    Gaussian1Dsampler gSampler(sigma);

    EXEC_RANGE(m, first, last)

    NTL::Pair<bool,long> extra(false,0);
    for (long j=first; j<last; j++)
    for (long i=0; i<n; i++){
        //for (long j=0; j<m; j++){
	E[i][j] = gSampler.getSample(/*mu=*/0.0, extra);
      }
      EXEC_RANGE_END
}

// Initialize to a convariance matrix, returns false on failure. The i'th
// vector in condSigmaV is the top row of the conditional covariance matrix
// of entries i,...,n ocnditioned on 1,...,i-1. Also initializes a 1D sample
// with variance condSigmaV[i][i] for each entry i.

#ifdef NTL_HAVE_AVX
// ******* AVX code
// Most of this was taken verbatim from NTL's mat_lzz_p.c

#define MAT_BLK_SZ (32)

#define PAR_THRESH_SQ (200)
#define PAR_THRESH (40000)

// MUL_ADD(a, b, c): a += b*c
#ifdef NTL_HAVE_FMA
#define MUL_ADD(a, b, c) a = _mm256_fmadd_pd(b, c, a)
#else
#define MUL_ADD(a, b, c) a = _mm256_add_pd(a, _mm256_mul_pd(b, c))
#endif

static
void muladd1_by_32(double *x, const double *a, const double *b, long n)
{
   __m256d acc0=_mm256_load_pd(x + 0*4);
   __m256d acc1=_mm256_load_pd(x + 1*4);
   __m256d acc2=_mm256_load_pd(x + 2*4);
   __m256d acc3=_mm256_load_pd(x + 3*4);
   __m256d acc4=_mm256_load_pd(x + 4*4);
   __m256d acc5=_mm256_load_pd(x + 5*4);
   __m256d acc6=_mm256_load_pd(x + 6*4);
   __m256d acc7=_mm256_load_pd(x + 7*4);

   long i = 0;
   for (; i <= n-4; i +=4) {

      // the following code sequences are a bit faster than
      // just doing 4 _mm256_broadcast_sd's
      // it requires a to point to aligned storage, however

     // this one seems slightly faster
      __m256d a0101 = _mm256_broadcast_pd((const __m128d*)(a+0));
      __m256d a2323 = _mm256_broadcast_pd((const __m128d*)(a+2));

      __m256d avec0 = _mm256_permute_pd(a0101, 0);
      __m256d avec1 = _mm256_permute_pd(a0101, 0xf);
      __m256d avec2 = _mm256_permute_pd(a2323, 0);
      __m256d avec3 = _mm256_permute_pd(a2323, 0xf);

      a += 4;

      __m256d bvec;

      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc0, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc1, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc2, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc3, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc4, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc5, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc6, avec0, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc7, avec0, bvec);

      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc0, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc1, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc2, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc3, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc4, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc5, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc6, avec1, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc7, avec1, bvec);

      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc0, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc1, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc2, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc3, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc4, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc5, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc6, avec2, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc7, avec2, bvec);

      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc0, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc1, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc2, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc3, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc4, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc5, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc6, avec3, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc7, avec3, bvec);
   }

   for (; i < n; i++) {
      __m256d avec = _mm256_broadcast_sd(a); a++;
      __m256d bvec;

      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc0, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc1, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc2, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc3, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc4, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc5, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc6, avec, bvec);
      bvec = _mm256_load_pd(b); b += 4; MUL_ADD(acc7, avec, bvec);
   }

   _mm256_store_pd(x + 0*4, acc0);
   _mm256_store_pd(x + 1*4, acc1);
   _mm256_store_pd(x + 2*4, acc2);
   _mm256_store_pd(x + 3*4, acc3);
   _mm256_store_pd(x + 4*4, acc4);
   _mm256_store_pd(x + 5*4, acc5);
   _mm256_store_pd(x + 6*4, acc6);
   _mm256_store_pd(x + 7*4, acc7);
}

// experiment: process two rows at a time
#ifndef NTL_HAVE_FMA
static
void muladd2_by_32(double *x, const double *a, const double *b, long n)
{
   __m256d avec0, avec1, bvec;
   __m256d acc00, acc01, acc02, acc03;
   __m256d acc10, acc11, acc12, acc13;

   // round 0

   acc00=_mm256_load_pd(x + 0*4 + 0*MAT_BLK_SZ);
   acc01=_mm256_load_pd(x + 1*4 + 0*MAT_BLK_SZ);
   acc02=_mm256_load_pd(x + 2*4 + 0*MAT_BLK_SZ);
   acc03=_mm256_load_pd(x + 3*4 + 0*MAT_BLK_SZ);

   acc10=_mm256_load_pd(x + 0*4 + 1*MAT_BLK_SZ);
   acc11=_mm256_load_pd(x + 1*4 + 1*MAT_BLK_SZ);
   acc12=_mm256_load_pd(x + 2*4 + 1*MAT_BLK_SZ);
   acc13=_mm256_load_pd(x + 3*4 + 1*MAT_BLK_SZ);

   for (long i = 0; i < n; i++) {
      avec0 = _mm256_broadcast_sd(&a[i]);
      avec1 = _mm256_broadcast_sd(&a[i+MAT_BLK_SZ]);

      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+0*4]); MUL_ADD(acc00, avec0, bvec); MUL_ADD(acc10, avec1, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+1*4]); MUL_ADD(acc01, avec0, bvec); MUL_ADD(acc11, avec1, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+2*4]); MUL_ADD(acc02, avec0, bvec); MUL_ADD(acc12, avec1, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+3*4]); MUL_ADD(acc03, avec0, bvec); MUL_ADD(acc13, avec1, bvec);
   }

   _mm256_store_pd(x + 0*4 + 0*MAT_BLK_SZ, acc00);
   _mm256_store_pd(x + 1*4 + 0*MAT_BLK_SZ, acc01);
   _mm256_store_pd(x + 2*4 + 0*MAT_BLK_SZ, acc02);
   _mm256_store_pd(x + 3*4 + 0*MAT_BLK_SZ, acc03);

   _mm256_store_pd(x + 0*4 + 1*MAT_BLK_SZ, acc10);
   _mm256_store_pd(x + 1*4 + 1*MAT_BLK_SZ, acc11);
   _mm256_store_pd(x + 2*4 + 1*MAT_BLK_SZ, acc12);
   _mm256_store_pd(x + 3*4 + 1*MAT_BLK_SZ, acc13);

   // round 1

   acc00=_mm256_load_pd(x + 4*4 + 0*MAT_BLK_SZ);
   acc01=_mm256_load_pd(x + 5*4 + 0*MAT_BLK_SZ);
   acc02=_mm256_load_pd(x + 6*4 + 0*MAT_BLK_SZ);
   acc03=_mm256_load_pd(x + 7*4 + 0*MAT_BLK_SZ);

   acc10=_mm256_load_pd(x + 4*4 + 1*MAT_BLK_SZ);
   acc11=_mm256_load_pd(x + 5*4 + 1*MAT_BLK_SZ);
   acc12=_mm256_load_pd(x + 6*4 + 1*MAT_BLK_SZ);
   acc13=_mm256_load_pd(x + 7*4 + 1*MAT_BLK_SZ);

   for (long i = 0; i < n; i++) {
      avec0 = _mm256_broadcast_sd(&a[i]);
      avec1 = _mm256_broadcast_sd(&a[i+MAT_BLK_SZ]);

      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+0*4+MAT_BLK_SZ/2]); MUL_ADD(acc00, avec0, bvec); MUL_ADD(acc10, avec1, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+1*4+MAT_BLK_SZ/2]); MUL_ADD(acc01, avec0, bvec); MUL_ADD(acc11, avec1, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+2*4+MAT_BLK_SZ/2]); MUL_ADD(acc02, avec0, bvec); MUL_ADD(acc12, avec1, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+3*4+MAT_BLK_SZ/2]); MUL_ADD(acc03, avec0, bvec); MUL_ADD(acc13, avec1, bvec);
   }

   _mm256_store_pd(x + 4*4 + 0*MAT_BLK_SZ, acc00);
   _mm256_store_pd(x + 5*4 + 0*MAT_BLK_SZ, acc01);
   _mm256_store_pd(x + 6*4 + 0*MAT_BLK_SZ, acc02);
   _mm256_store_pd(x + 7*4 + 0*MAT_BLK_SZ, acc03);

   _mm256_store_pd(x + 4*4 + 1*MAT_BLK_SZ, acc10);
   _mm256_store_pd(x + 5*4 + 1*MAT_BLK_SZ, acc11);
   _mm256_store_pd(x + 6*4 + 1*MAT_BLK_SZ, acc12);
   _mm256_store_pd(x + 7*4 + 1*MAT_BLK_SZ, acc13);

}
#endif

// experiment: process three rows at a time
// NOTE: this makes things slower on an AVX1 platform --- not enough registers
// it could be faster on AVX2/FMA, where there should be enough registers

static
void muladd3_by_32(double *x, const double *a, const double *b, long n)
{
   __m256d avec0, avec1, avec2, bvec;
   __m256d acc00, acc01, acc02, acc03;
   __m256d acc10, acc11, acc12, acc13;
   __m256d acc20, acc21, acc22, acc23;

   // round 0

   acc00=_mm256_load_pd(x + 0*4 + 0*MAT_BLK_SZ);
   acc01=_mm256_load_pd(x + 1*4 + 0*MAT_BLK_SZ);
   acc02=_mm256_load_pd(x + 2*4 + 0*MAT_BLK_SZ);
   acc03=_mm256_load_pd(x + 3*4 + 0*MAT_BLK_SZ);

   acc10=_mm256_load_pd(x + 0*4 + 1*MAT_BLK_SZ);
   acc11=_mm256_load_pd(x + 1*4 + 1*MAT_BLK_SZ);
   acc12=_mm256_load_pd(x + 2*4 + 1*MAT_BLK_SZ);
   acc13=_mm256_load_pd(x + 3*4 + 1*MAT_BLK_SZ);

   acc20=_mm256_load_pd(x + 0*4 + 2*MAT_BLK_SZ);
   acc21=_mm256_load_pd(x + 1*4 + 2*MAT_BLK_SZ);
   acc22=_mm256_load_pd(x + 2*4 + 2*MAT_BLK_SZ);
   acc23=_mm256_load_pd(x + 3*4 + 2*MAT_BLK_SZ);

   for (long i = 0; i < n; i++) {
      avec0 = _mm256_broadcast_sd(&a[i]);
      avec1 = _mm256_broadcast_sd(&a[i+MAT_BLK_SZ]);
      avec2 = _mm256_broadcast_sd(&a[i+2*MAT_BLK_SZ]);

      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+0*4]); MUL_ADD(acc00, avec0, bvec); MUL_ADD(acc10, avec1, bvec); MUL_ADD(acc20, avec2, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+1*4]); MUL_ADD(acc01, avec0, bvec); MUL_ADD(acc11, avec1, bvec); MUL_ADD(acc21, avec2, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+2*4]); MUL_ADD(acc02, avec0, bvec); MUL_ADD(acc12, avec1, bvec); MUL_ADD(acc22, avec2, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+3*4]); MUL_ADD(acc03, avec0, bvec); MUL_ADD(acc13, avec1, bvec); MUL_ADD(acc23, avec2, bvec);
   }

   _mm256_store_pd(x + 0*4 + 0*MAT_BLK_SZ, acc00);
   _mm256_store_pd(x + 1*4 + 0*MAT_BLK_SZ, acc01);
   _mm256_store_pd(x + 2*4 + 0*MAT_BLK_SZ, acc02);
   _mm256_store_pd(x + 3*4 + 0*MAT_BLK_SZ, acc03);

   _mm256_store_pd(x + 0*4 + 1*MAT_BLK_SZ, acc10);
   _mm256_store_pd(x + 1*4 + 1*MAT_BLK_SZ, acc11);
   _mm256_store_pd(x + 2*4 + 1*MAT_BLK_SZ, acc12);
   _mm256_store_pd(x + 3*4 + 1*MAT_BLK_SZ, acc13);

   _mm256_store_pd(x + 0*4 + 2*MAT_BLK_SZ, acc20);
   _mm256_store_pd(x + 1*4 + 2*MAT_BLK_SZ, acc21);
   _mm256_store_pd(x + 2*4 + 2*MAT_BLK_SZ, acc22);
   _mm256_store_pd(x + 3*4 + 2*MAT_BLK_SZ, acc23);

   // round 1

   acc00=_mm256_load_pd(x + 4*4 + 0*MAT_BLK_SZ);
   acc01=_mm256_load_pd(x + 5*4 + 0*MAT_BLK_SZ);
   acc02=_mm256_load_pd(x + 6*4 + 0*MAT_BLK_SZ);
   acc03=_mm256_load_pd(x + 7*4 + 0*MAT_BLK_SZ);

   acc10=_mm256_load_pd(x + 4*4 + 1*MAT_BLK_SZ);
   acc11=_mm256_load_pd(x + 5*4 + 1*MAT_BLK_SZ);
   acc12=_mm256_load_pd(x + 6*4 + 1*MAT_BLK_SZ);
   acc13=_mm256_load_pd(x + 7*4 + 1*MAT_BLK_SZ);

   acc20=_mm256_load_pd(x + 4*4 + 2*MAT_BLK_SZ);
   acc21=_mm256_load_pd(x + 5*4 + 2*MAT_BLK_SZ);
   acc22=_mm256_load_pd(x + 6*4 + 2*MAT_BLK_SZ);
   acc23=_mm256_load_pd(x + 7*4 + 2*MAT_BLK_SZ);

   for (long i = 0; i < n; i++) {
      avec0 = _mm256_broadcast_sd(&a[i]);
      avec1 = _mm256_broadcast_sd(&a[i+MAT_BLK_SZ]);
      avec2 = _mm256_broadcast_sd(&a[i+2*MAT_BLK_SZ]);

      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+0*4+MAT_BLK_SZ/2]); MUL_ADD(acc00, avec0, bvec); MUL_ADD(acc10, avec1, bvec); MUL_ADD(acc20, avec2, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+1*4+MAT_BLK_SZ/2]); MUL_ADD(acc01, avec0, bvec); MUL_ADD(acc11, avec1, bvec); MUL_ADD(acc21, avec2, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+2*4+MAT_BLK_SZ/2]); MUL_ADD(acc02, avec0, bvec); MUL_ADD(acc12, avec1, bvec); MUL_ADD(acc22, avec2, bvec);
      bvec = _mm256_load_pd(&b[i*MAT_BLK_SZ+3*4+MAT_BLK_SZ/2]); MUL_ADD(acc03, avec0, bvec); MUL_ADD(acc13, avec1, bvec); MUL_ADD(acc23, avec2, bvec);
   }

   _mm256_store_pd(x + 4*4 + 0*MAT_BLK_SZ, acc00);
   _mm256_store_pd(x + 5*4 + 0*MAT_BLK_SZ, acc01);
   _mm256_store_pd(x + 6*4 + 0*MAT_BLK_SZ, acc02);
   _mm256_store_pd(x + 7*4 + 0*MAT_BLK_SZ, acc03);

   _mm256_store_pd(x + 4*4 + 1*MAT_BLK_SZ, acc10);
   _mm256_store_pd(x + 5*4 + 1*MAT_BLK_SZ, acc11);
   _mm256_store_pd(x + 6*4 + 1*MAT_BLK_SZ, acc12);
   _mm256_store_pd(x + 7*4 + 1*MAT_BLK_SZ, acc13);

   _mm256_store_pd(x + 4*4 + 2*MAT_BLK_SZ, acc20);
   _mm256_store_pd(x + 5*4 + 2*MAT_BLK_SZ, acc21);
   _mm256_store_pd(x + 6*4 + 2*MAT_BLK_SZ, acc22);
   _mm256_store_pd(x + 7*4 + 2*MAT_BLK_SZ, acc23);

}

static inline
void muladd_all_by_32(long first, long last, double *x, const double *a, const double *b, long n)
{
   long i = first;
#ifdef NTL_HAVE_FMA
   // processing three rows at a time is faster
   for (; i <= last-3; i+=3)
      muladd3_by_32(x + i*MAT_BLK_SZ, a + i*MAT_BLK_SZ, b, n);
   for (; i < last; i++)
      muladd1_by_32(x + i*MAT_BLK_SZ, a + i*MAT_BLK_SZ, b, n);
#else
   // process only two rows at a time: not enough registers :-(
   for (; i <= last-2; i+=2)
      muladd2_by_32(x + i*MAT_BLK_SZ, a + i*MAT_BLK_SZ, b, n);
   for (; i < last; i++)
      muladd1_by_32(x + i*MAT_BLK_SZ, a + i*MAT_BLK_SZ, b, n);
#endif
}

// this assumes n is a multiple of 16
static inline
void muladd_interval(double * NTL_RESTRICT x, double * NTL_RESTRICT y, double c, long n)
{
   __m256d xvec0, xvec1, xvec2, xvec3;
   __m256d yvec0, yvec1, yvec2, yvec3;

   __m256d cvec = _mm256_broadcast_sd(&c);

   for (long i = 0; i < n; i += 16, x += 16, y += 16) {
      xvec0 = _mm256_load_pd(x+0*4);
      xvec1 = _mm256_load_pd(x+1*4);
      xvec2 = _mm256_load_pd(x+2*4);
      xvec3 = _mm256_load_pd(x+3*4);

      yvec0 = _mm256_load_pd(y+0*4);
      yvec1 = _mm256_load_pd(y+1*4);
      yvec2 = _mm256_load_pd(y+2*4);
      yvec3 = _mm256_load_pd(y+3*4);

      MUL_ADD(xvec0, yvec0, cvec);
      MUL_ADD(xvec1, yvec1, cvec);
      MUL_ADD(xvec2, yvec2, cvec);
      MUL_ADD(xvec3, yvec3, cvec);

      _mm256_store_pd(x + 0*4, xvec0);
      _mm256_store_pd(x + 1*4, xvec1);
      _mm256_store_pd(x + 2*4, xvec2);
      _mm256_store_pd(x + 3*4, xvec3);
   }
}

static
bool Alt_sampler(Vec< Vec<double> >& X, const Mat<long>& A)
{
   FHE_TIMER_START;

   long n = A.NumRows();

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

   if (NTL_OVERFLOW(n, MAT_BLK_SZ, 0)) ResourceError("dimension too large");

   long npanels = (n+MAT_BLK_SZ-1)/MAT_BLK_SZ;

   Vec< AlignedArray<double> > M;
   M.SetLength(npanels);
   for (long panel = 0; panel < npanels; panel++) {
      M[panel].SetLength(n*MAT_BLK_SZ);
      double *panelp = &M[panel][0];

      for (long r = 0; r < n*MAT_BLK_SZ; r++) panelp[r] = 0;
   }

   // copy A into panels
   for (long jj = 0, panel = 0; jj < n; jj += MAT_BLK_SZ, panel++) {
      long j_max = min(jj+MAT_BLK_SZ, n);
      double *panelp = &M[panel][0];

      for (long i = 0; i < n; i++, panelp += MAT_BLK_SZ) {
         const long *ap = A[i].elts() + jj;

         for (long j = jj; j < j_max; j++)
            panelp[j-jj] = ap[j-jj];
      }
   }

   X.SetLength(n);

   for (long kk = 0, kpanel = 0; kk < n; kk += MAT_BLK_SZ, kpanel++) {
      long k_max = min(kk+MAT_BLK_SZ, n);

      double * NTL_RESTRICT kpanelp = &M[kpanel][0];

      for (long k = kk; k < k_max; k++) {

         X[k].SetLength(n-k);
         double * NTL_RESTRICT Xk = X[k].elts();
         for (long i = k; i < n; i++)
            Xk[i-k] = kpanelp[i*MAT_BLK_SZ + (k-kk)];

         double * NTL_RESTRICT y = &kpanelp[k*MAT_BLK_SZ];
         double pivot = y[k-kk];
         y[k-kk] = 1;

         if (pivot <= 0) {
            return false;
         }

         double pivot_inv = 1/pivot;

         for (long i = k+1; i < n; i++) {
            double * NTL_RESTRICT x = &kpanelp[i*MAT_BLK_SZ];
            double t1 = -x[k-kk]*pivot_inv;
            x[k-kk] = 0;
            muladd_interval(x, y, t1, MAT_BLK_SZ);
         }

         for (long j = k; j < k_max; j++) y[j-kk] = 0;
      }

      // finished processing current kpanel
      // next, reduce and apply to all other kpanels

      bool seq = double(npanels-(kpanel+1))*double(n)*double(MAT_BLK_SZ)*double(MAT_BLK_SZ) < PAR_THRESH;

      NTL_GEXEC_RANGE(seq, npanels-(kpanel+1), first, last)
      NTL_IMPORT(n)
      NTL_IMPORT(kpanel)
      NTL_IMPORT(kpanelp)
      NTL_IMPORT(kk)
      NTL_IMPORT(k_max)

      AlignedArray<double> buf_store;
      buf_store.SetLength(MAT_BLK_SZ*MAT_BLK_SZ);
      double *buf = &buf_store[0];

      for (long index = first; index < last; index++) {
         long jpanel = index + kpanel+1;

         double * NTL_RESTRICT jpanelp = &M[jpanel][0];

         // copy block number kpanel (the one on the diagonal)  into buf

         for (long i = 0; i < (k_max-kk)*MAT_BLK_SZ; i++)
            buf[i] = jpanelp[kk*MAT_BLK_SZ+i];

         // jpanel += kpanel*buf

         muladd_all_by_32(kk, n, jpanelp, kpanelp, buf, k_max-kk);
      }

      NTL_GEXEC_RANGE_END

   }

   return true;

}
#endif

//create the nested covariance matrices
static
bool Plain_sampler(Vec< Vec<double> >& X, const Mat<long>& A)
{
    FHE_TIMER_START;

    if (A[0][0] < 0)   //sigma too small
    {
        return false; // failed, variance cannot be negative
    }

    long n = A.NumRows();

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

    X.SetLength(n); // allocate space for n vectors

    Mat<double> M;
    conv(M, A); // convert to floating point

    // 1st vector of the conditional covariance = 1st column of covariance
    X[0].SetLength(n);
    for (long k = 0; k < n; k++)
        X[0][k] = M[k][0];

    // Compute the rest of the conditional covariance, one vector at a time

    for (long k = 1; k<n; k++)
    {
        // create the new covariance: sig y|x = sig_yy - sig_yx *sig_xx^{-1} *sig_xy

        //get the subset covariance matrix

        NTL_GEXEC_RANGE(n-k < 100, n-k, first, last)

        NTL_IMPORT(n)
        NTL_IMPORT(k)

        double * NTL_RESTRICT M_0 = &M[k-1][0];
        double pivot = M_0[k-1];
        double pivot_inv = 1.0/pivot;

        for (long row = first; row < last; row++) {
            double * NTL_RESTRICT M_r = &M[k+row][0];
            double fac = -M_r[k-1]*pivot_inv;

            for (long col = k; col < n; col++) {
                M_r[col] += fac * (M_0[col]);
            }
        }

        NTL_GEXEC_RANGE_END

        if (M[k][k] < 0)
        {
            return false; //failed, sigma too small
        }

        // copy the result to the next conditional covariance vector
        X[k].SetLength(n-k);
        for (long i = 0; i < n-k; i++)
            X[k][i] = M[k+i][k];
    }

    return true;
}

bool DiscreteGaussianSampler::InitSampler(const Mat<long> &covMat)
{
    FHE_TIMER_START;

    n = covMat.NumRows();

    bool res = false;

#ifdef NTL_HAVE_AVX
#warning "using AVX code"
   if (n >= 512) {
      res = Alt_sampler(condSigmaV, covMat);
   }
   else
#endif
   {
      res = Plain_sampler(condSigmaV, covMat);
   }

   if (!res) return false;

    //clear if there are previous 1D-samplers
    for (long i = 0; i < oneDsamplers.length(); i++)
        if (oneDsamplers[i] != NULL) delete oneDsamplers[i];

    // create table of 1D-samplers
    oneDsamplers.SetLength(n);

    for (long iVar=0; iVar<n; iVar++)
    {
        if (condSigmaV[iVar][0] < 0)
        {
            cout << "conditional sigma too small" << endl;
            return false; //conditional sigma too small
            }

        oneDsamplers[iVar] =            //create a 1D sampler for this sigma
        (Gaussian1Dsampler*) new Gaussian1Dsampler(sqrt(condSigmaV[iVar][0]));

    }

    return true;
}

// FIXME: better to use UniquePtr or something to manage the entries
// in oneDsamplers, rather than new/delete

//delete the array of sampler instances
DiscreteGaussianSampler::~DiscreteGaussianSampler()
{
    FHE_TIMER_START;
    for (long i = 0; i < oneDsamplers.length(); i++)
        if (oneDsamplers[i]) delete oneDsamplers[i];
}

// Discrete sampling according to the conditional variance and mean. Choose
//each entry in x depending on the conditioned distribution of previous ones.

void DiscreteGaussianSampler::SampleDiscreteGaussian(vec_l& xOut, const Vec<double>& meanVec) const
{
    FHE_TIMER_START;

    //for this val, find the rest of the values

    //x,y are matrices
    //m y|x = m_y + sig_yx * sig ^-1_xx (x - m_x)
    //multiple matrices

    // scratch variables for internal computation
    Vec<double> marginalMeanVec = meanVec;
    Vec<double> nextMarginalMeanVec;

    // work with pointers for easier swapping
    Vec<double>* curMeanVec = &marginalMeanVec;
    Vec<double>* nextMeanVec = &nextMarginalMeanVec;

    //find marginal distribution for each variable

    xOut.SetLength(n);
    for (long iVar = 0; iVar<n; iVar++)
    {
        //get the next sample
        double mean = (*curMeanVec)[0];
        xOut[iVar] = oneDsamplers[iVar]->getSample(mean);

        // update the mean vector:
        //    m(i|j) = m(i) + sigma(i,j)*sigma(j,j)^-1*(x(j) - m(j))
        // only need the first element for the next item

        // Next mean vector has one dimension less
        nextMeanVec->SetLength(curMeanVec->length() -1);

        double diff = xOut[iVar] - mean;  // deviation from mean

        // Look at the next vector from the conditional covariance
        const Vec<double>& covVec = condSigmaV[iVar];

        // newMean = curMean[1..n-1] + covVec * diff / sigma_XX

        double invSig = 1.0 / covVec[0]; // 1/sigma_XX
        for (long row = 0; row<nextMeanVec->length(); row++)
            (*nextMeanVec)[row]
                = (*curMeanVec)[row+1] + (covVec[row+1] *invSig) *diff;

        // Set the updated mean vector as the current one
        swap(nextMeanVec,curMeanVec); // pointer swap
    }
}

long DiscreteGaussianSampler::writeToFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = fwrite(&n,sizeof(long),1,handle); // write dimension

    long nConvSig = condSigmaV.length();
    count += fwrite(&nConvSig,sizeof(long),1,handle);

    for (long i = 0; i < nConvSig; i++)
    {
    long nconvSigmaVi = condSigmaV[i].length();
    count += fwrite(&nconvSigmaVi,sizeof(long),1,handle);
    count += fwrite(condSigmaV[i].elts(),condSigmaV[i].length()*sizeof(double),1,handle);
    }

    long oLength = oneDsamplers.length();
    count += fwrite(&oLength,sizeof(long),1,handle);

    for (long i = 0; i < oLength; i++)
        count+= oneDsamplers[i]->writeToFile(handle);

    return count;
}

long DiscreteGaussianSampler::readFromFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = fread(&n,sizeof(long),1,handle); // read dimension

    long nConvSig;
    count += fread(&nConvSig,sizeof(long),1,handle);

    condSigmaV.SetLength(nConvSig);

    for (long i = 0; i < nConvSig; i++)
    {
    long nconvSigmaVi;
    count += fread(&nconvSigmaVi,sizeof(long),1,handle);
    condSigmaV[i].SetLength(nconvSigmaVi);
    count += fread(condSigmaV[i].elts(),nconvSigmaVi*sizeof(double),1,handle);
    }

    long oLength;
    count += fread(&oLength,sizeof(long),1,handle);
    oneDsamplers.SetLength(oLength);

    for (long iVar=0; iVar<oLength; iVar++)
    {
        oneDsamplers[iVar] =            //create a 1D sampler for this sigma
            (Gaussian1Dsampler*) new Gaussian1Dsampler(sqrt(condSigmaV[iVar][0]));
    }

    for (long i = 0; i < oLength; i++)
        count+= oneDsamplers[i]->readFromFile(handle);

    return count;
}


#ifndef BOOST_GEOMETRY_PROJECTIONS_OB_TRAN_HPP
#define BOOST_GEOMETRY_PROJECTIONS_OB_TRAN_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

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

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

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

#include <boost/geometry/util/math.hpp>
#include <boost/shared_ptr.hpp>

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>
#include <boost/geometry/extensions/gis/projections/impl/aasincos.hpp>

namespace boost { namespace geometry { namespace projections
{

    template <typename Geographic, typename Cartesian, typename Parameters> class factory;

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace ob_tran
    {

            static const double TOL = 1e-10;

            template <typename Geographic, typename Cartesian>
            struct par_ob_tran
            {
                boost::shared_ptr<projection<Geographic, Cartesian> > link;
                double    lamp;
                double    cphip, sphip;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_ob_tran_oblique : public base_t_fi<base_ob_tran_oblique<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_ob_tran<Geographic, Cartesian> m_proj_parm;

                inline base_ob_tran_oblique(const Parameters& par)
                    : base_t_fi<base_ob_tran_oblique<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(o_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double coslam, sinphi, cosphi;

                    coslam = cos(lp_lon);
                    sinphi = sin(lp_lat);
                    cosphi = cos(lp_lat);
                    lp_lon = adjlon(aatan2(cosphi * sin(lp_lon), this->m_proj_parm.sphip * cosphi * coslam +
                        this->m_proj_parm.cphip * sinphi) + this->m_proj_parm.lamp);
                    lp_lat = aasin(this->m_proj_parm.sphip * sinphi - this->m_proj_parm.cphip * cosphi * coslam);
                    m_proj_parm.link->fwd(lp_lon, lp_lat, xy_x, xy_y);
                }

                // INVERSE(o_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    double coslam, sinphi, cosphi;

                    m_proj_parm.link->inv(xy_x, xy_y, lp_lon, lp_lat);
                    if (lp_lon != HUGE_VAL) {
                        coslam = cos(lp_lon -= this->m_proj_parm.lamp);
                        sinphi = sin(lp_lat);
                        cosphi = cos(lp_lat);
                        lp_lat = aasin(this->m_proj_parm.sphip * sinphi + this->m_proj_parm.cphip * cosphi * coslam);
                        lp_lon = aatan2(cosphi * sin(lp_lon), this->m_proj_parm.sphip * cosphi * coslam -
                            this->m_proj_parm.cphip * sinphi);
                    }
                }

                static inline std::string get_name()
                {
                    return "ob_tran_oblique";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_ob_tran_transverse : public base_t_fi<base_ob_tran_transverse<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_ob_tran<Geographic, Cartesian> m_proj_parm;

                inline base_ob_tran_transverse(const Parameters& par)
                    : base_t_fi<base_ob_tran_transverse<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(t_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double cosphi, coslam;

                    cosphi = cos(lp_lat);
                    coslam = cos(lp_lon);
                    lp_lon = adjlon(aatan2(cosphi * sin(lp_lon), sin(lp_lat)) + this->m_proj_parm.lamp);
                    lp_lat = aasin(- cosphi * coslam);
                    m_proj_parm.link->fwd(lp_lon, lp_lat, xy_x, xy_y);
                }

                // INVERSE(t_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    double cosphi, t;

                    m_proj_parm.link->inv(xy_x, xy_y, lp_lon, lp_lat);
                    if (lp_lon != HUGE_VAL) {
                        cosphi = cos(lp_lat);
                        t = lp_lon - this->m_proj_parm.lamp;
                        lp_lon = aatan2(cosphi * sin(t), - sin(lp_lat));
                        lp_lat = aasin(cosphi * cos(t));
                    }
                }

                static inline std::string get_name()
                {
                    return "ob_tran_transverse";
                }

            };

            // General Oblique Transformation
            template <typename Geographic, typename Cartesian, typename Parameters>
            double setup_ob_tran(Parameters& par, par_ob_tran<Geographic, Cartesian>& proj_parm, bool create = true)
            {
                double phip;
                Parameters pj;

                /* get name of projection to be translated */
                pj.name = pj_param(par.params, "so_proj").s;
                /* copy existing header into new */
                par.es = 0.; /* force to spherical */
                pj.params = par.params;
                pj.over = par.over;
                pj.geoc = par.geoc;
                pj.a = par.a;
                pj.es = par.es;
                pj.ra = par.ra;
                pj.lam0 = par.lam0;
                pj.phi0 = par.phi0;
                pj.x0 = par.x0;
                pj.y0 = par.y0;
                pj.k0 = par.k0;
                /* force spherical earth */
                pj.one_es = pj.rone_es = 1.;
                pj.es = pj.e = 0.;

                if (create)
                {
                    factory<Geographic, Cartesian, Parameters> fac;
                    proj_parm.link.reset(fac.create_new(pj));
                    if (! proj_parm.link.get()) throw proj_exception(-26);
                }
                if (pj_param(par.params, "to_alpha").i) {
                    double lamc, phic, alpha;

                    lamc    = pj_param(par.params, "ro_lon_c").f;
                    phic    = pj_param(par.params, "ro_lat_c").f;
                    alpha    = pj_param(par.params, "ro_alpha").f;
            /*
                    if (fabs(phic) <= TOL ||
                        fabs(fabs(phic) - geometry::math::half_pi<double>()) <= TOL ||
                        fabs(fabs(alpha) - geometry::math::half_pi<double>()) <= TOL)
            */
                    if (fabs(fabs(phic) - geometry::math::half_pi<double>()) <= TOL)
                        throw proj_exception(-32);
                    proj_parm.lamp = lamc + aatan2(-cos(alpha), -sin(alpha) * sin(phic));
                    phip = aasin(cos(phic) * sin(alpha));
                } else if (pj_param(par.params, "to_lat_p").i) { /* specified new pole */
                    proj_parm.lamp = pj_param(par.params, "ro_lon_p").f;
                    phip = pj_param(par.params, "ro_lat_p").f;
                } else { /* specified new "equator" points */
                    double lam1, lam2, phi1, phi2, con;

                    lam1 = pj_param(par.params, "ro_lon_1").f;
                    phi1 = pj_param(par.params, "ro_lat_1").f;
                    lam2 = pj_param(par.params, "ro_lon_2").f;
                    phi2 = pj_param(par.params, "ro_lat_2").f;
                    if (fabs(phi1 - phi2) <= TOL ||
                        (con = fabs(phi1)) <= TOL ||
                        fabs(con - geometry::math::half_pi<double>()) <= TOL ||
                        fabs(fabs(phi2) - geometry::math::half_pi<double>()) <= TOL) throw proj_exception(-33);
                    proj_parm.lamp = atan2(cos(phi1) * sin(phi2) * cos(lam1) -
                        sin(phi1) * cos(phi2) * cos(lam2),
                        sin(phi1) * cos(phi2) * sin(lam2) -
                        cos(phi1) * sin(phi2) * sin(lam1));
                    phip = atan(-cos(proj_parm.lamp - lam1) / tan(phi1));
                }
                if (fabs(phip) > TOL) { /* oblique */
                    proj_parm.cphip = cos(phip);
                    proj_parm.sphip = sin(phip);
                } else { /* transverse */
                }
                // return phip to choose model
                return phip;
            }

        }} // namespace detail::ob_tran
    #endif // doxygen

    /*!
        \brief General Oblique Transformation projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Projection parameters
         - o_proj (string)
         - Plus projection parameters
         - o_lat_p (degrees)
         - o_lon_p (degrees)
         - New pole
         - o_alpha: Alpha (degrees)
         - o_lon_c (degrees)
         - o_lat_c (degrees)
         - o_lon_1 (degrees)
         - o_lat_1: Latitude of first standard parallel (degrees)
         - o_lon_2 (degrees)
         - o_lat_2: Latitude of second standard parallel (degrees)
        \par Example
        \image html ex_ob_tran.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct ob_tran_oblique : public detail::ob_tran::base_ob_tran_oblique<Geographic, Cartesian, Parameters>
    {
        inline ob_tran_oblique(const Parameters& par) : detail::ob_tran::base_ob_tran_oblique<Geographic, Cartesian, Parameters>(par)
        {
            detail::ob_tran::setup_ob_tran(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief General Oblique Transformation projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Projection parameters
         - o_proj (string)
         - Plus projection parameters
         - o_lat_p (degrees)
         - o_lon_p (degrees)
         - New pole
         - o_alpha: Alpha (degrees)
         - o_lon_c (degrees)
         - o_lat_c (degrees)
         - o_lon_1 (degrees)
         - o_lat_1: Latitude of first standard parallel (degrees)
         - o_lon_2 (degrees)
         - o_lat_2: Latitude of second standard parallel (degrees)
        \par Example
        \image html ex_ob_tran.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct ob_tran_transverse : public detail::ob_tran::base_ob_tran_transverse<Geographic, Cartesian, Parameters>
    {
        inline ob_tran_transverse(const Parameters& par) : detail::ob_tran::base_ob_tran_transverse<Geographic, Cartesian, Parameters>(par)
        {
            detail::ob_tran::setup_ob_tran(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class ob_tran_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    detail::ob_tran::par_ob_tran<Geographic, Cartesian> proj_parm;
                    Parameters p = par;
                    double phip = setup_ob_tran(p, proj_parm, false);

                    if (fabs(phip) > detail::ob_tran::TOL)
                        return new base_v_fi<ob_tran_oblique<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                    else
                        return new base_v_fi<ob_tran_transverse<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void ob_tran_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("ob_tran", new ob_tran_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_OB_TRAN_HPP


/*
Copyright 2009-2021 Nicolas Colombe

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

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

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

#include <gen/poissonsampling.hpp>
#include <math/geometrytraits.hpp>
#include <math/vector2.hpp>
#include <math/aabb2d.hpp>

#include <core/random.hpp>
#include <core/heapobject.hpp>

#include <boost/random.hpp>
//#include <boost/pool/pool_alloc.hpp>
#include <boost/geometry/index/rtree.hpp>

typedef std::pair<eXl::Vector2d, unsigned int> value;
typedef boost::geometry::index::rtree< value, boost::geometry::index::rstar<16, 4> > PointIndex;

namespace eXl
{

  static const unsigned int k_BootstrapLimit = 30;
  static const unsigned int k_GeneratedPoints = 30;

  //typedef boost::fast_pool_allocator<Vector2d,
  //  boost::default_user_allocator_new_delete,
  //  boost::details::pool::default_mutex,
  //  64, 128> allocator;

  typedef std::list<Vector2d/*, allocator*/> PointsStack;

  struct PoissonDiskSampling_Impl : public HeapObject
  {
    PoissonDiskSampling_Impl(Polygoni const& iPoly, Random& iGen) : m_PrecisePoly(iPoly), m_Gen(iGen)
    {
      //m_Stack.reserve(1024);
    }

    void AddPoint(Vector2d const& iPoint, float iCovering)
    {

      for(unsigned int i = 0; i < m_PointsIndex.size(); ++i)
      {
        double checkRadius = (m_Covering[i] + iCovering);
        AABB2Dd box = AABB2Dd::FromCenterAndSize(iPoint, Vector2d::ONE * checkRadius * 2);
        PointIndex const& index = m_PointsIndex[i];
        std::vector<value> returned_values;
        index.query(boost::geometry::index::within(box), std::back_inserter(returned_values));
        for(auto val : returned_values)
        {
          if((iPoint - val.first).Length() < checkRadius)
          {
            m_PointsValid[i][val.second] = false;
          }
        }
      }

      m_Stack.push_back(iPoint);
    }

    PointsStack m_Stack;

    std::vector<PointIndex>             m_PointsIndex;
    std::vector<std::vector<bool> >     m_PointsValid;
    std::vector<std::vector<Vector2d> > m_Points;
    std::vector<float>                  m_Covering;

    Polygond              m_PrecisePoly;
    Random&               m_Gen;
  };

  PoissonDiskSampling::PoissonDiskSampling(Polygoni const& iPoly, Random& iGen)
    :m_Impl(eXl_NEW PoissonDiskSampling_Impl(iPoly, iGen))
  {

  }

  PoissonDiskSampling::~PoissonDiskSampling()
  {
    eXl_DELETE m_Impl;
  }

  void PoissonDiskSampling::Sample(float iRadius, float iCovering, unsigned int iMaxPts)
  {
    Vector<Polygond> shrinkedPoly;
    {
      m_Impl->m_PrecisePoly.Shrink(iCovering, shrinkedPoly);
      if(shrinkedPoly.empty())
      {
        return;
      }
    }

    PointIndex curIndex;
    std::vector<Vector2d> curPoints;

    unsigned int numPts = 0;
    RandomWrapper rand(&m_Impl->m_Gen);

    for(auto poly : shrinkedPoly)
    {
      boost::random::uniform_real_distribution<double> distribX(poly.GetAABB().m_Data[0].X(), poly.GetAABB().m_Data[1].X());
      boost::random::uniform_real_distribution<double> distribY(poly.GetAABB().m_Data[0].Y(), poly.GetAABB().m_Data[1].Y());

      for(unsigned int i = 0; i<k_BootstrapLimit; ++i)
      {
        Vector2d origPt(distribX(rand),distribY(rand));
        if(poly.ContainsPoint(origPt))
        {
          m_Impl->AddPoint(origPt, iCovering);
          curIndex.insert(std::make_pair(origPt, numPts));
          curPoints.push_back(origPt);
          ++numPts;
          break;
        }
      }

      while(!m_Impl->m_Stack.empty()
      && (iMaxPts == 0 || numPts < iMaxPts))
      {
        boost::random::uniform_real_distribution<double> distribTheta(-Mathd::PI, Mathd::PI);
        Vector2d origPt = m_Impl->m_Stack.front();
        m_Impl->m_Stack.pop_front();

        for(unsigned int i = 0; i<k_GeneratedPoints; ++i)
        {
          double angle = distribTheta(rand);
          Vector2d point = origPt + Vector2d(Mathd::Cos(angle), Mathd::Sin(angle)) * iRadius * 2;
          if(poly.ContainsPoint(point))
          {
            AABB2Dd box = AABB2Dd::FromCenterAndSize(point, Vector2d(iRadius, iRadius) * 4);
            bool validPoint = true;
            std::vector<value> returned_values;
            curIndex.query(boost::geometry::index::within(box), std::back_inserter(returned_values));

            for(auto val : returned_values)
            {
              if((point - val.first).Length() < 2*iRadius)
              {
                validPoint = false;
                break;
              }
            }
            if(validPoint)
            {
              m_Impl->AddPoint(point, iCovering);
              curIndex.insert(std::make_pair(point, numPts));
              curPoints.push_back(point);
              ++numPts;
            }
          }
        }
      }
    }

    m_Impl->m_Covering.push_back(iCovering);
    m_Impl->m_Points.push_back(std::vector<Vector2d>());
    m_Impl->m_PointsValid.push_back(std::vector<bool>());
    m_Impl->m_PointsIndex.push_back(PointIndex());

    m_Impl->m_Points.back().swap(curPoints);
    m_Impl->m_PointsIndex.back().swap(curIndex);
    m_Impl->m_PointsValid.back().resize(numPts, true);

  }

  unsigned int PoissonDiskSampling::GetNumLayers() const
  {
    return m_Impl->m_PointsIndex.size();
  }

  void PoissonDiskSampling::GetLayer(unsigned int iLayer, Vector<Vector2d>& oPoints) const
  {
    oPoints.clear();
    if(iLayer < m_Impl->m_PointsIndex.size())
    {
      for(unsigned int i = 0; i<m_Impl->m_Points[iLayer].size(); ++i)
      {
        if(m_Impl->m_PointsValid[iLayer][i])
        {
          oPoints.push_back(m_Impl->m_Points[iLayer][i]);
        }
      }
    }
  }

}


//=============================================================================
// Copyright (C) 2011-2018 The pmp-library developers
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice, this
//   list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
// * Neither the name of the copyright holder nor the names of its
//   contributors may be used to endorse or promote products derived from
//   this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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.
//=============================================================================

#include <pmp/algorithms/SurfaceFairing.h>
#include <pmp/algorithms/DifferentialGeometry.h>

#include <utility>

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

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

namespace pmp {

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

using SparseMatrix = Eigen::SparseMatrix<double>;
using Triplet = Eigen::Triplet<double>;

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

SurfaceFairing::SurfaceFairing(SurfaceMesh& mesh) : m_mesh(mesh)
{
    // get & add properties
    m_points = m_mesh.vertexProperty<Point>("v:point");
    m_vselected = m_mesh.getVertexProperty<bool>("v:selected");
    m_vlocked = m_mesh.addVertexProperty<bool>("fairing:locked");
    m_vweight = m_mesh.addVertexProperty<double>("fairing:vweight");
    m_eweight = m_mesh.addEdgeProperty<double>("fairing:eweight");
    m_idx = m_mesh.addVertexProperty<int>("fairing:idx", -1);
}

//-----------------------------------------------------------------------------

SurfaceFairing::~SurfaceFairing()
{
    // remove properties
    m_mesh.removeVertexProperty(m_vlocked);
    m_mesh.removeVertexProperty(m_vweight);
    m_mesh.removeEdgeProperty(m_eweight);
    m_mesh.removeVertexProperty(m_idx);
}

//-----------------------------------------------------------------------------

void SurfaceFairing::fair(unsigned int k)
{
    // compute cotan weights
    for (auto v : m_mesh.vertices())
    {
        m_vweight[v] = 0.5 / voronoiArea(m_mesh, v);
    }
    for (auto e : m_mesh.edges())
    {
        m_eweight[e] = std::max(0.0, cotanWeight(m_mesh, e));
    }

    // check whether some vertices are selected
    bool noSelection = true;
    if (m_vselected)
    {
        for (auto v : m_mesh.vertices())
        {
            if (m_vselected[v])
            {
                noSelection = false;
                break;
            }
        }
    }

    // lock k locked boundary rings
    for (auto v : m_mesh.vertices())
    {
        // lock boundary
        if (m_mesh.isSurfaceBoundary(v))
        {
            m_vlocked[v] = true;

            // lock one-ring of boundary
            if (k > 1)
            {
                for (auto vv : m_mesh.vertices(v))
                {
                    m_vlocked[vv] = true;

                    // lock two-ring of boundary
                    if (k > 2)
                    {
                        for (auto vvv : m_mesh.vertices(vv))
                        {
                            m_vlocked[vvv] = true;
                        }
                    }
                }
            }
        }
    }

    // lock un-selected and isolated vertices
    for (auto v : m_mesh.vertices())
    {
        if (!noSelection && !m_vselected[v])
        {
            m_vlocked[v] = true;
        }

        if (m_mesh.isIsolated(v))
        {
            m_vlocked[v] = true;
        }
    }

    // collect free vertices
    std::vector<SurfaceMesh::Vertex> vertices;
    vertices.reserve(m_mesh.nVertices());
    for (auto v : m_mesh.vertices())
    {
        if (!m_vlocked[v])
        {
            m_idx[v] = vertices.size();
            vertices.push_back(v);
        }
    }

    // construct matrix & rhs
    const unsigned int n = vertices.size();
    SparseMatrix A(n, n);
    Eigen::MatrixXd B(n, 3);

    std::map<SurfaceMesh::Vertex, double> row;
    std::vector<Triplet> triplets;

    for (unsigned int i = 0; i < n; ++i)
    {
        B(i, 0) = 0.0;
        B(i, 1) = 0.0;
        B(i, 2) = 0.0;

        setupMatrixRow(vertices[i], m_vweight, m_eweight, k, row);

        for (auto r : row)
        {
            auto v = r.first;
            auto w = r.second;

            if (m_idx[v] != -1)
            {
                triplets.emplace_back(i, m_idx[v], w);
            }
            else
            {
                B(i, 0) -= w * m_points[v][0];
                B(i, 1) -= w * m_points[v][1];
                B(i, 2) -= w * m_points[v][2];
            }
        }
    }

    A.setFromTriplets(triplets.begin(), triplets.end());

    // solve A*X = B
    Eigen::SimplicialLDLT<SparseMatrix> solver(A);
    Eigen::MatrixXd X = solver.solve(B);

    if (solver.info() != Eigen::Success)
    {
        std::cerr << "SurfaceFairing: Could not solve linear system\n";
    }
    else
    {
        for (unsigned int i = 0; i < n; ++i)
        {
            auto v = vertices[i];
            m_points[v] = Point(X(m_idx[v], 0), X(m_idx[v], 1), X(m_idx[v], 2));
        }
    }
}

//-----------------------------------------------------------------------------

struct Triple
{
    Triple() = default;

    Triple(SurfaceMesh::Vertex v, double weight, unsigned int degree)
        : m_v(std::move(v)), m_weight(weight), m_degree(degree)
    {
    }

    SurfaceMesh::Vertex m_v;
    double m_weight;
    unsigned int m_degree;
};

//-----------------------------------------------------------------------------

void SurfaceFairing::setupMatrixRow(const SurfaceMesh::Vertex v,
                                    SurfaceMesh::VertexProperty<double> vweight,
                                    SurfaceMesh::EdgeProperty<double> eweight,
                                    unsigned int laplaceDegree,
                                    std::map<SurfaceMesh::Vertex, double>& row)
{
    Triple t(v, 1.0, laplaceDegree);

    // init
    static std::vector<Triple> todo;
    todo.reserve(50);
    todo.push_back(t);
    row.clear();

    while (!todo.empty())
    {
        t = todo.back();
        todo.pop_back();
        auto v = t.m_v;
        auto d = t.m_degree;

        if (d == 0)
        {
            row[v] += t.m_weight;
        }

        // else if (d == 1 && m_mesh.isSurfaceBoundary(v))
        // {
        //     // ignore?
        // }

        else
        {
            auto ww = 0.0;

            for (auto h : m_mesh.halfedges(v))
            {
                auto e = m_mesh.edge(h);
                auto vv = m_mesh.toVertex(h);
                auto w = eweight[e];

                if (d < laplaceDegree)
                    w *= vweight[v];

                w *= t.m_weight;
                ww -= w;

                todo.emplace_back(vv, w, d - 1);
            }

            todo.emplace_back(v, ww, d - 1);
        }
    }
}

//=============================================================================
} // namespace pmp
//=============================================================================


#pragma once

#include <deque>
#include <stdexcept>
#include <unordered_map>

#include <boost/pending/disjoint_sets.hpp>
#include <boost/functional/hash.hpp>
#include "xtensor/xtensor.hpp"
#include "xtensor/xarray.hpp"

#include "affogato/util.hxx"

namespace affogato {
namespace segmentation {

template<class EDGE_WEIGHTS, class NODE_WEIGHTS>
inline void get_node_weights(const xt::xexpression<EDGE_WEIGHTS> & edge_weights_exp,
                             const double lower_threshold,
                             xt::xexpression<NODE_WEIGHTS> & node_weights_exp,
                             const bool ignore_border = false) {

    typedef typename EDGE_WEIGHTS::value_type ValueType;
    const auto & edge_weights = edge_weights_exp.derived_cast();
    auto & node_weights = node_weights_exp.derived_cast();

    xt::xindex shape(node_weights.shape().begin(), node_weights.shape().end());
    const unsigned ndim = shape.size();

    // ValueType infinity = *std::max_element(edge_weights.begin(), edge_weights.end()) + .1;
    const ValueType infinity = 1.1;
    // iterate over all spatial coordinates. for each node, find all weigths that are connected to it
    util::for_each_coordinate(shape, [&](const xt::xindex coord){

        ValueType weight = 0;
        // get the weights connecting this node to other nodes
        for(unsigned d = 0; d < ndim; ++d) {
            // NOTE this assumes affinity offset conventions [[-1, 0], [0, -1]] etc.
            for(auto dir : {0, 1}) {
                // get the corresponding affi
                xt::xindex aff_coord(ndim + 1);
                std::copy(coord.begin(), coord.end(), aff_coord.begin() + 1);
                aff_coord[0] = d;
                aff_coord[d + 1] += dir;
                ValueType curr_weight;
                if(aff_coord[d + 1] > 0 && aff_coord[d + 1] < shape[d]) {
                    curr_weight = edge_weights[aff_coord];
                } else {
                    continue;
                }
                if(curr_weight > weight) {
                    weight = curr_weight;
                }
            }
        }
        node_weights[coord] = (weight > lower_threshold) ? infinity : weight;

        if(ignore_border) {
            bool at_border = false;
            for(unsigned d = 0; d < ndim; ++d) {
                if(coord[d] == 0 || coord[d] == shape[d] - 1) {
                    at_border = true;
                    break;
                }
            }
            if(at_border) {
                node_weights[coord] = infinity;
            }
        }
    });
}

template<class EDGE_WEIGHTS, class NODE_WEIGHTS, class LABELS>
inline uint64_t stream(const xt::xindex & start_coord,
                       const xt::xexpression<EDGE_WEIGHTS> & edge_weights_exp,
                       const xt::xexpression<NODE_WEIGHTS> & node_weights_exp,
                       const xt::xexpression<LABELS> & labels_exp,
                       const double upper_threshold,
                       std::vector<xt::xindex> & stream_coordinates
                       /*,std::ofstream debug*/ ) {

    typedef typename EDGE_WEIGHTS::value_type ValueType;
    // add pixel to the stream coordinates
    stream_coordinates.emplace_back(start_coord);
    const unsigned ndim = start_coord.size();

    const auto & edge_weights = edge_weights_exp.derived_cast();
    const auto & node_weights = node_weights_exp.derived_cast();
    const auto & labels = labels_exp.derived_cast();

    xt::xindex shape(labels.shape().begin(), labels.shape().end());

    // initialize pixel queue
    std::deque<xt::xindex> queue;
    queue.push_back(start_coord);

    const ValueType infinity = 1.1;

    while(!queue.empty()) {

        const auto & coord = queue.front();
        queue.pop_front();

        ValueType w_max = node_weights[coord];

        // iterate over the connection to neighbors for this coordinate
        for(unsigned d = 0; d < ndim; ++d) {
            // NOTE this assumes affinity offset conventions [[-1, 0], [0, -1]] etc.
            for(auto dir : {0, 1}) {
                // get the corresponding affinity coordinate and neighbor coordinate
                xt::xindex ngb_coord(ndim), aff_coord(ndim + 1);

                std::copy(coord.begin(), coord.end(), aff_coord.begin() + 1);
                aff_coord[0] = d;
                aff_coord[d + 1] += dir;

                std::copy(coord.begin(), coord.end(), ngb_coord.begin());
                ngb_coord[d] += (dir == 0) ? -1 : 1;

                // range check
                if(ngb_coord[d] <= 0 || ngb_coord[d] >= shape[d]) {
                    continue;
                }

                ValueType weight = edge_weights[aff_coord];
                weight = (weight > upper_threshold) ? infinity : weight;

                // only consider neighbor if its weight is equal to the nodes max-weight
                if(fabs(weight - w_max) > std::numeric_limits<ValueType>::epsilon()) {
                    continue;
                }

                // only consider neighbor if it's coordinate is not already in the stream
                if(std::find(stream_coordinates.begin(), stream_coordinates.end(), ngb_coord) != stream_coordinates.end()) {
                    continue;
                }

                // if we hit a labeled coordinate, return it's label
                if(labels[ngb_coord] != 0 ) {
                    return labels[ngb_coord];
                }

                // if the node weight of the considered pixel is smaller, we start depth first search from it
                else if(node_weights[ngb_coord] < w_max ) {
                    stream_coordinates.emplace_back(ngb_coord);
                    queue.clear();
                    queue.push_back(ngb_coord);
                    // break looping over the current neighbors and go to new pix
                    break;
                }
                else {
                    stream_coordinates.push_back(ngb_coord);
                    queue.push_back(ngb_coord);
                }
            }
        }
    }
    // return 0, if we have not found a labeled pixel in the stream
    return 0;
}

template<class EDGE_WEIGHTS, class NODE_WEIGHTS, class LABELS>
inline size_t run_zws(const xt::xexpression<EDGE_WEIGHTS> & edge_weights_exp,
                      const xt::xexpression<NODE_WEIGHTS> & node_weights_exp,
                      const double upper_threshold,
                      xt::xexpression<LABELS> & labels_exp,
                      bool const ignore_border=false) {
    typedef typename LABELS::value_type LabelType;
    LabelType next_label = 1;

    const auto & edge_weights = edge_weights_exp.derived_cast();
    const auto & node_weights = node_weights_exp.derived_cast();
    auto & labels = labels_exp.derived_cast();

    // ofstream for debug output
    //std::ofstream debug;
    //debug.open("run_wsgraph_debug.txt")
    //
    //
    const xt::xindex shape(node_weights.shape().begin(), node_weights.shape().end());
    const unsigned ndim = shape.size();

    util::for_each_coordinate(shape, [&](const xt::xindex & coord){
        if(ignore_border) {
            bool is_border = false;
            for(unsigned d = 0; d < ndim; ++d) {
                if(coord[d] == 0 || coord[d] == shape[d] - 1) {
                    is_border = true;
                    break;
                }
            }
            if(is_border) {
                return;
            }
        }

        auto label = labels[coord];
        // continue if the pixel is already labeled
        if(label != 0) {
            return;
        }

        // call stream -> finds the stream belonging to the current label and pixel coordinates belonging to the stream
        std::vector<xt::xindex> stream_coordinates;
        label = stream(coord, edge_weights, node_weights, labels,
                       upper_threshold, stream_coordinates); //debug

        // if stream returns 0, we found a new stream
        if(label == 0) {
            label = next_label++;
        }

        // write the new label to all coordinates in the stream
        for(const auto & coord : stream_coordinates) {
            labels[coord] = label;
        }

    });
    //debug.close();
    return next_label;
}

template<class EDGE_WEIGHTS, class LABELS, class LINK>
inline void get_region_weights(const xt::xexpression<EDGE_WEIGHTS> & edge_weights_exp,
                               const xt::xexpression<LABELS> & labels_exp,
                               const double upper_threshold,
                               std::unordered_map<LINK, typename EDGE_WEIGHTS::value_type, boost::hash<LINK>> & links) {

    typedef LINK Link;
    typedef typename EDGE_WEIGHTS::value_type ValueType;

    const ValueType infinity = 1.1;

    const auto & edge_weights = edge_weights_exp.derived_cast();
    const auto & labels = labels_exp.derived_cast();
    xt::xindex shape(labels.shape().begin(), labels.shape().end());
    const unsigned ndim = shape.size();

    util::for_each_coordinate(shape, [&](const xt::xindex & coord) {
        const auto label = labels[coord];
        for(unsigned d = 0; d < ndim ;++d) {

            // range check
            if(coord[d] > 0) {
                continue;
            }

            xt::xindex ngb_coord(coord);
            ngb_coord[d] -= 1;

            const auto ngb_label = labels[ngb_coord];
            if(ngb_label != label) {
                Link link(std::min(ngb_label, label),
                          std::max(ngb_label, label));
                xt::xindex aff_coord(ndim + 1);
                std::copy(coord.begin(), coord.end(), aff_coord.begin() + 1);
                aff_coord[0] = d;
                ValueType weight = edge_weights[aff_coord];
                weight = (weight > upper_threshold) ? infinity : weight;

                auto link_it = links.find(link);
                if(link_it != links.end()) {
                    link_it->second = std::max(weight, link_it->second);
                } else {
                    links.emplace(link, weight);
                }
            }
        }
    });
}

template<class LINK, class WEIGHT, class LABELS>
inline size_t apply_size_filter(const std::unordered_map<LINK, WEIGHT, boost::hash<LINK>> & region_weights,
                                const size_t n_labels,
                                const size_t size_threshold,
                                const double merge_threshold,
                                xt::xexpression<LABELS> & labels_exp) {
    typedef typename LABELS::value_type LabelType;
    auto & labels = labels_exp.derived_cast();

    // dump map into a vector and sort it by value
    std::vector<std::pair<LINK, WEIGHT>> rw_vec(region_weights.begin(),
                                                region_weights.end());

    std::sort(rw_vec.begin(), rw_vec.end(), [](const std::pair<LINK, WEIGHT> & a,
                                               const std::pair<LINK, WEIGHT> & b){return a.second < b.second;});

    xt::xindex shape(labels.shape().begin(), labels.shape().end());

    // find sizes of labels
    std::vector<size_t> sizes(n_labels);
    util::for_each_coordinate(shape, [&](const xt::xindex & coord){
        ++sizes[labels[coord]];
    });

    // make ufd
    std::vector<LabelType> ranks(n_labels);
    std::vector<LabelType> parents(n_labels);
    boost::disjoint_sets<LabelType*, LabelType*> ufd(&ranks[0], &parents[0]);
    for(LabelType label = 0; label < n_labels; ++label) {
        ufd.make_set(label);
    }

    // merge regions
    for(const auto & e_and_w : rw_vec) {

        // if we have reached the value threshold, we stop filtering
        if(e_and_w.second < merge_threshold) {
            break;
        }
        const auto & edge = e_and_w.first;

        const LabelType s1 = ufd.find_set(edge.first);
        const LabelType s2 = ufd.find_set(edge.second);

        // merge two regions, if at least one of them is below the size threshold
        if(s1 != s2 && (sizes[s1] < size_threshold || sizes[s2] < size_threshold)) {
            const size_t size = sizes[s1] + sizes[s2];
            sizes[s1] = 0;
            sizes[s2] = 0;
            ufd.link(s1, s2);
            sizes[ufd.find_set(s1)] = size;
        }
    }

    std::vector<LabelType> new_labels(n_labels);
    LabelType next_label = 1;
    for(LabelType label = 0; label < n_labels; ++label) {
        const auto repr = ufd.find_set(label);
        if(sizes[repr] < size_threshold) {
            new_labels[label] = 0;
        } else {
            new_labels[label] = next_label++;
        }
    }

    // write the new labels
    util::for_each_coordinate(shape, [&](const xt::xindex & coord){
        labels[coord] = new_labels[labels[coord]];
    });

    return next_label;
}

// z-watershed, implementation based on
// http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=4564470&tag=1
// and http://dspace.mit.edu/handle/1721.1/66820
template<class WEIGHTS, class LABELS>
size_t compute_zws_segmentation(const xt::xexpression<WEIGHTS> & edge_weights_exp,
							    const double lower_threshold,
        					    const double upper_threshold,
							    const double merge_threshold,
        					    const size_t size_threshold,
                                xt::xexpression<LABELS> & labels_exp) {

    typedef typename WEIGHTS::value_type ValueType;
    typedef typename LABELS::value_type LabelType;
    if(lower_threshold > upper_threshold) {
        throw std::runtime_error("Thresholds inverted!");
    }

    const auto & edge_weights = edge_weights_exp.derived_cast();
    // initialize the node weights, which are one dimension less than the edge weights
    typedef typename xt::xarray<ValueType>::shape_type ShapeType;
    ShapeType shape(edge_weights.shape().begin() + 1, edge_weights.shape().end());
    xt::xarray<ValueType> node_weights = xt::zeros<ValueType>(shape);

    // std::cout << "computing node weights from edge weights" << std::endl;
    get_node_weights(edge_weights, lower_threshold, node_weights);

    // threshold_edge_weights(edge_weights, upper_threshold);
    // std::cout << "run zws" << std::endl;
    size_t n_labels = run_zws(edge_weights, node_weights, upper_threshold, labels_exp);
    // std::cout << n_labels << " labels after zws" << std::endl;

    // if we don't do merging or size filtering, we can return here already
    if(merge_threshold == 0 && size_threshold == 0) {
        return n_labels;
    }

    typedef std::pair<LabelType, LabelType> Link;
    std::unordered_map<Link, ValueType, boost::hash<Link>> region_weights;
    // std::cout << "get region weights" << std::endl;
    get_region_weights(edge_weights, labels_exp, upper_threshold, region_weights);

    // std::cout << "apply size filter" << std::endl;
    n_labels = apply_size_filter(region_weights, n_labels,
                                 size_threshold, merge_threshold,
                                 labels_exp);

    return n_labels;
}

}
}


#ifndef ABSTRACT_INTERFACE_HH
#define ABSTRACT_INTERFACE_HH

#include <memory>
#include <iostream>
#include <boost/signals2.hpp>
#include <string>
#include <cmath>
#include <vector>

#include <dune/grid/config.h>
#include "linalg/triplet.hh"

namespace Kaskade
{
  /**
   * @file
   * @brief  Interfaces for function space oriented algorithms
   * @author Anton Schiela
   */
  /** \addtogroup abstract*/
  /**@{*/

  /// Abstract Vector for function space algorithms
  class AbstractFunctionSpaceElement
  {
  public:
    // No copy constructor.
    AbstractFunctionSpaceElement(AbstractFunctionSpaceElement const &) = delete;

    virtual ~AbstractFunctionSpaceElement() {}

    /// *this += alpha*l
    AbstractFunctionSpaceElement& axpy(double alpha, AbstractFunctionSpaceElement const& l, int component)
    {
      if(this==&l)
      {
        std::vector<double> alphaV(nComponents(),alpha+1.0);
        return doscale(alphaV);
      }
      else
        return doaxpy(alpha, l, component);
    }

    AbstractFunctionSpaceElement& axpy(double alpha, AbstractFunctionSpaceElement const&l, std::string const role)
    {
      return axpy_role(alpha,l,role);
    }

    AbstractFunctionSpaceElement& axpy_role(double alpha, AbstractFunctionSpaceElement const&l, std::string const role)
    {
      for(int i=0; i<nComponents(); ++i)
      {
        if(getRole(i)==role) axpy(alpha,l,i);
      }
      return *this;
    }

    /// *this += alpha*l
    AbstractFunctionSpaceElement& axpy(double alpha, AbstractFunctionSpaceElement const& l)
    {
      if(this==&l)
      {
        std::vector<double> alphaV(nComponents(),alpha+1.0);
        return doscale(alphaV);
      }
      else
        return doaxpy_(alpha, l);
    }

    /// Assignment
    AbstractFunctionSpaceElement& operator=(AbstractFunctionSpaceElement const& v)
    {
      if(this!=&v) return doassign(v);
      return *this;
    }

    /// Basic vector arithmetic
    AbstractFunctionSpaceElement& operator+=(AbstractFunctionSpaceElement const& v) { return doaxpy_(1.0,v); }
    /// Basic vector arithmetic
    AbstractFunctionSpaceElement& operator-=(AbstractFunctionSpaceElement const& v) { return doaxpy_(-1.0,v); }
    /// Basic vector arithmetic
    AbstractFunctionSpaceElement& operator*=(double lambda)
    {
      std::vector<double> lambdaV(nComponents(),lambda);
      return doscale(lambdaV);
    }

    /// Scaling each component of the vector separately
    AbstractFunctionSpaceElement& operator*=(std::vector<double>const& lambda)
    {
      return doscale(lambda);
    }

    /// Interpret *this as a dual vector, and apply it to v
    /** Duality is currently not represented by types, hence users have to make
  sure that the dual pairing <*this,v> is mathematically meaningful. The standard
 implementation of the dual pairing in Bridge::Vector is
 the dot-product between the coordinate representations of *this and v.
 For that *this should be the result of AbstractLinearization::evald
     */
    double applyAsDualTo(AbstractFunctionSpaceElement const& v, int component) const
    {
      return doapplyAsDualTo(v,component,component+1);
    }

    double applyAsDualTo(AbstractFunctionSpaceElement const& v) const
    {
      return doapplyAsDualTo(v,0,nComponents());
    }

    double applyAsDualTo(AbstractFunctionSpaceElement const& v, std::string const& role) const
    {
      return applyAsDualTo_role(v,role);
    }

    double applyAsDualTo_role(AbstractFunctionSpaceElement const&v, std::string const role) const
    {
      double res(0.0);
      for(int i=0; i<nComponents(); ++i)
      {
        if(getRole(i)==role) res+=applyAsDualTo(v,i);
      }
      return res;
    }

    virtual std::string getRole(int component) const = 0;

    /// Shallow swap
    void swap(AbstractFunctionSpaceElement& v) { doswap(v); }

    virtual int nComponents() const = 0;

    /// Optional output
    virtual void writeToFile(std::string const& file, bool append, int order=1) const {}

    /// Optional output
    virtual void print(std::string const& message="") const { std::cout << "AbstractFunctionSpaceElement: No printing available!" << std::endl;}

    /// Construction of a vector of the same type
    virtual std::unique_ptr<AbstractFunctionSpaceElement> clone() const = 0;

    /// Construction of a vector of the same type
    virtual std::unique_ptr<AbstractFunctionSpaceElement> initZeroVector() const = 0;

  private:
    /// Interface for implementation
    virtual AbstractFunctionSpaceElement& doaxpy(double alpha, AbstractFunctionSpaceElement const& l, int component) = 0;
    virtual AbstractFunctionSpaceElement& doaxpy_(double alpha, AbstractFunctionSpaceElement const& l) = 0;
    /// Interface for implementation
    virtual AbstractFunctionSpaceElement& doscale(std::vector<double> const& lambda) = 0;
    /// Interface for implementation
    virtual AbstractFunctionSpaceElement& doassign(AbstractFunctionSpaceElement const& l) = 0;
    /// Interface for implementation
    virtual void doswap(AbstractFunctionSpaceElement& l) = 0;
    virtual double doapplyAsDualTo(AbstractFunctionSpaceElement const& v,int vbegin, int vend) const = 0;

  protected:
    AbstractFunctionSpaceElement() {}
  };

  /// Abstract linearization.
  /** Given a nonlinear functional, this class represents second order information at the point of linearization
   */
  class AbstractLinearization
  {
  public:
    /// Evaluate functional
    virtual double eval() const = 0;
    /// Evaluate L1 norm of integrand
    virtual double evalL1norm() const = 0;
    /// Evaluate derivative
    virtual void evald(AbstractFunctionSpaceElement &g, int rbegin=0, int rend=-1) const = 0;
    /// Evaluate hessian times second argument: y = y+ddf*x
    void ddxpy(AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
    {
      d2axpy(1.0,y,x,rbegin,rend,cbegin,cend);
    }
    /// Evaluate hessian times second argument: y = y+ddf*x
    void ddtxpy(AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const
    {
      d2taxpy(1.0,y,x,rbegin,rend,cbegin,cend);
    }
    /// Evaluate hessian times second argument: y = y+a*ddf*x
    virtual void d2axpy(double a, AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const = 0;
    /// Evaluate hessian times second argument: y = y+a*ddf*x
    virtual void d2taxpy(double a, AbstractFunctionSpaceElement& y, AbstractFunctionSpaceElement const& x, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const = 0;
    /// Access matrix representation if available.
    virtual void getMatrixBlocks(MatrixAsTriplet<double>& mat, int rbegin=0, int rend=-1, int cbegin=0, int cend=-1) const = 0;//{ assert(!"not implemented"); }
    /// Get point of linearization
    virtual AbstractFunctionSpaceElement const& getOrigin() const = 0;

    virtual void precompute() = 0;

    virtual void flush() = 0;

    virtual ~AbstractLinearization() {}
  };

  class AbstractPreconditioner
  {
  public:
    virtual void setLinearization(AbstractLinearization&) = 0;
    virtual void apply(AbstractFunctionSpaceElement const& x, AbstractFunctionSpaceElement& Px) = 0;
  };

  /// Abstract connection.
  /**
   * Connect signal to flush() (in general for invalidating assembled data on grid refinement)
   */
  class AbstractFlushConnection
  {
  public:
    virtual ~AbstractFlushConnection(){}
    virtual void connectToSignalForFlush(boost::signals2::signal<void()>&) = 0;
  };

  class AbstractConnectedLinearization : public AbstractLinearization, public AbstractFlushConnection
  {};

  /// Representation of a nonlinear functional
  class AbstractFunctional
  {
  public:
    virtual double evaluate(AbstractFunctionSpaceElement const& x) const { assert(!"not implemented"); return 0; }
    virtual std::unique_ptr<AbstractLinearization> getLinearization(AbstractFunctionSpaceElement const& x) const = 0;
    virtual std::unique_ptr<AbstractFunctionSpaceElement> getImageVector(AbstractFunctionSpaceElement const& x) const = 0;
    virtual bool inDomain(AbstractFunctionSpaceElement const& x) const { return true; }
    virtual ~AbstractFunctional() {}
  };

  /// Representation of parameters
  class AbstractParameters
  {
  public:
    virtual ~AbstractParameters() {}
  };

  /// ...for parameter dependent functionals, implements AbstractParameters
  template<typename ParameterType>
  class Parameters : public AbstractParameters
  {
  public:
    explicit Parameters(ParameterType const& p) : pref(p) {}
    ParameterType const& getPars() const {return pref;}
  private:
    ParameterType const& pref;
  };

  ///
  template<class ParameterType>
  Parameters<ParameterType> makePars(ParameterType const& p) { return Parameters<ParameterType>(p); }

  /// Creates a functional from a homotopy of functionals by inserting a parameter
  class AbstractParameterFunctional
  {
  public:
    virtual std::unique_ptr<AbstractFunctional> getFunctional(AbstractParameters const&) const = 0;
    virtual std::unique_ptr<AbstractFunctional> getParameterLinFunctional(AbstractParameters const& p) const
          { assert(!"ParameterFunctional: Linearization not Implemented"); return getFunctional(p);}
    virtual std::unique_ptr<AbstractFunctional> getLinFunctionValue(AbstractParameters const& p) const
          { assert(!"ParameterFunctional: Linearization not Implemented"); return getFunctional(p);}
    virtual ~AbstractParameterFunctional() {}
  };

  /// Class that models the functionality of a (possibly inexact) linear solver
  class AbstractNewtonDirection
  {
  public:
    /** This method is not const, since the internal state of the solver may be modified */
    /** Computes an undamped ordinary Newton step, i.e., \f$correction=-F'(x)^{-1}F(x)\f$*/
    void ordinary(AbstractFunctionSpaceElement& correction, AbstractLinearization& linearization)
    {  doSolve(correction, linearization);
    correction *= -1.0;
    }
    /** Computes an undamped simplified Newton step, i.e., correction=-F'(xold)^{-1}F(x)*/
    void simplified(AbstractFunctionSpaceElement& correction, AbstractLinearization const& linearization, AbstractLinearization const& oldlinearization) const
    {  doResolve(correction, linearization, oldlinearization);
    correction *= -1.0;
    }

    /** Computes an undamped simplified Newton step, i.e., correction=-F'(xold)^{-1}F(x)*/
    /** where F'(xold)^{-1} has been kept from an earlier ordinary() call */
    void simplified(AbstractFunctionSpaceElement& correction, AbstractLinearization const& linearization) const
    {  doResolve(correction, linearization);
    correction *= -1.0;
    }

    /// Specify accuracy that should be achieved
    virtual void setRelativeAccuracy(double accuracy) = 0;
    /// Specify accuracy that should be achieved
    virtual void setAbsoluteAccuracy(double) {}
    /// Get accuracy that was actually achieved
    virtual double getRelativeAccuracy() = 0;
    /// Get accuracy that was actually achieved
    virtual double getAbsoluteAccuracy() = 0;

    virtual bool improvementPossible() = 0;

    virtual bool changedGrid() { return false; }

    virtual ~AbstractNewtonDirection() {}

    /// Solve Newton System: correction = +F'(iterate)^{-1}F(iterate)
    virtual void doSolve(AbstractFunctionSpaceElement& correction,
        AbstractLinearization& lin) = 0;
    /// Solve simplified Newton system: scorrection = +F'(iterate)^{-1}F(trialIterate)
    virtual void doResolve(AbstractFunctionSpaceElement& correction,
        AbstractLinearization const& lin,
        AbstractLinearization const& olin) const = 0;

    /// Solve simplified Newton system: scorrection = +F'(iterate)^{-1}F(trialIterate)
    virtual void doResolve(AbstractFunctionSpaceElement& correction,
        AbstractLinearization const& lin) const = 0;

    mutable boost::signals2::signal<void()> changed;
  };

  class AbstractChart
  {
  public:
    virtual void addPerturbation
    (AbstractFunctionSpaceElement& newIterate,
        AbstractFunctionSpaceElement const& perturbation,
        AbstractLinearization const& lin,
        std::vector<std::shared_ptr<AbstractFunctionSpaceElement> > basis = std::vector<std::shared_ptr<AbstractFunctionSpaceElement> >()) const = 0;

    virtual std::unique_ptr<AbstractChart> clone() const = 0;
  };

  class AbstractNorm
  {
  public:
    virtual ~AbstractNorm(){}
    virtual double operator()(AbstractFunctionSpaceElement const&) const = 0;
    virtual void setOrigin(AbstractLinearization const&) {}
  };

  class AbstractScalarProduct : public AbstractNorm
  {
  public:
    virtual double operator()(AbstractFunctionSpaceElement const&, AbstractFunctionSpaceElement const&) const = 0;
    double operator()(AbstractFunctionSpaceElement const& v) const
    {
      double sc = this->operator()(v,v);
      if(sc < 0) std::cout << "Warning: scalar product not positive definite! Taking absolute value" << std::endl;
      return sqrt(std::fabs(sc));
    }
  };

  ///Representation of an error estimate, i.e. the output of an error estimator
  class AbstractErrorEstimate
  {
  public:
    virtual double absoluteError() const = 0;
    virtual ~AbstractErrorEstimate() {}
  };

  /// Representation of an error estimator
  class AbstractErrorEstimator
  {
  public:
    virtual std::unique_ptr<AbstractErrorEstimate> createEstimate(AbstractFunctionSpaceElement const& correction,
        AbstractLinearization const& lin) const = 0;
    virtual ~AbstractErrorEstimator() {}
  };

  ///Representation of an adaptive grid and a simple set of operations thereon
  class AbstractAdaptiveGrid
  {
  public:
    /// Change grid according to marked elements
    virtual void adapt() = 0;
    /// Mark elements, using an error estimate that hold error indicators
    virtual void mark(AbstractErrorEstimate&, double portion) = 0;
    /// Remove all marks in the grid
    virtual void flushMarks() = 0;
    /// Number of patches
    virtual int size() = 0;
    virtual ~AbstractAdaptiveGrid() {}

    virtual int getNMarked() = 0;

    /// Inform others that the grid will change
    mutable boost::signals2::signal<void()> gridWillChange;
  };

  class AbstractHierarchicalErrorEstimator
  {
  public:
    virtual ~AbstractHierarchicalErrorEstimator(){}

    virtual void operator()(AbstractLinearization const& lin, AbstractFunctionSpaceElement const& x, AbstractFunctionSpaceElement const& dx, int, AbstractFunctionSpaceElement const& rhs) = 0;

    virtual void refineGrid() = 0;

    virtual double estimatedAbsoluteError() const = 0;

    virtual size_t gridSize() const = 0;

//    virtual void setNorm(AbstractNorm const&) = 0;
  };

  class ContinuousScalarFunction
  {
  public:
    virtual ~ContinuousScalarFunction() {}
    virtual double d0(std::vector<double> const&) const = 0;
  };

  class DifferentiableScalarFunction : public ContinuousScalarFunction
  {
  public:
    virtual ~DifferentiableScalarFunction() {}

    virtual std::vector<double> d1(std::vector<double> const&) const = 0;
  };

/** @}*/

}  // namespace Kaskade
#endif


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef EIGEN_UTILS_H
#define EIGEN_UTILS_H

#include <Eigen/Core>
#include <cassert>
#include <complex>
#include <iomanip>
#include <sstream>
#include <string>
#include <limits>

namespace flexiblesusy {

template <typename Derived>
int closest_index(double mass, const Eigen::ArrayBase<Derived>& v)
{
   int pos;
   typename Derived::PlainObject tmp;
   tmp.setConstant(mass);

   (v - tmp).abs().minCoeff(&pos);

   return pos;
}

template <class BinaryOp, class Derived>
Derived binary_map(
   const Eigen::DenseBase<Derived>& a, const Eigen::DenseBase<Derived>& b, BinaryOp op)
{
   typename Derived::PlainObject result(a.rows(), b.cols());

   assert(a.rows() == b.rows());
   assert(a.cols() == b.cols());

   for (int k = 0; k < a.cols(); k++)
      for (int i = 0; i < a.rows(); i++)
         result(i,k) = op(a(i,k), b(i,k));

   return result;
}

/**
 * Divides a by b element wise.  If the quotient is not finite, it is
 * set to zero.
 *
 * @param a numerator
 * @param b denominator
 */
template <class Derived>
Derived div_safe(
   const Eigen::DenseBase<Derived>& a, const Eigen::DenseBase<Derived>& b)
{
   using Scalar = typename Derived::Scalar;

   return binary_map(a, b, [](Scalar x, Scalar y){
         const Scalar q = x / y;
         return std::isfinite(q) ? q : Scalar{};
      });
}

/**
 * Calls eval() on any Eigen expression.  If the argument is not an
 * Eigen expression, the argument is returned.
 *
 * @param expr expression
 *
 * @return evaluated expression or argument
 */
template <class Derived>
auto Eval(const Eigen::DenseBase<Derived>& expr) -> decltype(expr.eval())
{
   return expr.eval();
}

inline char                 Eval(char                        expr) { return expr; }
inline short                Eval(short                       expr) { return expr; }
inline int                  Eval(int                         expr) { return expr; }
inline long                 Eval(long                        expr) { return expr; }
inline unsigned short       Eval(unsigned short              expr) { return expr; }
inline unsigned int         Eval(unsigned int                expr) { return expr; }
inline unsigned long        Eval(unsigned long               expr) { return expr; }
inline double               Eval(double                      expr) { return expr; }
inline std::complex<double> Eval(const std::complex<double>& expr) { return expr; }

/**
 * The element of v, which is closest to mass, is moved to the
 * position idx.
 *
 * @param idx new index of the mass eigenvalue
 * @param mass mass to compare against
 * @param v vector of masses
 * @param z corresponding mixing matrix
 */
template <typename DerivedArray, typename DerivedMatrix>
void move_goldstone_to(int idx, double mass, Eigen::ArrayBase<DerivedArray>& v,
                       Eigen::MatrixBase<DerivedMatrix>& z)
{
   int pos = closest_index(mass, v);
   if (pos == idx)
      return;

   const int sign = (idx - pos) < 0 ? -1 : 1;
   int steps = std::abs(idx - pos);

   // now we shuffle the states
   while (steps--) {
      const int new_pos = pos + sign;
      v.row(new_pos).swap(v.row(pos));
      z.row(new_pos).swap(z.row(pos));
      pos = new_pos;
   }
}

/**
 * Normalize each element of the given real matrix to be within the
 * interval [min, max].  Values < min are set to min.  Values > max
 * are set to max.
 *
 * @param m matrix
 * @param min minimum
 * @param max maximum
 */
template <int M, int N>
void normalize_to_interval(Eigen::Matrix<double,M,N>& m, double min = -1., double max = 1.)
{
   auto data = m.data();
   const auto size = m.size();

   for (int i = 0; i < size; i++) {
      if (data[i] < min)
         data[i] = min;
      else if (data[i] > max)
         data[i] = max;
   }
}

/**
 * Normalize each element of the given complex matrix to have a
 * magnitude within the interval [0, max].  If the magnitude of a
 * matrix element is > max, then the magnitude is set to max.  The
 * phase angles are not modified.
 *
 * @param m matrix
 * @param max_mag maximum magnitude
 */
template <int M, int N>
void normalize_to_interval(Eigen::Matrix<std::complex<double>,M,N>& m, double max_mag = 1.)
{
   auto data = m.data();
   const auto size = m.size();

   for (int i = 0; i < size; i++) {
      if (std::abs(data[i]) > max_mag)
         data[i] = std::polar(max_mag, std::arg(data[i]));
   }
}

namespace {
template <class T>
struct Is_not_finite {
   bool operator()(T x) const noexcept { return !std::isfinite(x); }
};
} // anonymous namespace

/**
 * Returns all elements from src, which are not close to the elements
 * in cmp.  The returned vector will have the length (src.size() -
 * cmp.size()).
 *
 * @param src source vector
 * @param cmp vector with elements to compare against
 * @return vector with elements of src not close to cmp
 */
template<class Real, int Nsrc, int Ncmp>
Eigen::Array<Real,Nsrc - Ncmp,1> remove_if_equal(
   const Eigen::Array<Real,Nsrc,1>& src,
   const Eigen::Array<Real,Ncmp,1>& cmp)
{
   Eigen::Array<Real,Nsrc,1> non_equal(src);
   Eigen::Array<Real,Nsrc - Ncmp,1> dst;

   for (int i = 0; i < Ncmp; i++) {
      const int idx = closest_index(cmp(i), non_equal);
      non_equal(idx) = std::numeric_limits<double>::infinity();
   }

   std::remove_copy_if(non_equal.data(), non_equal.data() + Nsrc,
                       dst.data(), Is_not_finite<Real>());

   return dst;
}

/**
 * @brief reorders vector v according to ordering in vector v2
 * @param v vector with elementes to be reordered
 * @param v2 vector with reference ordering
 */
template<class Real, int N>
void reorder_vector(
   Eigen::Array<Real,N,1>& v,
   const Eigen::Array<Real,N,1>& v2)
{
   Eigen::PermutationMatrix<N> p;
   p.setIdentity();
   std::sort(p.indices().data(), p.indices().data() + p.indices().size(),
             [&v2] (int i, int j) { return v2[i] < v2[j]; });

#if EIGEN_VERSION_AT_LEAST(3,1,4)
   v.matrix().transpose() *= p.inverse();
#else
   Eigen::Map<Eigen::Matrix<Real,N,1> >(v.data()).transpose() *= p.inverse();
#endif
}

/**
 * @brief reorders vector v according to ordering of diagonal elements in mass_matrix
 * @param v vector with elementes to be reordered
 * @param matrix matrix with diagonal elements with reference ordering
 */
template<class Derived>
void reorder_vector(
   Eigen::Array<double,Eigen::MatrixBase<Derived>::RowsAtCompileTime,1>& v,
   const Eigen::MatrixBase<Derived>& matrix)
{
   reorder_vector(v, matrix.diagonal().array().eval());
}

template<class Derived>
std::string print_scientific(const Eigen::DenseBase<Derived>& v,
                             int number_of_digits = std::numeric_limits<typename Derived::Scalar>::digits10 + 1)
{
   std::ostringstream sstr;

   for (int k = 0; k < v.cols(); k++) {
      for (int i = 0; i < v.rows(); i++) {
         sstr << std::setprecision(number_of_digits)
              << std::scientific << v(i,k) << ' ';
      }
      sstr << '\n';
   }

   return sstr.str();
}

} // namespace flexiblesusy

#endif