/**
 *
 * @file zcposv.c
 *
 *  PLASMA computational 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 Emmanuel Agullo
 * @date 2010-11-15
 * @precisions mixed zc -> ds
 *
 **/
#include <stdlib.h>
#include <math.h>
#include <lapacke.h>
#include "common.h"

#define PLASMA_zlag2c(_descA, _descSB)                \
  plasma_parallel_call_4(plasma_pzlag2c,              \
                         PLASMA_desc,      (_descA),  \
                         PLASMA_desc,      (_descSB), \
                         PLASMA_sequence*, sequence,  \
                         PLASMA_request*,  request)

#define PLASMA_clag2z(_descSA, _descB)                \
  plasma_parallel_call_4(plasma_pclag2z,              \
                         PLASMA_desc,      (_descSA), \
                         PLASMA_desc,      (_descB),  \
                         PLASMA_sequence*, sequence,  \
                         PLASMA_request*,  request)

#define PLASMA_zlange(_norm, _descA, _result, _work)   \
  _result = 0;                                         \
  plasma_parallel_call_6(plasma_pzlange,               \
                         PLASMA_enum,      (_norm),    \
                         PLASMA_desc,      (_descA),   \
                         double*,          (_work),    \
                         double*,          &(_result), \
                         PLASMA_sequence*, sequence,   \
                         PLASMA_request*,  request);

#define PLASMA_zlanhe(_norm, _uplo, _descA, _result, _work) \
  _result = 0;                                              \
  plasma_parallel_call_7(plasma_pzlanhe,                    \
                         PLASMA_enum,      (_norm),         \
                         PLASMA_enum,      (_uplo),         \
                         PLASMA_desc,      (_descA),        \
                         double*,          (_work),         \
                         double*,          &(_result),      \
                         PLASMA_sequence*, sequence,        \
                         PLASMA_request*,  request);

#define PLASMA_zlacpy(_descA, _descB)                        \
  plasma_parallel_call_5(plasma_pzlacpy,                     \
                         PLASMA_enum,      PlasmaUpperLower, \
                         PLASMA_desc,      (_descA),         \
                         PLASMA_desc,      (_descB),         \
                         PLASMA_sequence*, sequence,         \
                         PLASMA_request*,  request)

#define PLASMA_zgeadd(_alpha, _descA, _descB)           \
  plasma_parallel_call_5(plasma_pzgeadd,                \
                         PLASMA_Complex64_t, (_alpha),  \
                         PLASMA_desc,        (_descA),  \
                         PLASMA_desc,        (_descB),  \
                         PLASMA_sequence*,   sequence,  \
                         PLASMA_request*,    request)

/***************************************************************************//**
 *
 * @ingroup PLASMA_Complex64_t
 *
 *  PLASMA_zcposv - Computes the solution to a system of linear equations A * X = B,
 *  where A is an N-by-N symmetric positive definite (or Hermitian positive definite
 *  in the complex case) matrix and X and B are N-by-NRHS matrices.
 *  The Cholesky decomposition is used to factor A as
 *
 *    A = U**H * U, if uplo = PlasmaUpper, or
 *    A = L * L**H, if uplo =  PlasmaLower,
 *
 *  where U is an upper triangular matrix and  L is a lower triangular matrix.
 *  The factored form of A is then used to solve the system of equations A * X = B.
 *
 *  PLASMA_zcposv first attempts to factorize the matrix in COMPLEX and use this
 *  factorization within an iterative refinement procedure to produce a
 *  solution with COMPLEX*16 normwise backward error quality (see below).
 *  If the approach fails the method switches to a COMPLEX*16
 *  factorization and solve.
 *
 *  The iterative refinement is not going to be a winning strategy if
 *  the ratio COMPLEX performance over COMPLEX*16 performance is too
 *  small. A reasonable strategy should take the number of right-hand
 *  sides and the size of the matrix into account. This might be done
 *  with a call to ILAENV in the future. Up to now, we always try
 *  iterative refinement.
 *
 *  The iterative refinement process is stopped if ITER > ITERMAX or
 *  for all the RHS we have: RNRM < N*XNRM*ANRM*EPS*BWDMAX
 *  where:
 *
 *  - ITER is the number of the current iteration in the iterative refinement process
 *  - RNRM is the infinity-norm of the residual
 *  - XNRM is the infinity-norm of the solution
 *  - ANRM is the infinity-operator-norm of the matrix A
 *  - EPS is the machine epsilon returned by DLAMCH('Epsilon').
 *
 *  Actually, in its current state (PLASMA 2.1.0), the test is slightly relaxed.
 *
 *  The values ITERMAX and BWDMAX are fixed to 30 and 1.0D+00 respectively.
 *
 *******************************************************************************
 *
 * @param[in] uplo
 *          Specifies whether the matrix A is upper triangular or lower triangular:
 *          = PlasmaUpper: Upper triangle of A is stored;
 *          = PlasmaLower: Lower triangle of A is stored.
 *
 * @param[in] N
 *          The number of linear equations, i.e., the order of the matrix A. N >= 0.
 *
 * @param[in] NRHS
 *          The number of right hand sides, i.e., the number of columns of the matrix B.
 *          NRHS >= 0.
 *
 * @param[in] A
 *          The N-by-N symmetric positive definite (or Hermitian) coefficient matrix A.
 *          If uplo = PlasmaUpper, the leading N-by-N upper triangular part of A
 *          contains the upper triangular part of the matrix A, and the strictly lower triangular
 *          part of A is not referenced.
 *          If UPLO = 'L', the leading N-by-N lower triangular part of A contains the lower
 *          triangular part of the matrix A, and the strictly upper triangular part of A is not
 *          referenced.
 *          This matrix is not modified.
 *
 * @param[in] LDA
 *          The leading dimension of the array A. LDA >= max(1,N).
 *
 * @param[in] B
 *          The N-by-NRHS matrix of right hand side matrix B.
 *
 * @param[in] LDB
 *          The leading dimension of the array B. LDB >= max(1,N).
 *
 * @param[out] X
 *          If return value = 0, the N-by-NRHS solution matrix X.
 *
 * @param[in] LDX
 *          The leading dimension of the array B. LDX >= max(1,N).
 *
 * @param[out] ITER
 *          The number of the current iteration in the iterative refinement process
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval <0 if -i, the i-th argument had an illegal value
 *          \retval >0 if i, the leading minor of order i of A is not positive definite, so the
 *               factorization could not be completed, and the solution has not been computed.
 *
 *******************************************************************************
 *
 * @sa PLASMA_zcposv_Tile
 * @sa PLASMA_zcposv_Tile_Async
 * @sa PLASMA_dsposv
 * @sa PLASMA_zposv
 *
 ******************************************************************************/
int PLASMA_zcposv(PLASMA_enum uplo, int N, int NRHS,
                  PLASMA_Complex64_t *A, int LDA,
                  PLASMA_Complex64_t *B, int LDB,
                  PLASMA_Complex64_t *X, int LDX, int *ITER)
{
    int NB;
    int status;
    PLASMA_desc  descA;
    PLASMA_desc  descB;
    PLASMA_desc  descX;
    plasma_context_t *plasma;
    PLASMA_sequence *sequence = NULL;
    PLASMA_request request = PLASMA_REQUEST_INITIALIZER;

    plasma = plasma_context_self();
    if (plasma == NULL) {
        plasma_fatal_error("PLASMA_zcposv", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }

    *ITER = 0;

    /* Check input arguments */
    if (uplo != PlasmaUpper && uplo != PlasmaLower) {
        plasma_error("PLASMA_zcposv", "illegal value of uplo");
        return -1;
    }
    if (N < 0) {
        plasma_error("PLASMA_zcposv", "illegal value of N");
        return -2;
    }
    if (NRHS < 0) {
        plasma_error("PLASMA_zcposv", "illegal value of NRHS");
        return -3;
    }
    if (LDA < max(1, N)) {
        plasma_error("PLASMA_zcposv", "illegal value of LDA");
        return -5;
    }
    if (LDB < max(1, N)) {
        plasma_error("PLASMA_zcposv", "illegal value of LDB");
        return -7;
    }
    if (LDX < max(1, N)) {
        plasma_error("PLASMA_zcposv", "illegal value of LDX");
        return -9;
    }
    /* Quick return - currently NOT equivalent to LAPACK's
     * LAPACK does not have such check for ZCPOSV */
    if (min(N, NRHS) == 0)
        return PLASMA_SUCCESS;

    /* Tune NB depending on M, N & NRHS; Set NBNBSIZE */
    status = plasma_tune(PLASMA_FUNC_ZCPOSV, N, N, NRHS);
    if (status != PLASMA_SUCCESS) {
        plasma_error("PLASMA_zcposv", "plasma_tune() failed");
        return status;
    }

    NB = PLASMA_NB;

    plasma_sequence_create(plasma, &sequence);

    /* DOUBLE PRECISION INITIALIZATION */
    if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
        plasma_zooplap2tile( descA, A, NB, NB, LDA, N,    0, 0, N, N,    sequence, &request,
                             plasma_desc_mat_free(&(descA)) );
        plasma_zooplap2tile( descB, B, NB, NB, LDB, NRHS, 0, 0, N, NRHS, sequence, &request,
                             plasma_desc_mat_free(&(descA)); plasma_desc_mat_free(&(descB)) );
        plasma_zdesc_alloc(  descX, NB, NB, N, NRHS, 0, 0, N, NRHS, plasma_desc_mat_free(&(descA)); plasma_desc_mat_free(&(descB)); plasma_desc_mat_free(&(descX)) );
    } else {
        plasma_ziplap2tile( descA, A, NB, NB, LDA, N,    0, 0, N, N,
                            sequence, &request);
        plasma_ziplap2tile( descB, B, NB, NB, LDB, NRHS, 0, 0, N, NRHS,
                            sequence, &request);

        descX = plasma_desc_init(
            PlasmaComplexDouble, NB, NB, (NB*NB),
            LDX, NRHS, 0, 0, N, NRHS);
        descX.mat = X;
    }

    /* Call the native interface */
    status = PLASMA_zcposv_Tile_Async(uplo, &descA, &descB, &descX, ITER, sequence, &request);

    if (status == PLASMA_SUCCESS) {
        if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
            plasma_zooptile2lap( descX, X, NB, NB, LDX, NRHS,  sequence, &request);
            plasma_dynamic_sync();
            plasma_desc_mat_free(&descA);
            plasma_desc_mat_free(&descB);
            plasma_desc_mat_free(&descX);
        } else {
            plasma_ziptile2lap( descA, A, NB, NB, LDA, N,     sequence, &request);
            plasma_ziptile2lap( descB, B, NB, NB, LDB, NRHS,  sequence, &request);
            plasma_ziptile2lap( descX, X, NB, NB, LDX, NRHS,  sequence, &request);
            plasma_dynamic_sync();
        }
    }

    status = sequence->status;
    plasma_sequence_destroy(plasma, sequence);
    return status;
}

/***************************************************************************//**
 *
 * @ingroup PLASMA_Complex64_t_Tile
 *
 *  PLASMA_zcposv_Tile - Solves a symmetric positive definite or Hermitian positive definite
 *  system of linear equations using the Cholesky factorization and mixed-precision iterative refinement.
 *  Tile equivalent of PLASMA_zcposv().
 *  Operates on matrices stored by tiles.
 *  All matrices are passed through descriptors.
 *  All dimensions are taken from the descriptors.
 *
 *******************************************************************************
 *
 * @param[in] uplo
 *          Specifies whether the matrix A is upper triangular or lower triangular:
 *          = PlasmaUpper: Upper triangle of A is stored;
 *          = PlasmaLower: Lower triangle of A is stored.
 *
 * @param[in,out] A
 *          On entry, the N-by-N symmetric positive definite (or Hermitian) coefficient matrix A.
 *          If uplo = PlasmaUpper, the leading N-by-N upper triangular part of A
 *          contains the upper triangular part of the matrix A, and the strictly lower triangular
 *          part of A is not referenced.
 *          If UPLO = 'L', the leading N-by-N lower triangular part of A contains the lower
 *          triangular part of the matrix A, and the strictly upper triangular part of A is not
 *          referenced.
 *          - If the iterative refinement converged, A is not modified;
 *          - otherwise, it falled backed to double precision solution,
 *
 * @param[in] B
 *          On entry, the N-by-NRHS matrix of right hand side matrix B.
 *
 * @param[out] X
 *          On exit, if return value = 0, the N-by-NRHS solution matrix X.
 *
 * @param[out] ITER
 *          The number of the current iteration in the iterative refinement process
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval >0 if i, the leading minor of order i of A is not positive definite, so the
 *               factorization could not be completed, and the solution has not been computed.
 *
 *******************************************************************************
 *
 * @sa PLASMA_zcposv
 * @sa PLASMA_zcposv_Tile_Async
 * @sa PLASMA_dsposv_Tile
 * @sa PLASMA_zposv_Tile
 *
 ******************************************************************************/
