#include "AsynchronousUpdate.h"
#include <stdio.h>
#include <stdlib.h>
#include "constants.h"
#include <gsl/gsl_rng.h>
#include <omp.h>

int getBool(gsl_rng* r)
{
	return gsl_rng_uniform_int(r,2);
}

void singleStateUpdate(int n, int* state, int** topology, int seed, int* fixed_nodes)
{
	//Perform Asynchronous Updates
	gsl_rng* asyncer = gsl_rng_alloc(gsl_rng_ranlxs2);
	gsl_rng_set(asyncer,seed);

	for (int i = 0; i < ITER_ASYNCHRONOUS; ++i)
	{
		int node_to_update = gsl_rng_uniform_int(asyncer,n);
		if(fixed_nodes[node_to_update] == NORMAL_FLAG)
		{
			int val_temp = 0;

			for (int j = 0; j < n; ++j)
			{
				val_temp+= state[j]*topology[node_to_update][j];
			}

			if(val_temp > 0)
			{
				state[node_to_update] = 1;
			}
			else if(val_temp<0)
			{
				state[node_to_update] = LOWER_EXPRESSION;
			}
		}
	}
	//Free the rng
	gsl_rng_free(asyncer);
}

void getFinalState(int n, int state[n], gsl_rng* rangen, int** topology, int* fixed_nodes, int i)
{
	#pragma omp parallel for
	for (int j = 0; j < n; ++j)
	{
		if(fixed_nodes[j]==NORMAL_FLAG)
		{
			#pragma omp critical(rand)
			{
				state[j] = getBool(rangen);
			}
			if(state[j]==0)
			{
				state[j] = LOWER_EXPRESSION;
			}
		}
		else{
			state[j] = fixed_nodes[j];
		}
	}

	singleStateUpdate(n,state,topology,i+1,fixed_nodes);
}

asynclist* updateAsyncList(int n, int** topology, int* fixed_nodes, int height, int seed_init)
{
	if(height)
	{
		if(height%2)
		{
			gsl_rng* rangen = gsl_rng_alloc(gsl_rng_ranlxs2);
			gsl_rng_set(rangen,seed_init);
			asynclist* to_merge[2];
			int seed[2] = {gsl_rng_get(rangen),gsl_rng_get(rangen)};
			int i;
			#pragma omp parallel for shared(n,topology,fixed_nodes,seed,height) private(i)
			for (i = 0; i < 2; ++i)
			{
				to_merge[i] = updateAsyncList(n,topology,fixed_nodes,height-1,seed[i]);
			}
			return merge_asynclist(to_merge[0],to_merge[1],n);
		}
		else
		{
			gsl_rng* rangen = gsl_rng_alloc(gsl_rng_taus2);
			gsl_rng_set(rangen,seed_init);
			asynclist* to_merge[2];
			int seed[2] = {gsl_rng_get(rangen),gsl_rng_get(rangen)};
			int i;
			#pragma omp parallel for shared(n,topology,fixed_nodes,seed,height) private(i)
			for (i = 0; i < 2; ++i)
			{
				to_merge[i] = updateAsyncList(n,topology,fixed_nodes,height-1,seed[i]);
			}
			return merge_asynclist(to_merge[0],to_merge[1],n);
		}
	}
	else{
		asynclist* list = (asynclist*)malloc(sizeof(asynclist));
		init_asynclist(list);
		int state[n];

		gsl_rng* rangen = gsl_rng_alloc(gsl_rng_ranlxs2);
		gsl_rng_set(rangen,seed_init);

		for (int i = 0; i < n; ++i)
		{
			if(fixed_nodes[i] == NORMAL_FLAG)
			{
				state[i] = gsl_rng_uniform_int(rangen,2);
				if(state[i]==0)
				{
					state[i] = LOWER_EXPRESSION;
				}
			}
			else
			{
				state[i] = fixed_nodes[i];
			}
		}

		singleStateUpdate(n,state,topology,gsl_rng_get(rangen),fixed_nodes);
		list->n_elements = 1;
		asynclistnode* node = (asynclistnode*)malloc(sizeof(asynclistnode));
		if(!node)
		{
			exit(10);
		}
		node->n_occurances = 1;
		node->state = (int*)malloc(n*sizeof(int));
		if(!(node->state))
		{
			exit(11);
		}
		for (int i = 0; i < n; ++i)
		{
			node->state[i] = state[i];
		}
		node->next = NULL;
		node->prev = NULL;
		list->next = node;
		return list;
	}
}

void updateAsyncTree(int n, int** topology, int* fixed_nodes, base* stable)
{
	int iter = (1<<N_SAMPLES);

	gsl_rng* rangen = gsl_rng_alloc(gsl_rng_ranlxs2);
	gsl_rng_set(rangen,0);

	#pragma omp parallel for
	for (int i = 0; i < iter; ++i)
	{
		/*
		gsl_rng* rangen = gsl_rng_alloc(gsl_rng_ranlxs2);
		gsl_rng_set(rangen,i);
		*/
		//int* state=(int*)malloc(n*sizeof(int));
		int state[n];
		getFinalState(n,state,rangen,topology,fixed_nodes,i);

		#pragma omp critical(addToTree)
		{
			add_node(stable,n,state);
		}

		//free(state);
	}
	gsl_rng_free(rangen);
}


#pragma once

#include <bitset>
#include <type_traits>

#include <thrust/execution_policy.h>
#include <thrust/gather.h>
#include <thrust/iterator/counting_iterator.h>
#include <thrust/scatter.h>

#include <cuda/define_specifiers.hpp>

#include <gsl-lite/gsl-lite.hpp>

#include <thrustshift/bit.h>
#include <thrustshift/gather.h>
#include <thrustshift/managed-vector.h>
#include <thrustshift/not-a-vector.h>
#include <thrustshift/scatter.h>

namespace thrustshift {

namespace permutation {

/*! \brief Return \f$ P_b P_a^T \f$
 *
 *   \param result `result[i]` is the index of element \f$ a_i \f$
 *     after permutation \f$ P_b \f$ is applied to the natural order.
 */
template <class PermutationA,
          class PermutationB,
          class ResultPermutation,
          class MemoryResource>
void multiply(PermutationA&& a,
              PermutationB&& b,
              ResultPermutation&& result,
              MemoryResource& memory_resource) {

	gsl_Expects(a.size() == b.size());
	gsl_Expects(result.size() == a.size());

	using ResultIndex =
	    typename std::remove_reference<ResultPermutation>::type::value_type;

	auto [__, tmp] =
	    make_not_a_vector_and_span<ResultIndex>(result.size(), memory_resource);

	auto cit_begin = thrust::make_counting_iterator(ResultIndex(0));
	auto cit_end = thrust::make_counting_iterator(
	    gsl_lite::narrow<ResultIndex>(result.size()));

	// Rather use `.data()` in case the iterator class is not defined
	// in device code
	thrust::scatter(thrust::device, cit_begin, cit_end, b.data(), tmp.data());
	thrust::gather(thrust::device,
	               a.data(),
	               a.data() + a.size(),
	               tmp.data(),
	               result.data());
}

/*! \brief Multiply successive permutations
 *
 *  Assume you have \f$N\f$ permutations \f$P_{N-1},...,P_0\f$
 *  and \f$N\f$ linear maps \f$M_{N-1},...,M_0\f$ where each
 *  linear map \f$M_i\f$ operates on the new order described by
 *  \f$P_i\f$ relatively to the natural order. If every
 *  linear map is applied to a vector \f$x\f$ in natural order
 *  it yields to:
 *
 *  \f[
 *      P_{N-1}^T M_{N-1} P_{N-1} P_{N-2}^T M^{N-2} ... P_0^T M_0 P_0 x
 *  \f]
 *
 *  The \f$P_i\f$ permutation matrices are given in vectorial form \f$p_i\f$
 *  such that \f$(P_i)_mn = 1\f$ if \f$(p_i)_m = n\f$.
 *
 *  \param input_permutations Range of size N with ranges of length L each
 *  \param merged_permutations Range of size N - 1 with ranges of length L each.
 *    merged_permutations[i] = \f$P_{i+1} P_{i}^T\f$
 */
template <class InputPermutations,
          class MergedPermutations,
          class MemoryResource>
void multiply_successive(InputPermutations&& input_permutations,
                         MergedPermutations&& multiplied_permutations,
                         MemoryResource& memory_resource) {

	const auto N = input_permutations.size();

	if (N == 0) {
		return;
	}

	gsl_Expects(N > 0);
	gsl_Expects(N == multiplied_permutations.size() + 1);

	for (size_t i = 0; i < input_permutations.size() - 1; ++i) {
		multiply(input_permutations[i],
		         input_permutations[i + 1],
		         multiplied_permutations[i],
		         memory_resource);
	}
}

/*! \brief Bitoptimized successive pairwise permutation
 *
 *  This class saves a permutation of `I = {0,...,N-1}`, which
 *  is created by successive pairwise swaps of the sequence `I`:
 *
 *  ```cpp
 *  for (int j = 1; j < N; ++j) {
 *      if (do_swap[j - 1]) {
 *          swap(I[j - 1], I[j]);
 *      }
 *  }
 *  ```
 *
 *  The array `do_swap[]` saves if two values are swapped and is saved in
 *  a bit pattern within this class.
 *  This can also be seen as a path in a binary tree with `N` levels because
 *  the internal representation is a bit pattern, which sets the bit if the
 *  swap is not done. `N` is limited by the amount of Bits in `BitPatternT`.
 *  If `operator[]` is called once, no function to change the `do_swap`
 *  array should be called by the user. The state of the class is then a
 *  read-only mode. Moreover the array `I` can only be read successively
 *  by the `operator[]`. Thus reading the pattern starts with the last element
 *  `i = N - 1` and in the next call `i = i - 1` must be used as a function argument.
 *  This is an highly unsafe class design, which is chosen to avoid the
 *  usage of additional registers. E.g. the safety of the class can be
 *  greatly improved if one register is added, which is avoided here
 *  due to performance reasons.
 *
 *  \note When `std::bitset` is supported for device code. This should be
 *        used to save the bit pattern
 *  \note When compiled for device code CUDA currently supports built-in
 *        function to count leading zeros `clz` for `int` and `long long int`, which
 *        then must be used for `BitPatternT`.
 */
template <class BitPatternT>
class bit_successive_permutation_t {

   public:
	CUDA_FHD bit_successive_permutation_t(int N)
	    : bit_pattern_(BitPatternT(1) << (N - 1)) {
		gsl_Expects(N <= sizeof(BitPatternT) * 8);
#ifndef NDEBUG
		N_ = N;
#endif
	}

	CUDA_FHD void do_swap(int j) {
		gsl_Expects(j < sizeof(BitPatternT) * 8);
#ifndef NDEBUG
		gsl_Expects(!read_only_mode_);
		gsl_Expects(j < N_ - 1);
#endif
		bit_pattern_ &= ~(BitPatternT(1) << j);
	}

