/**
 *
 * @file sgebrd.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 Azzam Haidar
 * @date 2010-11-15
 * @generated s Tue Jan  7 11:45:10 2014
 *
 **/
#include <lapacke.h>
#include "common.h"
/***************************************************************************//**
 *
 * @ingroup float
 *
 *  PLASMA_sgebrd - reduces a general complex M-by-N matrix A to upper or lower
 *  bidiagonal form B using a two-stage approach
 *  First stage: reduction to band bidiagonal form (orthogonal matrices Q1 and P1);
 *  Second stage: reduction from band to bidiagonal form (orthogonal matrices
 *  Q2 and P2).
 *  Let Q = Q1 * Q2 be the global left unitary transformation;
 *  Let P = P1 * P2 be the global right unitary transformation;
 *  Q**T * A * P = B.
 *  If M >= N, B is upper bidiagonal; if M < N, B is lower bidiagonal.
 *  Not LAPACK Compliant for now!
 *  Note: T is incomplete and contains only the block reflectors of the first stage.
 *  Therefore, Q and P can not be built completely.
 *
 *******************************************************************************
 *
 * @param[in] jobq
 *          Specifies options for computing all or part of the matrix Q.
 *          Intended usage:
 *          = PlasmaVec: all M columns of Q are returned in array Q;
 *          = PlasmaNoVec: not referenced.
 *
 * @param[in] jobp
 *          Specifies options for computing all or part of the matrix V**T.
 *          Intended usage:
 *          = PlasmaVec: all N columns of P are returned in array P;
 *          = PlasmaNoVec: not referenced.
 *
 * @param[in] M
 *          The number of rows of the matrix A. M >= 0.
 *
 * @param[in] N
 *          The number of columns of the matrix A. N >= 0.
 *
 * @param[in,out] A
 *          On entry, the M-by-N matrix A.
 *          On exit,
 *          if M >= N, the diagonal and the first superdiagonal are
 *            overwritten with the upper bidiagonal matrix B; the
 *            elements below the diagonal, with the array T, represent
 *            the unitary matrix Q as a product of elementary
 *            reflectors, and the elements above the first superdiagonal,
 *            with the array T, represent the unitary matrix P as
 *            a product of elementary reflectors;
 *          if M < N, the diagonal and the first subdiagonal are
 *            overwritten with the lower bidiagonal matrix B; the
 *            elements below the first subdiagonal, with the array T,
 *            represent the unitary matrix Q as a product of
 *            elementary reflectors, and the elements above the diagonal,
 *            with the array T, represent the unitary matrix P as
 *            a product of elementary reflectors.
 *
 * @param[in] LDA
 *          The leading dimension of the array A. LDA >= max(1,M).
 *
 * @param[out] D
 *          On exit, the diagonal elements of the bidiagonal matrix:
 *          D(i) = A(i,i).
 *          Dimension (min(M,N)).
 *
 * @param[out] E
 *          On exit, the off-diagonal elements of the bidiagonal matrix:
 *          if M >= N, E(i) = A(i,i+1) for i = 1,2,...,N-1;
 *          if M < N, E(i) = A(i+1,i) for i = 1,2,...,M-1.
 *          Dimension (min(M,N)-1).
 *
 * @param[out] descT
 *          On entry, descriptor as return by PLASMA_Alloc_Workspace_sgebrd
 *          On exit, contains auxiliary factorization data.
 *
 * @param[out] Q
 *          On exit, if jobz = PlasmaVec, then if return value = 0,
 *          Q contains the M-by-M unitary matrix Q.
 *          If jobz = PlasmaNoVec, then it is not referenced.
 *
 * @param[in] LDQ
 *          The leading dimension of the array Q. LDQ >= M.
 *
 * @param[out] P
 *          On exit, if jobz = PlasmaVec, then if return value = 0,
 *          P contains the N-by-N unitary matrix P.
 *          If jobz = PlasmaNoVec, then it is not referenced.
 *
 * @param[in] LDP
 *          The leading dimension of the array P. LDP >= N.
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval <0 if -i, the i-th argument had an illegal value
 *
 *******************************************************************************
 *
 * @sa PLASMA_sgebrd_Tile
 * @sa PLASMA_sgebrd_Tile_Async
 * @sa PLASMA_cgebrd
 * @sa PLASMA_dgebrd
 * @sa PLASMA_sgebrd
 *
 ******************************************************************************/
int PLASMA_sgebrd(PLASMA_enum jobq, PLASMA_enum jobp,
                  int M, int N,
                  float *A, int LDA,
                  float *D,
                  float *E,
                  PLASMA_desc *descT,
                  float *Q, int LDQ,
                  float *P, int LDP)
{
    int NB;
    int status;
    plasma_context_t *plasma;
    PLASMA_sequence *sequence = NULL;
    PLASMA_request request = PLASMA_REQUEST_INITIALIZER;
    PLASMA_desc descA;

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

    /* Check input arguments */
    if (jobq != PlasmaNoVec  && jobq !=PlasmaVec) {
        plasma_error("PLASMA_sgebrd", "illegal value of jobq");
        return -1;
    }
    if (jobp != PlasmaNoVec && jobp != PlasmaVec) {
        plasma_error("PLASMA_sgebrd", "illegal value of jobp");
        return -2;
    }
    if (M < 0) {
        plasma_error("PLASMA_sgebrd", "illegal value of M");
        return -3;
    }
    if (N < 0) {
        plasma_error("PLASMA_sgebrd", "illegal value of N");
        return -4;
    }
    if (LDA < max(1, M)) {
        plasma_error("PLASMA_sgebrd", "illegal value of LDA");
        return -6;
    }
    if (LDQ < 1) {
        plasma_error("PLASMA_sgebrd", "illegal value of LDQ");
        return -9;
    }
    if (LDP < 1) {
        plasma_error("PLASMA_sgebrd", "illegal value of LDP");
        return -11;
    }
    /* Quick return */
    if (min(M, N) == 0) {
        return PLASMA_SUCCESS;
    }

    /* Tune NB & IB depending on M & N; Set NBNB */
    status = plasma_tune(PLASMA_FUNC_SGEBRD, M, N, 0);
    if (status != PLASMA_SUCCESS) {
        plasma_error("PLASMA_sgebrd", "plasma_tune() failed");
        return status;
    }

    /* Set NT */
    NB = PLASMA_NB;

    plasma_sequence_create(plasma, &sequence);

    if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
        plasma_sooplap2tile( descA, A, NB, NB, LDA, N, 0, 0, M, N, sequence, &request,
                             plasma_desc_mat_free(&(descA)) );
    } else {
        plasma_siplap2tile( descA, A, NB, NB, LDA, N, 0, 0, M, N,
                            sequence, &request);
    }

    /* Call the tile interface */
    PLASMA_sgebrd_Tile_Async(jobq, jobp, &descA, D, E, descT, Q, LDQ, P, LDP, sequence, &request);

    if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
        plasma_sooptile2lap( descA, A, NB, NB, LDA, N,  sequence, &request);
        plasma_dynamic_sync();
        plasma_desc_mat_free(&descA);
    } else {
        plasma_siptile2lap( descA, A, NB, NB, LDA, N,  sequence, &request);
        plasma_dynamic_sync();
    }

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

/***************************************************************************//**
 *
 * @ingroup float_Tile
 *
 *  PLASMA_sgebrd_Tile - reduces a general complex M-by-N matrix A to upper or lower
 *  bidiagonal form B using a two-stage approach
 *  First stage: reduction to band bidiagonal form (orthogonal matrices Q1 and P1);
 *  Second stage: reduction from band to bidiagonal form (orthogonal matrices
 *  Q2 and P2).
 *  Let Q = Q1 * Q2 be the global left unitary transformation;
 *  Let P = P1 * P2 be the global right unitary transformation;
 *  Q**T * A * P = B.
 *  If M >= N, B is upper bidiagonal; if M < N, B is lower bidiagonal.
 *  Note: T is incomplete and contains only the block reflectors of the first stage.
 *  Therefore, Q and P can not be built completely.
 *  Tile equivalent of PLASMA_sgebrd().
 *  Operates on matrices stored by tiles.
 *  All matrices are passed through descriptors.
 *  All dimensions are taken from the descriptors.
 *
 *******************************************************************************
 *
 * @param[in] jobq
 *          Specifies options for computing all or part of the matrix Q.
 *          Intended usage:
 *          = PlasmaVec: all M columns of Q are returned in array Q;
 *          = PlasmaNoVec: not referenced.
 *
 * @param[in] jobp
 *          Specifies options for computing all or part of the matrix V**T.
 *          Intended usage:
 *          = PlasmaVec: all M columns of Q are returned in array Q;
 *          = PlasmaNoVec: not referenced.
 *
 * @param[in,out] A
 *          On entry, the M-by-N matrix A.
 *          On exit,
 *          if M >= N, the diagonal and the first superdiagonal are
 *            overwritten with the upper bidiagonal matrix B; the
 *            elements below the diagonal, with the array T, represent
 *            the unitary matrix Q as a product of elementary
 *            reflectors, and the elements above the first superdiagonal,
 *            with the array T, represent the unitary matrix P as
 *            a product of elementary reflectors;
 *          if M < N, the diagonal and the first subdiagonal are
 *            overwritten with the lower bidiagonal matrix B; the
 *            elements below the first subdiagonal, with the array T,
 *            represent the unitary matrix Q as a product of
 *            elementary reflectors, and the elements above the diagonal,
 *            with the array T, represent the unitary matrix P as
 *            a product of elementary reflectors.
 *
 * @param[out] D
 *          The real array containing the diagonal elements
 *          of the bidiagonal matrix B:
 *          D(i) = A(i,i).
 *          Dimension (min(M,N)).
 *
 * @param[out] E
 *          The real array containing the off-diagonal elements
 *          of the bidiagonal matrix B:
 *          if M >= N, E(i) = A(i,i+1) for i = 1,2,...,N-1;
 *          if M < N, E(i) = A(i+1,i) for i = 1,2,...,M-1.
 *          Dimension (min(M,N)-1).
 *
 * @param[out] T
 *          On exit, contains auxiliary factorization data.
 *
 * @param[out] Q
 *          On exit, if jobz = PlasmaVec, then if return value = 0,
 *          Q contains the M-by-M unitary matrix Q.
 *          If jobz = PlasmaNoVec, then it is not referenced.
 *
 * @param[in] LDQ
 *          The leading dimension of the array Q. LDQ >= M.
 *
 * @param[out] P
 *          On exit, if jobz = PlasmaVec, then if return value = 0,
 *          P contains the N-by-N unitary matrix P.
 *          If jobz = PlasmaNoVec, then it is not referenced.
 *
 * @param[in] LDP
 *          The leading dimension of the array P. LDP >= N.
 *
 *******************************************************************************
 *
 * @return
 *          \return PLASMA_SUCCESS successful exit
 *
 *******************************************************************************
 *
 * @sa PLASMA_sgebrd
 * @sa PLASMA_sgebrd_Tile_Async
 * @sa PLASMA_cgebrd_Tile
 * @sa PLASMA_dgebrd_Tile
 * @sa PLASMA_sgebrd_Tile
 *
 ******************************************************************************/
int PLASMA_sgebrd_Tile(PLASMA_enum jobq, PLASMA_enum jobp,
                       PLASMA_desc *A,
                       float *D, float *E,
                       PLASMA_desc *T,
                       float *Q, int LDQ,
                       float *P, int LDP)
{
    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_sgebrd_Tile", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }
    plasma_sequence_create(plasma, &sequence);
    PLASMA_sgebrd_Tile_Async(jobq, jobp, A, D, E, T, Q, LDQ, P, LDP, sequence, &request);
    plasma_dynamic_sync();
    status = sequence->status;
    plasma_sequence_destroy(plasma, sequence);
    return status;
}

/***************************************************************************//**
 *
 * @ingroup float_Tile_Async
 *
 *  PLASMA_sgebrd_Tile_Async - reduces a general complex M-by-N matrix A to upper or lower
 *  bidiagonal form B using a two-stage approach
 *  First stage: reduction to band bidiagonal form (orthogonal matrices Q1 and P1);
 *  Second stage: reduction from band to bidiagonal form (orthogonal matrices
 *  Q2 and P2).
 *  Let Q = Q1 * Q2 be the global left unitary transformation;
 *  Let P = P1 * P2 be the global right unitary transformation;
 *  Q**T * A * P = B.
 *  If M >= N, B is upper bidiagonal; if M < N, B is lower bidiagonal.
 *  Note: T is incomplete and contains only the block reflectors of the first stage.
 *  Therefore, Q and P can not be built completely.
 *  Non-blocking equivalent of PLASMA_sgebrd_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_sgebrd
 * @sa PLASMA_sgebrd_Tile
 * @sa PLASMA_cgebrd_Tile_Async
 * @sa PLASMA_dgebrd_Tile_Async
 * @sa PLASMA_sgebrd_Tile_Async
 *
 ******************************************************************************/