int PLASMA_zcposv_Tile(PLASMA_enum uplo, PLASMA_desc *A, PLASMA_desc *B,
                       PLASMA_desc *X, int *ITER)
{
    plasma_context_t *plasma;
    PLASMA_sequence *sequence = NULL;
    PLASMA_request request = PLASMA_REQUEST_INITIALIZER;
    int status;

    plasma = plasma_context_self();
    if (plasma == NULL) {
        plasma_fatal_error("PLASMA_zcposv_Tile", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }
    plasma_sequence_create(plasma, &sequence);
    status = PLASMA_zcposv_Tile_Async(uplo, A, B, X, ITER, sequence, &request);
    if (status != PLASMA_SUCCESS)
        return status;
    plasma_dynamic_sync();
    status = sequence->status;
    plasma_sequence_destroy(plasma, sequence);
    return status;
}

/***************************************************************************//**
 *
 * @ingroup PLASMA_Complex64_t_Tile_Async
 *
 *  PLASMA_zcposv_Tile_Async - Solves a symmetric positive definite or Hermitian
 *  positive definite system of linear equations using the Cholesky factorization
 *  and mixed-precision iterative refinement.
 *  Non-blocking equivalent of PLASMA_zcposv_Tile().
 *  May return before the computation is finished.
 *  Allows for pipelining of operations at runtime.
 *
 *******************************************************************************
 *
 * @param[in] sequence
 *          Identifies the sequence of function calls that this call belongs to
 *          (for completion checks and exception handling purposes).
 *
 * @param[out] request
 *          Identifies this function call (for exception handling purposes).
 *
 *******************************************************************************
 *
 * @sa PLASMA_zcposv
 * @sa PLASMA_zcposv_Tile
 * @sa PLASMA_dsposv_Tile_Async
 * @sa PLASMA_zposv_Tile_Async
 *
 ******************************************************************************/
int PLASMA_zcposv_Tile_Async(PLASMA_enum uplo, PLASMA_desc *A, PLASMA_desc *B,
                             PLASMA_desc *X, int *ITER,
                             PLASMA_sequence *sequence, PLASMA_request *request)
{
    int N, NB;
    PLASMA_desc descA;
    PLASMA_desc descB;
    PLASMA_desc descX;
    plasma_context_t *plasma;
    double *work;
    PLASMA_desc descR, descSA, descSX;

    const int itermax = 30;
    const double bwdmax = 1.0;
    const PLASMA_Complex64_t negone = -1.0;
    const PLASMA_Complex64_t one = 1.0;
    int iiter;
    double Anorm, cte, eps, Rnorm, Xnorm;
    *ITER=0;

    plasma = plasma_context_self();
    if (plasma == NULL) {
        plasma_fatal_error("PLASMA_zcposv_Tile_Async", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }
    if (sequence == NULL) {
        plasma_fatal_error("PLASMA_zcposv_Tile_Async", "NULL sequence");
        return PLASMA_ERR_UNALLOCATED;
    }
    if (request == NULL) {
        plasma_fatal_error("PLASMA_zcposv_Tile_Async", "NULL request");
        return PLASMA_ERR_UNALLOCATED;
    }
    /* Check sequence status */
    if (sequence->status == PLASMA_SUCCESS)
        request->status = PLASMA_SUCCESS;
    else
        return plasma_request_fail(sequence, request, PLASMA_ERR_SEQUENCE_FLUSHED);

    /* Check descriptors for correctness */
    if (plasma_desc_check(A) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_zcposv_Tile_Async", "invalid first descriptor");
        return PLASMA_ERR_ILLEGAL_VALUE;
    } else {
        descA = *A;
    }
    if (plasma_desc_check(B) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_zcposv_Tile_Async", "invalid second descriptor");
        return PLASMA_ERR_ILLEGAL_VALUE;
    } else {
        descB = *B;
    }
    if (plasma_desc_check(X) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_zcposv_Tile_Async", "invalid third descriptor");
        return PLASMA_ERR_ILLEGAL_VALUE;
    } else {
        descX = *X;
    }
    /* Check input arguments */
    if (descA.nb != descA.mb || descB.nb != descB.mb || descX.nb != descX.mb) {
        plasma_error("PLASMA_zcposv_Tile_Async", "only square tiles supported");
        return PLASMA_ERR_ILLEGAL_VALUE;
    }
    if (uplo != PlasmaUpper && uplo != PlasmaLower) {
        plasma_error("PLASMA_zcposv_Tile_Async", "illegal value of uplo");
        return -1;
    }
    /* Quick return - currently NOT equivalent to LAPACK's
     * LAPACK does not have such check for DPOSV */

/*
    if (min(N, NRHS) == 0)
        return PLASMA_SUCCESS;
*/

    /* Set N, NRHS */
    N  = descA.m;
    NB = descA.nb;

    work = (double *)plasma_shared_alloc(plasma, PLASMA_SIZE, PlasmaRealDouble);
    if (work == NULL) {
        plasma_error("PLASMA_zcposv_Tile_Async", "plasma_shared_alloc() failed");
        plasma_shared_free(plasma, work);
        return PLASMA_ERR_OUT_OF_RESOURCES;
    }

    plasma_zdesc_alloc( descR,  NB, NB, descB.m, descB.n, 0, 0, descB.m, descB.n, plasma_shared_free( plasma, work ); plasma_desc_mat_free(&descR) );
    plasma_cdesc_alloc( descSA, NB, NB, descA.m, descA.n, 0, 0, descA.m, descA.n, plasma_shared_free( plasma, work ); plasma_desc_mat_free(&descR); plasma_desc_mat_free(&descSA) );
    plasma_cdesc_alloc( descSX, NB, NB, descX.m, descX.n, 0, 0, descX.m, descX.n, plasma_shared_free( plasma, work ); plasma_desc_mat_free(&descR); plasma_desc_mat_free(&descSA); plasma_desc_mat_free(&descSX) );

    /* Compute some constants */
    PLASMA_zlanhe(PlasmaInfNorm, uplo, descA, Anorm, work);
    eps = LAPACKE_dlamch_work('e');

    /* Convert B from double precision to single precision and store
       the result in SX. */
    PLASMA_zlag2c(descB, descSX);
    if (sequence->status != PLASMA_SUCCESS)
        return plasma_request_fail(sequence, request, PLASMA_ERR_SEQUENCE_FLUSHED);

    /* Convert A from double precision to single precision and store
       the result in SA. */
    PLASMA_zlag2c(descA, descSA);
    if (sequence->status != PLASMA_SUCCESS)
        return plasma_request_fail(sequence, request, PLASMA_ERR_SEQUENCE_FLUSHED);

    /* Compute the Cholesky factorization of SA */
    plasma_parallel_call_4(plasma_pcpotrf,
        PLASMA_enum, uplo,
        PLASMA_desc, descSA,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    /* Solve the system SA*SX = SB */
    /* Forward substitution */
    plasma_parallel_call_9(plasma_pctrsm,
        PLASMA_enum, PlasmaLeft,
        PLASMA_enum, uplo,
        PLASMA_enum, uplo == PlasmaUpper ? PlasmaConjTrans : PlasmaNoTrans,
        PLASMA_enum, PlasmaNonUnit,
        PLASMA_Complex32_t, 1.0,
        PLASMA_desc, descSA,
        PLASMA_desc, descSX,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    /* Backward substitution */
    plasma_parallel_call_9(plasma_pctrsm,
        PLASMA_enum, PlasmaLeft,
        PLASMA_enum, uplo,
        PLASMA_enum, uplo == PlasmaUpper ? PlasmaNoTrans : PlasmaConjTrans,
        PLASMA_enum, PlasmaNonUnit,
        PLASMA_Complex32_t, 1.0,
        PLASMA_desc, descSA,
        PLASMA_desc, descSX,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    /* Convert SX back to double precision */
    PLASMA_clag2z(descSX, descX);

    /* Compute R = B - AX. */
    PLASMA_zlacpy(descB,descR);
    plasma_parallel_call_9(plasma_pzhemm,
        PLASMA_enum, PlasmaLeft,
        PLASMA_enum, uplo,
        PLASMA_Complex64_t, negone,
        PLASMA_desc, descA,
        PLASMA_desc, descX,
        PLASMA_Complex64_t, one,
        PLASMA_desc, descR,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    /* Check whether the NRHS normwise backward error satisfies the
       stopping criterion. If yes return. Note that ITER=0 (already set). */
    PLASMA_zlange(PlasmaInfNorm, descX, Xnorm, work);
    PLASMA_zlange(PlasmaInfNorm, descR, Rnorm, work);

    /* Wait for the end of Anorm, Xnorm and Bnorm computations */
    plasma_dynamic_sync();

    cte = Anorm*eps*((double) N)*bwdmax;
    if (Rnorm < Xnorm * cte){
        /* The NRHS normwise backward errors satisfy the
           stopping criterion. We are good to exit. */
        plasma_desc_mat_free(&descSA);
        plasma_desc_mat_free(&descSX);
        plasma_desc_mat_free(&descR);
        plasma_shared_free(plasma, work);
        return PLASMA_SUCCESS;
    }

    /* Iterative refinement */
    for (iiter = 0; iiter < itermax; iiter++){

        /* Convert R from double precision to single precision
           and store the result in SX. */
        PLASMA_zlag2c(descR, descSX);

        /* Solve the system SA*SX = SR */
        /* Forward substitution */
        plasma_parallel_call_9(plasma_pctrsm,
            PLASMA_enum, PlasmaLeft,
            PLASMA_enum, uplo,
            PLASMA_enum, uplo == PlasmaUpper ? PlasmaConjTrans : PlasmaNoTrans,
            PLASMA_enum, PlasmaNonUnit,
            PLASMA_Complex32_t, 1.0,
            PLASMA_desc, descSA,
            PLASMA_desc, descSX,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        /* Backward substitution */
        plasma_parallel_call_9(plasma_pctrsm,
            PLASMA_enum, PlasmaLeft,
            PLASMA_enum, uplo,
            PLASMA_enum, uplo == PlasmaUpper ? PlasmaNoTrans : PlasmaConjTrans,
            PLASMA_enum, PlasmaNonUnit,
            PLASMA_Complex32_t, 1.0,
            PLASMA_desc, descSA,
            PLASMA_desc, descSX,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        /* Convert SX back to double precision and update the current
           iterate. */
        PLASMA_clag2z(descSX, descR);
        PLASMA_zgeadd(one, descR, descX);

        /* Compute R = B - AX. */
        PLASMA_zlacpy(descB,descR);
        plasma_parallel_call_9(plasma_pzhemm,
            PLASMA_enum, PlasmaLeft,
            PLASMA_enum, uplo,
            PLASMA_Complex64_t, negone,
            PLASMA_desc, descA,
            PLASMA_desc, descX,
            PLASMA_Complex64_t, one,
            PLASMA_desc, descR,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        /* Check whether the NRHS normwise backward errors satisfy the
           stopping criterion. If yes, set ITER=IITER>0 and return. */
        PLASMA_zlange(PlasmaInfNorm, descX, Xnorm, work);
        PLASMA_zlange(PlasmaInfNorm, descR, Rnorm, work);

        /* Wait for the end of Xnorm and Bnorm computations */
        plasma_dynamic_sync();

        if (Rnorm < Xnorm * cte){
            /* The NRHS normwise backward errors satisfy the
               stopping criterion. We are good to exit. */
            *ITER = iiter;

            plasma_desc_mat_free(&descSA);
            plasma_desc_mat_free(&descSX);
            plasma_desc_mat_free(&descR);
            plasma_shared_free(plasma, work);
            return PLASMA_SUCCESS;
        }
    }

    /* We have performed ITER=itermax iterations and never satisified
       the stopping criterion, set up the ITER flag accordingly and
       follow up on double precision routine. */
    *ITER = -itermax - 1;

    plasma_desc_mat_free(&descSA);
    plasma_desc_mat_free(&descSX);
    plasma_desc_mat_free(&descR);
    plasma_shared_free(plasma, work);

    /* Single-precision iterative refinement failed to converge to a
       satisfactory solution, so we resort to double precision. */

    plasma_parallel_call_4(plasma_pzpotrf,
        PLASMA_enum, uplo,
        PLASMA_desc, descA,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    PLASMA_zlacpy(descB,descX);

    plasma_parallel_call_9(plasma_pztrsm,
        PLASMA_enum, PlasmaLeft,
        PLASMA_enum, uplo,
        PLASMA_enum, uplo == PlasmaUpper ? PlasmaConjTrans : PlasmaNoTrans,
        PLASMA_enum, PlasmaNonUnit,
        PLASMA_Complex64_t, 1.0,
        PLASMA_desc, descA,
        PLASMA_desc, descX,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    plasma_parallel_call_9(plasma_pztrsm,
        PLASMA_enum, PlasmaLeft,
        PLASMA_enum, uplo,
        PLASMA_enum, uplo == PlasmaUpper ? PlasmaNoTrans : PlasmaConjTrans,
        PLASMA_enum, PlasmaNonUnit,
        PLASMA_Complex64_t, 1.0,
        PLASMA_desc, descA,
        PLASMA_desc, descX,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    return PLASMA_SUCCESS;
}


/* -*- linux-c -*- */
/* binsingle.c

   Copyright (C) 2002-2004 John M. Fregeau

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <getopt.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_rng.h>
#include "fewbody.h"
#include "binsingle_simple.h"

/* calculate the units used */
int calc_units(fb_obj_t *single, fb_obj_t *binary, fb_units_t *units)
{
	units->v = sqrt(FB_CONST_G*(single->m + binary->m)/(single->m * binary->m) * \
			(binary->obj[0]->m * binary->obj[1]->m / binary->a));
	units->l = binary->a;
	units->t = units->l / units->v;
	units->m = units->l * fb_sqr(units->v) / FB_CONST_G;
	units->E = units->m * fb_sqr(units->v);

	return(0);
}

/* the main attraction */
int main(int argc, char *argv[])
{
	int i, n;
	unsigned long int seed;
	double m0, m10, m11, r0, r10, r11, a1, e1;
	double rtid, vinf, b, m1, M, mu, Ei, t;
	fb_hier_t hier;
	fb_input_t input;
	fb_ret_t retval;
	fb_units_t units;
	char string1[FB_MAX_STRING_LENGTH], string2[FB_MAX_STRING_LENGTH];
	gsl_rng *rng;
	const gsl_rng_type *rng_type=gsl_rng_mt19937;

	/* set parameters to default values */
	m0 = FB_M0;
	m10 = FB_M10;
	m11 = FB_M11;
	r0 = FB_R0;
	r10 = FB_R10;
	r11 = FB_R11;
	a1 = FB_A1;
	e1 = FB_E1;
	vinf = FB_VINF;
	b = FB_B;
	input.ks = FB_KS;
	input.tstop = FB_TSTOP;
	input.Dflag = 0;
	input.dt = FB_DT;
	input.tcpustop = FB_TCPUSTOP;
	input.absacc = FB_ABSACC;
	input.relacc = FB_RELACC;
	input.ncount = FB_NCOUNT;
	input.tidaltol = FB_TIDALTOL;
	input.fexp = FB_FEXP;
	seed = FB_SEED;
	fb_debug = FB_DEBUG;

	/* initialize a few things for integrator */
	t = 0.0;
	fbui_new_hier(&hier, 3);

	/* initialize GSL rng */
	gsl_rng_env_setup();
	rng = gsl_rng_alloc(rng_type);
	gsl_rng_set(rng, seed);

	/* create binary */
	fbui_make_pair(fbui_hierarchy_binary(&hier, 0),
		       fbui_hierarchy_single(&hier, 1), fbui_hierarchy_single(&hier, 2));
	fbui_obj_t_set(fbui_hierarchy_binary(&hier, 0), t);

	/* give the objects some properties */
	fbui_initialize_single(fbui_hierarchy_single(&hier, 0), 0, "0");
	fbui_initialize_single(fbui_hierarchy_single(&hier, 1), 1, "1");
	fbui_initialize_single(fbui_hierarchy_single(&hier, 2), 2, "2");

	fbui_obj_radius_set(fbui_hierarchy_single(&hier, 0), r0);
	fbui_obj_radius_set(fbui_hierarchy_single(&hier, 1), r10);
	fbui_obj_radius_set(fbui_hierarchy_single(&hier, 2), r11);

	fbui_obj_mass_set(fbui_hierarchy_single(&hier, 0), m0);
	fbui_obj_mass_set(fbui_hierarchy_single(&hier, 1), m10);
	fbui_obj_mass_set(fbui_hierarchy_single(&hier, 2), m11);

	fbui_obj_mass_set(fbui_hierarchy_binary(&hier, 0), m10+m11);

	fbui_obj_a_set(fbui_hierarchy_binary(&hier, 0), a1);
	fbui_obj_e_set(fbui_hierarchy_binary(&hier, 0), e1);

	/* get the units and normalize */
	calc_units(fbui_hierarchy_single(&hier, 0), fbui_hierarchy_binary(&hier, 0), &units);
	fb_normalize(&hier, units);

	/* move hierarchies analytically in from infinity along hyperbolic orbit */
	m0 = fbui_obj_mass_get(fbui_hierarchy_single(&hier, 0));
	m1 = fbui_obj_mass_get(fbui_hierarchy_binary(&hier, 0));
	M = m0 + m1;
	mu = m0 * m1 / M;

	Ei = 0.5 * mu * fb_sqr(vinf);

	a1 = fbui_obj_a_get(fbui_hierarchy_binary(&hier, 0));
	e1 = fbui_obj_e_get(fbui_hierarchy_binary(&hier, 0));
	m10 = fbui_obj_mass_get(fbui_obj_left_child_get(fbui_hierarchy_binary(&hier, 0)));
	m11 = fbui_obj_mass_get(fbui_obj_right_child_get(fbui_hierarchy_binary(&hier, 0)));

	rtid = pow(2.0*(m0+m1)/(m1*input.tidaltol), 1.0/3.0) * a1 * (1.0+e1);

	fb_init_scattering(fbui_hierarchy_single(&hier, 0), fbui_hierarchy_binary(&hier, 0), vinf, b, rtid);

	/* trickle down the binary properties, then back up */
	fb_randorient(fbui_hierarchy_binary(&hier, 0), rng);
	fb_downsync(fbui_hierarchy_binary(&hier, 0), t);

	/* call fewbody! */
	retval = fewbody(input, &hier, &t);

	/* print information to screen */
	fprintf(stderr, "OUTCOME:\n");
	if (retval.retval == 1) {
		fprintf(stderr, "  encounter complete:  t=%.6g (%.6g yr)  %s  (%s)\n\n",
			t, t * units.t/FB_CONST_YR,
			fb_sprint_hier(hier, string1),
			fb_sprint_hier_hr(hier, string2));
	} else {
		fprintf(stderr, "  encounter NOT complete:  t=%.6g (%.6g yr)  %s  (%s)\n\n",
			t, t * units.t/FB_CONST_YR,
			fb_sprint_hier(hier, string1),
			fb_sprint_hier_hr(hier, string2));
	}

	fb_dprintf("there were %ld integration steps\n", retval.count);
	fb_dprintf("fb_classify() was called %ld times\n", retval.iclassify);

	fprintf(stderr, "FINAL:\n");
	fprintf(stderr, "  t_final=%.6g (%.6g yr)  t_cpu=%.6g s\n", \
		t, t*units.t/FB_CONST_YR, retval.tcpu);

	fprintf(stderr, "  DeltaL/L0=%.6g  DeltaL=%.6g\n", retval.DeltaLfrac, retval.DeltaL);
	fprintf(stderr, "  DeltaE/E0=%.6g  DeltaE=%.6g\n", retval.DeltaEfrac, retval.DeltaE);
	fprintf(stderr, "  Rmin=%.6g (%.6g RSUN)  Rmin_i=%d  Rmin_j=%d\n", \
		retval.Rmin, retval.Rmin*units.l/FB_CONST_RSUN, retval.Rmin_i, retval.Rmin_j);
	fprintf(stderr, "  Nosc=%d (%s)\n", retval.Nosc, (retval.Nosc>=1?"resonance":"non-resonance"));

	fprintf(stderr, "Resulting objects:\n");
	for (i=0; i<hier.nobj; i++) {
		n = fbui_obj_n_get(fbui_tree(&hier, i));
		if (n == 1) {
			fprintf(stderr, "  single: m=%g idstring=%s\n",
				fbui_obj_mass_get(fbui_tree(&hier, i)) * units.m / FB_CONST_MSUN,
				fbui_obj_idstring_get(fbui_tree(&hier, i)));
		} else if (n == 2) {
			fprintf(stderr, "  binary: m_i=%g,%g a=%g e=%g idstring=%s\n",
				fbui_obj_mass_get(fbui_obj_left_child_get(fbui_tree(&hier, i))) * units.m / FB_CONST_MSUN,
				fbui_obj_mass_get(fbui_obj_right_child_get(fbui_tree(&hier, i))) * units.m / FB_CONST_MSUN,
				fbui_obj_a_get(fbui_tree(&hier, i)) * units.l / FB_CONST_AU,
				fbui_obj_e_get(fbui_tree(&hier, i)),
				fbui_obj_idstring_get(fbui_tree(&hier, i)));
		}
	}

	/* free GSL stuff */
	gsl_rng_free(rng);

	/* free our own stuff */
	fbui_delete_hier(&hier);

	/* done! */
	return(0);
}


/*
ODE: a program to get optime Runge-Kutta and multi-steps methods.

Copyright 2011-2019, Javier Burguete Tolosa.

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.

THIS SOFTWARE IS PROVIDED BY Javier Burguete Tolosa ``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 Javier Burguete Tolosa 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.
*/

/**
 * \file rk_5_4.c
 * \brief Source file to optimize Runge-Kutta 5 steps 4th order methods.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2011-2019.
 */
#define _GNU_SOURCE
#include <string.h>
#include <math.h>
#include <libxml/parser.h>
#include <glib.h>
#include <libintl.h>
#include <gsl/gsl_rng.h>
#include "config.h"
#include "utils.h"
#include "optimize.h"
#include "rk.h"
#include "rk_5_4.h"

#define DEBUG_RK_5_4 0          ///< macro to debug.

/**
 * Function to obtain the coefficients of a 5 steps 4th order Runge-Kutta
 * method.
 */
int
rk_tb_5_4 (Optimize * optimize) ///< Optimize struct.
{
  long double A[4], B[4], C[4], D[4], E[4];
  long double *tb, *r;
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_tb_5_4: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t5 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  t3 (tb) = r[3];
  b31 (tb) = r[4];
  b32 (tb) = r[5];
  t4 (tb) = r[6];
  A[0] = t1 (tb);
  B[0] = t2 (tb);
  C[0] = t3 (tb);
  D[0] = t4 (tb);
  E[0] = 0.5L;
  A[1] = A[0] * t1 (tb);
  B[1] = B[0] * t2 (tb);
  C[1] = C[0] * t3 (tb);
  D[1] = D[0] * t4 (tb);
  E[1] = 1.L / 3.L;
  A[2] = A[1] * t1 (tb);
  B[2] = B[1] * t2 (tb);
  C[2] = C[1] * t3 (tb);
  D[2] = D[1] * t4 (tb);
  E[2] = 0.25L;
  A[3] = D[3] = 0.L;
  B[3] = b21 (tb) * t1 (tb) * (t2 (tb) - t4 (tb));
  C[3] = (b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb)) * (t3 (tb) - t4 (tb));
  E[3] = 0.125L - 1.L / 6.L * t4 (tb);
  solve_4 (A, B, C, D, E);
  if (isnan (E[0]) || isnan (E[1]) || isnan (E[2]) || isnan (E[3]))
    return 0;
  b54 (tb) = E[3];
  b53 (tb) = E[2];
  b52 (tb) = E[1];
  b51 (tb) = E[0];
  A[0] = t1 (tb);
  B[0] = t2 (tb);
  C[0] = t3 (tb);
  D[0] = 1.L / 6.L - b52 (tb) * b21 (tb) * t1 (tb)
    - b53 (tb) * (b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb));
  A[1] = A[0] * t1 (tb);
  B[1] = B[0] * t2 (tb);
  C[1] = C[0] * t3 (tb);
  D[1] = 1.L / 12.L - b52 (tb) * b21 (tb) * sqr (t1 (tb))
    - b53 (tb) * (b31 (tb) * sqr (t1 (tb)) + b32 (tb) * sqr (t2 (tb)));
  A[2] = 0.L;
  B[2] = b21 (tb) * t1 (tb);
  C[2] = b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb);
  D[2] = 1.L / 24.L - b53 (tb) * b32 (tb) * b21 (tb) * t1 (tb);
  solve_3 (A, B, C, D);
  b43 (tb) = D[2] / b54 (tb);
  if (isnan (b43 (tb)))
    return 0;
  b42 (tb) = D[1] / b54 (tb);
  if (isnan (b42 (tb)))
    return 0;
  b41 (tb) = D[0] / b54 (tb);
  if (isnan (b41 (tb)))
    return 0;
  rk_b_5 (tb);
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_tb_5_4: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 5 steps 4th order, 5th order in
 * equations depending only on time, Runge-Kutta method.
 */
int
rk_tb_5_4t (Optimize * optimize)        ///< Optimize struct.
{
  long double A[4], B[4], C[4], D[4], E[4];
  long double *tb, *r;
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_tb_5_4t: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t5 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  t3 (tb) = r[2];
  t4 (tb) = r[3];
  b31 (tb) = r[4];
  b21 (tb) = r[5];
  A[0] = t1 (tb);
  B[0] = t2 (tb);
  C[0] = t3 (tb);
  D[0] = t4 (tb);
  E[0] = 0.5L;
  A[1] = A[0] * t1 (tb);
  B[1] = B[0] * t2 (tb);
  C[1] = C[0] * t3 (tb);
  D[1] = D[0] * t4 (tb);
  E[1] = 1.L / 3.L;
  A[2] = A[1] * t1 (tb);
  B[2] = B[1] * t2 (tb);
  C[2] = C[1] * t3 (tb);
  D[2] = D[1] * t4 (tb);
  E[2] = 0.25L;
  A[3] = A[2] * t1 (tb);
  B[3] = B[2] * t2 (tb);
  C[3] = C[2] * t3 (tb);
  D[3] = D[2] * t4 (tb);
  E[3] = 0.2L;
  solve_4 (A, B, C, D, E);
  if (isnan (E[0]) || isnan (E[1]) || isnan (E[2]) || isnan (E[3]))
    return 0;
  b54 (tb) = E[3];
  b53 (tb) = E[2];
  b52 (tb) = E[1];
  b51 (tb) = E[0];
  b32 (tb) = (1.L / 6.L * t4 (tb) - 0.125L
              - t1 (tb) * (b52 (tb) * b21 (tb) * (t4 (tb) - t2 (tb))
                           + b53 (tb) * b31 (tb) * (t4 (tb) - t3 (tb))))
    / (b53 (tb) * t2 (tb) * (t4 (tb) - t3 (tb)));
  A[0] = t1 (tb);
  B[0] = t2 (tb);
  C[0] = t3 (tb);
  D[0] = 1.L / 6.L - b52 (tb) * b21 (tb) * t1 (tb)
    - b53 (tb) * (b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb));
  A[1] = A[0] * t1 (tb);
  B[1] = B[0] * t2 (tb);
  C[1] = C[0] * t3 (tb);
  D[1] = 1.L / 12.L - b52 (tb) * b21 (tb) * sqr (t1 (tb))
    - b53 (tb) * (b31 (tb) * sqr (t1 (tb)) + b32 (tb) * sqr (t2 (tb)));
  A[2] = 0.L;
  B[2] = b21 (tb) * t1 (tb);
  C[2] = b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb);
  D[2] = 1.L / 24.L - b53 (tb) * b32 (tb) * b21 (tb) * t1 (tb);
  solve_3 (A, B, C, D);
  b43 (tb) = D[2] / b54 (tb);
  if (isnan (b43 (tb)))
    return 0;
  b42 (tb) = D[1] / b54 (tb);
  if (isnan (b42 (tb)))
    return 0;
  b41 (tb) = D[0] / b54 (tb);
  if (isnan (b41 (tb)))
    return 0;
  rk_b_5 (tb);
#if DEBUG_RK_5_4
  rk_print_tb (optimize, "rk_tb_5_4t", stderr);
  fprintf (stderr, "rk_tb_5_4t: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 5 steps 3rd-4th order Runge-Kutta
 * pair.
 */
int
rk_tb_5_4p (Optimize * optimize)        ///< Optimize struct.
{
  long double A[3], B[3], C[3], D[3];
  long double *tb;
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_tb_5_4p: start\n");
#endif
  if (!rk_tb_5_4 (optimize))
    return 0;
  tb = optimize->coefficient;
  A[0] = t1 (tb);
  B[0] = t2 (tb);
  C[0] = t3 (tb);
  D[0] = 0.5L;
  A[1] = A[0] * t1 (tb);
  B[1] = B[0] * t2 (tb);
  C[1] = C[0] * t3 (tb);
  D[1] = 1.L / 3.L;
  A[2] = 0.L;
  B[2] = b21 (tb) * t1 (tb);
  C[2] = b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb);
  D[2] = 1.L / 6.L;
  solve_3 (A, B, C, D);
  if (isnan (D[0]) || isnan (D[1]) || isnan (D[2]))
    return 0;
  e53 (tb) = D[2];
  e52 (tb) = D[1];
  e51 (tb) = D[0];
  rk_e_5 (tb);
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_tb_5_4p: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 5 steps 3th-4th order, 4th-5th order
 * in equations depending only on time, Runge-Kutta pair.
 */
int
rk_tb_5_4tp (Optimize * optimize)       ///< Optimize struct.
{
  long double A[4], B[4], C[4], D[4], E[4];
  long double *tb, *r;
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_tb_5_4tp: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t5 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  t3 (tb) = r[2];
  t4 (tb) = r[3];
  b31 (tb) = r[4];
  A[0] = t1 (tb);
  B[0] = t2 (tb);
  C[0] = t3 (tb);
  D[0] = t4 (tb);
  E[0] = 0.5L;
  A[1] = A[0] * t1 (tb);
  B[1] = B[0] * t2 (tb);
  C[1] = C[0] * t3 (tb);
  D[1] = D[0] * t4 (tb);
  E[1] = 1.L / 3.L;
  A[2] = A[1] * t1 (tb);
  B[2] = B[1] * t2 (tb);
  C[2] = C[1] * t3 (tb);
  D[2] = D[1] * t4 (tb);
  E[2] = 0.25L;
  A[3] = A[2] * t1 (tb);
  B[3] = B[2] * t2 (tb);
  C[3] = C[2] * t3 (tb);
  D[3] = D[2] * t4 (tb);
  E[3] = 0.2L;
  solve_4 (A, B, C, D, E);
  if (isnan (E[0]) || isnan (E[1]) || isnan (E[2]) || isnan (E[3]))
    return 0;
  b54 (tb) = E[3];
  b53 (tb) = E[2];
  b52 (tb) = E[1];
  b51 (tb) = E[0];
  e53 (tb) = (0.25L - 1.L / 3.L * t1 (tb)
              - (1.L / 3.L - 0.5L * t1 (tb)) * t2 (tb))
    / (t3 (tb) * (t3 (tb) - t2 (tb)) * (t3 (tb) - t1 (tb)));
  if (isnan (e53 (tb)))
    return 0;
  e52 (tb) = (1.L / 3.L - 0.5L * t1 (tb)
              - t3 (tb) * (t3 (tb) - t1 (tb)) * e53 (tb))
    / (t2 (tb) * (t2 (tb) - t1 (tb)));
  if (isnan (e52 (tb)))
    return 0;
  e51 (tb) = (0.5L - t2 (tb) * e52 (tb) - t3 (tb) * e53 (tb)) / t1 (tb);
  if (isnan (e51 (tb)))
    return 0;
  b21 (tb) = (1.L / 6.L * b53 (tb) * (t4 (tb) - t3 (tb))
              + e53 (tb) * (0.125L - 1.L / 6.L * t4 (tb)))
    / (t1 (tb) * (e52 (tb) * b53 (tb) * (t4 (tb) - t3 (tb))
                  - e53 (tb) * b52 (tb) * (t4 (tb) - t2 (tb))));
  if (isnan (b21 (tb)))
    return 0;
  b32 (tb) = (1.L / 6.L * t4 (tb) - 0.125L
              - t1 (tb) * (b52 (tb) * b21 (tb) * (t4 (tb) - t2 (tb))
                           + b53 (tb) * b31 (tb) * (t4 (tb) - t3 (tb))))
    / (b53 (tb) * t2 (tb) * (t4 (tb) - t3 (tb)));
  if (isnan (b32 (tb)))
    return 0;
  A[0] = t1 (tb);
  B[0] = t2 (tb);
  C[0] = t3 (tb);
  D[0] = 1.L / 6.L - b52 (tb) * b21 (tb) * t1 (tb)
    - b53 (tb) * (b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb));
  A[1] = A[0] * t1 (tb);
  B[1] = B[0] * t2 (tb);
  C[1] = C[0] * t3 (tb);
  D[1] = 1.L / 12.L - b52 (tb) * b21 (tb) * sqr (t1 (tb))
    - b53 (tb) * (b31 (tb) * sqr (t1 (tb)) + b32 (tb) * sqr (t2 (tb)));
  A[2] = 0.L;
  B[2] = b21 (tb) * t1 (tb);
  C[2] = b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb);
  D[2] = 1.L / 24.L - b53 (tb) * b32 (tb) * b21 (tb) * t1 (tb);
  solve_3 (A, B, C, D);
  b43 (tb) = D[2] / b54 (tb);
  if (isnan (b43 (tb)))
    return 0;
  b42 (tb) = D[1] / b54 (tb);
  if (isnan (b42 (tb)))
    return 0;
  b41 (tb) = D[0] / b54 (tb);
  if (isnan (b41 (tb)))
    return 0;
  rk_b_5 (tb);
  rk_e_5 (tb);
#if DEBUG_RK_5_4
  rk_print_tb (optimize, "rk_tb_5_4tp", stderr);
  fprintf (stderr, "rk_tb_5_4tp: end\n");
#endif
  return 1;
}

/**
 * Function to calculate the objective function of a 5 steps 4th order
 * Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_5_4 (RK * rk)   ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_objective_tb_5_4: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b41 (tb) < 0.L)
    o += b41 (tb);
  if (b42 (tb) < 0.L)
    o += b42 (tb);
  if (b43 (tb) < 0.L)
    o += b43 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b51 (tb) < 0.L)
    o += b51 (tb);
  if (b52 (tb) < 0.L)
    o += b52 (tb);
  if (b53 (tb) < 0.L)
    o += b53 (tb);
  if (b54 (tb) < 0.L)
    o += b54 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), fmaxl (t3 (tb), t4 (tb)))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_objective_tb_5_4: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_5_4: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 5 steps 4th order, 5th
 * order in equations depending only on time, Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_5_4t (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_objective_tb_5_4t: start\n");
#endif
  tb = rk->tb->coefficient;
#if DEBUG_RK_5_4
  rk_print_tb (optimize, "rk_objective_tb_5_4t", stderr);
#endif
  o = fminl (0.L, b20 (tb));
  if (b21 (tb) < 0.L)
    o += b21 (tb);
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b32 (tb) < 0.L)
    o += b32 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b41 (tb) < 0.L)
    o += b41 (tb);
  if (b42 (tb) < 0.L)
    o += b42 (tb);
  if (b43 (tb) < 0.L)
    o += b43 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b51 (tb) < 0.L)
    o += b51 (tb);
  if (b52 (tb) < 0.L)
    o += b52 (tb);
  if (b53 (tb) < 0.L)
    o += b53 (tb);
  if (b54 (tb) < 0.L)
    o += b54 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), fmaxl (t3 (tb), t4 (tb)))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_objective_tb_5_4t: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_5_4t: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 5 steps 3rd-4th order
 * Runge-Kutta pair.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_5_4p (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_objective_tb_5_4p: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b41 (tb) < 0.L)
    o += b41 (tb);
  if (b42 (tb) < 0.L)
    o += b42 (tb);
  if (b43 (tb) < 0.L)
    o += b43 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b51 (tb) < 0.L)
    o += b51 (tb);
  if (b52 (tb) < 0.L)
    o += b52 (tb);
  if (b53 (tb) < 0.L)
    o += b53 (tb);
  if (b54 (tb) < 0.L)
    o += b54 (tb);
  if (e50 (tb) < 0.L)
    o += e50 (tb);
  if (e51 (tb) < 0.L)
    o += e51 (tb);
  if (e52 (tb) < 0.L)
    o += e52 (tb);
  if (e53 (tb) < 0.L)
    o += e53 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), fmaxl (t3 (tb), t4 (tb)))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_objective_tb_5_4p: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_5_4p: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 5 steps 3th-4th order,
 * 4th-5th order in equations depending only on time, Runge-Kutta pair.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_5_4tp (RK * rk) ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_objective_tb_5_4t: start\n");
#endif
  tb = rk->tb->coefficient;
#if DEBUG_RK_5_4
  rk_print_tb (optimize, "rk_objective_tb_5_4tp", stderr);
#endif
  o = fminl (0.L, b20 (tb));
  if (b21 (tb) < 0.L)
    o += b21 (tb);
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b32 (tb) < 0.L)
    o += b32 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b41 (tb) < 0.L)
    o += b41 (tb);
  if (b42 (tb) < 0.L)
    o += b42 (tb);
  if (b43 (tb) < 0.L)
    o += b43 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b51 (tb) < 0.L)
    o += b51 (tb);
  if (b52 (tb) < 0.L)
    o += b52 (tb);
  if (b53 (tb) < 0.L)
    o += b53 (tb);
  if (b54 (tb) < 0.L)
    o += b54 (tb);
  if (e50 (tb) < 0.L)
    o += e50 (tb);
  if (e51 (tb) < 0.L)
    o += e51 (tb);
  if (e52 (tb) < 0.L)
    o += e52 (tb);
  if (e53 (tb) < 0.L)
    o += e53 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), fmaxl (t3 (tb), t4 (tb)))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_5_4
  fprintf (stderr, "rk_objective_tb_5_4tp: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_5_4tp: end\n");
#endif
  return o;
}


/* Utility functions for working with GSL data types. */

#include <math.h>
#include <stdio.h>
#include <unistd.h>

#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_sort_vector.h>
#include <gsl/gsl_spmatrix.h>
#include <gsl/gsl_statistics.h>
#include <hdf5.h>
#include <hdf5_hl.h>
#include <osqp/osqp.h>

#include "qdm.h"

gsl_vector *
qdm_vector_seq(double from, double to, double by)
{
  size_t size = fabs(to - from) / by;
  gsl_vector *s = gsl_vector_alloc(size + 1);

  double value = from;
  for (size_t i = 0; i < s->size; i++) {
    gsl_vector_set(s, i, value);
    value += by;
  }

  return s;
}

void
qdm_vector_set_seq(gsl_vector *v, double from, double to)
{
  double by = (to - from) / (double)(v->size - 1);

  double value = from;
  for (size_t i = 0; i < v->size; i++) {
    gsl_vector_set(v, i, value);
    value += by;
  }
}

size_t
qdm_vector_search(const gsl_vector *v, double needle)
{
  double x = 0;
  size_t i = v->size - 1;

  for (; i > 0; i--) {
    x = gsl_vector_get(v, i);

    if (needle >= x) {
      break;
    }
  }

  return i;
}

void
qdm_vector_csv_fwrite(FILE *f, const gsl_vector *v)
{
  for (size_t i = 0; i < v->size; i++) {
    fprintf(f, "%.17g", gsl_vector_get(v, i));
    if (i < v->size - 1) {
      fprintf(f, ",");
    }
  }
  fprintf(f, "\n");
}

void
qdm_matrix_csv_fwrite(FILE *f, const gsl_matrix *m)
{
  if (m != NULL) {
    for (size_t i = 0; i < m->size1; i++) {
      gsl_vector_const_view row = gsl_matrix_const_row(m, i);
      qdm_vector_csv_fwrite(f, &row.vector);
    }
  }
}

/* Compute M^T * M */
int
qdm_matrix_tmm(gsl_matrix *m, gsl_matrix *result)
{
  return gsl_blas_dgemm(
      CblasTrans , CblasNoTrans , 1.0 ,
      m          , m            ,
      0.0        , result
  );
}

/* Compute det(M^T * M) */
int
qdm_matrix_det_tmm(gsl_matrix *m, double *det)
{
  int status = 0;
  gsl_matrix *c = gsl_matrix_alloc(m->size2, m->size2);
  gsl_permutation *p = gsl_permutation_alloc(c->size1);

  status = gsl_blas_dgemm(
      CblasTrans , CblasNoTrans , 1.0 ,
      m          , m            ,
      0.0        , c
  );
  if (status != 0) {
    goto cleanup;
  }

  int signum = 0;
  status = gsl_linalg_LU_decomp(c, p, &signum);
  if (status != 0) {
    goto cleanup;
  }

  *det = gsl_linalg_LU_det(c, signum);

cleanup:
  gsl_permutation_free(p);
  gsl_matrix_free(c);

  return status;
}

/* Create a sorted copy of the vector v. */
gsl_vector *
qdm_vector_sorted(const gsl_vector *v)
{
  gsl_vector *s = gsl_vector_alloc(v->size);

  gsl_vector_memcpy(s, v);
  gsl_sort_vector(s);

  return s;
}

gsl_vector *
qdm_vector_quantile(gsl_vector *data, gsl_vector *probs)
{
  gsl_vector *sorted = qdm_vector_sorted(data);
  gsl_vector *quantiles = gsl_vector_alloc(probs->size);

  for (size_t i = 0; i < probs->size; i++) {
    double q = gsl_stats_quantile_from_sorted_data(
        sorted->data,
        sorted->stride,
        sorted->size,
        gsl_vector_get(probs, i)
    );

    gsl_vector_set(quantiles, i, q);
  }

  free(sorted);

  return quantiles;
}

/* Compute the residual sum of squares:
 *
 * sum((y[i] - f(x[i])) ^ 2)
 */
double
qdm_vector_rss(const gsl_vector *y, const gsl_vector *fx)
{
  int status = 0;
  double rss = 0.0;

  gsl_vector *se = gsl_vector_alloc(y->size);
  status = gsl_vector_memcpy(se, y);
  if (status != 0) {
    goto cleanup;
  }

  status = gsl_vector_sub(se, fx);
  if (status != 0) {
    goto cleanup;
  }

  status = gsl_vector_mul(se, se);
  if (status != 0) {
    goto cleanup;
  }

  rss = qdm_vector_sum(se);

cleanup:
  gsl_vector_free(se);

  return rss;
}

/* Compute the summation of the vector.
 *
 * This uses the "iterative Kahan-Babuska algorithm" (aka Klein summation) as
 * outlined here:
 *
 * https://en.wikipedia.org/wiki/Kahan_summation_algorithm
 */
double
qdm_vector_sum(gsl_vector *v)
{
  double s = 0.0;
  double cs = 0.0;
  double ccs = 0.0;

  for (size_t i = 0; i < v->size; i++) {
    double t = s + gsl_vector_get(v, i);

    double c;
    if (fabs(s) >= fabs(gsl_vector_get(v, i))) {
      c = (s - t) + gsl_vector_get(v, i);
    } else {
      c = (gsl_vector_get(v, i) - t) + s;
    }

    s = t;
    t = cs + c;

    double cc;
    if (fabs(cs) >= fabs(c)) {
      cc = (cs - t) + c;
    } else {
      cc = (c - t) + cs;
    }

    cs = t;
    ccs = ccs + cc;
  }

  return s + cs + ccs;
}

/* Return the first index with a value greater than the value. */
size_t
qdm_vector_greater_than(const gsl_vector *v, double value)
{
  for (size_t i = 0; i < v->size; i++) {
    if (gsl_vector_get(v, i) > value) {
      return i;
    }
  }

  return 0;
}

/* Select the elements in the upper triangle of the matrix. All other elements
 * will be set to zero.
 */
void
qdm_matrix_select_upper_triangle(gsl_matrix *m)
{
  for (size_t i = 0; i < m->size1; i++) {
    for (size_t j = 0; j < m->size2; j++) {
      if (j < i) {
        gsl_matrix_set(m, i, j, 0);
      }
    }
  }
}

int
qdm_matrix_to_csc_matrix(csc **result, gsl_matrix *input)
{
  int status = 0;

  gsl_spmatrix *sm = gsl_spmatrix_alloc(input->size1, input->size2);

  status = gsl_spmatrix_d2sp(sm, input);
  if (status != 0) {
    goto cleanup_sm;
  }

  gsl_spmatrix *sm_csc = gsl_spmatrix_compress(sm, GSL_SPMATRIX_CSC);

  int m = sm_csc->size1;
  int n = sm_csc->size2;
  int nzmax = gsl_spmatrix_nnz(sm_csc);

  size_t x_size = sizeof(double) * nzmax;
  double *x = malloc(x_size);
  memcpy(x, sm_csc->data, x_size);

  size_t i_size = sizeof(int) * nzmax;
  int *i = malloc(i_size);
  memcpy(i, sm_csc->i, i_size);

  size_t p_size = sizeof(int) * (n + 1);
  int *p = malloc(p_size);
  memcpy(p, sm_csc->p, p_size);

  *result = csc_matrix(
      m,     // m     First dimension (rows)
      n,     // n     Second dimension (columns)
      nzmax, // nzmax Maximum number of nonzero elements
      x,     // x     Vector of data (size nzmax)
      i,     // i     Vector of row indices (size nzmax)
      p      // p     Vector of column pointers (size n+1)
  );

  gsl_spmatrix_free(sm_csc);

cleanup_sm:
  gsl_spmatrix_free(sm);

  return status;
}

gsl_vector *
qdm_vector_copy(const gsl_vector *src)
{
  gsl_vector *dst = gsl_vector_alloc(src->size);
  gsl_vector_memcpy(dst, src);

  return dst;
}

gsl_matrix *
qdm_matrix_copy(const gsl_matrix *src)
{
  gsl_matrix *dst = gsl_matrix_alloc(src->size1, src->size2);
  gsl_matrix_memcpy(dst, src);

  return dst;
}

int create_hd5(
    const char *file_path,
    const char *group_path
)
{
  int status = 0;

  hid_t file  = -1;
  hid_t group = -1;
  hid_t gcpl  = -1;

  /* Save old error handler */
  herr_t (*old_func)(hid_t, void*) = NULL;
  void *old_client_data = NULL;
  H5Eget_auto(H5E_DEFAULT, &old_func, &old_client_data);

  /* Turn off error handling */
  H5Eset_auto(H5E_DEFAULT, NULL, NULL);

  file = H5Fopen(file_path, H5F_ACC_RDWR, H5P_DEFAULT);
  if (file < 0) {
    file = H5Fcreate(file_path, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  }
  if (file < 0) {
    status = file;
    goto cleanup;
  }

  group = H5Gopen(file, group_path, H5P_DEFAULT);
  if (group < 0) {
    gcpl = H5Pcreate(H5P_LINK_CREATE);
    if (gcpl < 0) {
      status = gcpl;
      goto cleanup;
    }

    status = H5Pset_create_intermediate_group(gcpl, 1);
    if (status < 0) {
      goto cleanup;
    }

    group = H5Gcreate(file, group_path, gcpl, H5P_DEFAULT, H5P_DEFAULT);

    if (group < 0) {
      status = group;
      goto cleanup;
    }
  }

cleanup:
  if (gcpl >= 0) {
    H5Pclose(gcpl);
  }

  if (group >= 0) {
    H5Gclose(group);
  }

  if (file >= 0) {
    H5Fclose(file);
  }

  /* Restore previous error handler */
  H5Eset_auto(H5E_DEFAULT, old_func, old_client_data);

  if (status < 0) {
    H5Eprint(H5E_DEFAULT, stderr);
  }

  return status;
}

int
qdm_vector_hd5_read(
    hid_t id,
    const char *name,
    gsl_vector **v
)
{
  int status = 0;

  int rank = 0;

  status = H5LTget_dataset_ndims(id, name, &rank);
  if (status < 0) {
    return status;
  }

  hsize_t dims[rank];

  status = H5LTget_dataset_info(id, name, dims, NULL, NULL);
  if (status < 0) {
    return status;
  }

  size_t size = 1;
  for (int i = 0; i < rank; i++) {
    size *= dims[i];
  }

  gsl_vector *tmp = gsl_vector_alloc(size);

  status = H5LTread_dataset_double(id, name, tmp->data);
  if (status < 0) {
    gsl_vector_free(tmp);

    return status;
  }

  *v = tmp;

  return status;
}

int
qdm_vector_hd5_write(
    hid_t id,
    const char *name,
    const gsl_vector *v
)
{
  int status = 0;

  hid_t datatype  = -1;
  hid_t dataspace = -1;
  hid_t dataset   = -1;
  hid_t dcpl      = -1;

  if (v == NULL) {
    goto cleanup;
  }

  datatype = H5Tcopy(H5T_NATIVE_DOUBLE);

  status = H5Tset_order(datatype, H5T_ORDER_LE);
  if (status != 0) {
    goto cleanup;
  }

  hsize_t dims[1] = {
    v->size,
  };
  dataspace = H5Screate_simple(1, dims, NULL);
  if (dataspace < 0) {
    status = dataspace;
    goto cleanup;
  }

  dcpl = H5Pcreate(H5P_DATASET_CREATE);
  if (dcpl < 0) {
    status = dcpl;

    goto cleanup;
  }

  /* Only enable compression on larger vectors. */
  /*
  if (v->size > 1024) {
    status = H5Pset_deflate(dcpl, 9);
    if (status != 0) {
      goto cleanup;
    }

    hsize_t chunk_dims[1] = {
      v->size,
    };
    status = H5Pset_chunk(dcpl, 1, chunk_dims);
    if (status != 0) {
      goto cleanup;
    }
  }
  */

  dataset = H5Dcreate(id, name, datatype, dataspace, H5P_DEFAULT, dcpl, H5P_DEFAULT);
  if (dataset < 0) {
    status = dataset;
    goto cleanup;
  }

  status = H5Dwrite(dataset, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, v->data);
  if (status != 0) {
    goto cleanup;
  }

cleanup:
  if (dcpl >= 0) {
    H5Pclose(dcpl);
  }

  if (dataset >= 0) {
    H5Dclose(dataset);
  }

  if (dataspace >= 0) {
    H5Sclose(dataspace);
  }

  if (datatype >= 0) {
    H5Tclose(datatype);
  }

  H5Oflush(id);

  return status;
}

int
qdm_matrix_hd5_read(
    hid_t id,
    const char *name,
    gsl_matrix **m
)
{
  int status = 0;

  int rank = 0;

  status = H5LTget_dataset_ndims(id, name, &rank);
  if (status < 0) {
    return status;
  }

  if (rank < 2) {
    return -1;
  }

  hsize_t dims[rank];

  status = H5LTget_dataset_info(id, name, dims, NULL, NULL);
  if (status < 0) {
    return status;
  }

  size_t size1 = dims[0];
  size_t size2 = 1;
  for (int i = 1; i < rank; i++) {
    size2 *= dims[i];
  }

  gsl_matrix *tmp = gsl_matrix_alloc(size1, size2);

  status = H5LTread_dataset_double(id, name, tmp->data);
  if (status < 0) {
    gsl_matrix_free(tmp);

    return status;
  }

  *m = tmp;

  return status;
}

int
qdm_matrix_hd5_write(
    hid_t id,
    const char *name,
    const gsl_matrix *m
)
{
  int status = 0;

  hid_t datatype  = -1;
  hid_t dataspace = -1;
  hid_t dataset   = -1;
  hid_t dcpl      = -1;

  if (m == NULL) {
    goto cleanup;
  }

  datatype = H5Tcopy(H5T_NATIVE_DOUBLE);

  status = H5Tset_order(datatype, H5T_ORDER_LE);
  if (status != 0) {
    goto cleanup;
  }

  hsize_t dims[2] = {
    m->size1,
    m->size2,
  };
  dataspace = H5Screate_simple(2, dims, NULL);
  if (dataspace < 0) {
    status = dataspace;

    goto cleanup;
  }

  dcpl = H5Pcreate(H5P_DATASET_CREATE);
  if (dcpl < 0) {
    status = dcpl;

    goto cleanup;
  }

  /* Only enable compression on larger matrices. */
  /*
  if (m->size1 * m->size2 > 1024) {
    status = H5Pset_deflate(dcpl, 9);
    if (status != 0) {
      goto cleanup;
    }

    hsize_t chunk_dims[2] = {
      m->size1,
      m->size2
    };
    status = H5Pset_chunk(dcpl, 2, chunk_dims);
    if (status != 0) {
      goto cleanup;
    }
  }
  */

  dataset = H5Dcreate(id, name, datatype, dataspace, H5P_DEFAULT, dcpl, H5P_DEFAULT);
  if (dataset < 0) {
    status = dataset;

    goto cleanup;
  }

  status = H5Dwrite(dataset, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, m->data);
  if (status != 0) {
    goto cleanup;
  }

cleanup:
  if (dcpl >= 0) {
    H5Pclose(dcpl);
  }

  if (dataset >= 0) {
    H5Dclose(dataset);
  }

  if (dataspace >= 0) {
    H5Sclose(dataspace);
  }

  if (datatype >= 0) {
    H5Tclose(datatype);
  }

  H5Oflush(id);

  return status;
}

gsl_vector *
qdm_matrix_filter(
    const gsl_matrix *m,
    size_t needle_column,
    double needle_value,
    size_t select_column
)
{
  size_t size = 0;

  for (size_t i = 0; i < m->size1; i++) {
    if (gsl_matrix_get(m, i, needle_column) == needle_value) {
      size++;
    }
  }

  gsl_vector *v = gsl_vector_alloc(size);

  size_t j = 0;
  for (size_t i = 0; i < m->size1; i++) {
    double select_value = gsl_matrix_get(m, i, select_column);

    if (gsl_matrix_get(m, i, needle_column) == needle_value) {
      gsl_vector_set(v, j, select_value);
      j++;
    }
  }

  return v;
}


#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <nlopt.h>
#include <unistd.h>
#include <sys/time.h>

void quadbezier(const double *a, const double *b, const double *c,
		double t, double *out)
{
  double onemt = 1.0 - t;
  out[0] = onemt * (onemt * a[0] + t * b[0]) + t * (onemt * b[0] + t * c[0]);
  out[1] = onemt * (onemt * a[1] + t * b[1]) + t * (onemt * b[1] + t * c[1]);
}

void linearinterp(const double *a, const double *b,
		  double t, double *out)
{
  out[0] = (b[0] - a[0]) * t + a[0];
  out[1] = (b[1] - a[1]) * t + a[1];
}

typedef struct {
  int N;
  int steps;
  double *starts;
  double *stops;
  double *scratch;
} problem_t;

double randd()
{
  int n = rand() % (1<<24);
  return ((double)n) / ((double)(1<<24));
}

double distance(const double *a, const double *b)
{
  return sqrt((b[0] - a[0]) * (b[0] - a[0]) +
	      (b[1] - a[1]) * (b[1] - a[1]));
}

#define RADIUS 5.0

double errorf(unsigned n, const double *pts, double *grad,
	      void *data)
{
  problem_t *setup = (problem_t *)data;
  double error = 0.0;

  for(int step=0; step<setup->steps; step++) {
    double t = (1.0+step) / (1.0+setup->steps);
    for(int i=0; i<setup->N; i++)
      quadbezier(&setup->starts[2*i],
		 &pts[2*i],
		 &setup->stops[2*i],
		 t,
		 &setup->scratch[2*i]);

    for(int i=0; i<setup->N; i++)
      for(int j=i+1; j<setup->N; j++) {
	double d = distance(&setup->scratch[2*i],
			    &setup->scratch[2*j]);
	d /= RADIUS;
	error += (1.0/d) * (1.0/d);
      }
  }

  return error / setup->steps;
}

int main(int argc, char *argv[])
{
  int N = 12;

  double *starts = (double *)malloc(sizeof(double)*N*2);
  double *stops = (double *)malloc(sizeof(double)*N*2);

  srand(2);
  nlopt_srand(1);

  double scaling = 100.0;
  for(int i=0; i<N; i++) {
    starts[2*i+0] = scaling * randd();
    starts[2*i+1] = scaling * randd();
    stops[2*i+0]  = scaling * randd();
    stops[2*i+1]  = scaling * randd();
  }

  double hs = scaling / 2.0;
  for(int i=0; i<N; i++) {
    double theta = ((double)i)/N * 2 * 3.141592;
    starts[2*i+0] = hs * cos(theta) + hs;
    starts[2*i+1] = hs * sin(theta) + hs;
    stops[2*i+0]  = hs * cos(theta + 3.141592) + hs;
    stops[2*i+1]  = hs * sin(theta + 3.141592) + hs;
  }

  struct timeval start, stop;

  double *guesses = (double *)malloc(sizeof(double)*N*2);

  gettimeofday(&start, NULL);
  for(int i=0; i<N; i++) {
    linearinterp(&starts[2*i], &stops[2*i], 0.5, &guesses[2*i]);
  }

  nlopt_opt global = nlopt_create(NLOPT_GN_ISRES, 2*N);
  nlopt_opt local = nlopt_create(NLOPT_LN_SBPLX, 2*N);

  problem_t *setup = (problem_t *)malloc(sizeof(problem_t));
  setup->N = N;
  setup->starts = starts;
  setup->stops = stops;
  setup->steps = 50;
  setup->scratch = (double *)malloc(sizeof(double)*N*2);

  nlopt_result res;

  res = nlopt_set_min_objective(global, errorf, setup);
  if(res!=1) { exit(-1); }

  res = nlopt_set_lower_bounds1(global, -25.0);
  if(res!=1) { exit(-1); }

  res = nlopt_set_upper_bounds1(global, 125.0);
  if(res!=1) { exit(-1); }

  res = nlopt_set_stopval(global, 1.0);
  if(res!=1) { exit(-1); }

  res = nlopt_set_maxeval(global, 100);
  if(res!=1) { exit(-1); }

  double final_error;
  res = nlopt_optimize(global, guesses, &final_error);

  res = nlopt_set_min_objective(local, errorf, setup);
  if(res!=1) { exit(-1); }

  res = nlopt_set_lower_bounds1(local, -25.0);
  if(res!=1) { exit(-1); }

  res = nlopt_set_upper_bounds1(local, 125.0);
  if(res!=1) { exit(-1); }

  res = nlopt_set_stopval(local, 1.0);
  if(res!=1) { exit(-1); }

  res = nlopt_set_maxeval(local, 1000);
  if(res!=1) { exit(-1); }

  res = nlopt_optimize(local, guesses, &final_error);
  gettimeofday(&stop, NULL);

  fprintf(stderr, "final: %f\n", final_error);

  for(int i=0; i<N; i++) {
    printf("%f %f %f %f %f %f\n",
	   starts[2*i+0], starts[2*i+1],
	   guesses[2*i+0], guesses[2*i+1],
	   stops[2*i+0], stops[2*i+1]);
  }

  double startd, stopd;
  startd = start.tv_sec;
  startd += ((double)(start.tv_usec)) / 1e6;
  stopd = stop.tv_sec;
  stopd += ((double)(stop.tv_usec)) / 1e6;

  //printf("%f\n", stopd - startd);
}


#include <errno.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <time.h>
#include <clapack.h>
#include <cblas.h>
#include <omp.h>

#define DIM 16
#define NB 256

//#define DIM 96
//#define NB 128

extern void spotrf_(char *, int *, float *, int *, int *);

/*#pragma css task input(NB) inout(A[NB][NB]) highpriority */
void smpSs_spotrf_tile(float *A)
{

unsigned char LO='L';
int INFO;
int nn=NB;
   spotrf_(&LO,
          &nn,
          A,&nn,
          &INFO);
}

/*#pragma css task input(A[NB][NB], B[NB][NB], NB) inout(C[NB][NB])*/
void smpSs_sgemm_tile(float  *A, float *B, float *C)
{
unsigned char TR='T', NT='N';
float DONE=1.0, DMONE=-1.0;
    cblas_sgemm(
        CblasColMajor,
        CblasNoTrans, CblasTrans,
        NB, NB, NB,
        -1.0, A, NB,
              B, NB,
         1.0, C, NB);

}

/*#pragma css task input(T[NB][NB], NB) inout(B[NB][NB])*/
void smpSs_strsm_tile(float *T, float *B)
{
unsigned char LO='L', TR='T', NU='N', RI='R';
float DONE=1.0;

    cblas_strsm(
        CblasColMajor,
        CblasRight, CblasLower, CblasTrans, CblasNonUnit,
        NB, NB,
        1.0, T, NB,
             B, NB);

}

/*#pragma css task input(A[NB][NB], NB) inout(C[NB][NB])*/
void smpSs_ssyrk_tile( float *A, float *C)
{
unsigned char LO='L', NT='N';
float DONE=1.0, DMONE=-1.0;

    cblas_ssyrk(
        CblasColMajor,
        CblasLower,CblasNoTrans,
        NB, NB,
        -1.0, A, NB,
         1.0, C, NB);

}

void compute(struct timeval *start, struct timeval *stop, float *A[DIM][DIM])
{
#pragma omp parallel num_threads(48)
#pragma omp single
{
 gettimeofday(start,NULL);
  double t1 = omp_get_wtime();

  long j,k,i;
  for (j = 0; j < DIM; j++)
  {
    for (k= 0; k< j; k++)
    {
      for (i = j+1; i < DIM; i++)
      {
      #pragma omp task shared(A) firstprivate(i,k,j)  depend(in : A[i][k], A[j][k]) depend(inout:A[i][j])
        smpSs_sgemm_tile(  A[i][k], A[j][k], A[i][j]);
      }
    }
    for (i = 0; i < j; i++)
    {
    #pragma omp task shared(A) firstprivate(i,j) depend (in : A[j][i]) depend(inout : A[j][j])
      smpSs_ssyrk_tile( A[j][i], A[j][j]);
    }

   #pragma omp task shared(A) firstprivate(j) depend(inout : A[j][j])
    smpSs_spotrf_tile( A[j][j]);

    for (i = j+1; i < DIM; i++)
    {
  #pragma omp task shared(A) firstprivate(i,j) depend(in : A[j][j]) depend (inout : A[i][j])
      smpSs_strsm_tile( A[j][j], A[i][j]);
    }

  }
#pragma omp taskwait

  double t2 = omp_get_wtime();
  fprintf(stderr,"Time: %f\n",t2-t1);
}
 gettimeofday(stop,NULL);
 exit(0);
}

static void init(int argc, char **argv,  long *N_p);

float **A;
float * Alin;

long N;

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

unsigned char LO='L';
int  INFO;

  struct timeval start;
  struct timeval stop;
  unsigned long elapsed;

  init(argc, argv, &N);
  fprintf(stderr,"Computing cholesky...\n");

  compute(&start, &stop, (void *)A);

  int nn=N;

  elapsed = 1000000 * (stop.tv_sec - start.tv_sec);
  elapsed += stop.tv_usec - start.tv_usec;

  printf ("%lu;\t", elapsed);
  printf("%d\n", (int)((0.33*N*N*N+0.5*N*N+0.17*N)/elapsed));

  printf("par_sec_time_us:%lu\n",elapsed);
  return 0;
}

static void convert_to_blocks(long N, float *Alin, float *A[DIM][DIM])
{
  long i,j;
  for (i = 0; i < N; i++)
  {
    for (j = 0; j < N; j++)
    {
      A[j/NB][i/NB][(i%NB)*NB+j%NB] = Alin[i*N+j];
    }
  }

}

void fill_random(float *Alin, int NN)
{
  int i;
  for (i = 0; i < NN; i++)
  {
    Alin[i]=((float)rand())/((float)RAND_MAX);
  }
}

static void init(int argc, char **argv,  long *N_p)
{
  long ISEED[4] = {0,0,0,1};
  long IONE=1;

  long N = NB*DIM;
  long NN = N * N;

  *N_p = N;

   Alin = (float *) malloc(NN * sizeof(float));

  fill_random(Alin,NN);
  long i;
  for(i=0; i<N; i++)
  {
    Alin[i*N + i] += N;
  }

  A = (float **) malloc(DIM*DIM*sizeof(float *));
  for ( i = 0; i < DIM*DIM; i++)
    {
//     #pragma omp memory
     A[i] = (float *) malloc(NB*NB*sizeof(float));
      int z;
      for (z=0;z<DIM*DIM;z++)
	   { float *zz = (float *) A[i];
	     zz[z] = Alin[(DIM*DIM)*i + z]; }
    }

  convert_to_blocks(N, Alin, (void *)A);

}


/**
 * @copyright (c) 2017 King Abdullah University of Science and Technology (KAUST).
 *                     All rights reserved.
 **/
/**
 * @file time_zpotrf_tile.c
 *
 * This file shows how to generate tile low-rank (TLR) matrix and factorize it using Cholesky factorization.
 *
 * HiCMA is a software package provided by King Abdullah University of Science and Technology (KAUST)
 *
 * @version 0.1.1
 * @author Kadir Akbudak
 * @date 2018-11-08
 **/

/*
 * @copyright (c) 2009-2014 The University of Tennessee and The University
 *                          of Tennessee Research Foundation.
 *                          All rights reserved.
 * @copyright (c) 2012-2016 Inria. All rights reserved.
 * @copyright (c) 2012-2014 Bordeaux INP, CNRS (LaBRI UMR 5800), Inria, Univ. Bordeaux. All rights reserved.
 */

/**
 * The meaning of the descriptors:
 * - AUV: U and V, side by side
 * - AD : U*V
 * - A  : the original, non-approximated problem
 * - Ark: rank of U and V, each tile of the matrix is a single integer in fact a double.
 *
 **/

#include "morse.h"
#include "timing.h"
#include "hicma_constants.h"
#include "hicma_struct.h"
#include "hicma_z.h"
#include <stdio.h>
#include <time.h>
#include <sys/time.h>
//#include <mpi.h> //MPI_Wtime()

#include "starpu.h"
#ifdef MKL
#include <mkl.h>
//#pragma message("MKL is used")
#else
#include <cblas.h>
#ifdef LAPACKE_UTILS
#include <lapacke_utils.h>
#endif
#include <lapacke.h>
//#pragma message("MKL is NOT used")
#endif

#include "starsh-spatial.h"

#include <assert.h>
#include "hicma_z.h"
#include "auxcompute_z.h"
#include "auxdescutil.h"
#include "hicma.h"
#include <math.h>
#include <time.h>

#undef  CBLAS_SADDR
#define CBLAS_SADDR(_val) (_val)

// zgytlr uses starsh in MPI mode.
STARSH_blrf *mpiF;

int print_progress = 1;   // Print progress about the execution
char datebuf[128];
time_t timer;
struct tm* tm_info;
#define PROGRESS(str) \
    if(print_progress){ \
        int myrank = MORSE_My_Mpi_Rank();\
        time(&timer); \
        tm_info = localtime(&timer); \
        strftime(datebuf, 26, "%Y-%m-%d %H:%M:%S",tm_info); \
        fprintf(stderr, "%d:%s\t%d\t%s\t%s\n", myrank, datebuf, __LINE__, __func__, str);\
        fflush(stderr);\
    }
//#undef PROGRESS
//#define PROGRESS(str)

int store_only_diagonal_tiles = 0;
int global_check = 0;
int global_always_fixed_rank = 0;
int global_fixed_rank = 0;
int global_omit_computation = 1;
int num_mpi_ranks;
int run_potrf = 1;
int diag_nrows = 0;
int main_print_index = 0;
int print_index = 0;
int print_index_end = 0;
int main_print_mat = 0;
int print_mat = 0;
int use_scratch = 1; // Use scratch memory provided by starpu
int calc_rank_stat = 1;

void fwrite_array(int m, int n, int ld, double* arr, char* file){
        FILE* fp = fopen(file, "w");
        if(fp == NULL){
            fprintf(stderr, "File %s cannot be opened to write\n", file);
            exit(1);
        }
        int i, j;
        fprintf(fp, "%d %d\n", m, n);
        for(i = 0; i < m; i++){
            for(j = 0; j < n; j++){
                fprintf(fp, "%d\t", (int)arr[ld*j+i] );
            }
            fprintf(fp, "\n" );
        }
        fclose(fp);
}

double timediff(struct timeval begin, struct timeval end){
    double elapsed = (end.tv_sec - begin.tv_sec) +
        ((end.tv_usec - begin.tv_usec)/1000000.0);
    return elapsed;
}

    int
RunTest(int *iparam, double *dparam, morse_time_t *t_, char* rankfile)
{
    // print progress info only on ROOT process
    if(MORSE_My_Mpi_Rank() != 0)
        print_progress = 0;
    PROGRESS("RunTest started");

    // this paramater enables storing only diagonal tiles in a tall and skinny matrix
    store_only_diagonal_tiles = 1;
    //chameleon/runtime/starpu/control/runtime_descriptor.c
    //MORSE_user_tag_size(31,26);
    //MORSE_user_tag_size(31,29);
    MORSE_user_tag_size(31,27);// When I added tile_to_lapack for descArk, I got not enough number of desc error

    // get parameters coming from command line
    PASTE_CODE_IPARAM_LOCALS( iparam );

    // set global variable so that p.. files can fill dense matrix
    global_check = check;
    // calculate total number of mpi processes (it is not used for now)
    num_mpi_ranks = P*Q;
    print_index = iparam[IPARAM_HICMA_PRINTINDEX];
    print_index_end = iparam[IPARAM_HICMA_PRINTINDEXEND];
    print_mat   = iparam[IPARAM_HICMA_PRINTMAT];
    int64_t _nb = iparam[IPARAM_NB];
    LDA = chameleon_max(M, iparam[IPARAM_LDA]);
    int hicma_maxrank  = iparam[IPARAM_HICMA_MAXRANK];
    global_always_fixed_rank = iparam[IPARAM_HICMA_ALWAYS_FIXED_RANK];

    int saveNB = NB;
    NB = MB;
    size_t ncols_AD;
    int saveP = P;
    int saveQ = Q;
    if (store_only_diagonal_tiles == 1) {
        ncols_AD = MB;
    } else {
        ncols_AD = M;
    }
    int saveN = N;
    N = ncols_AD;
    PASTE_CODE_ALLOCATE_MATRIX_TILE( descAD, 1, double, MorseRealDouble, LDA, M, N );
    N = saveN;
    P = saveP;
    Q = saveQ;
    PROGRESS("descAD is allocated");

    size_t ncols_Dense;
    size_t ld_Dense;
    int saveMB = MB;
    if(check == 0) {
        ncols_Dense = MT;
        MB = NB = 1;
        ld_Dense = MT;
    } else {
        ncols_Dense = M;
        ld_Dense = M;
    }
    /*descDense is full matrix if numerical accuracy will be checked.
     * Otherwise it is MB-by-MB matrix with 1-by-1 tiles */
    PASTE_CODE_ALLOCATE_MATRIX_TILE( descDense, 1, double, MorseRealDouble, ld_Dense, ncols_Dense, ncols_Dense );
    if(check == 0) {
        MB = saveMB;
    } else {
    }
    PROGRESS("descDense is allocated");
    NB = saveNB;

    int MTMB = MT * MB; // roundup number of rows/columns for AUV
    int nrows_AUV = MTMB;
    int ld_AUV = MTMB;
    // allocate descUV
    saveN = N;
    N = N * 2;
    saveNB = NB;
    NB = NB * 2;
    //printf("N:%d NB:%d\n", N, NB);
    PASTE_CODE_ALLOCATE_MATRIX_TILE( descAUV, 1, double, MorseRealDouble, ld_AUV, nrows_AUV, N );
    N = saveN;
    NB = saveNB;
    PROGRESS("descAUV is allocated");

    /* tile dimension of rank descriptor must be 1 */
    /* when LD for rk matrices is 1, program exits*/
    int bigMB = MB;
    int bigNB = NB;
    MB = NB = 1;
    PASTE_CODE_ALLOCATE_MATRIX_TILE( descArk, 1, double, MorseRealDouble, MT, MT, NT);
    PROGRESS("descA's are allocated");
    MB = bigMB;
    NB = bigNB;

    int diag_dense = 1;
    int fixedrank = iparam[IPARAM_RK]; //genargs->k
    double fixedacc = pow(10, -1.0*iparam[IPARAM_ACC]);

    char sym;
    if (run_potrf)
        sym = 'S';
    else
        sym = 'N';
    int probtype = iparam[IPARAM_HICMA_STARSH_PROB];
    int maxrank  = iparam[IPARAM_HICMA_STARSH_MAXRANK];
    //double ddecay = pow(10, -1.0*iparam[IPARAM_HICMA_STARSH_DECAY]);
    double ddecay = dparam[IPARAM_HICMA_STARSH_DECAY];

    int  initial_maxrank, final_maxrank;
    double initial_avgrank, final_avgrank;
    HICMA_problem_t hicma_problem;

    hicma_problem.ndim = 2;

    //BEGIN: rndtiled
    if(iparam[IPARAM_HICMA_STARSH_PROB] == HICMA_STARSH_PROB_RND) {
        hicma_problem.noise    = 1.0; //value added to diagonal
    }
    //END:   rndtiled

    //BEGIN: geostat
    //double theta[3] = {1, 0.1, 0.5}; //initially
    double theta[3] = {
        1.0, //sigma
        0.01, //beta
        10.0 //nu Aleks used 10.0 in his paper
    };
    if(iparam[IPARAM_HICMA_STARSH_PROB] == HICMA_STARSH_PROB_GEOSTAT) {
        hicma_problem.theta = theta;
        hicma_problem.noise = 0.0;
        hicma_problem.noise = 1.e-2;
        hicma_problem.kernel_type = STARSH_SPATIAL_MATERN2_SIMD;
    }
    //END: geostat

    //BEGIN: ss
    if(iparam[IPARAM_HICMA_STARSH_PROB] == HICMA_STARSH_PROB_SS) {
        //sigma=1.0 default value line 193 of stars-h/src/applications/spatial.c
        // Correlation length
        hicma_problem.beta  = 0.1;
        //If fixed rank is required set beta=1 and a sample case will be like this nb=25 maxrank=10 m=2500 So ranks will decrease.

        // Smoothing parameter for Matern kernel
        hicma_problem.nu    = 0.5;
        // Shift added to diagonal elements
        hicma_problem.noise = 1.e-4; //not enough for matrices larger than 600K
        hicma_problem.noise = 5.e-4; //works for 640K but does not work for 10M
        hicma_problem.noise = 1.e-2; //
    }
    //END: ss

    //BEGIN: edsin
    if(iparam[IPARAM_HICMA_STARSH_PROB] == HICMA_STARSH_PROB_EDSIN) {
        // Wave number, >= 0
        hicma_problem.wave_k = dparam[IPARAM_HICMA_STARSH_WAVE_K];
        hicma_problem.diag = M;
    //printf("%s %d: %g\n", __FILE__, __LINE__, hicma_problem.wave_k);
    }
    //END: edsin

    PROGRESS("generating coordinates started");
    struct timeval tvalBefore, tvalAfter;  // removed comma
    gettimeofday (&tvalBefore, NULL);
    HICMA_zgenerate_problem(probtype, sym, ddecay, M, MB, MT, NT, &hicma_problem);
    gettimeofday (&tvalAfter, NULL);
    if(MORSE_My_Mpi_Rank()==0){
        printf("Tproblem:%g\n",
                (tvalAfter.tv_sec - tvalBefore.tv_sec)
                 +(tvalAfter.tv_usec - tvalBefore.tv_usec)/1000000.0
              );
        fflush(stderr);
        fflush(stdout);
    }
    PROGRESS("generating coordinates ended");
    mpiF = hicma_problem.starsh_format; // This is assignment will be hidden from user in release

	// DO NOT enforce compression of diagonal tiles
	int compress_diag = 0;
    PROGRESS("nompi zgytlr starting");
    //descDense original problem
    gettimeofday (&tvalBefore, NULL);
    HICMA_zgytlr_Tile(MorseLower, descAUV, descAD, descArk, 0, maxrank, fixedacc, compress_diag, descDense);
    gettimeofday (&tvalAfter, NULL);
    if(MORSE_My_Mpi_Rank()==0){
        printf("Tcompress:%g\n",
                (tvalAfter.tv_sec - tvalBefore.tv_sec)
                 +(tvalAfter.tv_usec - tvalBefore.tv_usec)/1000000.0
                );
        fflush(stderr);
        fflush(stdout);
    }
    PROGRESS("nompi zgytlr finished");
    fflush(stderr);
    fflush(stdout);
    /*return 0; //TODO*/

    if(calc_rank_stat == 1) {
        PASTE_TILE_TO_LAPACK( descArk, Ark_initial, 1, double, MT, NT );
        if(MORSE_My_Mpi_Rank()==0){

            sprintf(rankfile, "%s-1", rankfile);
            fwrite_array(descArk->m, descArk->n, descArk->m, Ark_initial, rankfile);

            HICMA_stat_t hicma_statrk_initial;
            zget_stat(MorseLower, Ark_initial, MT, NT, MT,  &hicma_statrk_initial);
            printf("initial_ranks:");
            zprint_stat(hicma_statrk_initial);
            fflush(stderr);
            fflush(stdout);
        }
    }

    if (global_always_fixed_rank == 1) {
        fprintf(stderr, "%s %d Fixed rank: %d\n", __FILE__, __LINE__, global_fixed_rank);
    }

    if(0 && num_mpi_ranks == 1 && initial_maxrank > N){ //FIXME Enable for distributed mem
        fprintf(stderr,"%s %d %d\t|N:%d is less than actual maxrank:%d\n", __FILE__, __LINE__, MORSE_My_Mpi_Rank(), N, initial_maxrank);
        exit(1);
    }
    int set_diag = 0;

    /* Save A for check */
    PROGRESS("pasting original dense descAD into Adense and Adense2 started");
    // Adense: original dense problem.
    PASTE_TILE_TO_LAPACK( descDense, Adense, check, double, LDA, M );
    double one = 1.0, zero = 0.0, minusone = -1.0, diagVal = M;
    double* swork = NULL;
    //double* cp_L_Adense =  calloc(LDA*M, sizeof(double));
    if(check){
        swork  = calloc(2*M, sizeof(double));
        {size_t i, j;
            double* orgAdense = calloc(LDA*M, sizeof(double));
            for(j = 0; j < M; j++){
                for(i = 0; i < M; i++){
                    orgAdense[j*LDA+i] = Adense[j*LDA+i];
                }
            }
            int info = LAPACKE_dpotrf_work(
                    LAPACK_COL_MAJOR,
                    'L',
                    M, orgAdense, LDA);
            if(0 && info != 0){ //FIXME
                fprintf(stderr, "%s\t|%d\t|Error in LAPACK potrf. info:%d, This errors means "
                        "that the matrix generated is not positive definite\n", __FILE__, __LINE__, info);
            }
            for(j = 0; j < M; j++){
                for(i = 0; i < j; i++){
                    orgAdense[j*LDA+i] = zero;
                }
            }
            /*for(j = 0; j < M; j++) {                */
                /*for(i = 0; i < M; i++){*/
                    /*cp_L_Adense[j*LDA+i] = orgAdense[j*LDA+i];*/
                /*}*/
            /*}*/
            if(main_print_mat ){printf("L of Adense\n");printmat(orgAdense,M,M,LDA,MB, MB);}
            double normOrgAdense = 0.0;
            /*HICMA_znormest(M, M, orgAdense, &normOrgAdense, swork);*/
            /*printf("norm_L_OrgAdense:%e\n",normOrgAdense);*/
            free(orgAdense);
        }
    }
    PASTE_TILE_TO_LAPACK( descDense, Adense2, check, double, LDA, M );
    PROGRESS("pasting original dense descAD into Adense and Adense2 finished");
    PROGRESS("potrf started");
    START_TIMING();
    HICMA_zpotrf_Tile(MorseLower, descAUV, descAD, descArk, fixedrank, maxrank, fixedacc );
    STOP_TIMING();
    fflush(stderr);
    fflush(stdout);
    PROGRESS("potrf finished");
    if(check){
        HICMA_zuncompress(MorseLower, descAUV, descDense, descArk);
        HICMA_zdiag_vec2mat(descAD, descDense);
        PASTE_CODE_FREE_MATRIX( descAD ); //@KADIRLBL001
        descAD = descDense; // descAD was only diagonals.
        // After this line, descAD is dense matrix containing approximate L
        // So no need to adapt below code for descAD containg only diagonals.
    }
    if(calc_rank_stat == 1) {
        PASTE_TILE_TO_LAPACK( descArk, Ark_final, 1, double, MT, NT );
        if(MORSE_My_Mpi_Rank()==0){
            sprintf(rankfile, "%s-2", rankfile);
            fwrite_array(descArk->m, descArk->n, descArk->m, Ark_final, rankfile);
            HICMA_stat_t hicma_statrk_final;
            zget_stat(MorseLower, Ark_final, MT, NT, MT,  &hicma_statrk_final);
            printf("final_ranks:");
            zprint_stat(hicma_statrk_final);
            fflush(stderr);
            fflush(stdout);
        }
    }

    int check_dense = 0;
    int check_app = 1;
    if(check == 0){
        check_dense = check_app = 0;
    }
    if(check_app ) {
        PROGRESS("checking accuracy");
        if( MORSE_My_Mpi_Rank()==0){
#ifndef COMPLEX
            if(main_print_mat){printf("Adense2\n");printmat(Adense2,M,M,LDA,MB, MB);}
            double normA;
            {size_t i, j;
                for(j = 0; j < M; j++){
                    for(i = 0; i < j; i++){
                        Adense2[j*LDA+i] = zero;
                    }
                }
            }
            PROGRESS("normaA started");
            HICMA_znormest(M, M, Adense2, &normA, swork);
            // Ahicma: result of TLR potrf
            PASTE_TILE_TO_LAPACK( descAD, Ahicma, check, double, LDA, M );
            /*if(0){size_t i,j;*/
                /*for(j = 0; j < M; j++) {                */
                    /*for(i = 0; i < M; i++){*/
                        /*Ahicma[j*LDA+i] = cp_L_Adense[j*LDA+i];*/
                    /*}*/
                /*}*/
            /*}*/
            double normAhicma = 0.0;
            {size_t i, j;
                for(j = 0; j < M; j++){
                    for(i = 0; i < j; i++){
                        Ahicma[j*LDA+i] = zero;
                    }
                }
                double* orgAhicma = calloc(LDA*M, sizeof(double));
                for(j = 0; j < M; j++){
                    for(i = 0; i < M; i++){
                        orgAhicma[j*LDA+i] = Ahicma[j*LDA+i];
                    }
                }
                HICMA_znormest(M, M, orgAhicma, &normAhicma, swork);
                free(orgAhicma);
            }
            if(set_diag){size_t j; for(j = 0; j < M; j++){ Ahicma[j*LDA+j] = diagVal; } }
            if(main_print_mat){printf("Ahicma\n");printmat(Ahicma,M,M,LDA, MB, MB);}
            //LAPACKE_dpotrf(LAPACK_COL_MAJOR, 'L', M, Ahicma, LDA);
            // AhicmaT: transpose of Ahicma
            PROGRESS("copy descAd into AhicmaT started");
            PASTE_TILE_TO_LAPACK( descAD,  AhicmaT, check, double, LDA, M );

            {size_t i, j;
                for(j = 0; j < M; j++){
                    for(i = 0; i < j; i++){
                        Adense[j*LDA+i] = zero;
                    }
                }
            }

            if(main_print_mat){printf("Ahicma-upperzero\n");printmat(Ahicma,M,M,LDA, MB, MB);}
            PROGRESS("Transpose A started");
            LAPACKE_dge_trans(LAPACK_COL_MAJOR, M, M, Ahicma, LDA, AhicmaT, LDA);
            if(main_print_mat){printf("AhicmaT\n");printmat(AhicmaT,M,M,LDA, MB, MB);}
            PROGRESS("TRMM started");
            cblas_dtrmm (CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasNonUnit, M, M, one, Ahicma, LDA, AhicmaT, LDA);
            if(main_print_mat){printf("Ahicma*AhicmaT\n");printmat(AhicmaT,M,M,LDA, MB, MB);}
            //double tmpnorm;normest(M, M, AhicmaT, &tmpnorm, swork);printf("tmpnorm:%e\n",tmpnorm);
            {size_t i, j;
                for(j = 0; j < M; j++){
                    for(i = 0; i < j; i++){
                        AhicmaT[j*LDA+i] = zero;
                    }
                }
            }

            size_t nelm = M * M;
            if(main_print_mat)printf("nelm:%zu M:%d N:%d\n", nelm, M, N);
            PROGRESS("DAXPY started");
            cblas_daxpy(nelm, minusone, AhicmaT, 1, Adense, 1);
            if(main_print_mat){printf("Adense-(Ahicma*AhicmaT)\n");printmat(Adense,M,M,LDA, MB, MB);}

            double normDenseAppDiff;
            PROGRESS("Norm of difference started");
            HICMA_znormest(M, M, Adense, &normDenseAppDiff, swork);
            double accuracyDenseAppDiff = normDenseAppDiff/normA;
            //printf("normA:%.2e normDenseAppdiff:%.2e Accuracy: %.2e\n", normA, normDenseAppDiff,  accuracyDenseAppDiff);
            dparam[IPARAM_RES] = normDenseAppDiff;
            dparam[IPARAM_ANORM] = normA;
            dparam[IPARAM_XNORM] = normA;
            dparam[IPARAM_BNORM] = normAhicma;
#endif
        } else {
            PASTE_TILE_TO_LAPACK( descAD, Ahicma, check, double, LDA, M );
            PASTE_TILE_TO_LAPACK( descAD,  AhicmaT, check, double, LDA, M );
        }
        PROGRESS("checking accuracy is finished");
    }

    PASTE_CODE_FREE_MATRIX( descAUV );
    PROGRESS("descAUV is freed");
    if(check == 0) { // If there is no check, then descAD and descDense are different. Refer to @KADIRLBL001
        PASTE_CODE_FREE_MATRIX( descAD );
        PROGRESS("descAD is freed");
    }
    PASTE_CODE_FREE_MATRIX( descArk );
    PROGRESS("descArk is freed");
    PASTE_CODE_FREE_MATRIX( descDense );
    PROGRESS("descDense is freed");
    PROGRESS("freed descs");
    return 0;
}