	CUDA_FHD void do_not_swap(int j) {
		gsl_Expects(j < sizeof(BitPatternT) * 8);
#ifndef NDEBUG
		gsl_Expects(!read_only_mode_);
		gsl_Expects(j < N_ - 1);
#endif
		bit_pattern_ |= (BitPatternT(1) << j);
	}

	CUDA_FHD void set(int j, bool do_swap_) {
		if (do_swap_) {
			do_swap(j);
		}
		else {
			do_not_swap(j);
		}
	}

	CUDA_FHD int operator[](int i) {
		gsl_Expects(i < sizeof(BitPatternT) * 8);
#ifndef NDEBUG
		gsl_Expects(i < N_);
		if (!read_only_mode_) {
			read_only_mode_ = true;
			last_i_ = N_;
		}
		gsl_Expects(i == last_i_ - 1);
		last_i_ = i;
#endif
		gsl_Expects(i >= 0);
		const bool flag = (1ll << i) & bit_pattern_;
		bit_pattern_ &= ~(1ll << i);

		if (flag) {
			const int lz = count_leading_zeros(bit_pattern_);
			return sizeof(BitPatternT) * 8 - lz;
		}
		else {
			return i + 1;
		}
	}

   private:
#ifndef NDEBUG
	bool read_only_mode_ = false;
	int N_;
	int last_i_;
#endif
	BitPatternT bit_pattern_;
};

} // namespace permutation

} // namespace thrustshift


/**
 *
 * @file testing_zpotri.c
 *
 *  PLASMA testing routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Hatem Ltaief
 * @date 2010-11-15
 * @precisions normal z -> c d s
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <core_blas.h>
#include "testing_zmain.h"

enum blas_order_type {
            blas_rowmajor = 101,
            blas_colmajor = 102 };

enum blas_cmach_type {
            blas_base      = 151,
            blas_t         = 152,
            blas_rnd       = 153,
            blas_ieee      = 154,
            blas_emin      = 155,
            blas_emax      = 156,
            blas_eps       = 157,
            blas_prec      = 158,
            blas_underflow = 159,
            blas_overflow  = 160,
            blas_sfmin     = 161};

enum blas_norm_type {
            blas_one_norm       = 171,
            blas_real_one_norm  = 172,
            blas_two_norm       = 173,
            blas_frobenius_norm = 174,
            blas_inf_norm       = 175,
            blas_real_inf_norm  = 176,
            blas_max_norm       = 177,
            blas_real_max_norm  = 178 };

static void
BLAS_error(char *rname, int err, int val, int x) {
  fprintf( stderr, "%s %d %d %d\n", rname, err, val, x );
  abort();
}

static
void
BLAS_zge_norm(enum blas_order_type order, enum blas_norm_type norm,
  int m, int n, const PLASMA_Complex64_t *a, int lda, double *res) {
  int i, j; float anorm, v;
  char rname[] = "BLAS_zge_norm";

  if (order != blas_colmajor) BLAS_error( rname, -1, order, 0 );

  if (norm == blas_frobenius_norm) {
    anorm = 0.0f;
    for (j = n; j; --j) {
      for (i = m; i; --i) {
        v = a[0];
        anorm += v * v;
        a++;
      }
      a += lda - m;
    }
    anorm = sqrt( anorm );
  } else if (norm == blas_inf_norm) {
    anorm = 0.0f;
    for (i = 0; i < m; ++i) {
      v = 0.0f;
      for (j = 0; j < n; ++j) {
        v += cabs( a[i + j * lda] );
      }
      if (v > anorm)
        anorm = v;
    }
  } else if (norm == blas_one_norm) {
    anorm = 0.0f;
    for (i = 0; i < m; ++i) {
      v = 0.0f;
      for (j = 0; j < n; ++j) {
        v += cabs( a[i + j * lda] );
      }
      if (v > anorm)
        anorm = v;
    }
  } else {
    BLAS_error( rname, -2, norm, 0 );
    return;
  }

  if (res) *res = anorm;
}

static
double
BLAS_dpow_di(double x, int n) {
  double rv = 1.0;

  if (n < 0) {
    n = -n;
    x = 1.0 / x;
  }

  for (; n; n >>= 1, x *= x) {
    if (n & 1)
      rv *= x;
  }

  return rv;
}

static
double
BLAS_dfpinfo(enum blas_cmach_type cmach) {
  double eps = 1.0, r = 1.0, o = 1.0, b = 2.0;
  int t = 53, l = 1024, m = -1021;
  char rname[] = "BLAS_dfpinfo";

  if ((sizeof eps) == sizeof(float)) {
    t = 24;
    l = 128;
    m = -125;
  } else {
    t = 53;
    l = 1024;
    m = -1021;
  }

  /* for (i = 0; i < t; ++i) eps *= half; */
  eps = BLAS_dpow_di( b, -t );
  /* for (i = 0; i >= m; --i) r *= half; */
  r = BLAS_dpow_di( b, m-1 );

  o -= eps;
  /* for (i = 0; i < l; ++i) o *= b; */
  o = (o * BLAS_dpow_di( b, l-1 )) * b;

  switch (cmach) {
    case blas_eps: return eps;
    case blas_sfmin: return r;
    default:
      BLAS_error( rname, -1, cmach, 0 );
      break;
  }
  return 0.0;
}