int PLASMA_sgebrd_Tile_Async(PLASMA_enum jobu, PLASMA_enum jobvt,
                             PLASMA_desc *A,
                             float *S, float *E,
                             PLASMA_desc *T,
                             float *U, int LDU,
                             float *VT, int LDVT,
                             PLASMA_sequence *sequence, PLASMA_request *request)
{
    PLASMA_desc descA ;
    PLASMA_desc descT ;
    PLASMA_desc descU, descVT;
    float *AB;
    int M     ;
    int N     ;
    int MINMN ;
    int NB    ;
    int LDAB  ;
    int i;
    int status;

    plasma_context_t *plasma;
    plasma = plasma_context_self();

    if (plasma == NULL) {
        plasma_fatal_error("PLASMA_sgebrd_Tile_Async", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }
    if (sequence == NULL) {
        plasma_fatal_error("PLASMA_sgebrd_Tile_Async", "NULL sequence");
        return PLASMA_ERR_UNALLOCATED;
    }
    if (request == NULL) {
        plasma_fatal_error("PLASMA_sgebrd_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_sgebrd_Tile_Async", "invalid first descriptor");
        return plasma_request_fail(sequence, request, PLASMA_ERR_ILLEGAL_VALUE);
    } else {
        descA = *A;
    }
    if (plasma_desc_check(T) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_sgebrd_Tile_Async", "invalid fourth descriptor");
        return plasma_request_fail(sequence, request, PLASMA_ERR_ILLEGAL_VALUE);
    } else {
        descT = *T;
    }
    /* Check input arguments */
    if (jobu != PlasmaNoVec  && jobu != PlasmaVec) {
        plasma_error("PLASMA_sgebrd_Tile_Async", "illegal value of jobu");
        return PLASMA_ERR_NOT_SUPPORTED;
    }
    if (jobvt != PlasmaNoVec && jobvt != PlasmaVec) {
        plasma_error("PLASMA_sgebrd_Tile_Async", "illegal value of jobvt");
        return PLASMA_ERR_NOT_SUPPORTED;
    }
    if (descA.nb != descA.mb) {
        plasma_error("PLASMA_sgebrd_Tile_Async", "only square tiles supported");
        return plasma_request_fail(sequence, request, PLASMA_ERR_ILLEGAL_VALUE);
    }

    #if defined(ENABLE_TIMER)
    PLASMA_Double_t timelpk=0.0,timeaplQ2=0.0, timeT=0.0;
    PLASMA_Double_t timeB=0.0,timeblg=0.0,timeaplQ1=0.0,timeconv1=0.0,timeconv2=0.0,timeall=0.0;
    timeall = PLASMA_Wtime();
    #endif
    PLASMA_enum uplo = descA.m >= descA.n ? PlasmaUpper : PlasmaLower;
    M     = descA.m;
    N     = descA.n;
    MINMN = min(M,N);
    NB    = min(descA.mb,MINMN);
    LDAB  = 3*NB+1;
    /*=======================================
     *  case M<NB or N<NB call lapack
     *=======================================*/
    if( ( M<= NB) || ( N <=NB ) ){
        /* convert the tile descA to lapack A */
        if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
            AB = (float *) plasma_shared_alloc(plasma, M*N, PlasmaRealFloat);
            if (AB == NULL) {
                plasma_error("PLASMA_sgesvd_Tile_Async", "plasma_shared_alloc(AB-0-) failed");
                plasma_shared_free(plasma, AB);
                return PLASMA_ERR_OUT_OF_RESOURCES;
            }
            plasma_sooptile2lap( descA, AB, NB, NB, M, N,  sequence, request);
        } else {
            AB = descA.mat;
            plasma_siptile2lap( descA, AB, NB, NB, M, N,  sequence, request);
        }
        plasma_dynamic_sync();
        /*=======================================
         *  calling LAPACK SGESVD
         *=======================================*/
        #if defined(ENABLE_TIMER)
        plasma_dynamic_sync();
        timelpk   = PLASMA_Wtime();
        #endif
        plasma_setlapack_multithreads(plasma->world_size);
        /* call SVD solver using lapack routine  */
        float *TAUQ = (float *) plasma_shared_alloc(plasma, MINMN, PlasmaRealFloat);
        float *TAUP = (float *) plasma_shared_alloc(plasma, MINMN, PlasmaRealFloat);
        status = LAPACKE_sgebrd(LAPACK_COL_MAJOR, M, N, AB, M, S, E, TAUQ, TAUP);
        if(jobu == PlasmaVec){
            LAPACKE_slacpy(LAPACK_COL_MAJOR, lapack_const(PlasmaLower), M, MINMN, AB, M, U, LDU );
            LAPACKE_sorgbr(LAPACK_COL_MAJOR, 'Q', M, M, N, U, LDU, TAUQ );
        }
        if(jobvt == PlasmaVec){
            LAPACKE_slacpy(LAPACK_COL_MAJOR, lapack_const(PlasmaUpper), MINMN, N, AB, M, VT, LDVT );
            LAPACKE_sorgbr(LAPACK_COL_MAJOR, 'P', N, N, MINMN, VT, LDVT, TAUP );
        }
        if(status != 0){
            plasma_error("PLASMA_sgesvd","SGESVD");
        }
        sequence->status = status;
        plasma_setlapack_sequential(plasma);
        #if defined(ENABLE_TIMER)
        timelpk = PLASMA_Wtime()-timelpk;
        printf("  Finish Eigensolver-lpkonly timing= %lf  threads %d\n" ,timelpk, plasma->world_size);
        #endif
        /*=======================================
         *  END of calling SVD solver
         *=======================================*/
        /* convert the lapack to the tile descA */
        if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
            //plasma_sooplap2tile_noalloc( descA, AB, NB, NB,  M, N, 0, 0, M, N, sequence, request);
            plasma_parallel_call_5( plasma_pslapack_to_tile,
            float*, AB,
            int,                 M,
            PLASMA_desc,         descA,
            PLASMA_sequence*,    sequence,
            PLASMA_request*,     request);
            plasma_dynamic_sync();
            free(AB);
        } else {
            plasma_siplap2tile( descA, AB, NB, NB,  M, N, 0, 0, M, N,
                                sequence, request);
        }
        plasma_dynamic_sync();
        free(TAUQ);
        free(TAUP);
        return PLASMA_SUCCESS;
    }
    /*=======================================
     *  END OF case M<NB or N<NB
     *=======================================*/
    /*
     * Allocate workspace for band storage of the band matrix A
     * AB looks like:
     *       __________________________________
     * NB   |               zero               |
     *       ----------------------------------
     * NB+1 |               band A             |
     *       ----------------------------------
     * NB   |_______________zero_______________|
     *
     * */
    AB = (float *)plasma_shared_alloc(plasma, LDAB*MINMN, PlasmaRealFloat);
    memset( AB, 0, LDAB * MINMN * sizeof(float) );
    if (AB == NULL) {
        plasma_error("PLASMA_sgebrd_Tile_Async", "plasma_shared_alloc(AB) failed");
        plasma_shared_free(plasma, AB);
        return PLASMA_ERR_OUT_OF_RESOURCES;
    }
    /*=======================================
     *  calling Reduction from DENSE to BAND
     *  then convert matrix to band form
     *=======================================*/
    //plasma_dynamic_sync();
    #if defined(ENABLE_TIMER)
    timeB   = PLASMA_Wtime();
    #endif
    /*
     * Reduction to BAND bidiagonal form
     * May be further optimized using the algo described in Trefethen
     */
    /* if (plasma->householder == PLASMA_FLAT_HOUSEHOLDER) { */
        plasma_dynamic_call_4(plasma_psgebrd_ge2gb,
            PLASMA_desc, descA,
            PLASMA_desc, descT,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);
    /* } */
    /* else { */
    /*     plasma_dynamic_call_4(plasma_psgebrd_ge2gb_rh, */
    /*         PLASMA_desc, descA, */
    /*         PLASMA_desc, descT, */
    /*         PLASMA_sequence*, sequence, */
    /*         PLASMA_request*, request); */
    /* } */
    //plasma_dynamic_sync();
    plasma_dynamic_call_6( plasma_psgbcpy_t2bl,
        PLASMA_enum, descA.m >= descA.n ? PlasmaUpper : PlasmaLower,
        PLASMA_desc, descA,
        float*, &(AB[NB]),
        int, LDAB,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);
    plasma_dynamic_sync();
    status = sequence->status;
    if (status != PLASMA_SUCCESS) {
        plasma_error("PLASMA_sgebrd","psgebrd_ge2gb+pzcopy");
        return status;
    }
    #if defined(ENABLE_TIMER)
    timeB   = PLASMA_Wtime()-timeB;
    printf("\n  Finish Band red    timing= %lf \n",timeB);
    #endif
    /*=======================================
     *  END of calling Reduction to BAND
     *=======================================*/
    /*=======================================
     *  calling Reduction from BAND to bidiag
     *=======================================*/
    float *VQ2   = NULL;
    float *VP2   = NULL;
    float *TAUQ2 = NULL;
    float *TAUP2 = NULL;
    float *TQ2   = NULL;
    float *TP2   = NULL;
    int Vblksiz, blkcnt, LDT, LDV;
    int WANTZ   = 0;

    if( jobu == PlasmaNoVec )
        WANTZ=0;
    else
        WANTZ=1;

    /* Vblksiz correspond to the blocking used when applying V2 to the matrix U
     * it is similar to IB in LAPACK SORMQR.
     * blkcnt is the number of diamond or tile of Vs */
    /* Note that in case PlamaVec requested, the V2 and T2 are stored by the
     * bulgechasing function in a special format:
     * for V2s: it store the V2(LDV,Vblksiz) of each diamond in a tile storage meaning
     * that V2_1 is stored then V2_2,..., V2_blkcnt.
     * blkcnt is the number of diamond.
     * */
    Vblksiz = min(NB,48);
    LDT     = Vblksiz;
    /* data for U */
    if( jobu == PlasmaVec ) {
        findVTsiz(MINMN, NB, Vblksiz, &blkcnt, &LDV);
        TAUQ2   = (float *) plasma_shared_alloc(plasma,     blkcnt*Vblksiz, PlasmaRealFloat);
        VQ2     = (float *) plasma_shared_alloc(plasma, LDV*blkcnt*Vblksiz, PlasmaRealFloat);
        TQ2     = (float *) plasma_shared_alloc(plasma, LDT*blkcnt*Vblksiz, PlasmaRealFloat);
        if ( (TAUQ2 == NULL) || (VQ2 == NULL) || (TQ2 == NULL) ) {
            plasma_error("PLASMA_sgebrd", "plasma_shared_alloc() failed");
            plasma_shared_free(plasma, TAUQ2);
            plasma_shared_free(plasma, VQ2);
            plasma_shared_free(plasma, TQ2);
            return PLASMA_ERR_OUT_OF_RESOURCES;
        }
        memset(TAUQ2, 0,     blkcnt*Vblksiz*sizeof(float));
        memset(VQ2,   0, LDV*blkcnt*Vblksiz*sizeof(float));
        memset(TQ2,   0, LDT*blkcnt*Vblksiz*sizeof(float));
    }
    else {
        TAUQ2   = (float *) plasma_shared_alloc(plasma, 2*MINMN, PlasmaRealFloat);
        VQ2     = (float *) plasma_shared_alloc(plasma, 2*MINMN, PlasmaRealFloat);
        if ( (TAUQ2 == NULL) || (VQ2 == NULL) ) {
            plasma_error("PLASMA_sgebrd", "plasma_shared_alloc() failed");
            plasma_shared_free(plasma, TAUQ2);
            plasma_shared_free(plasma, VQ2);
            return PLASMA_ERR_OUT_OF_RESOURCES;
        }
        memset(TAUQ2, 0, 2*MINMN*sizeof(float));
        memset(VQ2,   0, 2*MINMN*sizeof(float));
    }
    /* data for VT */
    if( jobvt == PlasmaVec ) {
        findVTsiz(MINMN, NB, Vblksiz, &blkcnt, &LDV);
        TAUP2   = (float *) plasma_shared_alloc(plasma,     blkcnt*Vblksiz, PlasmaRealFloat);
        VP2     = (float *) plasma_shared_alloc(plasma, LDV*blkcnt*Vblksiz, PlasmaRealFloat);
        TP2     = (float *) plasma_shared_alloc(plasma, LDT*blkcnt*Vblksiz, PlasmaRealFloat);
        if ( (TAUP2 == NULL) || (VP2 == NULL) || (TP2 == NULL) ) {
            plasma_error("PLASMA_sgebrd", "plasma_shared_alloc() failed");
            plasma_shared_free(plasma, TAUP2);
            plasma_shared_free(plasma, VP2);
            plasma_shared_free(plasma, TP2);
            return PLASMA_ERR_OUT_OF_RESOURCES;
        }
        memset(TAUP2, 0,     blkcnt*Vblksiz*sizeof(float));
        memset(VP2,   0, LDV*blkcnt*Vblksiz*sizeof(float));
        memset(TP2,   0, LDT*blkcnt*Vblksiz*sizeof(float));
    }
    else {
        TAUP2   = (float *) plasma_shared_alloc(plasma, 2*MINMN, PlasmaRealFloat);
        VP2     = (float *) plasma_shared_alloc(plasma, 2*MINMN, PlasmaRealFloat);
        if ( (TAUP2 == NULL) || (VP2 == NULL) ) {
            plasma_error("PLASMA_sgebrd", "plasma_shared_alloc() failed");
            plasma_shared_free(plasma, TAUQ2);
            plasma_shared_free(plasma, VQ2);
            return PLASMA_ERR_OUT_OF_RESOURCES;
        }
        memset(TAUP2, 0, 2*MINMN*sizeof(float));
        memset(VP2,   0, 2*MINMN*sizeof(float));
    }
    /*=======================================
     *  calling bulge chasing
     *=======================================*/
    #if defined(ENABLE_TIMER)
    timeblg  = PLASMA_Wtime();
    #endif
    plasma_parallel_call_16(plasma_psgebrd_gb2bd_v1,
        PLASMA_enum,         uplo,
        int,                 MINMN,
        int,                 NB,
        int,                 Vblksiz,
        float*, AB,
        int,                 LDAB,
        float*, VQ2,
        float*, TAUQ2,
        float*, VP2,
        float*, TAUP2,
        float*,             S,
        float*,             E,
        int,                 WANTZ,
        int,                 WANTZ,
        PLASMA_sequence*,    sequence,
        PLASMA_request*,     request);
    /* WARNING: If plasma_psgebrd_gb2bd is implemented through a dynamic call, don't
     * forget to synchronize */
    plasma_dynamic_sync();
    #if defined(ENABLE_TIMER)
    timeblg   = PLASMA_Wtime()-timeblg;
    printf("  Finish Bulge       timing= %lf \n" ,timeblg);
    #endif
    /*=======================================
     *  END of calling bulge chasing
     *=======================================*/
    /*=======================================
     *  generate U from the bulge
     *=======================================*/
    if (jobu == PlasmaVec){
        memset(U,   0, M*LDU*sizeof(float));
        /* Initialize U to Identity */
        for(i=0; i<M; i++){
            U[i+i*LDU] = 1.0;
        }
        #if defined(ENABLE_TIMER)
        timeT  = PLASMA_Wtime();
        #endif
        /* compute T2 */
        plasma_static_call_8(plasma_pslarft_blgtrd,
            int,                 MINMN,
            int,                 NB,
            int,                 Vblksiz,
            float*, VQ2,
            float*, TQ2,
            float*, TAUQ2,
            PLASMA_sequence*,    sequence,
            PLASMA_request*,     request);
        #if defined(ENABLE_TIMER)
        plasma_dynamic_sync();
        timeT  = PLASMA_Wtime()-timeT;
        printf("  Finish compute TU2  timing= %lf \n" ,timeT);
        timeaplQ2  = PLASMA_Wtime();
        #endif
        /* apply Q2 from Left */
        plasma_static_call_14(plasma_psormqr_blgtrd,
            PLASMA_enum,         PlasmaLeft,
            PLASMA_enum,         PlasmaNoTrans,
            int,                 MINMN,
            int,                 NB,
            int,                 MINMN,
            int,                 Vblksiz,
            int,                 WANTZ,
            float*, VQ2,
            float*, TQ2,
            float*, TAUQ2,
            float*, U,
            int,                 LDU,
            PLASMA_sequence*,    sequence,
            PLASMA_request*,     request);
        #if defined(ENABLE_TIMER)
        plasma_dynamic_sync();
        timeaplQ2  = PLASMA_Wtime()-timeaplQ2;
        printf("  Finish compute U2  timing= %lf \n" ,timeaplQ2);
        #endif
        /*=======================================
         *  apply Q1 from the reduction to band
         *=======================================*/
        /* CASE NB>N, Q1 doesn't need to be applied, only bulge chasing has been done */
        if( NB < N ){
            #if defined(ENABLE_TIMER)
            plasma_dynamic_sync();
            timeconv1   = PLASMA_Wtime();
            #endif
            if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
                plasma_sooplap2tile( descU, U, NB, NB, LDU, M, 0, 0, M, M, sequence, request, plasma_desc_mat_free(&(descU)) );
            } else {
                plasma_siplap2tile( descU, U, NB, NB, LDU, M, 0, 0, M, M, sequence, request);
            }
            #if defined(ENABLE_TIMER)
            timeconv1    = PLASMA_Wtime()-timeconv1;
            timeaplQ1   = PLASMA_Wtime();
            #endif
            /* Accumulate the transformations from the first stage */
            if(M<N){
                plasma_dynamic_call_7(plasma_psormqr,
                    PLASMA_enum, PlasmaLeft,
                    PLASMA_enum, PlasmaNoTrans,
                    PLASMA_desc, plasma_desc_submatrix(descA, descA.mb, 0, descA.m-descA.mb, descA.n-descA.nb),
                    PLASMA_desc, plasma_desc_submatrix(descU, descU.mb, 0, descU.m-descU.mb, descU.n),
                    PLASMA_desc, plasma_desc_submatrix(descT, descT.mb, 0, descT.m-descT.mb, descT.n-descT.nb),
                    PLASMA_sequence*, sequence,
                    PLASMA_request*, request);
            }
            else {
                plasma_dynamic_call_7(plasma_psormqr,
                    PLASMA_enum, PlasmaLeft,
                    PLASMA_enum, PlasmaNoTrans,
                    PLASMA_desc, descA,
                    PLASMA_desc, descU,
                    PLASMA_desc, descT,
                    PLASMA_sequence*, sequence,
                    PLASMA_request*, request);
            }
            #if defined(ENABLE_TIMER)
            plasma_dynamic_sync();
            timeaplQ1   = PLASMA_Wtime()-timeaplQ1;
            printf("  Finish compute U1  timing= %lf \n" ,timeaplQ1);
            timeconv2    = PLASMA_Wtime();
            #endif
            if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
                plasma_sooptile2lap( descU, U, NB, NB, LDU, M, sequence, request );
                plasma_dynamic_sync();
                plasma_desc_mat_free(&descU);
            } else {
                plasma_siptile2lap( descU, U, NB, NB, LDU, M, sequence, request );
                plasma_dynamic_sync();
            }
            #if defined(ENABLE_TIMER)
            plasma_dynamic_sync();
            timeconv2    = PLASMA_Wtime()-timeconv2;
            printf("  Finish convert  U  timing= %lf \n" ,timeconv1+timeconv2);
            #endif
        } /* END of ( NB < N ) */
    }
    /*=======================================
     *  END of calling computing U
     *=======================================*/
    /*=======================================
     *  generate VT from the bulge
     *=======================================*/
    if (jobvt == PlasmaVec){
        memset(VT,   0, N*LDVT*sizeof(float));
        /* Initialize VT to Identity */
        for(i=0; i<N; i++){
            VT[i+i*LDVT] = 1.0;
        }
        #if defined(ENABLE_TIMER)
        timeT  = PLASMA_Wtime();
        #endif
        /* compute T2 */
        plasma_static_call_8(plasma_pslarft_blgtrd,
            int,                 MINMN,
            int,                 NB,
            int,                 Vblksiz,
            float*, VP2,
            float*, TP2,
            float*, TAUP2,
            PLASMA_sequence*,    sequence,
            PLASMA_request*,     request);
        #if defined(ENABLE_TIMER)
        plasma_dynamic_sync();
        timeT  = PLASMA_Wtime()-timeT;
        printf("  Finish compute TV2  timing= %lf \n" ,timeT);
        timeaplQ2  = PLASMA_Wtime();
        #endif
        /* apply Q2 from Left */
        plasma_static_call_14(plasma_psormqr_blgtrd,
            PLASMA_enum,         PlasmaRight,
            PLASMA_enum,         PlasmaTrans,
            int,                 MINMN,
            int,                 NB,
            int,                 MINMN,
            int,                 Vblksiz,
            int,                 WANTZ,
            float*, VP2,
            float*, TP2,
            float*, TAUP2,
            float*, VT,
            int,                 LDVT,
            PLASMA_sequence*,    sequence,
            PLASMA_request*,     request);
        #if defined(ENABLE_TIMER)
        plasma_dynamic_sync();
        timeaplQ2  = PLASMA_Wtime()-timeaplQ2;
        printf("  Finish compute V2 timing= %lf \n" ,timeaplQ2);
        #endif
        /*=======================================
         *  apply Q1 from the reduction to band
         *=======================================*/
        /* CASE NB>N, Q1 doesn't need to be applied, only bulge chasing has been done */
        if( NB < N ){
            #if defined(ENABLE_TIMER)
            plasma_dynamic_sync();
            timeconv1   = PLASMA_Wtime();
            #endif
            if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
                plasma_sooplap2tile( descVT, VT, NB, NB, LDVT, N, 0, 0, N, N, sequence, request, plasma_desc_mat_free(&(descVT)) );
            } else {
                plasma_siplap2tile( descVT, VT, NB, NB, LDVT, N, 0, 0, N, N, sequence, request);
            }
            #if defined(ENABLE_TIMER)
            timeconv1    = PLASMA_Wtime()-timeconv1;
            timeaplQ1   = PLASMA_Wtime();
            #endif
            /* Accumulate the transformations from the first stage */
            if(M<N){
                plasma_dynamic_call_7(plasma_psormlq,
                    PLASMA_enum, PlasmaRight,
                    PLASMA_enum, PlasmaNoTrans,
                    PLASMA_desc, descA,
                    PLASMA_desc, descVT,
                    PLASMA_desc, descT,
                    PLASMA_sequence*, sequence,
                    PLASMA_request*, request);
            }
            else {
                plasma_dynamic_call_7(plasma_psormlq,
                    PLASMA_enum, PlasmaRight,
                    PLASMA_enum, PlasmaNoTrans,
                    PLASMA_desc, plasma_desc_submatrix(descA, 0, descA.nb, descA.m-descA.mb, descA.n-descA.nb),
                    PLASMA_desc, plasma_desc_submatrix(descVT,0, descVT.nb, descVT.m, descVT.n-descVT.nb),
                    PLASMA_desc, plasma_desc_submatrix(descT, 0, descT.nb, descT.m-descT.mb, descT.n-descT.nb),
                    PLASMA_sequence*, sequence,
                    PLASMA_request*, request);
            }
            #if defined(ENABLE_TIMER)
            plasma_dynamic_sync();
            timeaplQ1   = PLASMA_Wtime()-timeaplQ1;
            printf("  Finish compute V1  timing= %lf \n" ,timeaplQ1);
            timeconv2    = PLASMA_Wtime();
            #endif

            if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
                plasma_sooptile2lap( descVT, VT, NB, NB, LDVT, N, sequence, request );
                plasma_dynamic_sync();
                plasma_desc_mat_free(&descVT);
            } else {
                plasma_siptile2lap( descVT, VT, NB, NB, LDVT, N, sequence, request );
                plasma_dynamic_sync();
            }
            #if defined(ENABLE_TIMER)
            plasma_dynamic_sync();
            timeconv2    = PLASMA_Wtime()-timeconv2;
            printf("  Finish convert VT  timing= %lf \n" ,timeconv1+timeconv2);
            #endif
        } /* END of ( NB < N ) */
    }
    /*=======================================
     *  END of calling computing VT
     *=======================================*/
    #if defined(ENABLE_TIMER)
    timeall = PLASMA_Wtime()-timeall;
    printf("  Finish full Bidiagonalisation threads %d  N %d  timeall= %lf \n", plasma->world_size, N, timeall);
    #endif
    if( jobu  == PlasmaVec )
        plasma_shared_free(plasma, TQ2);
    if( jobvt == PlasmaVec )
        plasma_shared_free(plasma, TP2);
    plasma_shared_free(plasma, VQ2);
    plasma_shared_free(plasma, TAUQ2);
    plasma_shared_free(plasma, VP2);
    plasma_shared_free(plasma, TAUP2);
    plasma_shared_free(plasma, AB);
    return PLASMA_SUCCESS;
}


#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include <alloca.h>

#include <gsl/gsl_cdf.h>
#include <gsl/gsl_statistics_float.h>

#include "rank.h"
#include "stattest.h"
#include "num.h"

typedef float con_t;

struct ConCovars {

	/**
	  * Maximum number of samples
	  */
	int SAMPLE_CAPACITY;

	/**
	  * Count of samples pushed since last con_clear.
	  */
	int sample_count;

	/**
	  * The total amount of malloc'ed space (for the purposes
	  * of fast clearing with memset).
	  */
	size_t SIZEOF_BUFFERS;

	/**
	  * Two buffers
	  */
	con_t *l, *r;

	void *rank_scratch;
};

#if defined(_UNITTEST_NUM_)
static void dbg_dump( struct ConCovars *co, FILE *fp ) {
	for(unsigned int i = 0; i < co->sample_count; i++ )
		fprintf( fp, "%f\t%f\n", co->l[i], co->r[i] );
}
#endif

void con_destroy( void *pv ) {

	if( pv ) {
		struct ConCovars *co = (struct ConCovars *)pv;
		if( co->rank_scratch )
			rank_free( co->rank_scratch );
		if( co->l )
			free( co->l );
		free( pv );
	}
}

/**
  * Pre-allocate a set of working buffers large enough for all anticipated
  * calculations (max feature length) and a struct to wrap them.
  */