static int check_factorization(int, PLASMA_Complex64_t*, PLASMA_Complex64_t*, int, int, double);
static int check_inverse(int, PLASMA_Complex64_t *, PLASMA_Complex64_t *, int, int, double);

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

    /* Check for number of arguments*/
    if (argc != 2){
        USAGE("POTRI", "N LDA",
              "   - N    : the size of the matrix\n"
              "   - LDA  : leading dimension of the matrix A\n");
        return -1;
    }

    int N     = atoi(argv[0]);
    int LDA   = atoi(argv[1]);
    double eps;
    int uplo;
    int info_inverse, info_factorization;

    PLASMA_Complex64_t *A1   = (PLASMA_Complex64_t *)malloc(LDA*N*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *A2   = (PLASMA_Complex64_t *)malloc(LDA*N*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *WORK = (PLASMA_Complex64_t *)malloc(2*LDA*sizeof(PLASMA_Complex64_t));

    /* Check if unable to allocate memory */
    if ((!A1)||(!A2)){
        printf("Out of Memory \n ");
        return -2;
    }

    eps = BLAS_dfpinfo( blas_eps );

    uplo = PlasmaUpper;

    /*-------------------------------------------------------------
    *  TESTING ZPOTRI
    */

    /* Initialize A1 and A2 for Symmetric Positif Matrix */
    PLASMA_zplghe( (double)N, N, A1, LDA, 51 );
    PLASMA_zlacpy( PlasmaUpperLower, N, N, A1, LDA, A2, LDA );

    printf("\n");
    printf("------ TESTS FOR PLASMA ZPOTRI ROUTINE -------  \n");
    printf("            Size of the Matrix %d by %d\n", N, N);
    printf("\n");
    printf(" The matrix A is randomly generated for each test.\n");
    printf("============\n");
    printf(" The relative machine precision (eps) is to be %e \n", eps);
    printf(" Computational tests pass if scaled residuals are less than 60.\n");

    /* PLASMA ZPOTRF */
    PLASMA_zpotrf(uplo, N, A2, LDA);

    /* Check the factorization */
    info_factorization = check_factorization( N, A1, A2, LDA, uplo, eps);

    /* PLASMA ZPOTRI */
    PLASMA_zpotri(uplo, N, A2, LDA);

    /* Check the inverse */
    info_inverse = check_inverse(N, A1, A2, LDA, uplo, eps);

    if ( (info_inverse == 0) && (info_factorization == 0) ) {
        printf("***************************************************\n");
        printf(" ---- TESTING ZPOTRI ..................... PASSED !\n");
        printf("***************************************************\n");
    }
    else {
        printf("***************************************************\n");
        printf(" - TESTING ZPOTRI ... FAILED !\n");
        printf("***************************************************\n");
    }

    free(A1); free(A2); free(WORK);

    return 0;
}

/*------------------------------------------------------------------------
 *  Check the factorization of the matrix A2
 */
static int check_factorization(int N, PLASMA_Complex64_t *A1, PLASMA_Complex64_t *A2, int LDA, int uplo, double eps)
{
    double Anorm, Rnorm;
    PLASMA_Complex64_t alpha;
    int info_factorization;
    int i,j;

    PLASMA_Complex64_t *Residual = (PLASMA_Complex64_t *)malloc(N*N*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *L1       = (PLASMA_Complex64_t *)malloc(N*N*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *L2       = (PLASMA_Complex64_t *)malloc(N*N*sizeof(PLASMA_Complex64_t));
    double *work              = (double *)malloc(N*sizeof(double));

    memset((void*)L1, 0, N*N*sizeof(PLASMA_Complex64_t));
    memset((void*)L2, 0, N*N*sizeof(PLASMA_Complex64_t));

    alpha= 1.0;

    LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,' ', N, N, A1, LDA, Residual, N);

    /* Dealing with L'L or U'U  */
    if (uplo == PlasmaUpper){
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L1, N);
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L2, N);
        cblas_ztrmm(CblasColMajor, CblasLeft, CblasUpper, CblasConjTrans, CblasNonUnit, N, N, CBLAS_SADDR(alpha), L1, N, L2, N);
    }
    else{
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L1, N);
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L2, N);
        cblas_ztrmm(CblasColMajor, CblasRight, CblasLower, CblasConjTrans, CblasNonUnit, N, N, CBLAS_SADDR(alpha), L1, N, L2, N);
    }

    /* Compute the Residual || A -L'L|| */
    for (i = 0; i < N; i++)
        for (j = 0; j < N; j++)
           Residual[j*N+i] = L2[j*N+i] - Residual[j*N+i];

    BLAS_zge_norm( blas_colmajor, blas_inf_norm, N, N, Residual, N, &Rnorm );
    BLAS_zge_norm( blas_colmajor, blas_inf_norm, N, N, A1, LDA, &Anorm );

    printf("============\n");
    printf("Checking the Cholesky Factorization \n");
    printf("-- ||L'L-A||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));

    if ( isnan(Rnorm/(Anorm*N*eps)) || isinf(Rnorm/(Anorm*N*eps)) || (Rnorm/(Anorm*N*eps) > 60.0) ){
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else{
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(Residual); free(L1); free(L2); free(work);

    return info_factorization;
}

/*------------------------------------------------------------------------
 *  Check the accuracy of the computed inverse
 */

static int check_inverse(int N, PLASMA_Complex64_t *A1, PLASMA_Complex64_t *A2, int LDA, int uplo, double eps )
{
    int info_inverse;
    int i, j;
    double Rnorm, Anorm, Ainvnorm, result;
    PLASMA_Complex64_t alpha, beta, zone;
    PLASMA_Complex64_t *work = (PLASMA_Complex64_t *)malloc(N*N*sizeof(PLASMA_Complex64_t));

    alpha = -1.0;
    beta  = 0.0;
    zone = 1.0;

    /* Rebuild the other part of the inverse matrix */
    if(uplo == PlasmaUpper){
       for(j=0; j<N; j++)
          for(i=0; i<j; i++)
             *(A2+j+i*LDA) = *(A2+i+j*LDA);
       cblas_zhemm(CblasColMajor, CblasLeft, CblasUpper, N, N, CBLAS_SADDR(alpha), A2, LDA, A1, LDA, CBLAS_SADDR(beta), work, N);

    }
    else {
       for(j=0; j<N; j++)
          for(i=j; i<N; i++)
             *(A2+j+i*LDA) = *(A2+i+j*LDA);
       cblas_zhemm(CblasColMajor, CblasLeft, CblasLower, N, N, CBLAS_SADDR(alpha), A2, LDA, A1, LDA, CBLAS_SADDR(beta), work, N);
    }

    /* Add the identity matrix to work */
    for(i=0; i<N; i++)
        *(work+i+i*N) = *(work+i+i*N) + zone;

    BLAS_zge_norm( blas_colmajor, blas_one_norm, N, N, work, N, &Rnorm );
    BLAS_zge_norm( blas_colmajor, blas_one_norm, N, N, A1, LDA, &Anorm );
    BLAS_zge_norm( blas_colmajor, blas_one_norm, N, N, A2, LDA, &Ainvnorm );

    if (getenv("PLASMA_TESTING_VERBOSE"))
      printf( "||A||_1=%f\n||Ainv||_1=%f\n||Id - A*Ainv||_1=%e\n", Anorm, Ainvnorm, Rnorm );

    result = Rnorm / ( (Anorm*Ainvnorm)*N*eps ) ;
    printf("============\n");
    printf("Checking the Residual of the inverse \n");
    printf("-- ||Id - A*Ainv||_1/((||A||_1||Ainv||_1).N.eps) = %e \n", result);

    if (  isnan(Ainvnorm) || isinf(Ainvnorm) || isnan(result) || isinf(result) || (result > 60.0) ) {
        printf("-- The inverse is suspicious ! \n");
        info_inverse = 1;
     }
    else{
        printf("-- The inverse is CORRECT ! \n");
        info_inverse = 0;
    }

    free(work);

    return info_inverse;
}


/***************************************************************************/
/*                                                                         */
/* QR_decomp.c - LU Decomposition class for mruby                          */
/* Copyright (C) 2015 Paolo Bosetti                                        */
/* paolo[dot]bosetti[at]unitn.it                                           */
/* Department of Industrial Engineering, University of Trento              */
/*                                                                         */
/* This library is free software.  You can redistribute it and/or          */
/* modify it under the terms of the GNU GENERAL PUBLIC LICENSE 2.0.        */
/*                                                                         */
/* This library 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           */
/* Artistic License 2.0 for more details.                                  */
/*                                                                         */
/* See the file LICENSE                                                    */
/*                                                                         */
/***************************************************************************/

#include <gsl/gsl_linalg.h>
#include <stdio.h>
#include "matrix.h"
#include "vector.h"
#include "QR_decomp.h"

#ifndef MIN
#define MAX(a, b)                                                              \
  ({                                                                           \
    __typeof__(a) _a = (a);                                                    \
    __typeof__(b) _b = (b);                                                    \
    _a < _b ? _a : _b;                                                         \
  })
#endif

#pragma mark -
#pragma mark • Utilities

// Garbage collector handler, for play_data struct
// if play_data contains other dynamic data, free it too!
// Check it with GC.start
void qr_decomp_destructor(mrb_state *mrb, void *p_) {
  qr_decomp_data_s *lu = (qr_decomp_data_s *)p_;
  gsl_matrix_free(lu->mat);
  gsl_vector_free(lu->tau);
  free(lu);
};

// Creating data type and reference for GC, in a const struct
const struct mrb_data_type qr_decomp_data_type = {"qr_decomp_data",
                                                  qr_decomp_destructor};

// Utility function for getting the struct out of the wrapping IV @data
void mrb_qr_decomp_get_data(mrb_state *mrb, mrb_value self,
                            qr_decomp_data_s **data) {
  mrb_value data_value;
  data_value = mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@data"));

  // Loading data from data_value into p_data:
  Data_Get_Struct(mrb, data_value, &qr_decomp_data_type, *data);
  if (!*data)
    mrb_raise(mrb, E_RUNTIME_ERROR, "Could not access @data");
}

#pragma mark -
#pragma mark • Initializations

// Data Initializer C function (not exposed!)
static mrb_value mrb_qr_initialize(mrb_state *mrb, mrb_value self) {
  mrb_value data_value;            // this IV holds the data
  qr_decomp_data_s *p_data = NULL; // pointer to the C struct
  mrb_value matrix;
  gsl_matrix *p_mat = NULL;
  mrb_int size1, size2;

  mrb_get_args(mrb, "o", &matrix);
  if (!mrb_obj_is_kind_of(mrb, matrix, mrb_class_get(mrb, "Matrix"))) {
    mrb_raise(mrb, E_ARGUMENT_ERROR, "Argument must be a Matrix");
  }

  mrb_matrix_get_data(mrb, matrix, &p_mat);
  // if (p_mat->size1 < p_mat->size2) {
  //   mrb_raise(mrb, E_ARGUMENT_ERROR, "Argument must be square or horizontal
  //   Matrix");
  // }
  size1 = p_mat->size1;
  size2 = p_mat->size2;

  data_value = mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@data"));
  // if @data already exists, free its content:
  if (!mrb_nil_p(data_value)) {
    Data_Get_Struct(mrb, data_value, &qr_decomp_data_type, p_data);
    free(p_data);
  }
  // Allocate and zero-out the data struct:
  p_data = (qr_decomp_data_s *)malloc(sizeof(qr_decomp_data_s));
  if (!p_data) {
    mrb_raise(mrb, E_RUNTIME_ERROR, "Could not allocate @data");
  }
  p_data->size1 = size1;
  p_data->size2 = size2;
  p_data->minsize = MIN(size1, size2);
  p_data->mat = gsl_matrix_calloc(size1, size2);
  p_data->tau = gsl_vector_calloc(p_data->minsize);

  // copy argument matrix into local object data
  gsl_matrix_memcpy(p_data->mat, p_mat);
  // invert in-place
  gsl_linalg_QR_decomp(p_data->mat, p_data->tau);
  mrb_iv_set(mrb, self, mrb_intern_lit(mrb, "@size1"), mrb_fixnum_value(size1));
  mrb_iv_set(mrb, self, mrb_intern_lit(mrb, "@size2"), mrb_fixnum_value(size2));
  mrb_iv_set(mrb, self, mrb_intern_lit(mrb, "@minsize"),
             mrb_fixnum_value(p_data->minsize));
  mrb_iv_set(mrb, self, mrb_intern_lit(mrb, "@residuals"), mrb_nil_value());
  // Wrap struct into @data:
  mrb_iv_set(mrb, self, mrb_intern_lit(mrb, "@data"), // set @data
             mrb_obj_value( // with value hold in struct
                 Data_Wrap_Struct(mrb, mrb->object_class, &qr_decomp_data_type,
                                  p_data)));
  return mrb_nil_value();
}

#pragma mark -
#pragma mark • Accessors

static mrb_value mrb_qr_residuals(mrb_state *mrb, mrb_value self) {
  return mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@residuals"));
}

static mrb_value mrb_qr_minsize(mrb_state *mrb, mrb_value self) {
  return mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@minsize"));
}

static mrb_value mrb_qr_size1(mrb_state *mrb, mrb_value self) {
  return mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@size1"));
}

static mrb_value mrb_qr_size2(mrb_state *mrb, mrb_value self) {
  return mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@size2"));
}

static mrb_value mrb_qr_matrix(mrb_state *mrb, mrb_value self) {
  mrb_value result;
  qr_decomp_data_s *p_data = NULL;
  gsl_matrix *p_res = NULL;
  mrb_value args[2];

  // call utility for unwrapping @data into p_data:
  mrb_qr_decomp_get_data(mrb, self, &p_data);
  args[0] = mrb_fixnum_value(p_data->size1);
  args[1] = mrb_fixnum_value(p_data->size2);
  result = mrb_obj_new(mrb, mrb_class_get(mrb, "Matrix"), 2, args);
  mrb_matrix_get_data(mrb, result, &p_res);
  gsl_matrix_memcpy(p_res, p_data->mat);
  return result;
}

static mrb_value mrb_qr_tau(mrb_state *mrb, mrb_value self) {
  mrb_value result;
  qr_decomp_data_s *p_data = NULL;
  gsl_vector *p_res = NULL;
  mrb_value args[1];

  // call utility for unwrapping @data into p_data:
  mrb_qr_decomp_get_data(mrb, self, &p_data);
  args[0] = mrb_fixnum_value(p_data->minsize);
  result = mrb_obj_new(mrb, mrb_class_get(mrb, "Vector"), 1, args);
  mrb_vector_get_data(mrb, result, &p_res);
  gsl_vector_memcpy(p_res, p_data->tau);
  return result;
}

#pragma mark -
#pragma mark • Operations

// int gsl_linalg_QR_solve (const gsl_matrix * QR, const gsl_vector * tau, const
// gsl_vector * b, gsl_vector * x)
static mrb_value mrb_qr_solve(mrb_state *mrb, mrb_value self) {
  mrb_value result, b_vec;
  qr_decomp_data_s *p_data = NULL;
  gsl_vector *p_result = NULL, *p_b = NULL;
  mrb_value args[1];

  mrb_get_args(mrb, "o", &b_vec);
  if (!mrb_obj_is_kind_of(mrb, b_vec, mrb_class_get(mrb, "Vector"))) {
    mrb_raise(mrb, E_ARGUMENT_ERROR, "Argument must be a Vector");
  }

  // call utility for unwrapping @data into p_data:
  mrb_qr_decomp_get_data(mrb, self, &p_data);
  if (p_data->size1 != p_data->size2) {
    mrb_raise(mrb, E_QR_DECOMP_ERROR, "Matrix must be square");
  }

  args[0] = mrb_fixnum_value(p_data->tau->size);
  result = mrb_obj_new(mrb, mrb_class_get(mrb, "Vector"), 1, args);
  mrb_vector_get_data(mrb, result, &p_result);
  mrb_vector_get_data(mrb, b_vec, &p_b);

  if (gsl_linalg_QR_solve(p_data->mat, p_data->tau, p_b, p_result)) {
    mrb_raise(mrb, E_QR_DECOMP_ERROR, "Singular matrix");
  }
  mrb_iv_set(mrb, self, mrb_intern_lit(mrb, "@residuals"), mrb_nil_value());
  return result;
}

// int gsl_linalg_QR_lssolve (const gsl_matrix * QR, const gsl_vector * tau,
// const gsl_vector * b, gsl_vector * x, gsl_vector * residual)
static mrb_value mrb_qr_lssolve(mrb_state *mrb, mrb_value self) {
  mrb_value result, b_vec, residuals;
  qr_decomp_data_s *p_data = NULL;
  gsl_vector *p_result = NULL, *p_b = NULL, *p_residuals;
  mrb_value args[1];

  mrb_get_args(mrb, "o", &b_vec);
  if (!mrb_obj_is_kind_of(mrb, b_vec, mrb_class_get(mrb, "Vector"))) {
    mrb_raise(mrb, E_ARGUMENT_ERROR, "Argument must be a Vector");
  }

  // call utility for unwrapping @data into p_data:
  mrb_qr_decomp_get_data(mrb, self, &p_data);
  if (p_data->size1 <= p_data->size2) {
    mrb_raise(mrb, E_QR_DECOMP_ERROR,
              "Matrix must have more rows than columns");
  }

  args[0] = mrb_fixnum_value(p_data->size2);
  result = mrb_obj_new(mrb, mrb_class_get(mrb, "Vector"), 1, args);
  mrb_vector_get_data(mrb, result, &p_result);
  mrb_vector_get_data(mrb, b_vec, &p_b);

  args[0] = mrb_fixnum_value(p_b->size);
  residuals = mrb_obj_new(mrb, mrb_class_get(mrb, "Vector"), 1, args);
  mrb_vector_get_data(mrb, residuals, &p_residuals);
  mrb_iv_set(mrb, self, mrb_intern_lit(mrb, "@residuals"), residuals);

  if (gsl_linalg_QR_lssolve(p_data->mat, p_data->tau, p_b, p_result,
                            p_residuals)) {
    mrb_raise(mrb, E_QR_DECOMP_ERROR, "Singular matrix");
  }
  return result;
}

#pragma mark -
#pragma mark • Gem setup

void mrb_gsl_qr_decomp_init(mrb_state *mrb) {
  struct RClass *lu;

  mrb_load_string(mrb, "class QRDecompError < Exception; end");

  lu = mrb_define_class(mrb, "QRDecomp", mrb->object_class);
  mrb_define_method(mrb, lu, "residuals", mrb_qr_residuals, MRB_ARGS_NONE());
  mrb_define_method(mrb, lu, "matrix", mrb_qr_matrix, MRB_ARGS_NONE());
  mrb_define_method(mrb, lu, "tau", mrb_qr_tau, MRB_ARGS_NONE());
  mrb_define_method(mrb, lu, "size1", mrb_qr_size1, MRB_ARGS_NONE());
  mrb_define_method(mrb, lu, "size2", mrb_qr_size2, MRB_ARGS_NONE());
  mrb_define_method(mrb, lu, "minsize", mrb_qr_minsize, MRB_ARGS_NONE());

  mrb_define_method(mrb, lu, "initialize", mrb_qr_initialize, MRB_ARGS_REQ(1));
  mrb_define_method(mrb, lu, "solve", mrb_qr_solve, MRB_ARGS_REQ(1));
  mrb_define_method(mrb, lu, "lssolve", mrb_qr_lssolve, MRB_ARGS_REQ(1));
}


///
/// @file
///
/// @author Angelika Schwarz (angies@cs.umu.se), Umeå University
///
/// @internal LICENSE
///
/// Copyright (c) 2019-2020, Umeå Universitet
///
/// Redistribution and use in source and binary forms, with or without
/// modification, are permitted provided that the following conditions are met:
///
/// 1. Redistributions of source code must retain the above copyright notice,
///    this list of conditions and the following disclaimer.
///
/// 2. 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.
///
/// 3. 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 <starneig_config.h>
#include <starneig/configuration.h>

#include "typedefs.h"
#include "cpu.h"
// #include "tasks.h"  // <--- moved to misc/
#include "robust.h"

#include "../../common/common.h"
#include "../../common/tiles.h"
#include <starpu.h>
#include <cblas.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <float.h>

static double vec_real_infnorm(int n, const double *x)
{
    double norm = 0.0;
    for (int i = 0; i < n; ++i) {
        double abs = fabs(x[i]);
        if (abs > norm)
            norm = abs;
    }

    return norm;
}

static double vec_cmplx_infnorm(int n, const double *x_re, const double *x_im)
{
    double norm = 0.0;
    for (int i = 0; i < n; ++i) {
        // Compute len = sqrt(x_re[i]*x_re[i]+x_im[i]*x_im[i]) robustly.
        double maxabs = MAX(fabs(x_re[i]), fabs(x_im[i]));
        double len = maxabs*sqrt(  (x_re[i]/maxabs)*(x_re[i]/maxabs)
                                 + (x_im[i]/maxabs)*(x_im[i]/maxabs));

        if (len > norm)
            norm = len;
    }

    return norm;
}

static double mat_infnorm(int m, int n, const double *A, int ldA)
{
#define A(i,j) A[(i) + (j) * (size_t)ldA]

    double *rowsums = calloc(m, sizeof(double));

    for (int j = 0; j < n; ++j)
        for (int i = 0; i < m; i++)
            rowsums[i] += fabs(A(i,j));

    double norm = rowsums[0];
    for (int i = 1; i < m; i++)
        if (rowsums[i] > norm)
            norm = rowsums[i];

    free(rowsums);

    return norm;

#undef A
}

static void find_max(
    int num_rows, int num_selected, int n,
    const double *restrict const X, int ldX,
    const int *restrict const lambda_type, const int *restrict const selected,
    double *restrict emax)
{
    int si = num_selected-1;
    for (int ki = n-1; ki >= 0; ki--) {
        if (!selected[ki]) {
            // Proceed with the next eigenvalue.
            if (lambda_type[ki] == 1) { // CMPLX
                // A complex conjugate pair of eigenvalues is not selected, so
                // skip the next diagonal entry.
                ki--;
            }
        }
        else { // ki-th eigenvalue is selected
            if (lambda_type[ki] == 0) { // REAL
                // Locate eigenvector to ki-th eigenvalue.
               const double *X_re = X+si*(size_t)ldX;

               // Find max entry.
               emax[si] = vec_real_infnorm(num_rows, X_re);
               si--;
            }
            else { // CMPLX
                // Locate real and imaginary part of complex eigenvector for
                // complex conjugate pair of eigenvalues (ki, ki-1).
                const double *X_re = X+(si-1)*(size_t)ldX;
                const double *X_im = X+si*(size_t)ldX;

                // Find max entry.
                emax[si] = 0.0;
                for (int i = 0; i < num_rows; i++)
                    if (fabs(X_re[i])+fabs(X_im[i]) > emax[si])
                        emax[si] = fabs(X_re[i])+fabs(X_im[i]);

                // Duplicate max entry for real and imaginary column.
                emax[si-1] = emax[si];

                ki--;
                si -= 2;
            }
        }
    }
}

void starneig_eigvec_std_unify_scaling(int num_tiles, int *first_row, int *first_col,
    scaling_t *restrict scales,
    double *restrict X, int ldX,
    const int *restrict lambda_type, const int *restrict selected)
{
    // TODO: Is it possible to add a #pragma parallel for here?

    const int num_selected = first_col[num_tiles];

    //
    // Compute the most constraining scaling factor.
    //
    scaling_t *smin = (scaling_t *) malloc(num_selected*sizeof(scaling_t));
    starneig_eigvec_std_init_scaling_factor(num_selected, smin);

    starneig_eigvec_std_find_smallest_scaling(num_tiles, num_selected, scales, smin);

#ifndef STARNEIG_ENABLE_INTEGER_SCALING

    //
    // Check if the range of double-precision scaling factors was sufficient
    // and replace flushed entries with 1/Omega to avoid NaNs.
    //

    for (int i = 0; i < num_selected; i++) {
        if (smin[i] == 0.0) {
            if (lambda_type[i] == 1) { // CMPLX
                starneig_warning("Scaling factor of complex eigenvector "
                                 "X(:,%d:%d) was flushed. Rerun with integer "
                                 "scaling factors.", i, i+1);
                smin[i] = DBL_MIN;
                smin[i+1] = DBL_MIN;
                i++;
            }
            else { // REAL
                starneig_warning("Scaling factor of real eigenvector X(:,%d) "
                                 "was flushed. Rerun with integer "
                                 "scaling factors.", i);
                smin[i] = DBL_MIN;

            }
        }
    }

    for (int i = 0; i < (size_t)num_tiles*num_selected;i++)
        if (scales[i] == 0.0)
            scales[i] = DBL_MIN;

#endif

    double *tmp = (double *) malloc(num_selected * sizeof(double));
    double *emax = (double *) malloc(num_selected * sizeof(double));
    memset(emax, 0.0, num_selected * sizeof(double));

#define scales(col, tilerow) scales[(col) + (tilerow) * (size_t)num_selected]

    for (int blkj = 0; blkj < num_tiles; blkj++) {
        for (int blki = 0; blki <= blkj; blki++) {
            const int num_rows = first_row[blki+1]-first_row[blki];
            const int num_sel = first_col[blkj+1]-first_col[blkj];
            const int num_cols = first_row[blkj+1]-first_row[blkj];

            double *tile = X+(size_t)first_col[blkj]*ldX+first_row[blki];
            const int *lambda_type_tile = lambda_type+first_row[blkj];
            const int *selected_tile = selected+first_row[blkj];
            double *tmp_tile = tmp+first_col[blkj];
            memset(tmp_tile, 0.0, num_sel*sizeof(double));

            find_max(num_rows, num_sel, num_cols,
                tile, ldX, lambda_type_tile, selected_tile, tmp_tile);

            // Reduce to maximum normalization factor.
            for (int j = first_col[blkj]; j < first_col[blkj+1]; j++) {
                // Compute normalization factor simulating consistent scaling.
               double s = starneig_eigvec_std_compute_upscaling(smin[j], scales(j, blki));
               emax[j] = MAX(s * tmp[j], emax[j]);
            }
        }

        // Apply scaling.
        for (int blki = 0; blki <= blkj; blki++) {
            const int num_rows = first_row[blki+1]-first_row[blki];
            const int num_cols = first_col[blkj+1]-first_col[blkj];

            // Scale column.
            for (int j = 0; j < num_cols; j++) {
                // The current column index.
                size_t col = (size_t)first_col[blkj]+j;

                // The current column.
                double *x = X+col*ldX+first_row[blki];
                double s =
                    starneig_eigvec_std_compute_upscaling(smin[col], scales(col, blki));

                // Avoid oo.
                if (isinf(s))
                    s = DBL_MIN;

                for (int i = 0; i < num_rows; i++)
                    x[i] = (s*x[i])/emax[col];
            }
        }
    }

    free(tmp);
    free(emax);
#undef scales
}

void starneig_eigvec_std_cpu_bound_DM(void *buffers[], void *cl_args)
{
    struct packing_info packing_info;
    starpu_codelet_unpack_args(cl_args, &packing_info);

    // extract tile dimensions from the packing information struct
    int m = packing_info.rend - packing_info.rbegin;
    int n = packing_info.cend - packing_info.cbegin;

    int k = 0;

    double *norm = (double *) STARPU_VARIABLE_GET_PTR(buffers[k]);
    k++;

    double *T = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldT = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    struct starpu_matrix_interface **A_i =
        (struct starpu_matrix_interface **)buffers + k;
    k += packing_info.handles;

    starneig_join_diag_window(&packing_info, ldT, A_i, T, 0);

    *norm = mat_infnorm(m, n, T, ldT);
}

void starneig_eigvec_std_cpu_bound(void *buffers[], void *cl_args)
{
    double *T = (double *) STARPU_MATRIX_GET_PTR(buffers[0]);
    int ldT = STARPU_MATRIX_GET_LD(buffers[0]);
    int m = STARPU_MATRIX_GET_NX(buffers[0]);
    int n = STARPU_MATRIX_GET_NY(buffers[0]);

    double *tnorm = (double *) STARPU_VARIABLE_GET_PTR(buffers[1]);
    double ub = mat_infnorm(m, n, T, ldT);
    *tnorm = ub;
}

static void backsolve(
    int n, const double *restrict T, int ldT, double tnorm,
    double *restrict X, int ldX, scaling_t *restrict const scales,
    double *restrict Xnorms, const int *restrict lambda_type,
    const int *restrict selected, int num_selected,
    double smlnum, int *restrict infos)
{
#define T(i,j) T[(i) + (j) * (size_t)ldT]

    // Solve T * X = X * \Lambda.

    // The i-th selected eigenvalue.
    int si = num_selected-1;

    // Loop over eigenvalues from bottom to top.
    for (int ki = n-1; ki >= 0; ki--) {
        if (!selected[ki]) {
            // Proceed with the next eigenvalue.
            if (lambda_type[ki] == 1) { // CMPLX
                // A complex conjugate pair of eigenvalues is not selected,
                // so skip the next diagonal entry.
                ki--;
            }
        }
        else { // ki-th eigenvalue is selected

            // Error flag.
            int info = 0;

            if (lambda_type[ki] == 0) {
                // Compute a real right eigenvector.
                double lambda = T(ki,ki);

                // Locate eigenvector to ki-th eigenvalue.
                double *X_re = X+si*(size_t)ldX;

                // Locate corresponding scaling factor.
                scaling_t *beta = scales+si;

                // Critical threshold to detect unsafe divisions.
                const double smin = MAX(DBL_EPSILON/2*fabs(lambda), smlnum);

                // Form right-hand side.
                X_re[ki] = 1.0;
                for (int i = 0; i < ki; i++)
                    X_re[i] = -T(i, ki);
                for (int i = ki+1; i < n; i++)
                    X_re[i] = 0.0;

                // Compute norm of entire vector.
                double norm = vec_real_infnorm(ki+1, X_re);

                // Solve the upper quasi-triangular system.
                // (T(0:ki-1,0:ki-1) - lambda I) \ X.
                for (int j = ki-1; j >= 0; j--) {
                    // if next block is 1-by-1 diagonal block:
                    if (lambda_type[j] == 0) { // REAL
                        scaling_t phi;
                        starneig_eigvec_std_init_scaling_factor(1, &phi);
                        info |= starneig_eigvec_std_solve_1x1_real_system(
                            smin, T(j,j), lambda, X_re+j, &phi);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Scale remaining parts of the vector.
                        starneig_eigvec_std_scale(j, X_re, &phi);
                        starneig_eigvec_std_scale(ki-(j+1), X_re+(j+1), &phi);
                        starneig_eigvec_std_update_norm(&norm, phi);

                        // Protect against overflow in the linear update.
                        phi =
                            starneig_eigvec_std_protect_update(tnorm, fabs(X_re[j]), norm);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Apply the scaling to the whole eigenvector.
                        starneig_eigvec_std_scale(ki+1, X_re, &phi);

                        // Now it is safe to execute the linear update.
                        for (int i = 0; i < j; i++)
                            X_re[i] = X_re[i]-T(i,j)*X_re[j];

                        // Recompute norm excluding the entries j:n.
                        norm = vec_real_infnorm(j, X_re);
                    }
                    // if next block is 2-by-2 diagonal block:
                    else {
                        scaling_t phi;
                        starneig_eigvec_std_init_scaling_factor(1, &phi);
                        info |= starneig_eigvec_std_solve_2x2_real_system(smin,
                            &T(j-1,j-1), ldT, lambda, &X_re[j-1], &phi);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Scale remaining parts of vector.
                        starneig_eigvec_std_scale(j-1, X_re, &phi);
                        starneig_eigvec_std_scale(ki-(j+1), X_re+(j+1), &phi);
                        starneig_eigvec_std_update_norm(&norm, phi);

                        // Protect against overflow in the first linear update.
                        phi = starneig_eigvec_std_protect_update(
                            tnorm, fabs(X_re[j-1]), norm);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Apply the scaling to the whole eigenvector.
                        starneig_eigvec_std_scale(ki+1, X_re, &phi);

                        // Now it is safe to execute the first linear update.
                        for (int i = 0; i < j-1; i++)
                            X_re[i] = X_re[i]-T(i,j-1)*X_re[j-1];

                        // Recompute norm excluding the entries j:n.
                        norm = vec_real_infnorm(j, X_re);

                        // Protect against overflow in the second linear update.
                        phi =
                            starneig_eigvec_std_protect_update(tnorm, fabs(X_re[j]), norm);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Apply the scaling to the whole eigenvector.
                        starneig_eigvec_std_scale(ki+1, X_re, &phi);

                        // Now it is safe to execute the second linear update.
                        for (int i = 0; i < j-1; i++)
                            X_re[i] = X_re[i]-T(i,j)*X_re[j];

                        // Recompute norm excluding the entries j-1:n.
                        norm = vec_real_infnorm(j-1, X_re);

                        // We processed a 2-by-2 block, so skip the next entry.
                        j--;
                    }
                }

                // The ki-th real eigenvector has been computed. Recompute norm.
                Xnorms[si] = vec_real_infnorm(ki+1, X_re);

                // Record error flag.
                infos[si] = info;

                // This eigenvector spans 1 column. Update selected counter.
                si--;
            }
            else { // lambda_type[ki] == CMPLX
                // Locate real and imaginary part of complex eigenvector for
                // complex conjugate pair of eigenvalues (ki, ki-1).
                double *X_re = X+(si-1)*(size_t)ldX;
                double *X_im = X+si*(size_t)ldX;

                // Locate corresponding scaling factor, one per complex pair.
                scaling_t *beta = scales+si;

                // Compute eigenvalue as lambda = lambda_re + i * lambda_im.
                // A 2-by-2 block in canonical Schur form has the shape
                // [ T(ki-1,ki-1) T(ki-1,ki) ] = [ a b ]  or [ a -b ]
                // [ T(ki, ki-1)  T(ki, ki)  ]   [-c a ]     [ c  a ].
                double lambda_re = T(ki,ki);
                double lambda_im =
                    sqrt(fabs(T(ki,ki-1)))*sqrt(fabs(T(ki-1,ki)));

                // Critical threshold to detect unsafe divisions.
                const double smin = MAX(
                    DBL_EPSILON/2*(fabs(lambda_re)+fabs(lambda_im)), smlnum);

                // Form right-hand side.
                if (fabs(T(ki-1,ki)) >= fabs(T(ki,ki-1))) {
                    X_re[ki-1] = 1.0;
                    X_im[ki] = lambda_im/T(ki-1,ki);
                }
                else {
                    X_re[ki-1] = -lambda_im/T(ki,ki-1);
                    X_im[ki] = 1.0;
                }
                X_re[ki] = 0.0;
                X_im[ki-1] = 0.0;
                for (int i = 0; i < ki-1; i++) {
                    X_re[i] = -X_re[ki-1]*T(i,ki-1);
                    X_im[i] = -X_im[ki]*T(i,ki);
                }
                for (int i = ki+1; i < n; i++) {
                    X_re[i] = 0.0;
                    X_im[i] = 0.0;
                }

                // Compute norm of the entire vector.
                double norm = vec_cmplx_infnorm(ki+1, X_re, X_im);

                // Solve the upper quasi-triangular system
                // (T(0:ki-2,0:ki-2) - (lambda * I) \ (X_re + i * X_im).

                // Loop over triangular matrix above the eigenvalue. Note ki-2!
                for (int j = ki-2; j >= 0; j--) {
                    // If next block is 1-by-1 diagonal bock:
                    if (lambda_type[j] == 0) { // REAL
                        scaling_t phi;
                        starneig_eigvec_std_init_scaling_factor(1, &phi);
                        info |= starneig_eigvec_std_solve_1x1_cmplx_system(
                            smin, T(j,j), lambda_re, lambda_im,
                            X_re+j, X_im+j, &phi);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Scale the remaining parts of the vector.
                        starneig_eigvec_std_scale(j, X_re, &phi);
                        starneig_eigvec_std_scale(ki-(j+1), X_re+(j+1), &phi);
                        starneig_eigvec_std_scale(j, X_im, &phi);
                        starneig_eigvec_std_scale(ki-(j+1), X_im+(j+1), &phi);
                        starneig_eigvec_std_update_norm(&norm, phi);

                        // Protect against overflow in the linear update.
                        double absmax = MAX(fabs(X_re[j]), fabs(X_im[j]));
                        phi = starneig_eigvec_std_protect_update(tnorm, absmax, norm);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Apply scaling to the whole eigenvector.
                        starneig_eigvec_std_scale(ki+1, X_re, &phi);
                        starneig_eigvec_std_scale(ki+1, X_im, &phi);

                        // Now it is safe to execute the linear update.
                        for (int i = 0; i < j; i++) {
                            X_re[i] = X_re[i]-T(i,j)*X_re[j];
                            X_im[i] = X_im[i]-T(i,j)*X_im[j];
                        }

                        // Recompute norm excluding the entries j:n.
                        norm = vec_cmplx_infnorm(j, X_re, X_im);
                    }
                    // If next block is 2-by-2 diagonal block:
                    else {
                        scaling_t phi;
                        starneig_eigvec_std_init_scaling_factor(1, &phi);
                        info |= starneig_eigvec_std_solve_2x2_cmplx_system(
                            smin, &T(j-1,j-1), ldT, lambda_re, lambda_im,
                            X_re+j-1, X_im+j-1, &phi);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Scale the remaining parts of the vector.
                        starneig_eigvec_std_scale(j-1, X_re, &phi);
                        starneig_eigvec_std_scale(ki-j, X_re+(j+1), &phi);
                        starneig_eigvec_std_scale(j-1, X_im, &phi);
                        starneig_eigvec_std_scale(ki-j, X_im+(j+1), &phi);
                        starneig_eigvec_std_update_norm(&norm, phi);

                        // Protect against overflow in the first linear update.
                        double absmax = MAX(fabs(X_re[j-1]), fabs(X_im[j-1]));
                        phi = starneig_eigvec_std_protect_update(tnorm, absmax, norm);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Apply scaling to the whole eigenvector.
                        starneig_eigvec_std_scale(ki+1, X_re, &phi);
                        starneig_eigvec_std_scale(ki+1, X_im, &phi);

                        // Now it is safe to execute the first linear update.
                        for (int i = 0; i < j-1; i++) {
                            X_re[i] = X_re[i]-T(i,j-1)*X_re[j-1];
                            X_im[i] = X_im[i]-T(i,j-1)*X_im[j-1];
                        }

                        // Recompute norm excluding the entries j+1:n.
                        norm = vec_cmplx_infnorm(j+1, X_re, X_im);

                        // Protect against overflow in the second linear update.
                        absmax = MAX(fabs(X_re[j]), fabs(X_im[j]));
                        phi = starneig_eigvec_std_protect_update(tnorm, absmax, norm);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Apply scaling to the whole eigenvector.
                        starneig_eigvec_std_scale(ki+1, X_re, &phi);
                        starneig_eigvec_std_scale(ki+1, X_im, &phi);

                        // Now it is safe to execute the second linear update.
                        for (int i = 0; i < j-1; i++) {
                            X_re[i] = X_re[i]-T(i,j)*X_re[j];
                            X_im[i] = X_im[i]-T(i,j)*X_im[j];
                        }

                        // Recompute norm excluding the entries j:n.
                        norm = vec_cmplx_infnorm(j-1, X_re, X_im);

                        // We processed a 2-by-2 block, so skip the next entry.
                        j--;
                    }
                }

                // Note that the second column of a complex conjugate pair is
                // never allocated or computed. Obtaining it is left to the
                // user. If the positions si - 1, si mark a 2-by-2 block, then
                // the eigenvector corresponding to lambda = alpha + i beta
                // is X(:, si - 1) + i * X(:, si).
                // The complex conjugate eigenvector corresponding to
                // lambda = alpha - i beta can be derived as
                // conj(X) := X(:, si -1) - i * X(:, si).

                // The real part and the imaginary part are scaled alike.
                scales[si-1] = scales[si];

                // The ki-th complex eigenvector has been computed. Recompute
                // norm of the complex eigenvector.
                Xnorms[si] = vec_cmplx_infnorm(ki+1, X_re, X_im);
                Xnorms[si-1] = Xnorms[si];

                // Record error flag.
                infos[si] = info;
                infos[si-1] = info;

                // We processed a complex conjugate pair of eigenvalues,
                // so skip the next entry.
                ki--;

                // This eigenvector spans 2 columns. Update selected counter.
                si -= 2;
            }
        }
    }

    // Note that the eigenvectors are not normalized.

#undef T

}

void starneig_eigvec_std_cpu_backsolve(void *buffers[], void *cl_args)
{
    double *T = (double *) STARPU_MATRIX_GET_PTR(buffers[0]);
    int n = STARPU_MATRIX_GET_NX(buffers[0]);
    int ldT = STARPU_MATRIX_GET_LD(buffers[0]);

    double *ubT = (double *) STARPU_VARIABLE_GET_PTR(buffers[1]);
    double tnorm = *ubT;

    double *X = (double *) STARPU_MATRIX_GET_PTR(buffers[2]);
    int num_selected = STARPU_MATRIX_GET_NY(buffers[2]);
    int ldX = STARPU_MATRIX_GET_LD(buffers[2]);

    scaling_t *scales = (scaling_t *) STARPU_VECTOR_GET_PTR(buffers[3]);

    double *Xnorms = (double *) STARPU_VECTOR_GET_PTR(buffers[4]);

    int *lambda_type = (int *) STARPU_VECTOR_GET_PTR(buffers[5]);

    int *selected = (int *) STARPU_VECTOR_GET_PTR(buffers[6]);

    int *infos = (int *) STARPU_VECTOR_GET_PTR(buffers[7]);

    double smlnum;
    starpu_codelet_unpack_args(cl_args, &smlnum);

    backsolve(n, T, ldT, tnorm, X, ldX, scales, Xnorms, lambda_type,
        selected, num_selected, smlnum, infos);
}

void starneig_eigvec_std_cpu_solve(void *buffers[], void *cl_args)
{
    double *T = (double *) STARPU_MATRIX_GET_PTR(buffers[0]);
    int n = STARPU_MATRIX_GET_NX(buffers[0]);
    int ldT = STARPU_MATRIX_GET_LD(buffers[0]);

    double *ubT = (double *) STARPU_VARIABLE_GET_PTR(buffers[1]);
    double tnorm = *ubT;

    double *X = (double *) STARPU_MATRIX_GET_PTR(buffers[2]);
    int num_selected = STARPU_MATRIX_GET_NY(buffers[2]);
    int ldX = STARPU_MATRIX_GET_LD(buffers[2]);

    scaling_t *scales = (scaling_t *) STARPU_VECTOR_GET_PTR(buffers[3]);

    double *Xnorms = (double *) STARPU_VECTOR_GET_PTR(buffers[4]);

    double *lambda = (double *) STARPU_VECTOR_GET_PTR(buffers[5]);
    int num_rhs = STARPU_VECTOR_GET_NX(buffers[5]);

    int *lambda_type = (int *) STARPU_VECTOR_GET_PTR(buffers[6]);

    int *selected = (int *) STARPU_VECTOR_GET_PTR(buffers[7]);

    int *diag_type = (int *) STARPU_VECTOR_GET_PTR(buffers[8]);

    int *infos = (int *) STARPU_VECTOR_GET_PTR(buffers[9]);

    double smlnum;
    starpu_codelet_unpack_args(cl_args, &smlnum);

#define T(i,j) T[(i) + (j) * (size_t)ldT]
    // The i-th selected eigenvalue.
    int si = num_selected-1;

    // Loop over eigenvalues.
    for (int k = num_rhs-1; k >= 0; k--) {
        if (!selected[k]) {
            // Proceed with the next eigenvalue.
            if (lambda_type[k] == 1) { // CMPLX
                // A complex conjugate pair of eigenvalues is not selected,
                // so skip the next diagonal entry.
                k--;
            }
        }
        else { // k-th eigenvalue is selected

            // Error flag.
            int info = 0;

            if (lambda_type[k] == 0) { // REAL
                // Locate eigenvector to k-th eigenvalue.
                double *X_re = X+si*(size_t)ldX;

                // Locate corresponding scaling factor.
                scaling_t *beta = scales+si;

                // Compute norm of entire vector.
                double norm = vec_real_infnorm(n, X_re);

                // Critical threshold to detect unsafe divisions.
                const double smin = MAX(DBL_EPSILON/2*fabs(lambda[k]), smlnum);

                for (int j = n-1; j >= 0; j--) {
                    // if next block is 1-by-1 diagonal block:
                    if (diag_type[j] == 0) { // REAL
                        scaling_t phi;
                        starneig_eigvec_std_init_scaling_factor(1, &phi);
                        info |= starneig_eigvec_std_solve_1x1_real_system(
                            smin, T(j,j), lambda[k], X_re+j, &phi);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Scale remaining parts of vector.
                        starneig_eigvec_std_scale(j, X_re, &phi);
                        starneig_eigvec_std_scale(n-(j+1), X_re+(j+1), &phi);
                        starneig_eigvec_std_update_norm(&norm, phi);

                        // Protect against overflow in the linear update.
                        phi =
                            starneig_eigvec_std_protect_update(tnorm, fabs(X_re[j]), norm);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Apply the scaling to the whole eigenvector.
                        starneig_eigvec_std_scale(n, X_re, &phi);

                        // Now it is safe to execute the linear update.
                        for (int i = 0; i < j; i++)
                            X_re[i] = X_re[i]-T(i,j)*X_re[j];

                        // Recompute norm excluding the entries j:n.
                        norm = vec_real_infnorm(j, X_re);
                    }
                    // if next block is 2-by-2 block:
                    else {
                        scaling_t phi;
                        starneig_eigvec_std_init_scaling_factor(1, &phi);
                        info |= starneig_eigvec_std_solve_2x2_real_system(smin,
                            &T(j-1,j-1), ldT, lambda[k], &X_re[j-1], &phi);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Scale remaining parts of vector.
                        starneig_eigvec_std_scale(j-1, X_re, &phi);
                        starneig_eigvec_std_scale(n-(j+1), X_re+(j+1), &phi);
                        starneig_eigvec_std_update_norm(&norm, phi);

                        // Protect first linear update against overflow.
                        phi = starneig_eigvec_std_protect_update(
                            tnorm, fabs(X_re[j-1]), norm);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Apply the scaling to the whole eigenvector.
                        starneig_eigvec_std_scale(n, X_re, &phi);

                        // Now it is safe to execute the linear udpate.
                        for (int i = 0; i < j-1; i++)
                            X_re[i] = X_re[i]-T(i,j-1)*X_re[j-1];

                        // Recompute norm excluding the entries j:n.
                        norm = vec_real_infnorm(j, X_re);

                        // Protect second linear update against overflow.
                        phi =
                            starneig_eigvec_std_protect_update(tnorm, fabs(X_re[j]), norm);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Apply the scaling to the whole eigenvector.
                        starneig_eigvec_std_scale(n, X_re, &phi);

                        // Now it is safe to execute the linear update.
                        for (int i = 0; i < j - 1; i++)
                            X_re[i] = X_re[i]-T(i,j)*X_re[j];

                        // Recompute norm excluding the entries j-1:n.
                        norm = vec_real_infnorm(j-1, X_re);

                        // We processed a 2-by-2 block, so skip the next
                        // diagonal entry.
                        j--;
                    }
                }

                // The k-th real eigenvector has been computed.
                // Recompute norm.
                Xnorms[si] = vec_real_infnorm(n, X_re);

                // Record error flag.
                infos[si] = info;

                // This eigenvalue spans 1 column. Update selected counter.
                si--;
            }
            else { // lambda_type[k] == CMPLX
                // Locate real and imaginary part of complex eigenvector
                // for complex conjugate pair of eigenvalues (k, k-1).
                double *X_re = X+(si-1)*(size_t)ldX;
                double *X_im = X+si*(size_t)ldX;

                // The eigenvalue is lambda = lambda_re+i*lambda_im.
                double lambda_re = lambda[k-1];
                double lambda_im = fabs(lambda[k]);

                // Locate corresponding scaling factor.
                scaling_t *beta = scales+si;

                // Compute norm of entire vector.
                double norm = vec_cmplx_infnorm(n, X_re, X_im);

                // Critical threshold to detect unsafe divisions.
                const double smin = MAX(
                    DBL_EPSILON/2*(fabs(lambda_re)+fabs(lambda_im)), smlnum);

                for (int j = n-1; j >= 0; j--) {
                    // if the next block is 1-by-1 diagonal block:
                    if (diag_type[j] == 0) { // REAL
                        scaling_t phi;
                        starneig_eigvec_std_init_scaling_factor(1, &phi);
                        info |= starneig_eigvec_std_solve_1x1_cmplx_system(smin, T(j,j),
                            lambda_re, lambda_im, X_re+j, X_im+j, &phi);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Scale the remaining parts of the 2 columns.
                        starneig_eigvec_std_scale(j, X_re, &phi);
                        starneig_eigvec_std_scale(n-(j+1), X_re+(j+1), &phi);
                        starneig_eigvec_std_scale(j, X_im, &phi);
                        starneig_eigvec_std_scale(n-(j+1), X_im+(j+1), &phi);
                        starneig_eigvec_std_update_norm(&norm, phi);

                        // Protect against overflow in the linear update.
                        double absmax = MAX(fabs(X_re[j]), fabs(X_im[j]));
                        phi = starneig_eigvec_std_protect_update(tnorm, absmax, norm);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Apply scaling to the whole eigenvector.
                        starneig_eigvec_std_scale(n, X_re, &phi);
                        starneig_eigvec_std_scale(n, X_im, &phi);

                        // Now it is safe to execute the linear update.
                        for (int i = 0; i < j; i++) {
                            X_re[i] = X_re[i]-T(i,j)*X_re[j];
                            X_im[i] = X_im[i]-T(i,j)*X_im[j];
                        }

                        // Recompute norm excluding the entries j:n.
                        norm = vec_cmplx_infnorm(j, X_re, X_im);
                    }
                    // if next block is 2-by-2 diagonal block:
                    else {
                        scaling_t phi;
                        starneig_eigvec_std_init_scaling_factor(1, &phi);
                        info |= starneig_eigvec_std_solve_2x2_cmplx_system(smin,
                            &T(j-1,j-1), ldT, lambda_re, lambda_im,
                            X_re+j-1, X_im+j-1, &phi);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Scale remaining parts of vector.
                        starneig_eigvec_std_scale(j-1, X_re, &phi);
                        starneig_eigvec_std_scale(n-(j+1), X_re+(j+1), &phi);
                        starneig_eigvec_std_scale(j-1, X_im, &phi);
                        starneig_eigvec_std_scale(n-(j+1), X_im+(j+1), &phi);
                        starneig_eigvec_std_update_norm(&norm, phi);

                        // Protect against overflow in the first linear update.
                        double absmax = MAX(fabs(X_re[j-1]), fabs(X_im[j-1]));
                        phi = starneig_eigvec_std_protect_update(tnorm, absmax, norm);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Apply scaling to the whole eigenvector.
                        starneig_eigvec_std_scale(n, X_re, &phi);
                        starneig_eigvec_std_scale(n, X_im, &phi);

                        // Now it is safe to execute the first linear update.
                        for (int i = 0; i < j-1; i++) {
                            X_re[i] = X_re[i]-T(i,j-1)*X_re[j-1];
                            X_im[i] = X_im[i]-T(i,j-1)*X_im[j-1];
                        }

                        // Recompute norm excluding the entries j+1:n.
                        norm = vec_cmplx_infnorm(j+1, X_re, X_im);

                        // Protect against overflow in the second linear update.
                        absmax = MAX(fabs(X_re[j]), fabs(X_im[j]));
                        phi = starneig_eigvec_std_protect_update(tnorm, absmax, norm);
                        starneig_eigvec_std_update_global_scaling(beta, phi);

                        // Apply scaling to the whole eigenvector.
                        starneig_eigvec_std_scale(n, X_re, &phi);
                        starneig_eigvec_std_scale(n, X_im, &phi);

                        // Now it is safe to execute the second linear update.
                        for (int i = 0; i < j-1; i++) {
                            X_re[i] = X_re[i]-T(i,j)*X_re[j];
                            X_im[i] = X_im[i]-T(i,j)*X_im[j];
                        }

                        // Recompute norm excluding the entries j-1:n.
                        norm = vec_cmplx_infnorm(j-1, X_re, X_im);

                        // We processed a 2-by-2 block, so skip the next
                        // diagonal entry.
                        j--;
                    }
                }

                // Note that the second column of a complex conjugate pair is
                // never allocated or computed. Obtaining it is left to the
                // user. If the positions si - 1, si mark a 2-by-2 block, then
                // the eigenvector corresponding to lambda = alpha + i beta
                // is X(:, si - 1) + i * X(:, si).
                // The complex conjugate eigenvector corresponding to
                // lambda = alpha - i beta can be derived as
                // conj(X) := X(:, si -1) - i * X(:, si).

                // The real part and the imaginary part are scaled alike.
                scales[si-1] = scales[si];

                // The ki-th complex eigenvector has been computed.
                // Recompute norm of the complex eigenvector.
                Xnorms[si] = vec_cmplx_infnorm(n, X_re, X_im);
                Xnorms[si-1] = Xnorms[si];

                // Record error flag.
                infos[si] = info;
                infos[si-1] = info;

                // We processed a complex conjugate pair of eigenvalues,
                // so skip the next entry.
                k--;

                // This eigenvector spans 2 cols. Update selected counter.
                si -= 2;
            }
        }
    }

#undef T
}

void starneig_eigvec_std_cpu_update(void *buffers[], void *cl_args)
{
    // T is n-by-m.
    double *T = (double *) STARPU_MATRIX_GET_PTR(buffers[0]);
    int ldT = STARPU_MATRIX_GET_LD(buffers[0]);
    int n = STARPU_MATRIX_GET_NX(buffers[0]);
    int m = STARPU_MATRIX_GET_NY(buffers[0]);

    double *ubT = (double *) STARPU_VARIABLE_GET_PTR(buffers[1]);
    double tnorm = *ubT;

    double *Xin = (double *) STARPU_MATRIX_GET_PTR(buffers[2]);
    int num_rhs = STARPU_MATRIX_GET_NY(buffers[2]);
    int ldX = STARPU_MATRIX_GET_LD(buffers[2]);

    scaling_t *Xscales = (scaling_t *) STARPU_VECTOR_GET_PTR(buffers[3]);

    double *Xinnorms = (double *) STARPU_VECTOR_GET_PTR(buffers[4]);

    double *Y = (double *) STARPU_MATRIX_GET_PTR(buffers[5]);
    int ldY = STARPU_MATRIX_GET_LD(buffers[5]);

    scaling_t *Yscales = (scaling_t *) STARPU_VECTOR_GET_PTR(buffers[6]);

    double *Ynorms = (double *) STARPU_VECTOR_GET_PTR(buffers[7]);

    int *lambda_type = (int *) STARPU_VECTOR_GET_PTR(buffers[8]);

    // Pointer to X - either a copy or the original memory.
    double *X;

    // Pointer to norms of X - either a copy or the original memory.
    double *Xnorms;

    // Status flag if X has to be rescaled.
    int rescale_X = 0;

    // Status flag if Y has to be rescaled.
    int rescale_Y = 0;

    // Indicator if xinnorms has to be rescaled. Note that this only affects
    // consistency scaling and not overflow protection.
    int rescale_xnorms = 0;

    // Workspace to store locally computed scaling factors.
    scaling_t tmp_scales[num_rhs];

    //
    // Compute scaling factor.
    //

    for (int k = 0; k < num_rhs; k++) {
        if (Yscales[k] < Xscales[k]) {
            rescale_xnorms = 1;
            break;
        }
    }

    if (rescale_xnorms) {
        // As X is read-only, copy xnorms.
        Xnorms = (double *) malloc(num_rhs * sizeof(double));
        memcpy(Xnorms, Xinnorms, num_rhs * sizeof(double));

        // Simulate the consistency scaling.
        for (int k = 0; k < num_rhs; k++) {
            if (Yscales[k] < Xscales[k]) {
                // The common scaling factor is Yscales[k].
                const double s =
                    starneig_eigvec_std_compute_upscaling(Yscales[k], Xscales[k]);

                // Mark X for scaling. Physical rescaling is deferred.
                rescale_X = 1;

                // Update norm.
                Xnorms[k] = s*Xinnorms[k];
            }
            else if (Xscales[k] < Yscales[k]) {
                // The common scaling factor is Xscales[k].
                const double s =
                    starneig_eigvec_std_compute_upscaling(Xscales[k], Yscales[k]);

                // Mark Y for scaling. Physical rescaling is deferred.
                rescale_Y = 1;

                // Update norm: norm(s * Y) = s * norm(Y).
                Ynorms[k] = s*Ynorms[k];
            }
        }
    }
    else { // !rescale_xnorms
        // No changes to Xinnorms necessary. Operate on original memory.
        Xnorms = Xinnorms;

        // Xnorms does not need scaling, but Ynorms may do.
        for (int k = 0; k < num_rhs; k++) {
            if (Xscales[k] < Yscales[k]) {
                // The common scaling factor is Xscales[k].
                const double s =
                    starneig_eigvec_std_compute_upscaling(Xscales[k], Yscales[k]);

                // Mark Y for scaling. Phyiscal rescaling is deferred.
                rescale_Y = 1;

                // Update norm: norm(s * Y) = s * norm(Y).
                Ynorms[k] = s*Ynorms[k];
            }
        }
    }

    //
    // Apply scaling.
    //

    // Status flag if update is safe to execute or if rescaling is required.
    int rescale;

    // Compute scaling factors needed to survive the linear update.
    rescale = starneig_eigvec_std_protect_multi_rhs_update(
        Xnorms, num_rhs, tnorm, Ynorms, lambda_type, tmp_scales);

    if (rescale) {
        rescale_X = 1;
        rescale_Y = 1;
    }

    // If X has to be rescaled, take a copy of X and do scaling on the copy.
    if (rescale_X) {
        X = (double *) malloc((size_t)ldX * num_rhs * sizeof(double));

        for (int k = 0; k < num_rhs; k++) {
            if (Yscales[k] < Xscales[k]) {
                // Copy X and simultaneously rescale.
                const double s = starneig_eigvec_std_compute_combined_upscaling(
                    Yscales[k], Xscales[k], tmp_scales[k]);
                for (int i = 0; i < m; i++)
                    X[(size_t)ldX*k+i] = s*Xin[(size_t)ldX*k+i];
            }
            else if (Xscales[k] < Yscales[k]) {
                // Copy X and simultaneously rescale with robust update factor.
                const double s = starneig_eigvec_std_convert_scaling(tmp_scales[k]);
                for (int i = 0; i < m; i++)
                    X[(size_t)ldX*k+i] = s*Xin[(size_t)ldX*k+i];
            }
            else {
                // Xscales[k] == Yscales[k].

                // Copy X and simultaneously rescale with robust update factor.
                const double s = starneig_eigvec_std_convert_scaling(tmp_scales[k]);
                for (int i = 0; i < m; i++)
                    X[(size_t)ldX*k+i] = s*Xin[(size_t)ldX*k+i];
            }
        }
    }
    else { // !rescale_X
        // No changes to X necessary. Operate on original memory.
        X = Xin;
    }

    // If Y has to be rescaled, directly modify Y.
    if (rescale_Y) {
        for (int k = 0; k < num_rhs; k++) {
            if (Yscales[k] < Xscales[k]) {
                // The common scaling factor is Yscales[k]. Rescale Y with
                // robust update factor, if necessary.
                starneig_eigvec_std_scale(n, Y+(size_t)ldY*k, tmp_scales+k);
            }
            else if (Xscales[k] < Yscales[k]) {
                // The common scaling factor is Xscales[k]. Combine with
                // robust update scaling factor.
                const double s = starneig_eigvec_std_compute_combined_upscaling(
                    Xscales[k], Yscales[k], tmp_scales[k]);
                for (int i = 0; i < n; i++)
                    Y[(size_t)ldY*k+i] = s*Y[(size_t)ldY*k+i];
            }
            else {
                // Xscales[k] == Yscales[k].

                // Rescale Y with robust update factor, if necessary.
                starneig_eigvec_std_scale(n, Y+(size_t)ldY*k, tmp_scales+k);
            }
        }
    }

    //
    // Update global scaling of Y.
    //

#ifdef STARNEIG_ENABLE_INTEGER_SCALING
    for (int k = 0; k < num_rhs; k++)
        Yscales[k] = MIN(Yscales[k], Xscales[k])+tmp_scales[k];
#else
    for (int k = 0; k < num_rhs; k++)
        Yscales[k] = MIN(Yscales[k], Xscales[k])*tmp_scales[k];
#endif

    //
    // Compute update.
    //

    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
        n, num_rhs, m, -1.0, T, ldT, X, ldX, 1.0, Y, ldY);

    //
    // Recompute norms.
    //

    for (int k = 0; k < num_rhs; k++) {
        if (lambda_type[k] == 1) { // CMPLX
            // We store only one scaling factor per complex eigenvector pair.
            // So interpret columns as real and imaginary part.
            const double *Y_re = Y+(size_t)k*ldY;
            const double *Y_im = Y+(size_t)(k+1)*ldY;

            Ynorms[k] = vec_cmplx_infnorm(n, Y_re, Y_im);

            // Duplicate norm for real and imaginary column.
            Ynorms[k+1] = Ynorms[k];

            k++;
        }
        else { // lambda_type[k] == REAL
            Ynorms[k] = vec_real_infnorm(n, Y+(size_t)k*ldY);
        }
    }

    //
    // Clean up.
    //

    if (rescale_X)
        free(X);

    if (rescale_xnorms)
        free(Xnorms);
}

void starneig_eigvec_std_cpu_backtransform(void *buffers[], void *cl_args)
{
    double *Q = (double *) STARPU_MATRIX_GET_PTR(buffers[0]);
    int ldQ = STARPU_MATRIX_GET_LD(buffers[0]);

    double *X = (double *) STARPU_MATRIX_GET_PTR(buffers[1]);
    int ldX = STARPU_MATRIX_GET_LD(buffers[1]);

    double *Y = (double *) STARPU_MATRIX_GET_PTR(buffers[2]);
    int ldY = STARPU_MATRIX_GET_LD(buffers[2]);
    int m = STARPU_MATRIX_GET_NX(buffers[2]);
    int n = STARPU_MATRIX_GET_NY(buffers[2]);

    int k;
    starpu_codelet_unpack_args(cl_args, &k);

    //   Yij  :=   Qi:  *   X:j
    // (m x n)   (m x k)  (k x n)

    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
        m, n, k, 1.0, Q, ldQ, X, ldX, 0.0, Y, ldY);

}