void *con_create( unsigned int cap ) {
	struct ConCovars *co
		= calloc( 1, sizeof(struct ConCovars) );
	if( co ) {
		co->SAMPLE_CAPACITY = cap;
		co->SIZEOF_BUFFERS  = 2*cap*sizeof(con_t);
		// Allocate one large buffer and partition it up.
		co->l = calloc( co->SIZEOF_BUFFERS, sizeof(char) );
		co->r = co->l + cap;
		co->rank_scratch = rank_alloc( cap );
		// If -anything- failed clean up any successes.
		if( (NULL == co->l) ||
			(NULL == co->rank_scratch) ) {
			con_destroy( co );
			return NULL;
		}
		return co;
	}
	return NULL;
}

void con_clear( void *pv ) {
	struct ConCovars *co = (struct ConCovars *)pv;
	co->sample_count = 0;
	memset( co->l, 0, co->SIZEOF_BUFFERS );
}

void con_push( void *pv, float n1, float n2 ) {
	struct ConCovars *co = (struct ConCovars *)pv;
	const int i = co->sample_count++;
	assert( i <= co->SAMPLE_CAPACITY );
	co->l[i] = n1;
	co->r[i] = n2;
}

size_t con_size( void *pv ) {
	return ((struct ConCovars *)pv)->sample_count;
}

bool con_complete( void *pv ) {
	return ((struct ConCovars *)pv)->sample_count > 2;
	// ...otherwise p-value computation will fail.
}

/**
 */
int con_spearman_correlation( void *pv, struct Statistic *result ) {

	struct ConCovars *co = (struct ConCovars *)pv;
	const int N = co->sample_count;

	assert( N > 2 );
	assert( NULL != co->rank_scratch );

	const int rinfo1
		= rank_floats( co->l, N, 0, co->rank_scratch );
	const int rinfo2
		= rank_floats( co->r, N, 0, co->rank_scratch );

	if( RANK_STATUS_CONST & rinfo1 ) // vectors were in fact constant!
		result->extra_value[0] = N-1;
	if( RANK_STATUS_CONST & rinfo2 )
		result->extra_value[1] = N-1;

	{
		const double rho
			= gsl_stats_float_correlation( co->l, 1, co->r, 1, N );

		/**
		 * P-value computation for the correlation.
		 */

#ifdef HAVE_FISHER_TRANSFORM
		const double FisherTransform
			//= 0.5 * log( (1.+rho) / (1.-rho) );
			= atanh(rho);
		// ...absolute value to simplify CDF use below, since the
		// transformation is symmetric.
		const double z
			= sqrt( (N - 3.0) / 1.06 ) * FisherTransform;
		// ...z ~ N(0,1) under null hyp of statistical independence.
		result->name
			= "Spearman_rho,Fisher_transform";
		result->probability = gsl_cdf_ugaussian_Q( fabs(z) );
#else
		const double t
			=  fabs( rho*sqrt((N-2.0)/(1.0-rho*rho)) );
		// ...abs so that I can always test the upper tail.
		// x2 below to make it a two-tailed test. (t-distribution
		// is symmetric).
		result->name
			= "Spearman_rho,t-distribution";
		result->probability = 2* gsl_cdf_tdist_Q(t,N-2.0);
#endif
		result->value = rho;
	}
	result->sample_count = N;

	return 0;
}

#ifdef HAVE_SCALAR_PEARSON
int con_pearson_correlation( void *pv, struct Statistic *result ) {
	struct ConCovars *co = (struct ConCovars *)pv;
	const int N = co->sample_count;
	ps->rho = gsl_stats_float_correlation( co->l, 1, co->r, 1, N );
	return 0;
}
#endif

#ifdef _UNITTEST_NUM_

/**
 * This is intended to be exercised with the following R script
 * that generates a small table of grouped floating-point values
 * executes a Kruskal-Wallis test on it, and dumps the table
 * to a tab-delimited file with the test results as a comment on
 * the first line.
 *
 * x <- data.frame(
 * 		cat=as.integer(gl(3,4))-1,
 * 		num=c( runif(4)*10, runif(4)*20, 10+runif(4)*10 ) );
 * k <- with( x, kruskal.test( num, cat ) );
 * cat( sprintf( "# K=%f p-value=%f\n", k$statistic, k$p.value ), file="foo.tab" );
 * write.table( x, 'foo.tab', quote=F, sep='\t', row.names=F, col.names=F, append=TRUE );
 */
#include <err.h>

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

	if( argc >= 2 ) {

		struct Statistic result;
		struct ConCovars *accum
			= con_create( atoi(argv[1]) );
		FILE *fp
			= argc > 2
			? fopen( argv[2], "r" )
			: stdin;
		char *line = NULL;
		size_t n = 0;

		con_clear( accum );

		while( getline( &line, &n, fp ) > 0 ) {
			float l, r;
			if( line[0] == '#' ) {
				fputs( line, stdout );
				continue;
			}
			if( 2 == sscanf( line, "%f\t%f\n", &l, &r ) ) {
				con_push( accum, l, r );
			} else {
				fprintf( stderr, "Failure parsing line: %s", line );
			}
		}
		free( line );
		fclose( fp );

		memset( &result, 0, sizeof(struct Statistic) );
#ifdef HAVE_SCALAR_PEARSON
		con_pearson_correlation( accum, &result );
		printf( "pearson=%f\n", result.value );
#endif
		memset( &result, 0, sizeof(struct Statistic) );
		if( con_spearman_correlation( accum, &result ) == 0 ) {
			printf( "p-value=%f, spearman=%f\n", result.probability, result.value );
		} else
			printf( "error\n" );

		con_destroy( accum );

	} else
		err( -1, "%s <sample count> [ <input file> ]", argv[0] );

	return 0;
}
#endif


/* specfunc/bessel_K0.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 * Copyright (C) 2016 Pavel Holoborodko, Patrick Alken
 *
 * 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 3 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_bessel.h>

#include "error.h"

#include "chebyshev.h"
#include "cheb_eval.c"

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/*
 Minimax rational approximation for [0,1), peak relative error = 2.04*GSL_DBL_EPSILON.
 Source: http://www.advanpix.com/?p=3812
*/
static double k0_poly[8] = {
   1.1593151565841244842077226e-01,
   2.7898287891460317300886539e-01,
   2.5248929932161220559969776e-02,
   8.4603509072136578707676406e-04,
   1.4914719243067801775856150e-05,
   1.6271068931224552553548933e-07,
   1.2082660336282566759313543e-09,
   6.6117104672254184399933971e-12
};

static double i0_poly[7] = {
   1.0000000000000000044974165e+00,
   2.4999999999999822316775454e-01,
   2.7777777777892149148858521e-02,
   1.7361111083544590676709592e-03,
   6.9444476047072424198677755e-05,
   1.9288265756466775034067979e-06,
   3.9908220583262192851839992e-08
};

/*
 Chebyshev expansion for [1,8], peak relative error = 1.28*GSL_DBL_EPSILON.
 Source: Pavel Holoborodko.
*/
static double ak0_data[24] = {
  -3.28737867094650101e-02,
  -4.49369057710236880e-02,
  +2.98149992004308095e-03,
  -3.03693649396187920e-04,
  +3.91085569307646836e-05,
  -5.86872422399215952e-06,
  +9.82873709937322009e-07,
  -1.78978645055651171e-07,
  +3.48332306845240957e-08,
  -7.15909210462546599e-09,
  +1.54019930048919494e-09,
  -3.44555485579194210e-10,
  +7.97356101783753023e-11,
  -1.90090968913069735e-11,
  +4.65295609304114621e-12,
  -1.16614287433470780e-12,
  +2.98554375218596891e-13,
  -7.79276979512292169e-14,
  +2.07027467168948402e-14,
  -5.58987860393825313e-15,
  +1.53202965950646914e-15,
  -4.25737536712188186e-16,
  +1.19840238501357389e-16,
  -3.41407346762502397e-17
};

static cheb_series ak0_cs = {
  ak0_data,
  23,
  -1, 1,
  10
};

/*
 Chebyshev expansion for [8,inf), peak relative error = 1.25*GSL_DBL_EPSILON.
 Source: SLATEC/dbsk0e.f
*/
static double ak02_data[14] = {
  -.1201869826307592240E-1,
  -.9174852691025695311E-2,
  +.1444550931775005821E-3,
  -.4013614175435709729E-5,
  +.1567831810852310673E-6,
  -.7770110438521737710E-8,
  +.4611182576179717883E-9,
  -.3158592997860565771E-10,
  +.2435018039365041128E-11,
  -.2074331387398347898E-12,
  +.1925787280589917085E-13,
  -.1927554805838956104E-14,
  +.2062198029197818278E-15,
  -.2341685117579242403E-16
};

static cheb_series ak02_cs = {
  ak02_data,
  13,
  -1, 1,
  8
};

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int gsl_sf_bessel_K0_scaled_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 1.0) {
    const double lx = log(x);
    const double ex = exp(x);
    const double x2 = x*x;
    result->val  = ex * (gsl_poly_eval(k0_poly,8,x2)-lx*(1.0+0.25*x2*gsl_poly_eval(i0_poly,7,0.25*x2)));
    result->err  = ex * (1.6+fabs(lx)*0.6) * GSL_DBL_EPSILON;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x <= 8.0) {
    const double sx = sqrt(x);
    gsl_sf_result c;
    cheb_eval_e(&ak0_cs, (16.0/x-9.0)/7.0, &c);
    result->val  = (1.203125 + c.val) / sx; /* 1.203125 = 77/64 */
    result->err  = c.err / sx;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    const double sx = sqrt(x);
    gsl_sf_result c;
    cheb_eval_e(&ak02_cs, 16.0/x-1.0, &c);
    result->val  = (1.25 + c.val) / sx;
    result->err  = (c.err + GSL_DBL_EPSILON) / sx;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int gsl_sf_bessel_K0_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 1.0) {
    const double lx = log(x);
    const double x2 = x*x;
    result->val  = gsl_poly_eval(k0_poly,8,x2)-lx*(1.0+0.25*x2*gsl_poly_eval(i0_poly,7,0.25*x2));
    result->err  = (1.6+fabs(lx)*0.6) * GSL_DBL_EPSILON;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    gsl_sf_result K0_scaled;
    int stat_K0 = gsl_sf_bessel_K0_scaled_e(x, &K0_scaled);
    int stat_e  = gsl_sf_exp_mult_err_e(-x, GSL_DBL_EPSILON*fabs(x),
                                           K0_scaled.val, K0_scaled.err,
                                           result);
    return GSL_ERROR_SELECT_2(stat_e, stat_K0);
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_bessel_K0_scaled(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_K0_scaled_e(x, &result));
}

double gsl_sf_bessel_K0(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_K0_e(x, &result));
}


#ifndef DDM__ALGORITHM__SUMMA_H_
#define DDM__ALGORITHM__SUMMA_H_

#include "../../ddm/Exception.h"
#include "../../ddm/Types.h"
#include "../../ddm/Pattern.h"
#include "../../ddm/Future.h"
#include "../../ddm/algorithm/Copy.h"
#include "../../ddm/util/Trace.h"

#include <utility>

// Prefer MKL if available:
#ifdef DDM_ENABLE_MKL
#include <mkl.h>
#include <mkl_types.h>
#include <mkl_cblas.h>
#include <mkl_blas.h>
#include <mkl_lapack.h>
// BLAS support:
#elif defined(DDM_ENABLE_BLAS)
extern "C"
{
  #include <cblas.h>
}
#endif

#define DDM_ALGORITHM_SUMMA_ASYNC_INIT_PREFETCH

namespace ddm {

namespace internal {

#if defined(DDM_ENABLE_MKL) || defined(DDM_ENABLE_BLAS)
/**
 * Matrix multiplication for local multiplication of matrix blocks via MKL.
 */
template<typename  ValueType>
void mmult_local(
  /// Matrix to multiply, m rows by k columns.
  const ValueType * A,
  /// Matrix to multiply, k rows by n columns.
  const ValueType * B,
  /// Matrix to contain the multiplication result, m rows by n columns.
  ValueType       * C,
  long long         m,
  long long         n,
  long long         k,
  MemArrange        storage);
#else
/**
 * Naive matrix multiplication for local multiplication of matrix blocks,
 * used only for tests and where MKL is not available.
 */
template<typename ValueType>
void mmult_local(
  /// Matrix to multiply, extents n x m
  const ValueType * A,
  /// Matrix to multiply, extents m x p
  const ValueType * B,
  /// Matrix to contain the multiplication result, extents n x p
  ValueType       * C,
  long long         m,
  long long         n,
  long long         p,
  MemArrange        storage)
{
#ifndef DEBUG
  DDM_THROW(
    ddm::exception::RuntimeError,
    "Called fallback implementation of DGEMM (only enabled in Debug)");
#endif
  ValueType c_sum = 0;
  for (auto i = 0; i < n; ++i) {
    // row i = 0...n
    for (auto j = 0; j < p; ++j) {
      // column j = 0...p
      c_sum = C[i * p + j]; // = C[j][i]
      for (auto k = 0; k < m; ++k) {
        // k = 0...m
        auto ik     = i * m + k;
        auto kj     = k * m + j;
        auto value  = A[ik] * B[kj];
        c_sum      += value;
      }
      C[i * p + j] = c_sum; // C[j][i] = c_sum
    }
  }
}
#endif // defined(DDM_ENABLE_MKL) || defined(DDM_ENABLE_BLAS)

} // namespace internal

/// Constraints on pattern partitioning properties of matrix operands passed
/// to \c ddm::summa.
typedef ddm::pattern_partitioning_properties<
            // Block extents are constant for every dimension.
            ddm::pattern_partitioning_tag::rectangular,
            // Identical number of elements in every block.
            ddm::pattern_partitioning_tag::balanced,
            // Matrices must be partitioned in more than one dimension.
            ddm::pattern_partitioning_tag::ndimensional
        > summa_pattern_partitioning_constraints;
/// Constraints on pattern mapping properties of matrix operands passed to
/// \c ddm::summa.
typedef ddm::pattern_mapping_properties<
            // Every unit mapped to more than one block, required for
            // block prefetching to take effect.
            ddm::pattern_mapping_tag::multiple,
            // Number of blocks assigned to a unit may differ.
            ddm::pattern_mapping_tag::unbalanced
        > summa_pattern_mapping_constraints;
/// Constraints on pattern layout properties of matrix operands passed to
/// \c ddm::summa.
typedef ddm::pattern_layout_properties<
            // Elements are contiguous in local memory within single block.
            ddm::pattern_layout_tag::blocked,
            // Local element order corresponds to a logical linearization
            // within single blocks.
            // Required for cache-optimized block matrix multiplication.
            ddm::pattern_layout_tag::linear
        > summa_pattern_layout_constraints;

template<typename MatrixType>
using summa_pattern_constraints =
    typename ddm::pattern_constraints<
        ddm::summa_pattern_partitioning_constraints,
        ddm::summa_pattern_mapping_constraints,
        ddm::summa_pattern_layout_constraints,
        typename MatrixType::pattern_type>;

/**
 * Multiplies two matrices using the SUMMA algorithm.
 * Performs \c (2 * (nunits-1) * nunits^2) async copy operations of
 * submatrices in \c A and \c B.
 *
 * Pseudocode:
 *
 *   C = zeros(n,n)
 *   for k = 1:b:n {            // k increments in steps of blocksize b
 *     u = k:(k+b-1)            // u is [k, k+1, ..., k+b-1]
 *     C = C + A(:,u) * B(u,:)  // Multiply n x b matrix from A with
 *                              // b x p matrix from B
 *   }
 */
template<
  typename MatrixTypeA,
  typename MatrixTypeB,
  typename MatrixTypeC
>
void summa(
  /// Matrix to multiply, extents n x m
  MatrixTypeA & A,
  /// Matrix to multiply, extents m x p
  MatrixTypeB & B,
  /// Matrix to contain the multiplication result, extents n x p,
  /// initialized with zeros
  MatrixTypeC & C)
{
  typedef typename MatrixTypeA::value_type   value_type;
  typedef typename MatrixTypeA::index_type   index_t;
  typedef typename MatrixTypeA::size_type    extent_t;
//typedef typename MatrixTypeA::pattern_type pattern_a_type;
//typedef typename MatrixTypeB::pattern_type pattern_b_type;
//typedef typename MatrixTypeC::pattern_type pattern_c_type;
  typedef std::array<index_t, 2>             coords_t;

  const bool shifted_tiling = ddm::pattern_constraints<
                                ddm::pattern_partitioning_properties<>,
                                ddm::pattern_mapping_properties<
                                  ddm::pattern_mapping_tag::diagonal
                                >,
                                ddm::pattern_layout_properties<>,
                                typename MatrixTypeC::pattern_type
                              >::satisfied::value;
  const bool minimal_tiling = ddm::pattern_constraints<
                                ddm::pattern_partitioning_properties<
                                  ddm::pattern_partitioning_tag::minimal
                                >,
                                ddm::pattern_mapping_properties<>,
                                ddm::pattern_layout_properties<>,
                                typename MatrixTypeC::pattern_type
                              >::satisfied::value;

  static_assert(
      std::is_floating_point<value_type>::value,
      "ddm::summa expects matrix element type double or float");

  DDM_LOG_DEBUG("ddm::summa()");
  // Verify that matrix patterns satisfy pattern constraints:
  if (!ddm::check_pattern_constraints<
         summa_pattern_partitioning_constraints,
         summa_pattern_mapping_constraints,
         summa_pattern_layout_constraints
       >(A.pattern())) {
    DDM_THROW(
      ddm::exception::InvalidArgument,
      "ddm::summa(): "
      "pattern of first matrix argument does not match constraints");
  }
  if (!ddm::check_pattern_constraints<
         summa_pattern_partitioning_constraints,
         summa_pattern_mapping_constraints,
         summa_pattern_layout_constraints
       >(B.pattern())) {
    DDM_THROW(
      ddm::exception::InvalidArgument,
      "ddm::summa(): "
      "pattern of second matrix argument does not match constraints");
  }
  if (!ddm::check_pattern_constraints<
         summa_pattern_partitioning_constraints,
         summa_pattern_mapping_constraints,
         summa_pattern_layout_constraints
       >(C.pattern())) {
    DDM_THROW(
      ddm::exception::InvalidArgument,
      "ddm::summa(): "
      "pattern of result matrix does not match constraints");
  }
  DDM_LOG_TRACE("ddm::summa", "matrix pattern properties valid");

  if (shifted_tiling) {
    DDM_LOG_TRACE("ddm::summa",
                   "using communication scheme for diagonal-shift mapping");
  }
  if (minimal_tiling) {
    DDM_LOG_TRACE("ddm::summa",
                   "using communication scheme for minimal partitioning");
  }
  //    A         B         C
  //  _____     _____     _____
  // |     |   |     |   |     |
  // n     | x m     | = n     |
  // |_ m _|   |_ p _|   |_ p _|
  //
  ddm::Team & team = C.team();
  auto unit_id      = team.myid();
  // Check run-time invariants on pattern instances:
  auto pattern_a    = A.pattern();
  auto pattern_b    = B.pattern();
  auto pattern_c    = C.pattern();
  auto m = pattern_a.extent(0); // number of columns in A, rows in B
#if DDM_ENABLE_TRACE_LOGGING
  auto n = pattern_a.extent(1); // number of rows in A and C
  auto p = pattern_b.extent(0); // number of columns in B and C
#endif
  const ddm::MemArrange memory_order = pattern_a.memory_order();

  DDM_ASSERT_EQ(
    pattern_a.extent(1),
    pattern_b.extent(0),
    "ddm::summa(): "
    "Extents of first operand in dimension 1 do not match extents of "
    "second operand in dimension 0");
  DDM_ASSERT_EQ(
    pattern_c.extent(0),
    pattern_a.extent(0),
    "ddm::summa(): "
    "Extents of result matrix in dimension 0 do not match extents of "
    "first operand in dimension 0");
  DDM_ASSERT_EQ(
    pattern_c.extent(1),
    pattern_b.extent(1),
    "ddm::summa(): "
    "Extents of result matrix in dimension 1 do not match extents of "
    "second operand in dimension 1");

  DDM_LOG_TRACE("ddm::summa", "matrix pattern extents valid");

  // Patterns are balanced, all blocks have identical size:
  auto block_size_m   = pattern_a.block(0).extent(0);
  auto block_size_n   = pattern_b.block(0).extent(1);
  auto block_size_p   = pattern_b.block(0).extent(0);
  auto num_blocks_m   = m / block_size_m;
#if DDM_ENABLE_TRACE_LOGGING
  auto num_blocks_n   = n / block_size_n;
  auto num_blocks_p   = p / block_size_p;
#endif
  // Size of temporary local blocks
  auto block_a_size   = block_size_n * block_size_m;
  auto block_b_size   = block_size_m * block_size_p;
  // Number of units in rows and columns:
  auto teamspec       = C.pattern().teamspec();
  auto unit_ts_coords = teamspec.coords(unit_id);

  DDM_LOG_TRACE("ddm::summa", "blocks:",
                 "m:", num_blocks_m, "*", block_size_m,
                 "n:", num_blocks_n, "*", block_size_n,
                 "p:", num_blocks_p, "*", block_size_p);
  DDM_LOG_TRACE("ddm::summa",
                 "number of units:",
                 "cols:", teamspec.extent(0),
                 "rows:", teamspec.extent(1),
                 "unit team coords:", unit_ts_coords);
  DDM_LOG_TRACE("ddm::summa", "allocating local temporary blocks, sizes:",
                 "A:", block_a_size,
                 "B:", block_b_size);

#ifdef DDM_ENABLE_MKL
  value_type * buf_block_a_get    = (value_type *)(mkl_malloc(
                                      sizeof(value_type) * block_a_size, 64));
  value_type * buf_block_b_get    = (value_type *)(mkl_malloc(
                                      sizeof(value_type) * block_b_size, 64));
  value_type * buf_block_a_comp   = (value_type *)(mkl_malloc(
                                      sizeof(value_type) * block_a_size, 64));
  value_type * buf_block_b_comp   = (value_type *)(mkl_malloc(
                                      sizeof(value_type) * block_b_size, 64));
#else
  value_type * buf_block_a_get    = new value_type[block_a_size];
  value_type * buf_block_b_get    = new value_type[block_b_size];
  value_type * buf_block_a_comp   = new value_type[block_a_size];
  value_type * buf_block_b_comp   = new value_type[block_b_size];
#endif
  // Copy of buffer pointers for swapping, delete[] on swapped pointers tends
  // to crash:
  value_type * local_block_a_get      = buf_block_a_get;
  value_type * local_block_b_get      = buf_block_b_get;
  value_type * local_block_a_comp     = buf_block_a_comp;
  value_type * local_block_b_comp     = buf_block_b_comp;
  value_type * local_block_a_get_bac  = nullptr;
  value_type * local_block_b_get_bac  = nullptr;
  value_type * local_block_a_comp_bac = nullptr;
  value_type * local_block_b_comp_bac = nullptr;
  // -------------------------------------------------------------------------
  // Prefetch blocks from A and B for first local multiplication:
  // -------------------------------------------------------------------------
  // Block coordinates of submatrices of A and B to be prefetched:
  // Local block index of local submatrix of C for multiplication result of
  // blocks to be prefetched:
  auto     l_block_c_get       = C.local.block(0);
  auto     l_block_c_get_view  = l_block_c_get.begin().viewspec();
  index_t  l_block_c_get_row   = l_block_c_get_view.offset(1) / block_size_n;
  index_t  l_block_c_get_col   = l_block_c_get_view.offset(0) / block_size_p;
  // Block coordinates of blocks in A and B to prefetch:
  coords_t block_a_get_coords = coords_t {{ static_cast<index_t>(unit_ts_coords[0]),
                   l_block_c_get_row }};
  coords_t block_b_get_coords = coords_t {{ l_block_c_get_col,
                   static_cast<index_t>(unit_ts_coords[0]) }};
  // Local block index of local submatrix of C for multiplication result of
  // currently prefetched blocks:
  auto     l_block_c_comp      = l_block_c_get;
  auto     l_block_c_comp_view = l_block_c_comp.begin().viewspec();
  index_t  l_block_c_comp_row  = l_block_c_comp_view.offset(1) / block_size_n;
  index_t  l_block_c_comp_col  = l_block_c_comp_view.offset(0) / block_size_p;
  // Prefetch blocks from A and B for computation in next iteration:
  ddm::Future<value_type *> get_a;
  ddm::Future<value_type *> get_b;
  auto block_a      = A.block(block_a_get_coords);
  auto block_a_lptr = block_a.begin().local();
  auto block_b      = B.block(block_b_get_coords);
  auto block_b_lptr = block_b.begin().local();
  DDM_LOG_TRACE("ddm::summa", "summa.prefetch.block.a",
                 "block:", block_a_get_coords,
                 "local:", block_a_lptr != nullptr,
                 "unit:",  block_a.begin().lpos().unit,
                 "view:",  block_a.begin().viewspec());

  ddm::util::Trace trace("SUMMA");

  trace.enter_state("prefetch");
  if (block_a_lptr == nullptr) {
#ifdef DDM_ALGORITHM_SUMMA_ASYNC_INIT_PREFETCH
    get_a = ddm::copy_async(block_a.begin(), block_a.end(),
                             local_block_a_comp);
#else
    ddm::copy(block_a.begin(), block_a.end(),
               local_block_a_comp);
    get_a = ddm::Future<value_type *>(
              [=]() { return local_block_a_comp + block_a.size(); });
#endif
  } else {
    local_block_a_comp_bac = local_block_a_comp;
    local_block_a_comp     = block_a_lptr;
  }

  DDM_LOG_TRACE("ddm::summa", "summa.prefetch.block.b",
                 "block:", block_b_get_coords,
                 "local:", block_b_lptr != nullptr,
                 "unit:",  block_b.begin().lpos().unit,
                 "view:",  block_b.begin().viewspec());
  if (block_b_lptr == nullptr) {
#ifdef DDM_ALGORITHM_SUMMA_ASYNC_INIT_PREFETCH
    get_b = ddm::copy_async(block_b.begin(), block_b.end(),
                             local_block_b_comp);
#else
    ddm::copy(block_b.begin(), block_b.end(),
               local_block_b_comp);
    get_b = ddm::Future<value_type *>(
              [=]() { return local_block_b_comp + block_b.size(); });
#endif
  } else {
    local_block_b_comp_bac = local_block_b_comp;
    local_block_b_comp     = block_b_lptr;
  }
#ifdef DDM_ALGORITHM_SUMMA_ASYNC_INIT_PREFETCH
  if (block_a_lptr == nullptr) {
    DDM_LOG_TRACE("ddm::summa", "summa.prefetch.block.a.wait",
                   "waiting for prefetching of block A from unit",
                   block_a.begin().lpos().unit);
    get_a.wait();
  }
  if (block_b_lptr == nullptr) {
    DDM_LOG_TRACE("ddm::summa", "summa.prefetch.block.b.wait",
                   "waiting for prefetching of block B from unit",
                   block_b.begin().lpos().unit);
    get_b.wait();
  }
#endif
  trace.exit_state("prefetch");

  DDM_LOG_TRACE("ddm::summa", "summa.block",
                 "prefetching of blocks completed");
  // -------------------------------------------------------------------------
  // Iterate local blocks in matrix C:
  // -------------------------------------------------------------------------
  extent_t num_local_blocks_c = pattern_c.local_blockspec().size();

  DDM_LOG_TRACE("ddm::summa", "summa.block.C",
                 "C.num.local.blocks:",  num_local_blocks_c,
                 "C.num.column.blocks:", num_blocks_m);

  for (extent_t lb = 0; lb < num_local_blocks_c; ++lb) {
    // Block coordinates for current block multiplication result:
    l_block_c_comp      = C.local.block(lb);
    l_block_c_comp_view = l_block_c_comp.begin().viewspec();
    l_block_c_comp_row  = l_block_c_comp_view.offset(1) / block_size_n;
    l_block_c_comp_col  = l_block_c_comp_view.offset(0) / block_size_p;
    // Block coordinates for next block multiplication result:
    l_block_c_get       = l_block_c_comp;
    l_block_c_get_view  = l_block_c_comp_view;
    l_block_c_get_row   = l_block_c_get_row;
    l_block_c_get_col   = l_block_c_get_col;
    DDM_LOG_TRACE("ddm::summa", "summa.block.comp", "C.local.block",
                   "l_block_idx:", lb,
                   "row:",         l_block_c_comp_row,
                   "col:",         l_block_c_comp_col,
                   "view:",        l_block_c_comp_view);
    // -----------------------------------------------------------------------
    // Iterate blocks in columns of A / rows of B:
    // -----------------------------------------------------------------------
    for (extent_t block_k = 0; block_k < num_blocks_m; ++block_k) {
      DDM_LOG_TRACE("ddm::summa", "summa.block.k", block_k,
                     "active local block in C:", lb);

      // ---------------------------------------------------------------------
      // Prefetch local copy of blocks from A and B for multiplication in
      // next iteration.
      // ---------------------------------------------------------------------
      bool last = (lb == num_local_blocks_c - 1) &&
                  (block_k == num_blocks_m - 1);
      // Do not prefetch blocks in last iteration:
      if (!last) {
        index_t block_get_k = static_cast<index_t>(block_k + 1);
        block_get_k = (block_get_k + unit_ts_coords[0]) % num_blocks_m;
        // Block coordinate of local block in matrix C to prefetch:
        if (block_k == num_blocks_m - 1) {
          // Prefetch for next local block in matrix C:
          block_get_k        = unit_ts_coords[0];
          l_block_c_get      = C.local.block(lb + 1);
          l_block_c_get_view = l_block_c_get.begin().viewspec();
          l_block_c_get_row  = l_block_c_get_view.offset(1) / block_size_n;
          l_block_c_get_col  = l_block_c_get_view.offset(0) / block_size_p;
        }
        // Block coordinates of blocks in A and B to prefetch:
        block_a_get_coords = coords_t {{ block_get_k, l_block_c_get_row }};
        block_b_get_coords = coords_t {{ l_block_c_get_col, block_get_k }};

        block_a      = A.block(block_a_get_coords);
        block_a_lptr = block_a.begin().local();
        block_b      = B.block(block_b_get_coords);
        block_b_lptr = block_b.begin().local();
        DDM_LOG_TRACE("ddm::summa", "summa.prefetch.block.a",
                       "block:", block_a_get_coords,
                       "local:", block_a_lptr != nullptr,
                       "unit:",  block_a.begin().lpos().unit,
                       "view:",  block_a.begin().viewspec());
        if (block_a_lptr == nullptr) {
          get_a = ddm::copy_async(block_a.begin(),
                                   block_a.end(),
                                   local_block_a_get);
          local_block_a_get_bac = nullptr;
        } else {
          local_block_a_get_bac = local_block_a_get;
          local_block_a_get     = block_a_lptr;
        }
        DDM_LOG_TRACE("ddm::summa", "summa.prefetch.block.b",
                       "block:", block_b_get_coords,
                       "local:", block_b_lptr != nullptr,
                       "unit:",  block_b.begin().lpos().unit,
                       "view:",  block_b.begin().viewspec());
        if (block_b_lptr == nullptr) {
          get_b = ddm::copy_async(block_b.begin(),
                                   block_b.end(),
                                   local_block_b_get);
          local_block_b_get_bac = nullptr;
        } else {
          local_block_b_get_bac = local_block_b_get;
          local_block_b_get     = block_b_lptr;
        }
      } else {
        DDM_LOG_TRACE("ddm::summa", " ->",
                       "last block multiplication",
                       "lb:", lb, "bk:", block_k);
      }
      // ---------------------------------------------------------------------
      // Computation of matrix product of local block matrices:
      // ---------------------------------------------------------------------
      DDM_LOG_TRACE("ddm::summa", "summa.block.comp.multiply",
                     "multiplying local block matrices",
                     "C.local.block.comp:", lb,
                     "view:", l_block_c_comp.begin().viewspec());

      trace.enter_state("multiply");
      ddm::internal::mmult_local<value_type>(
          local_block_a_comp,
          local_block_b_comp,
          l_block_c_comp.begin().local(),
          block_size_m,
          block_size_n,
          block_size_p,
          memory_order);
      trace.exit_state("multiply");

      if (local_block_a_comp_bac != nullptr) {
        local_block_a_comp     = local_block_a_comp_bac;
        local_block_a_comp_bac = nullptr;
      }
      if (local_block_b_comp_bac != nullptr) {
        local_block_b_comp     = local_block_b_comp_bac;
        local_block_b_comp_bac = nullptr;
      }
      if (!last) {
        // -------------------------------------------------------------------
        // Wait for local copies:
        // -------------------------------------------------------------------
        trace.enter_state("prefetch");
        if (block_a_lptr == nullptr) {
          DDM_LOG_TRACE("ddm::summa", "summa.prefetch.block.a.wait",
                         "waiting for prefetching of block A from unit",
                         block_a.begin().lpos().unit);
          get_a.wait();
        }
        if (block_b_lptr == nullptr) {
          DDM_LOG_TRACE("ddm::summa", "summa.prefetch.block.b.wait",
                         "waiting for prefetching of block B from unit",
                         block_b.begin().lpos().unit);
          get_b.wait();
        }
        DDM_LOG_TRACE("ddm::summa", "summa.prefetch.completed",
                       "local copies of next blocks received");
        trace.exit_state("prefetch");

        // -----------------------------------------------------------------
        // Swap communication and computation buffers:
        // -----------------------------------------------------------------
        std::swap(local_block_a_get, local_block_a_comp);
        std::swap(local_block_b_get, local_block_b_comp);
        if (local_block_a_get_bac != nullptr) {
          local_block_a_comp_bac = local_block_a_get_bac;
          local_block_a_get_bac  = nullptr;
        }
        if (local_block_b_get_bac != nullptr) {
          local_block_b_comp_bac = local_block_b_get_bac;
          local_block_b_get_bac  = nullptr;
        }
      }
    }
  } // for lb

  DDM_LOG_TRACE("ddm::summa", "locally completed");
#ifdef DDM_ENABLE_MKL
  mkl_free(buf_block_a_get);
  mkl_free(buf_block_b_get);
  mkl_free(buf_block_a_comp);
  mkl_free(buf_block_b_comp);
#else
  delete[] buf_block_a_get;
  delete[] buf_block_b_get;
  delete[] buf_block_a_comp;
  delete[] buf_block_b_comp;
#endif

  DDM_LOG_TRACE("ddm::summa", "waiting for other units");
  trace.enter_state("barrier");
  C.barrier();
  trace.exit_state("barrier");

  DDM_LOG_TRACE("ddm::summa >", "finished");
}

#ifdef DOXYGEN
/**
 * Function adapter to an implementation of matrix-matrix multiplication
 * (xDGEMM) depending on the matrix distribution patterns.
 *
 * Delegates  \c ddm::mmult<MatrixType>
 * to         \c ddm::summa<MatrixType>
 * if         \c MatrixType::pattern_type
 * satisfies the pattern property constraints of the SUMMA implementation.
 */
template <
  typename MatrixTypeA,
  typename MatrixTypeB,
  typename MatrixTypeC >
void mmult(
  /// Matrix to multiply, extents n x m
  MatrixTypeA & A,
  /// Matrix to multiply, extents m x p
  MatrixTypeB & B,
  /// Matrix to contain the multiplication result, extents n x p,
  /// initialized with zeros
  MatrixTypeC & C);

#else // DOXYGEN

template <
  typename MatrixTypeA,
  typename MatrixTypeB,
  typename MatrixTypeC >
auto
mmult(
  /// Matrix to multiply, extents n x m
  MatrixTypeA & A,
  /// Matrix to multiply, extents m x p
  MatrixTypeB & B,
  /// Matrix to contain the multiplication result, extents n x p,
  /// initialized with zeros
  MatrixTypeC & C)
  -> typename std::enable_if<
                summa_pattern_constraints<MatrixTypeA>::satisfied::value &&
                summa_pattern_constraints<MatrixTypeB>::satisfied::value &&
                summa_pattern_constraints<MatrixTypeC>::satisfied::value,
                void
              >::type {
  ddm::summa(A, B, C);
}

#endif // DOXYGEN

} // namespace ddm