#include <algorithm>
#include <numeric>

#ifdef HAS_MKL
#include <mkl.h>
#else
#include <cblas.h>
#endif

#ifdef __SSE4_2__
#include <emmintrin.h>
#endif

typedef double Real;

inline void GetTotalTraction_(Real *TotalTraction,
    const Real *ElectricDisplacementx, int ndim) {
    // if (ndim==3) {
    //     TotalTraction[0] = ElectricDisplacementx[0];
    //     TotalTraction[1] = ElectricDisplacementx[1];
    //     TotalTraction[2] = ElectricDisplacementx[2];
    // }
    // else if (ndim == 2) {
    //     TotalTraction[0] = ElectricDisplacementx[0];
    //     TotalTraction[1] = ElectricDisplacementx[1];
    // }
    std::copy(ElectricDisplacementx,ElectricDisplacementx+ndim,TotalTraction);
}

inline void FillConstitutiveB_(Real *B, const Real* SpatialGradient,
                     int ndim, int rows, int cols) {
    int i = 0;

    if (ndim == 3) {

        for (; i<rows; ++i) {
            // Store in registers
            const Real a0 = SpatialGradient[i*ndim];
            const Real a1 = SpatialGradient[i*ndim+1];
            const Real a2 = SpatialGradient[i*ndim+2];

            // ELECTROSTATIC TERMS
            B[i*cols+0]     = a0;
            B[i*cols+1]     = a1;
            B[i*cols+2]     = a2;
        }
    }

    else if (ndim == 2) {

        for (; i<rows; ++i) {
            // Store in registers
            const Real a0 = SpatialGradient[i*ndim];
            const Real a1 = SpatialGradient[i*ndim+1];

            // ELECTROSTATIC TERMS
            B[i*cols+0]     = a0;
            B[i*cols+1]     = a1;
        }
    }
}
#include <iostream>
inline void _ConstitutiveStiffnessLaplacian_Filler_(Real *stiffness, Real *traction,
    const Real* SpatialGradient,
    const Real* ElectricDisplacementx,
    const Real* H_Voigt,
    const Real* detJ,
    int ngauss,
    int noderpelem,
    int ndim,
    int nvar,
    int H_VoigtSize,
    int requires_geometry_update) {

    int local_size = nvar*noderpelem;

    Real *t;

#ifdef __SSE4_2__
    t = (Real*)_mm_malloc(ndim*sizeof(Real),32);
    Real *B = (Real*)_mm_malloc(H_VoigtSize*local_size*sizeof(Real),32);
    Real *HBT = (Real*)_mm_malloc(H_VoigtSize*local_size*sizeof(Real),32);
    Real *BDB_1 = (Real*)_mm_malloc(local_size*local_size*sizeof(Real),32);
#else
    t = (Real*)malloc(ndim*sizeof(Real));
    Real *B = (Real*)malloc(H_VoigtSize*local_size*sizeof(Real));
    Real *HBT = (Real*)malloc(H_VoigtSize*local_size*sizeof(Real));
    Real *BDB_1 = (Real*)malloc(local_size*local_size*sizeof(Real));
#endif

    std::fill(B,B+H_VoigtSize*local_size,0.);

    for (int igauss = 0; igauss < ngauss; ++igauss) {

        FillConstitutiveB_(B,&SpatialGradient[igauss*ndim*noderpelem],ndim,noderpelem,H_VoigtSize);

        cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans,
            H_VoigtSize, local_size, H_VoigtSize, 1.0, &H_Voigt[igauss*H_VoigtSize*H_VoigtSize], H_VoigtSize, B, H_VoigtSize, 0.0, HBT, local_size);

        cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
            local_size, local_size, H_VoigtSize, 1.0, B, H_VoigtSize, HBT, local_size, 0.0, BDB_1, local_size);

        // Multiply stiffness with detJ
        const Real detJ_igauss = detJ[igauss];
        for (int i=0; i<local_size*local_size; ++i) {
            stiffness[i] += BDB_1[i]*detJ_igauss;
        }

        // Compute tractions
        GetTotalTraction_(t,
            &ElectricDisplacementx[igauss*ndim], ndim);

        // Multiply B with traction - for loop is okay
        for (int i=0; i<local_size; ++i) {
            Real tmp = 0;
            for (int j=0; j<H_VoigtSize; ++j) {
                tmp += B[i*H_VoigtSize+j]*t[j];
            }
            traction[i] += tmp*detJ_igauss;
        }
    }