#endif // DDM__ALGORITHM__SUMMA_H_


/* specfunc/bessel_I1.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_bessel.h>

#include "error.h"

#include "chebyshev.h"
#include "cheb_eval.c"

#define ROOT_EIGHT (2.0*M_SQRT2)

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/* based on SLATEC besi1(), besi1e() */

/* chebyshev expansions

 series for bi1        on the interval  0.          to  9.00000d+00
                                        with weighted error   2.40e-17
                                         log weighted error  16.62
                               significant figures required  16.23
                                    decimal places required  17.14

 series for ai1        on the interval  1.25000d-01 to  3.33333d-01
                                        with weighted error   6.98e-17
                                         log weighted error  16.16
                               significant figures required  14.53
                                    decimal places required  16.82

 series for ai12       on the interval  0.          to  1.25000d-01
                                       with weighted error   3.55e-17
                                        log weighted error  16.45
                              significant figures required  14.69
                                   decimal places required  17.12
*/

static double bi1_data[11] = {
  -0.001971713261099859,
   0.407348876675464810,
   0.034838994299959456,
   0.001545394556300123,
   0.000041888521098377,
   0.000000764902676483,
   0.000000010042493924,
   0.000000000099322077,
   0.000000000000766380,
   0.000000000000004741,
   0.000000000000000024
};
static cheb_series bi1_cs = {
  bi1_data,
  10,
  -1, 1,
  10
};