#ifdef __SSE4_2__
    _mm_free(t);
    _mm_free(B);
    _mm_free(HBT);
    _mm_free(BDB_1);
#else
    free(t);
    free(B);
    free(HBT);
    free(BDB_1);
#endif
}


/*
 * applyForceToolFrame.h
 *
 *  Created on: Sep 11, 2015
 *      Author: mas
 */
#ifndef TANGENTIAL_VELOCITY_H_
#define TANGENTIAL_VELOCITY_H_
#include <cmath>
#include <barrett/units.h>
#include <barrett/systems.h>
#include <barrett/products/product_manager.h>
#include <libconfig.h++>
#include <barrett/detail/ca_macro.h>
#include <barrett/cdlbt/calgrav.h>
#include <barrett/systems/abstract/system.h>
#include <barrett/systems/abstract/single_io.h>
#include <barrett/systems/kinematics_base.h>
#include <barrett/math/kinematics.h>
#include <stdio.h>
#include <iostream>
#include <string>
#include <cstdlib>  // For std::atexit()
#include <barrett/os.h>  // For btsleep()
#include <barrett/math.h>  // For barrett::math::saturate()
#include <Eigen/Dense>
#include <Eigen/Core>
#include <barrett/standard_main_function.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#define BARRETT_SMF_VALIDATE_ARGS
#include <barrett/standard_main_function.h>

using namespace barrett;
using namespace systems;
using systems::connect;
BARRETT_UNITS_FIXED_SIZE_TYPEDEFS;

// --------- tangential velocity controller --------
// inputs: v = wam.toolVelocity, output: tangential velocity Vt =  dotproduct (v , tanDir)
template<size_t DOF>
class tangentialVelocityCompute : public systems::System
{
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

// IO  (inputs)
public:
Input<cv_type> toolVelocityInput;
Input<cp_type> tangentDirInput;

// IO  (outputs)
public:
Output<double> tangentVelOutput;     // tangential velocity

protected:
typename Output<double>::Value* tangentVelOutputValue;

public:
double cpomputedTangentialVel; 	 // can be used to display later in the program

public:
tangentialVelocityCompute(
      const std::string& sysName = "tangentialVelocityCompute") :
      System(sysName), toolVelocityInput(this), tangentDirInput(this),
      tangentVelOutput(this, &tangentVelOutputValue)
{
}
virtual ~tangentialVelocityCompute()
{
this->mandatoryCleanUp();
}
protected:
    cp_type tanDir;
    cv_type vel;

    double tanVel;

virtual void operate()
{

  tanDir = tangentDirInput.getValue();
  vel = toolVelocityInput.getValue();

  tanVel = tanDir.dot(vel) ;
  cpomputedTangentialVel = tanVel;

  tangentVelOutputValue->setData( & tanVel );

}

private:
DISALLOW_COPY_AND_ASSIGN(tangentialVelocityCompute);
};
// -----------------------------------

#endif /*TANGENTIAL_VELOCITY_H_*/


#include <cblas.h>
int main() { double *x, *y; cblas_ddot(0, x, 0, y, 0); }