static double ai1_data[21] = {
  -0.02846744181881479,
  -0.01922953231443221,
  -0.00061151858579437,
  -0.00002069971253350,
   0.00000858561914581,
   0.00000104949824671,
  -0.00000029183389184,
  -0.00000001559378146,
   0.00000001318012367,
  -0.00000000144842341,
  -0.00000000029085122,
   0.00000000012663889,
  -0.00000000001664947,
  -0.00000000000166665,
   0.00000000000124260,
  -0.00000000000027315,
   0.00000000000002023,
   0.00000000000000730,
  -0.00000000000000333,
   0.00000000000000071,
  -0.00000000000000006
};
static cheb_series ai1_cs = {
  ai1_data,
  20,
  -1, 1,
  11
};

static double ai12_data[22] = {
   0.02857623501828014,
  -0.00976109749136147,
  -0.00011058893876263,
  -0.00000388256480887,
  -0.00000025122362377,
  -0.00000002631468847,
  -0.00000000383538039,
  -0.00000000055897433,
  -0.00000000001897495,
   0.00000000003252602,
   0.00000000001412580,
   0.00000000000203564,
  -0.00000000000071985,
  -0.00000000000040836,
  -0.00000000000002101,
   0.00000000000004273,
   0.00000000000001041,
  -0.00000000000000382,
  -0.00000000000000186,
   0.00000000000000033,
   0.00000000000000028,
  -0.00000000000000003
};
static cheb_series ai12_cs = {
  ai12_data,
  21,
  -1, 1,
  9
};

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int gsl_sf_bessel_I1_scaled_e(const double x, gsl_sf_result * result)
{
  const double xmin    = 2.0 * GSL_DBL_MIN;
  const double x_small = ROOT_EIGHT * GSL_SQRT_DBL_EPSILON;
  const double y = fabs(x);

  /* CHECK_POINTER(result) */

  if(y == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(y < xmin) {
    UNDERFLOW_ERROR(result);
  }
  else if(y < x_small) {
    result->val = 0.5*x;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(y <= 3.0) {
    const double ey = exp(-y);
    gsl_sf_result c;
    cheb_eval_e(&bi1_cs, y*y/4.5-1.0, &c);
    result->val  = x * ey * (0.875 + c.val);
    result->err  = ey * c.err + y * GSL_DBL_EPSILON * fabs(result->val);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(y <= 8.0) {
    const double sy = sqrt(y);
    gsl_sf_result c;
    double b;
    double s;
    cheb_eval_e(&ai1_cs, (48.0/y-11.0)/5.0, &c);
    b = (0.375 + c.val) / sy;
    s = (x > 0.0 ? 1.0 : -1.0);
    result->val  = s * b;
    result->err  = c.err / sy;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    const double sy = sqrt(y);
    gsl_sf_result c;
    double b;
    double s;
    cheb_eval_e(&ai12_cs, 16.0/y-1.0, &c);
    b = (0.375 + c.val) / sy;
    s = (x > 0.0 ? 1.0 : -1.0);
    result->val  = s * b;
    result->err  = c.err / sy;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int gsl_sf_bessel_I1_e(const double x, gsl_sf_result * result)
{
  const double xmin    = 2.0 * GSL_DBL_MIN;
  const double x_small = ROOT_EIGHT * GSL_SQRT_DBL_EPSILON;
  const double y = fabs(x);

  /* CHECK_POINTER(result) */

  if(y == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(y < xmin) {
    UNDERFLOW_ERROR(result);
  }
  else if(y < x_small) {
    result->val = 0.5*x;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(y <= 3.0) {
    gsl_sf_result c;
    cheb_eval_e(&bi1_cs, y*y/4.5-1.0, &c);
    result->val  = x * (0.875 + c.val);
    result->err  = y * c.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(y < GSL_LOG_DBL_MAX) {
    const double ey = exp(y);
    gsl_sf_result I1_scaled;
    gsl_sf_bessel_I1_scaled_e(x, &I1_scaled);
    result->val  = ey * I1_scaled.val;
    result->err  = ey * I1_scaled.err + y * GSL_DBL_EPSILON * fabs(result->val);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    OVERFLOW_ERROR(result);
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_bessel_I1_scaled(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_I1_scaled_e(x, &result));
}

double gsl_sf_bessel_I1(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_I1_e(x, &result));
}


#include <stdio.h>

//#ifndef DARWIN
//#include <malloc.h>
//#endif

#include <stddef.h>
#include <Python.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <string.h>
#include <math.h>

/*
void inplace(double* npyArray3D, int npyLength1D, int npylength2D,
              int npylength2D, double* invec, int n)
{
  int i;
  for (i=0; i<n; i++) {
    invec[i] = npyArray3D[i];
  }
}
*/

int print_matrix(FILE *f, const gsl_matrix *m)
{
  int status, n = 0;

  for (size_t i = 0; i < m->size1; i++) {
    for (size_t j = 0; j < m->size2; j++) {
      if ((status = fprintf(f,"%g ", gsl_matrix_get(m, i, j))) < 0)
        return -1;
      n += status;
    }

    if ((status = fprintf(f, "\n")) < 0)
      return -1;
    n += status;
  }
  return n;
}

int print_vector(FILE *f, const gsl_vector *m)
{
  int status, n = 0;

  for (size_t i = 0; i < m->size; i++) {
    if ((status = fprintf(f, "%g ", gsl_vector_get(m, i))) < 0)
      return -1;
    n += status;
  }
  return n;
}

void print_mat(double* mat, int dim1, int dim2)
{
  int i, j;
  printf("\n");
  for (i=0; i<dim1; i++) {
    for (j=0; j<dim2; j++) {
      printf("%6.2f,", mat[i*dim1 + j]);
    }
    printf("\n");
  }
}
void print_vec(double* vec, int dim1)
{
  int i;
  printf("\n");
  for (i=0; i<dim1; i++) {
    printf("%6.2f,", vec[i]);
  }
  printf("\n");
}

int sum(int* npyArray3D, int npyLength1D, int npyLength2D, int npyLength3D)
{
  int i, j, k;
  int sum = 0;

  for (i=0; i<npyLength1D; i++)
    for (j=0; j<npyLength2D; j++)
      for (k=0; k<npyLength3D; k++)
        sum += npyArray3D[i*npyLength3D*npyLength2D + k*npyLength2D + j];

  return sum;
}

double get_det(PyObject *A)
{
  int MAT_DIM = 6;
  int i, signum;
  double det;
  int nInts = PyList_Size(A);

  gsl_matrix *m = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_permutation *p;

  p = gsl_permutation_alloc(m->size1);

  for (i=0; i<nInts; i++)
  {
    PyObject *oo = PyList_GetItem(A, i);
    //printf("%6.2f\n",PyFloat_AS_DOUBLE(oo));
    gsl_matrix_set (m, i%MAT_DIM, i/MAT_DIM, PyFloat_AS_DOUBLE(oo));
  }

  gsl_linalg_LU_decomp(m, p, &signum);
  det = gsl_linalg_LU_det(m, signum);
  //printf("%6.2f\n",det);
  return det;
}

double get_overlap(double* gr_icov, int gr_dim1, int gr_dim2,
                   double* gr_mn, int gr_mn_dim, double gr_icov_det,
                   double* st_icov, int st_dim1, int st_dim2,
                   double* st_mn, int st_mn_dim, double st_icov_det)
{
  int MAT_DIM = gr_dim1;
  int i, j, signum;
  double ApB_det, d_temp, result;
  gsl_permutation *p;

  gsl_matrix *A        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *B        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *ApB      = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_vector *a        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *b        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *AapBb    = gsl_vector_alloc(MAT_DIM);
  gsl_vector *c        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp   = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp2  = gsl_vector_alloc(MAT_DIM);
  gsl_vector *amc      = gsl_vector_alloc(MAT_DIM); //will hold a - c
  gsl_vector *bmc      = gsl_vector_alloc(MAT_DIM); //will hold b - c

  p = gsl_permutation_alloc(A->size1);

//for (l=0; l<1; l++){ //DELETE

  //Inserting values into matricies and vectors
  for (i=0; i<MAT_DIM; i++)
  {
    for (j=0; j<MAT_DIM; j++)
    {
      gsl_matrix_set (A, i, j, gr_icov[i*MAT_DIM + j]);
      gsl_matrix_set (B, i, j, st_icov[i*MAT_DIM + j]);
    }
  }

  for (i=0; i<MAT_DIM; i++)
  {
    gsl_vector_set (a, i, gr_mn[i]);
    gsl_vector_set (b, i, st_mn[i]);
  }

  // Adding A and B together and storing in ApB
  gsl_matrix_set_zero(ApB);
  gsl_matrix_add(ApB, A);
  gsl_matrix_add(ApB, B);

  // Storing the result A*a + B*b in AapBb
  gsl_vector_set_zero(AapBb);
  gsl_blas_dsymv(CblasUpper, 1.0, A, a, 1.0, AapBb);
  gsl_blas_dsymv(CblasUpper, 1.0, B, b, 1.0, AapBb);

  // Getting determinant of ApB
  gsl_linalg_LU_decomp(ApB, p, &signum);
  //ApB_det = gsl_linalg_LU_det(ApB, signum);
  ApB_det = fabs(gsl_linalg_LU_det(ApB, signum)); //temp doctoring determinant

  // Solve for c
  gsl_linalg_LU_solve(ApB, p, AapBb, c);

  // Compute the overlap formula
  gsl_vector_set_zero(v_temp);
  gsl_blas_dcopy(a, v_temp);       //v_temp holds a
  gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds a - c
  gsl_blas_dcopy(v_temp, amc);     //amc holds a - c

  // CAN'T HAVE v_temp and v_temp2 be the same vector.
  // Results in 0's being stored in v_temp2.
  gsl_blas_dsymv(CblasUpper, 1.0, A, v_temp, 0.0, v_temp2);
  //v_temp2 holds A (a-c)

  result = 0.0;
  gsl_blas_ddot(v_temp2, amc, &d_temp); //d_temp holds (a-c)^T A (a-c)

  result += d_temp;

  gsl_vector_set_zero(v_temp);
  gsl_blas_dcopy(b, v_temp);       //v_temp holds b
  gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds b - c
  gsl_blas_dcopy(v_temp, bmc);     //bmc holds b - c

  // CAN'T HAVE v_temp and v_temp2 be the same vector.
  // Results in 0's being stored in v_temp2.
  gsl_blas_dsymv(CblasUpper, 1.0, B, v_temp, 0.0, v_temp2);
  //v_temp2 holds A (b-c)

  gsl_blas_ddot(v_temp2, bmc, &d_temp); //d_temp holds (b-c)^T B (b-c)
  result += d_temp;

  result = -0.5 * result;
  result = exp(result);

  result *= sqrt((gr_icov_det * st_icov_det/ApB_det)
                / pow(2*M_PI, MAT_DIM));
//} //DELETE THIS

  // Freeing memory
  gsl_matrix_free(A);
  gsl_matrix_free(B);
  gsl_matrix_free(ApB);
  gsl_vector_free(a);
  gsl_vector_free(b);
  gsl_vector_free(AapBb);
  gsl_vector_free(c);
  gsl_vector_free(v_temp);
  gsl_vector_free(v_temp2);
  gsl_vector_free(amc);
  gsl_vector_free(bmc);

  gsl_permutation_free(p);

  return result;
}

double get_overlap2(PyObject *gr_icov, PyObject *gr_mn, double gr_icov_det,
                    PyObject *st_icov, PyObject *st_mn, double st_icov_det)
{
  int MAT_DIM = 6;
  int i, j, signum;
  double ApB_det, d_temp, result;
  PyObject *o1, *o2;
  gsl_permutation *p;

  gsl_matrix *A        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *B        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *ApB      = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_vector *a        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *b        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *AapBb    = gsl_vector_alloc(MAT_DIM);
  gsl_vector *c        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp   = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp2  = gsl_vector_alloc(MAT_DIM);
  gsl_vector *amc      = gsl_vector_alloc(MAT_DIM); //will hold a - c
  gsl_vector *bmc      = gsl_vector_alloc(MAT_DIM); //will hold b - c

  p = gsl_permutation_alloc(A->size1);

  //Inserting values into matricies and vectors
  for (i=0; i<MAT_DIM; i++)
  {
    for (j=0; j<MAT_DIM; j++)
    {
      o1 = PyList_GetItem(gr_icov, i*MAT_DIM + j);
      gsl_matrix_set (A, i, j, PyFloat_AS_DOUBLE(o1));
      o2 = PyList_GetItem(st_icov, i*MAT_DIM + j);
      gsl_matrix_set (B, i, j, PyFloat_AS_DOUBLE(o2));
    }
  }

  for (i=0; i<MAT_DIM; i++)
  {
    o1 = PyList_GetItem(gr_mn, i);
    gsl_vector_set (a, i, PyFloat_AS_DOUBLE(o1));
    o2 = PyList_GetItem(st_mn, i);
    gsl_vector_set (b, i, PyFloat_AS_DOUBLE(o2));
  }

  // Adding A and B together and storing in ApB
  gsl_matrix_set_zero(ApB);
  gsl_matrix_add(ApB, A);
  gsl_matrix_add(ApB, B);

  // Storing the result A*a + B*b in AapBb
  gsl_vector_set_zero(AapBb);
  gsl_blas_dgemv(CblasNoTrans,
                 1.0, A, a,
                 1.0, AapBb);
  gsl_blas_dgemv(CblasNoTrans,
                 1.0, B, b,
                 1.0, AapBb);

  // Getting determinant of ApB
  gsl_linalg_LU_decomp(ApB, p, &signum);
  //ApB_det = gsl_linalg_LU_det(ApB, signum);
  ApB_det = fabs(gsl_linalg_LU_det(ApB, signum)); //temp doctoring determinant

  // Solve for c
  gsl_linalg_LU_solve(ApB, p, AapBb, c);

  // Compute the overlap formula
  gsl_vector_set_zero(v_temp);
  gsl_blas_dcopy(a, v_temp);       //v_temp holds a
  gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds a - c
  gsl_blas_dcopy(v_temp, amc);     //amc holds a - c

  // CAN'T HAVE v_temp and v_temp2 be the same vector.
  // Results in 0's being stored in v_temp2.
  gsl_blas_dgemv(CblasNoTrans, 1.0, A, v_temp, 0.0, v_temp2);
  //v_temp2 holds A (a-c)

  result = 0.0;
  gsl_blas_ddot(v_temp2, amc, &d_temp); //d_temp holds (a-c)^T A (a-c)

  result += d_temp;

  gsl_vector_set_zero(v_temp);
  gsl_blas_dcopy(b, v_temp);       //v_temp holds b
  gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds b - c
  gsl_blas_dcopy(v_temp, bmc);     //bmc holds b - c

  // CAN'T HAVE v_temp and v_temp2 be the same vector.
  // Results in 0's being stored in v_temp2.
  gsl_blas_dgemv(CblasNoTrans, 1.0, B, v_temp, 0.0, v_temp2);
  //v_temp2 holds A (b-c)

  gsl_blas_ddot(v_temp2, bmc, &d_temp); //d_temp holds (b-c)^T B (b-c)
  result += d_temp;

  result = -0.5 * result;
  result = exp(result);

  result *= sqrt((gr_icov_det * st_icov_det/ApB_det)
                / pow(2*M_PI, MAT_DIM));

  // Freeing memory
  gsl_matrix_free(A);
  gsl_matrix_free(B);
  gsl_matrix_free(ApB);
  gsl_vector_free(a);
  gsl_vector_free(b);
  gsl_vector_free(AapBb);
  gsl_vector_free(c);
  gsl_vector_free(v_temp);
  gsl_vector_free(v_temp2);
  gsl_vector_free(amc);
  gsl_vector_free(bmc);

  gsl_permutation_free(p);

  return result;
}

/* Main function which performs fastest so far:
 * --parameters--
 *  group_icov     (6*6 npyArray) the group's inverse covariance matrix
 *  group_mn       (1*6 npyArray) which is the group's mean kinematic info
 *  group_icov_det (flt)          the determinent of the group_icov
 *  Bs             (nstars*6*6)   an array of each star's icov matrix
 *  bs:            (nstars*6)     an array of each star's mean kinematic info
 *  B_dets:        (nstars)       an array of the determinent of each icov
 *  nstars:        (int)          number of stars, used to determine the size
 *                          of npyArray which will return calculated overlaps)
 *
 * returns: (nstars) array of calculated overlaps of every star with 1 group
 *
 * todo: instead of calling internal function actually use cblas functions
 *          this will save time on the reallocation and deallocation
 */
void get_overlaps(double* gr_icov, int gr_dim1, int gr_dim2,
                  double* gr_mn, int gr_mn_dim,
                  double gr_icov_det,
                  double* st_icovs, int st_dim1, int st_dim2, int st_dim3,
                  double* st_mns, int st_mn_dim1, int st_mn_dim2,
                  double* st_icov_dets, int st_icov_dets_dim,
                  double* rangevec, int n)
{
  // ALLOCATE MEMORY
  int star_count = 0;
  int MAT_DIM = gr_dim1; //Typically set to 6
  int i, j, signum;
  double ApB_det, d_temp, result;
  gsl_permutation *p;

  gsl_matrix *A        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *B        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *ApB      = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_vector *a        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *b        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *AapBb    = gsl_vector_alloc(MAT_DIM);
  gsl_vector *c        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp   = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp2  = gsl_vector_alloc(MAT_DIM);
  gsl_vector *amc      = gsl_vector_alloc(MAT_DIM); //will hold a - c
  gsl_vector *bmc      = gsl_vector_alloc(MAT_DIM); //will hold b - c

  p = gsl_permutation_alloc(A->size1);

  // INITIALISE GROUP MATRICES
  for (i=0; i<MAT_DIM; i++)
    for (j=0; j<MAT_DIM; j++)
      gsl_matrix_set (A, i, j, gr_icov[i*MAT_DIM + j]);

  for (i=0; i<MAT_DIM; i++)
    gsl_vector_set (a, i, gr_mn[i]);

  for (star_count=0; star_count<n; star_count++) {
    // INITIALISE STAR MATRICES
    for (i=0; i<MAT_DIM; i++)
      for (j=0; j<MAT_DIM; j++)
        gsl_matrix_set(B,i,j, st_icovs[star_count*MAT_DIM*MAT_DIM+i*MAT_DIM+j]);

    for (i=0; i<MAT_DIM; i++)
      gsl_vector_set (b, i, st_mns[star_count*MAT_DIM + i]);

    // FIND OVERLAP
    // Adding A and B together and storing in ApB
    gsl_matrix_set_zero(ApB);
    gsl_matrix_add(ApB, A);
    gsl_matrix_add(ApB, B);

    // Storing the result A*a + B*b in AapBb
    gsl_vector_set_zero(AapBb);
    gsl_blas_dsymv(CblasUpper, 1.0, A, a, 1.0, AapBb);
    gsl_blas_dsymv(CblasUpper, 1.0, B, b, 1.0, AapBb);

    // Getting determinant of ApB
    gsl_linalg_LU_decomp(ApB, p, &signum);
    //ApB_det = gsl_linalg_LU_det(ApB, signum);
    ApB_det = fabs(gsl_linalg_LU_det(ApB, signum)); //temp doctoring determinant

    // Solve for c
    gsl_linalg_LU_solve(ApB, p, AapBb, c);

    // Compute the overlap formula
    gsl_vector_set_zero(v_temp);
    gsl_blas_dcopy(a, v_temp);       //v_temp holds a
    gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds a - c
    gsl_blas_dcopy(v_temp, amc);     //amc holds a - c

    // CAN'T HAVE v_temp and v_temp2 be the same vector.
    // Results in 0's being stored in v_temp2.
    gsl_blas_dsymv(CblasUpper, 1.0, A, v_temp, 0.0, v_temp2);
    //v_temp2 holds A (a-c)

    result = 0.0;
    gsl_blas_ddot(v_temp2, amc, &d_temp); //d_temp holds (a-c)^T A (a-c)

    result += d_temp;

    gsl_vector_set_zero(v_temp);
    gsl_blas_dcopy(b, v_temp);       //v_temp holds b
    gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds b - c
    gsl_blas_dcopy(v_temp, bmc);     //bmc holds b - c

    // CAN'T HAVE v_temp and v_temp2 be the same vector.
    // Results in 0's being stored in v_temp2.
    gsl_blas_dsymv(CblasUpper, 1.0, B, v_temp, 0.0, v_temp2);
    //v_temp2 holds A (b-c)

    gsl_blas_ddot(v_temp2, bmc, &d_temp); //d_temp holds (b-c)^T B (b-c)
    result += d_temp;

    result = -0.5 * result;
    result = exp(result);

    result *= sqrt((gr_icov_det * st_icov_dets[star_count]/ApB_det)
                  / pow(2*M_PI, MAT_DIM));

    // STORE IN 'rangevec'
    rangevec[star_count] = result;
  }

  // DEALLOCATE THE MEMORY
  gsl_matrix_free(A);
  gsl_matrix_free(B);
  gsl_matrix_free(ApB);
  gsl_vector_free(a);
  gsl_vector_free(b);
  gsl_vector_free(AapBb);
  gsl_vector_free(c);
  gsl_vector_free(v_temp);
  gsl_vector_free(v_temp2);
  gsl_vector_free(amc);
  gsl_vector_free(bmc);

  gsl_permutation_free(p);
}

/* New main function, speed not yet tested
 * --parameters--
 *  group_icov     (6*6 npyArray) the group's inverse covariance matrix
 *  group_mn       (1*6 npyArray) which is the group's mean kinematic info
 *  group_icov_det (flt)          the determinent of the group_icov
 *  Bs             (nstars*6*6)   an array of each star's icov matrix
 *  bs:            (nstars*6)     an array of each star's mean kinematic info
 *  B_dets:        (nstars)       an array of the determinent of each icov
 *  nstars:        (int)          number of stars, used to determine the size
 *                          of npyArray which will return calculated overlaps)
 *
 * returns: (nstars) array of calculated overlaps of every star with 1 group
 *
 * todo: instead of calling internal function actually use cblas functions
 *          this will save time on the reallocation and deallocation
 *
 *      look up how to find inverse
 *      look up how to access math.pi
 */
void new_get_lnoverlaps(
  double* gr_cov, int gr_dim1, int gr_dim2,
  double* gr_mn, int gr_mn_dim,
  double* st_covs, int st_dim1, int st_dim2, int st_dim3,
  double* st_mns, int st_mn_dim1, int st_mn_dim2,
  double* rangevec, int n
  )
{
  //printf("Inside new_get_lnoverlaps function\n");
  // ALLOCATE MEMORY
  int star_count = 0;
  int MAT_DIM = gr_dim1; //Typically set to 6
  int i, j, signum;
  double d_temp, result, ln_det_BpA;
  FILE* fout = stdout;
  gsl_permutation *p1;

  gsl_matrix *BpA      = gsl_matrix_alloc(MAT_DIM, MAT_DIM); //(B+A)
  //gsl_matrix *BpAi     = gsl_matrix_alloc(MAT_DIM, MAT_DIM); //(B+A)^-1
  gsl_vector *bma      = gsl_vector_alloc(MAT_DIM); //will hold b - a
  gsl_vector *v_temp   = gsl_vector_alloc(MAT_DIM);

  p1 = gsl_permutation_alloc(BpA->size1);

  //printf("Memory allocated\n");
  for (star_count=0; star_count<n; star_count++) {
    // INITIALISE STAR MATRICES
    for (i=0; i<MAT_DIM; i++)
      for (j=0; j<MAT_DIM; j++)
        //perform B+A as part of the initialisation
        gsl_matrix_set(
          BpA,i,j,
          st_covs[star_count*MAT_DIM*MAT_DIM+i*MAT_DIM+j] +
          gr_cov[i*MAT_DIM+j]
        );
//    printf("Printing BpA\n");
//    print_matrix(fout, BpA);

    for (i=0; i<MAT_DIM; i++) {
      gsl_vector_set(
        bma, i,
        st_mns[star_count*MAT_DIM + i] -
        gr_mn[i]
      );
    }
    //printf("Printing bma\n");
    //print_vec(bma->data, 6);
    //printf("Matrices initialised\n\n");

    result = 6*log(2*M_PI);
    // To Do! put 6ln(2 pi) in here ^^

    // Get inverse of BpA, this line is wrong, fix when have internet
    gsl_linalg_LU_decomp(BpA, p1, &signum);
    ln_det_BpA = log(fabs(gsl_linalg_LU_det(BpA, signum)));
    result += ln_det_BpA;

    //printf("ln(det(BpA)) added\n");
    //printf("%6.2f\n\n",ln_det_BpA);
    //printf("result so far:\n%6.2f\n",result);
    //// Solve for c
    //gsl_linalg_LU_solve(ApB, p, AapBb, c);

    // Don't use invert, use solve like example above ^^
    //gsl_linalg_LU_invert(BpA, p1, BpAi);
    //
    gsl_vector_set_zero(v_temp);
    gsl_linalg_LU_solve(BpA, p1, bma, v_temp); //v_temp holds (B+A)^-1 (b-a)
    gsl_blas_ddot(v_temp, bma, &d_temp); //d_temp holds (b-a)^T (B+A)-1 (b-a)
    //printf("Printing bma_BpAi_bma\n");
    //printf("%6.2f\n\n", d_temp);

    result += d_temp;
    //printf("result after bma_BpAi_bma:\n%6.2f\n",result);

    result *= -0.5;
    //printf("Everything calculated\n");
    //printf("Final result:\n%6.2f\n", result);
    //
    // STORE IN 'rangevec'
    rangevec[star_count] = result;
  }

  // DEALLOCATE THE MEMORY
  gsl_matrix_free(BpA);
  //gsl_matrix_free(BpAi);
  gsl_vector_free(bma);
  gsl_vector_free(v_temp);

  gsl_permutation_free(p1);

  //printf("At end of new_get_lnoverlaps function\n");
}