/**
 *
 * @file zgesdd.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
 * @precisions normal z -> s d c
 *
 **/
#include <lapacke.h>
#include "common.h"

#define COMPLEX
/***************************************************************************//**
 *
 * @ingroup PLASMA_Complex64_t
 *
 *  PLASMA_zgesdd - computes the singular value decomposition (SVD) of a complex
 *  M-by-N matrix A, optionally computing the left and/or right singular
 *  vectors, by using divide-and-conquer method. The SVD is written
 *
 *       A = U * SIGMA * conjugate-transpose(V)
 *
 *  where SIGMA is an M-by-N matrix which is zero except for its
 *  min(m,n) diagonal elements, U is an M-by-M unitary matrix, and
 *  V is an N-by-N unitary matrix.  The diagonal elements of SIGMA
 *  are the singular values of A; they are real and non-negative, and
 *  are returned in descending order.  The first min(m,n) columns of
 *  U and V are the left and right singular vectors of A.
 *
 *  Note that the routine returns VT = V**H, not V.
 *
 *  The divide and conquer algorithm makes very mild assumptions about
 *  floating point arithmetic. It will work on machines with a guard
 *  digit in add/subtract, or on those binary machines without guard
 *  digits which subtract like the Cray X-MP, Cray Y-MP, Cray C-90, or
 *  Cray-2. It could conceivably fail on hexadecimal or decimal machines
 *  without guard digits, but we know of none.
 *
 *  NOTE that the input parameter of this function differ from the
 *  LAPACK ZGESDD. We have jobu and jobvt instead of only jobz first
 *  to be consistent with ZGESVD and second to allow a computation
 *  of only U or VT.
 *******************************************************************************
 *
 * @param[in] jobu
 *          Specifies options for computing all or part of the matrix U.
 *          Intended usage:
 *          = PlasmaVec   = 'A'(lapack):  all M columns of U are returned
 *                        in array U;
 *          = PlasmaNoVec = 'N':  no columns of U (no left singular vectors)
 *                        are computed.
 *          = PlasmaSVec  = 'S': the first min(m,n) columns of U (the left
 *                        singular vectors) are returned in the array U;
 *                        NOT SUPPORTTED YET
 *          = PlasmaOVec  = 'O': the first min(m,n) columns of U (the left
 *                        singular vectors) are overwritten on the array A;
 *                        NOT SUPPORTTED YET
 *
 * @param[in] jobvt
 *          Specifies options for computing all or part of the matrix V**H.
 *          Intended usage:
 *          = PlasmaVec   = 'A'(lapack): all N rows of V**H are returned
 *                        in the array VT;
 *          = PlasmaNoVec = 'N': no rows of V**H (no right singular vectors)
 *                        are computed.
 *          = PlasmaSVec  = 'S': the first min(m,n) rows of V**H (the right
 *                        singular vectors) are returned in the array VT;
 *                        NOT SUPPORTTED YET
 *          = PlasmaOVec  = 'O': the first min(m,n) rows of V**H (the right
 *                        singular vectors) are overwritten on the array A;
 *                        NOT SUPPORTTED YET
 *
 *          Note: jobu and jobvt cannot both be PlasmaOVec.
 *
 * @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 JOBU = 'O',  A is overwritten with the first min(m,n)
 *                          columns of U (the left singular vectors,
 *                          stored columnwise);
 *          if JOBVT = 'O', A is overwritten with the first min(m,n)
 *                          rows of V**H (the right singular vectors,
 *                          stored rowwise);
 *          if JOBU .ne. 'O' and JOBVT .ne. 'O', the contents of A
 *                          are destroyed.
 *
 * @param[in] LDA
 *          The leading dimension of the array A. LDA >= max(1,M).
 *
 * @param[out] S
 *          The double precision singular values of A, sorted so that S(i) >= S(i+1).
 *
 * @param[in, out] descT
 *          On entry, descriptor as return by PLASMA_Alloc_Workspace_zgesdd
 *          On exit, contains auxiliary factorization data.
 *
 * @param[out] U
 *          (LDU,M) if JOBU = 'A' or (LDU,min(M,N)) if JOBU = 'S'.
 *          If JOBU = 'A', U contains the M-by-M unitary matrix U;
 *          if JOBU = 'S', U contains the first min(m,n) columns of U
 *          (the left singular vectors, stored columnwise);
 *          if JOBU = 'N' or 'O', U is not referenced.
 *
 * @param[in] LDU
 *          The leading dimension of the array U.  LDU >= 1; if
 *          JOBU = 'S' or 'A', LDU >= M.
 *
 * @param[out] VT
 *         If JOBVT = 'A', VT contains the N-by-N unitary matrix
 *         V**H;
 *         if JOBVT = 'S', VT contains the first min(m,n) rows of
 *         V**H (the right singular vectors, stored rowwise);
 *         if JOBVT = 'N' or 'O', VT is not referenced.
 *
 * @param[in] LDVT
 *         The leading dimension of the array VT.  LDVT >= 1; if
 *         JOBVT = 'A', LDVT >= N; if JOBVT = 'S', LDVT >= min(M,N).
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval <0 if -i, the i-th argument had an illegal value
 *
 *******************************************************************************
 *
 * @sa PLASMA_zgesdd_Tile
 * @sa PLASMA_zgesdd_Tile_Async
 * @sa PLASMA_cgesdd
 * @sa PLASMA_dgesdd
 * @sa PLASMA_sgesdd
 *
 ******************************************************************************/
int PLASMA_zgesdd(PLASMA_enum jobu, PLASMA_enum jobvt, int M, int N,
                  PLASMA_Complex64_t *A, int LDA,
                  double *S,
                  PLASMA_desc *descT,
                  PLASMA_Complex64_t *U, int LDU,
                  PLASMA_Complex64_t *VT, int LDVT)
{
    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_zgesdd", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }

    /* Check input arguments */
    if (jobu != PlasmaNoVec  && jobu !=PlasmaVec) {
        plasma_error("PLASMA_zgesdd", "illegal value of jobu");
        return -1;
    }
    if (jobvt != PlasmaNoVec && jobvt != PlasmaVec) {
        plasma_error("PLASMA_zgesdd", "illegal value of jobvt");
        return -2;
    }
    if (jobvt != jobu) {
        plasma_error("PLASMA_zgesdd", "in this version: jobu should be equal jobvt");
        return -22;
    }
    if (M < 0) {
        plasma_error("PLASMA_zgesdd", "illegal value of M");
        return -3;
    }
    if (N < 0) {
        plasma_error("PLASMA_zgesdd", "illegal value of N");
        return -4;
    }
    if (LDA < max(1, M)) {
        plasma_error("PLASMA_zgesdd", "illegal value of LDA");
        return -6;
    }
    if (LDU < 1) {
        plasma_error("PLASMA_zgesdd", "illegal value of LDU");
        return -9;
    }
    if (LDVT < 1) {
        plasma_error("PLASMA_zgesdd", "illegal value of LDVT");
        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_ZGESVD, M, N, 0);
    if (status != PLASMA_SUCCESS) {
        plasma_error("PLASMA_zgesdd", "plasma_tune() failed");
        return status;
    }

    /* Set MT, NT */
    NB = PLASMA_NB;

    plasma_sequence_create(plasma, &sequence);

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

    /* Call the tile interface */
    PLASMA_zgesdd_Tile_Async(jobu, jobvt, &descA, S, descT, U, LDU, VT, LDVT, sequence, &request);

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

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

/***************************************************************************//**
 *
 * @ingroup PLASMA_Complex64_t_Tile
 *
 *  PLASMA_zgesdd_Tile - computes the singular value decomposition (SVD) of a complex
 *  M-by-N matrix A, optionally computing the left and/or right singular
 *  vectors.
 *  Tile equivalent of PLASMA_zgesdd().
 *  Operates on matrices stored by tiles.
 *  All matrices are passed through descriptors.
 *  All dimensions are taken from the descriptors.
 *
 *******************************************************************************
 *
 * @param[in] jobu
 *          Specifies options for computing all or part of the matrix U.
 *          Intended usage:
 *          = PlasmaVec   = 'A'(lapack):  all M columns of U are returned
 *                        in array U;
 *          = PlasmaNoVec = 'N':  no columns of U (no left singular vectors)
 *                        are computed.
 *          = PlasmaSVec  = 'S': the first min(m,n) columns of U (the left
 *                        singular vectors) are returned in the array U;
 *                        NOT SUPPORTTED YET
 *          = PlasmaOVec  = 'O': the first min(m,n) columns of U (the left
 *                        singular vectors) are overwritten on the array A;
 *                        NOT SUPPORTTED YET
 *
 * @param[in] jobvt
 *          Specifies options for computing all or part of the matrix V**H.
 *          Intended usage:
 *          = PlasmaVec   = 'A'(lapack): all N rows of V**H are returned
 *                        in the array VT;
 *          = PlasmaNoVec = 'N': no rows of V**H (no right singular vectors)
 *                        are computed.
 *          = PlasmaSVec  = 'S': the first min(m,n) rows of V**H (the right
 *                        singular vectors) are returned in the array VT;
 *                        NOT SUPPORTTED YET
 *          = PlasmaOVec  = 'O': the first min(m,n) rows of V**H (the right
 *                        singular vectors) are overwritten on the array A;
 *                        NOT SUPPORTTED YET
 *
 *          Note: jobu and jobvt cannot both be PlasmaOVec.
 *
 * @param[in,out] A
 *          On entry, the M-by-N matrix A.
 *          On exit,
 *          if JOBU = 'O',  A is overwritten with the first min(m,n)
 *                          columns of U (the left singular vectors,
 *                          stored columnwise);
 *          if JOBVT = 'O', A is overwritten with the first min(m,n)
 *                          rows of V**H (the right singular vectors,
 *                          stored rowwise);
 *          if JOBU .ne. 'O' and JOBVT .ne. 'O', the contents of A
 *                          are destroyed.
 *
 * @param[out] S
 *          The singular values of A, sorted so that S(i) >= S(i+1).
 *
 * @param[in, out] T
 *          On entry, descriptor as return by PLASMA_Alloc_Workspace_zgesdd
 *          On exit, contains auxiliary factorization data.
 *
 * @param[out] U
 *          (LDU,M) if JOBU = 'A' or (LDU,min(M,N)) if JOBU = 'S'.
 *          If JOBU = 'A', U contains the M-by-M unitary matrix U;
 *          if JOBU = 'S', U contains the first min(m,n) columns of U
 *          (the left singular vectors, stored columnwise);
 *          if JOBU = 'N' or 'O', U is not referenced.
 *
 * @param[in] LDU
 *          The leading dimension of the array U.  LDU >= 1; if
 *          JOBU = 'S' or 'A', LDU >= M.
 *
 * @param[out] VT
 *         If JOBVT = 'A', VT contains the N-by-N unitary matrix
 *         V**H;
 *         if JOBVT = 'S', VT contains the first min(m,n) rows of
 *         V**H (the right singular vectors, stored rowwise);
 *         if JOBVT = 'N' or 'O', VT is not referenced.
 *
 * @param[in] LDVT
 *         The leading dimension of the array VT.  LDVT >= 1; if
 *         JOBVT = 'A', LDVT >= N; if JOBVT = 'S', LDVT >= min(M,N).
 *
 *******************************************************************************
 *
 * @return
 *          \return PLASMA_SUCCESS successful exit
 *
 *******************************************************************************
 *
 * @sa PLASMA_zgesdd
 * @sa PLASMA_zgesdd_Tile_Async
 * @sa PLASMA_cgesdd_Tile
 * @sa PLASMA_dgesdd_Tile
 * @sa PLASMA_sgesdd_Tile
 *
 ******************************************************************************/
int PLASMA_zgesdd_Tile(PLASMA_enum jobu, PLASMA_enum jobvt,
                       PLASMA_desc *A,
                       double *S,
                       PLASMA_desc *T,
                       PLASMA_Complex64_t *U, int LDU,
                       PLASMA_Complex64_t *VT, int LDVT)
{
    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_zgesdd_Tile", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }
    plasma_sequence_create(plasma, &sequence);
    PLASMA_zgesdd_Tile_Async(jobu, jobvt, A, S, T, U, LDU, VT, LDVT, sequence, &request);
    plasma_dynamic_sync();
    status = sequence->status;
    plasma_sequence_destroy(plasma, sequence);
    return status;
}

/***************************************************************************//**
 *
 * @ingroup PLASMA_Complex64_t_Tile_Async
 *
 *  PLASMA_zgesdd_Tile_Async - computes the singular value decomposition (SVD) of a complex
 *  M-by-N matrix A, optionally computing the left and/or right singular
 *  vectors.
 *  Non-blocking equivalent of PLASMA_zgesdd_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_zgesdd
 * @sa PLASMA_zgesdd_Tile
 * @sa PLASMA_cgesdd_Tile_Async
 * @sa PLASMA_dgesdd_Tile_Async
 * @sa PLASMA_sgesdd_Tile_Async
 *
 ******************************************************************************/
int PLASMA_zgesdd_Tile_Async(PLASMA_enum jobu, PLASMA_enum jobvt,
                             PLASMA_desc *A,
                             double *S,
                             PLASMA_desc *T,
                             PLASMA_Complex64_t *U, int LDU,
                             PLASMA_Complex64_t *VT, int LDVT,
                             PLASMA_sequence *sequence, PLASMA_request *request)
{
    PLASMA_desc descA;
    PLASMA_desc descT;
    PLASMA_desc descU, descVT;
    PLASMA_Complex64_t *AB;
    double *E;
    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_zgesdd_Tile_Async", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }
    if (sequence == NULL) {
        plasma_fatal_error("PLASMA_zgesdd_Tile_Async", "NULL sequence");
        return PLASMA_ERR_UNALLOCATED;
    }
    if (request == NULL) {
        plasma_fatal_error("PLASMA_zgesdd_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_zgesdd_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_zgesdd_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_zgesdd_Tile_Async", "illegal value of jobu");
        return PLASMA_ERR_NOT_SUPPORTED;
    }
    if (jobvt != PlasmaNoVec && jobvt != PlasmaVec) {
        plasma_error("PLASMA_zgesdd_Tile_Async", "illegal value of jobvt");
        return PLASMA_ERR_NOT_SUPPORTED;
    }
    if (jobvt != jobu) {
        plasma_error("PLASMA_zgesdd_Tile_Async", "in this version: jobu should be equal jobvt");
        return PLASMA_ERR_NOT_SUPPORTED;
    }
    if (descA.nb != descA.mb) {
        plasma_error("PLASMA_zgesdd_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 = (PLASMA_Complex64_t *) plasma_shared_alloc(plasma, M*N, PlasmaComplexDouble);
            if (AB == NULL) {
                plasma_error("PLASMA_zgesvd_Tile_Async", "plasma_shared_alloc(AB-0-) failed");
                plasma_shared_free(plasma, AB);
                return PLASMA_ERR_OUT_OF_RESOURCES;
            }
            plasma_zooptile2lap( descA, AB, NB, NB, M, N,  sequence, request);
        } else {
            AB = descA.mat;
            plasma_ziptile2lap( descA, AB, NB, NB, M, N,  sequence, request);
        }
        plasma_dynamic_sync();
        /*=======================================
         *  calling LAPACK ZGESVD
         *=======================================*/
        #if defined(ENABLE_TIMER)
        plasma_dynamic_sync();
        timelpk   = PLASMA_Wtime();
        #endif
        plasma_setlapack_multithreads(plasma->world_size);
        /* call SVD solver using lapack routine  */
        PLASMA_enum jobz  = jobu  == PlasmaVec ? PlasmaAllVec : PlasmaNoVec;
        status = LAPACKE_zgesdd(LAPACK_COL_MAJOR, lapack_const(jobz),
                                M, N, AB, M, S, U, LDU, VT, LDVT);
        if(status != 0){
            plasma_error("PLASMA_zgesvd","ZGESVD");
        }
        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_zooplap2tile_noalloc( descA, AB, NB, NB,  M, N, 0, 0, M, N, sequence, request);
            plasma_parallel_call_5( plasma_pzlapack_to_tile,
            PLASMA_Complex64_t*, AB,
            int,                 M,
            PLASMA_desc,         descA,
            PLASMA_sequence*,    sequence,
            PLASMA_request*,     request);
            plasma_dynamic_sync();
            free(AB);
        } else {
            plasma_ziplap2tile( descA, AB, NB, NB,  M, N, 0, 0, M, N,
                                sequence, request);
        }
        plasma_dynamic_sync();
        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 = (PLASMA_Complex64_t *)plasma_shared_alloc(plasma, LDAB*MINMN, PlasmaComplexDouble);
    memset( AB, 0, LDAB * MINMN * sizeof(PLASMA_Complex64_t) );
    if (AB == NULL) {
        plasma_error("PLASMA_zgesdd_Tile_Async", "plasma_shared_alloc(AB) failed");
        plasma_shared_free(plasma, AB);
        return PLASMA_ERR_OUT_OF_RESOURCES;
    }
    E = (double *)plasma_shared_alloc(plasma, MINMN, PlasmaRealDouble);
    if (E == NULL) {
        plasma_error("PLASMA_zgesdd_Tile_Async", "plasma_shared_alloc(E) failed");
        plasma_shared_free(plasma, E);
        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_pzgebrd_ge2gb,
            PLASMA_desc, descA,
            PLASMA_desc, descT,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);
    /* } */
    /* else { */
    /*     plasma_dynamic_call_4(plasma_pzgebrd_ge2gb_rh, */
    /*         PLASMA_desc, descA, */
    /*         PLASMA_desc, descT, */
    /*         PLASMA_sequence*, sequence, */
    /*         PLASMA_request*, request); */
    /* } */
    //plasma_dynamic_sync();
    plasma_dynamic_call_6( plasma_pzgbcpy_t2bl,
        PLASMA_enum, descA.m >= descA.n ? PlasmaUpper : PlasmaLower,
        PLASMA_desc, descA,
        PLASMA_Complex64_t*, &(AB[NB]),
        int, LDAB,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);
    plasma_dynamic_sync();
    status = sequence->status;
    if (status != PLASMA_SUCCESS) {
        plasma_error("PLASMA_zgesdd","pzgebrd_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
     *=======================================*/
    PLASMA_Complex64_t *VQ2   = NULL;
    PLASMA_Complex64_t *VP2   = NULL;
    PLASMA_Complex64_t *TAUQ2 = NULL;
    PLASMA_Complex64_t *TAUP2 = NULL;
    PLASMA_Complex64_t *TQ2   = NULL;
    PLASMA_Complex64_t *TP2   = NULL;
    int Vblksiz, blkcnt, LDT, LDV;
    int WANTZ   = 0;
    /* compute fraction of singular vectors */
    /* for future use when a portion of the eigenvectors are requested */
    int ME      = M;
    int NE      = N;
    int MINMNE  = MINMN;
    /*
    int fraction = 100;
    if((fraction<100)&&(fraction>0)) {
        MINMNE  = plasma_ceildiv(fraction*MINMN,100);
        ME      = MINMNE;
        NE      = MINMNE;
    }
    */
    if( jobu == PlasmaNoVec )
        WANTZ=0;
    else
        WANTZ=2;
    /* Vblksiz correspond to the blocking used when applying V2 to the matrix U
     * it is similar to IB in LAPACK ZUNMQR.
     * 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   = (PLASMA_Complex64_t *) plasma_shared_alloc(plasma,     blkcnt*Vblksiz, PlasmaComplexDouble);
        VQ2     = (PLASMA_Complex64_t *) plasma_shared_alloc(plasma, LDV*blkcnt*Vblksiz, PlasmaComplexDouble);
        TQ2     = (PLASMA_Complex64_t *) plasma_shared_alloc(plasma, LDT*blkcnt*Vblksiz, PlasmaComplexDouble);
        if ( (TAUQ2 == NULL) || (VQ2 == NULL) || (TQ2 == NULL) ) {
            plasma_error("PLASMA_zgesdd", "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(PLASMA_Complex64_t));
        memset(VQ2,   0, LDV*blkcnt*Vblksiz*sizeof(PLASMA_Complex64_t));
        memset(TQ2,   0, LDT*blkcnt*Vblksiz*sizeof(PLASMA_Complex64_t));
    }
    else {
        TAUQ2   = (PLASMA_Complex64_t *) plasma_shared_alloc(plasma, 2*MINMN, PlasmaComplexDouble);
        VQ2     = (PLASMA_Complex64_t *) plasma_shared_alloc(plasma, 2*MINMN, PlasmaComplexDouble);
        if ( (TAUQ2 == NULL) || (VQ2 == NULL) ) {
            plasma_error("PLASMA_zgesdd", "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(PLASMA_Complex64_t));
        memset(VQ2,   0, 2*MINMN*sizeof(PLASMA_Complex64_t));
    }
    /* data for VT */
    if( jobvt == PlasmaVec ) {
        findVTsiz(MINMN, NB, Vblksiz, &blkcnt, &LDV);
        TAUP2   = (PLASMA_Complex64_t *) plasma_shared_alloc(plasma,     blkcnt*Vblksiz, PlasmaComplexDouble);
        VP2     = (PLASMA_Complex64_t *) plasma_shared_alloc(plasma, LDV*blkcnt*Vblksiz, PlasmaComplexDouble);
        TP2     = (PLASMA_Complex64_t *) plasma_shared_alloc(plasma, LDT*blkcnt*Vblksiz, PlasmaComplexDouble);
        if ( (TAUP2 == NULL) || (VP2 == NULL) || (TP2 == NULL) ) {
            plasma_error("PLASMA_zgesdd", "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(PLASMA_Complex64_t));
        memset(VP2,   0, LDV*blkcnt*Vblksiz*sizeof(PLASMA_Complex64_t));
        memset(TP2,   0, LDT*blkcnt*Vblksiz*sizeof(PLASMA_Complex64_t));
    }
    else {
        TAUP2   = (PLASMA_Complex64_t *) plasma_shared_alloc(plasma, 2*MINMN, PlasmaComplexDouble);
        VP2     = (PLASMA_Complex64_t *) plasma_shared_alloc(plasma, 2*MINMN, PlasmaComplexDouble);
        if ( (TAUP2 == NULL) || (VP2 == NULL) ) {
            plasma_error("PLASMA_zgesdd", "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(PLASMA_Complex64_t));
        memset(VP2,   0, 2*MINMN*sizeof(PLASMA_Complex64_t));
    }
    /*=======================================
     *  calling bulge chasing
     *=======================================*/
    #if defined(ENABLE_TIMER)
    timeblg  = PLASMA_Wtime();
    #endif
    plasma_parallel_call_16(plasma_pzgebrd_gb2bd_v1,
        PLASMA_enum,         uplo,
        int,                 MINMN,
        int,                 NB,
        int,                 Vblksiz,
        PLASMA_Complex64_t*, AB,
        int,                 LDAB,
        PLASMA_Complex64_t*, VQ2,
        PLASMA_Complex64_t*, TAUQ2,
        PLASMA_Complex64_t*, VP2,
        PLASMA_Complex64_t*, TAUP2,
        double*,             S,
        double*,             E,
        int,                 WANTZ,
        int,                 WANTZ,
        PLASMA_sequence*,    sequence,
        PLASMA_request*,     request);
    /* WARNING: If plasma_pzgebrd_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
     *=======================================*/
    /*=======================================
     *  calling SVD solver
     *=======================================*/
    plasma_setlapack_multithreads(plasma->world_size);
    /* call SVD solver using lapack routine for our resulting bidiag [S E] */
    /* call eigensolver using lapack routine for our resulting tridiag [D E] */
    if((jobu == PlasmaNoVec)&&(jobvt == PlasmaNoVec)){
        #if defined(ENABLE_TIMER)
        timelpk   = PLASMA_Wtime();
        #endif
        status = LAPACKE_dbdsdc(LAPACK_COL_MAJOR, lapack_const(uplo), lapack_const(PlasmaNoVec),
                                MINMN, S, E, NULL, LDU, NULL, LDVT, NULL, NULL);
        #if defined(ENABLE_TIMER)
        timelpk = PLASMA_Wtime()-timelpk;
        #endif
    } else {
        // set U and VT to zero
        memset(VT,   0, N*LDVT*sizeof(PLASMA_Complex64_t));
        memset(U,   0, M*LDU*sizeof(PLASMA_Complex64_t));
        /* Initialize U(N+1:M,N+1:M) and VT(M+1:N,M+1:N) to Identity */

        for(i=N; i<M; i++){
            U[i+i*LDU] = 1.0;
        }
         for(i=M; i<N; i++){
            VT[i+i*LDVT] = 1.0;
        }
#if defined COMPLEX
        int j;
        double *RU    = NULL;
        double *RVT   = NULL;
        RU   = (double *) plasma_shared_alloc(plasma,     MINMN*MINMN, PlasmaRealDouble);
        RVT  = (double *) plasma_shared_alloc(plasma,     MINMN*MINMN, PlasmaRealDouble);
        #if defined(ENABLE_TIMER)
        timelpk   = PLASMA_Wtime();
        #endif
        status = LAPACKE_dbdsdc(LAPACK_COL_MAJOR, lapack_const(uplo), lapack_const(PlasmaIvec),
                                MINMN, S, E, RU, MINMN, RVT, MINMN, NULL, NULL);
        #if defined(ENABLE_TIMER)
        timelpk = PLASMA_Wtime()-timelpk;
        #endif
        // copy the real U and VT matrix to the complex
        //LAPACKE_zlacp2(LAPACK_COL_MAJOR, lapack_const(uplo), MINMN, MINMN, RU, MINMN, U, LDU );
        //LAPACKE_zlacp2(LAPACK_COL_MAJOR, lapack_const(uplo), MINMN, MINMN, RVT, MINMN, VT, LDVT );
        for(j=0; j<MINMN; j++){
            for(i=0; i<MINMN; i++){
                U[i+j*LDU] = (PLASMA_Complex64_t) RU[i+j*MINMN];
            }
        }
        for(j=0; j<MINMN; j++){
            for(i=0; i<MINMN; i++){
                VT[i+j*LDVT] = (PLASMA_Complex64_t) RVT[i+j*MINMN];
            }
        }
        plasma_shared_free(plasma, RU);
        plasma_shared_free(plasma, RVT);
#else
        #if defined(ENABLE_TIMER)
        timelpk   = PLASMA_Wtime();
        #endif
        status = LAPACKE_dbdsdc(LAPACK_COL_MAJOR, lapack_const(uplo), lapack_const(PlasmaIvec),
                                MINMN, S, E, U, LDU, VT, LDVT, NULL, NULL);
        #if defined(ENABLE_TIMER)
        timelpk = PLASMA_Wtime()-timelpk;
        #endif
#endif
    }
    if(status != 0){
        plasma_error("PLASMA_zbdedc","ZBDEDC");
        /*return status;*/
    }
    sequence->status = status;
    plasma_setlapack_sequential(plasma);
    #if defined(ENABLE_TIMER)
    int fraction = (MINMNE*100)/N;
    printf("  Finish Eigensolver timing= %lf  WANTZ %d threads %d fraction %d  MINMNE %d \n" ,timelpk, WANTZ,  plasma->world_size, fraction, MINMNE);
    #endif
    /*=======================================
     *  END of calling SVD solver
     *=======================================*/
    /*=======================================
     *  generate U from the bulge
     *=======================================*/
    if (jobu == PlasmaVec){
        #if defined(ENABLE_TIMER)
        timeT  = PLASMA_Wtime();
        #endif
        /* compute T2 */
        plasma_static_call_8(plasma_pzlarft_blgtrd,
            int,                 MINMN,
            int,                 NB,
            int,                 Vblksiz,
            PLASMA_Complex64_t*, VQ2,
            PLASMA_Complex64_t*, TQ2,
            PLASMA_Complex64_t*, 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_pzunmqr_blgtrd,
            PLASMA_enum,         PlasmaLeft,
            PLASMA_enum,         PlasmaNoTrans,
            int,                 MINMN,
            int,                 NB,
            int,                 MINMNE,
            int,                 Vblksiz,
            int,                 WANTZ,
            PLASMA_Complex64_t*, VQ2,
            PLASMA_Complex64_t*, TQ2,
            PLASMA_Complex64_t*, TAUQ2,
            PLASMA_Complex64_t*, 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_zooplap2tile( descU, U, NB, NB, LDU, ME, 0, 0, M, ME, sequence, request, plasma_desc_mat_free(&(descU)) );
            } else {
                plasma_ziplap2tile( descU, U, NB, NB, LDU, ME, 0, 0, M, ME, 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_pzunmqr,
                    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_pzunmqr,
                    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_zooptile2lap( descU, U, NB, NB, LDU, ME, sequence, request );
                plasma_dynamic_sync();
                plasma_desc_mat_free(&descU);
            } else {
                plasma_ziptile2lap( descU, U, NB, NB, LDU, ME, 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){
        #if defined(ENABLE_TIMER)
        timeT  = PLASMA_Wtime();
        #endif
        /* compute T2 */
        plasma_static_call_8(plasma_pzlarft_blgtrd,
            int,                 MINMN,
            int,                 NB,
            int,                 Vblksiz,
            PLASMA_Complex64_t*, VP2,
            PLASMA_Complex64_t*, TP2,
            PLASMA_Complex64_t*, 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_pzunmqr_blgtrd,
            PLASMA_enum,         PlasmaRight,
            PLASMA_enum,         PlasmaConjTrans,
            int,                 MINMN,
            int,                 NB,
            int,                 MINMNE,
            int,                 Vblksiz,
            int,                 WANTZ,
            PLASMA_Complex64_t*, VP2,
            PLASMA_Complex64_t*, TP2,
            PLASMA_Complex64_t*, TAUP2,
            PLASMA_Complex64_t*, 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_zooplap2tile( descVT, VT, NB, NB, LDVT, N, 0, 0, NE, N, sequence, request, plasma_desc_mat_free(&(descVT)) );
            } else {
                plasma_ziplap2tile( descVT, VT, NB, NB, LDVT, N, 0, 0, NE, 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_pzunmlq,
                    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_pzunmlq,
                    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_zooptile2lap( descVT, VT, NB, NB, LDVT, N, sequence, request );
                plasma_dynamic_sync();
                plasma_desc_mat_free(&descVT);
            } else {
                plasma_ziptile2lap( 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 eigenproblem threads %d  N %d  fraction %d  MINMNE %d  timeall= %lf \n", plasma->world_size, MINMN, fraction, MINMNE, 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, E);
    plasma_shared_free(plasma, AB);
    return PLASMA_SUCCESS;
}


/* randist/gsl_randist.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, Brian Gough
 *
 * 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.
 */

#ifndef __GSL_RANDIST_H__
#define __GSL_RANDIST_H__
#include <gsl/gsl_rng.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

unsigned int gsl_ran_bernoulli (const gsl_rng * r, double p);
double gsl_ran_bernoulli_pdf (const unsigned int k, double p);

double gsl_ran_beta (const gsl_rng * r, const double a, const double b);
double gsl_ran_beta_pdf (const double x, const double a, const double b);

unsigned int gsl_ran_binomial (const gsl_rng * r, double p, unsigned int n);
unsigned int gsl_ran_binomial_knuth (const gsl_rng * r, double p, unsigned int n);
unsigned int gsl_ran_binomial_tpe (const gsl_rng * r, double p, unsigned int n);
double gsl_ran_binomial_pdf (const unsigned int k, const double p, const unsigned int n);

double gsl_ran_exponential (const gsl_rng * r, const double mu);
double gsl_ran_exponential_pdf (const double x, const double mu);

double gsl_ran_exppow (const gsl_rng * r, const double a, const double b);
double gsl_ran_exppow_pdf (const double x, const double a, const double b);

double gsl_ran_cauchy (const gsl_rng * r, const double a);
double gsl_ran_cauchy_pdf (const double x, const double a);

double gsl_ran_chisq (const gsl_rng * r, const double nu);
double gsl_ran_chisq_pdf (const double x, const double nu);

void gsl_ran_dirichlet (const gsl_rng * r, const size_t K, const double alpha[], double theta[]);
double gsl_ran_dirichlet_pdf (const size_t K, const double alpha[], const double theta[]);
double gsl_ran_dirichlet_lnpdf (const size_t K, const double alpha[], const double theta[]);

double gsl_ran_erlang (const gsl_rng * r, const double a, const double n);
double gsl_ran_erlang_pdf (const double x, const double a, const double n);

double gsl_ran_fdist (const gsl_rng * r, const double nu1, const double nu2);
double gsl_ran_fdist_pdf (const double x, const double nu1, const double nu2);

double gsl_ran_flat (const gsl_rng * r, const double a, const double b);
double gsl_ran_flat_pdf (double x, const double a, const double b);

double gsl_ran_gamma (const gsl_rng * r, const double a, const double b);
double gsl_ran_gamma_int (const gsl_rng * r, const unsigned int a);
double gsl_ran_gamma_pdf (const double x, const double a, const double b);
double gsl_ran_gamma_mt (const gsl_rng * r, const double a, const double b);
double gsl_ran_gamma_knuth (const gsl_rng * r, const double a, const double b);

double gsl_ran_gaussian (const gsl_rng * r, const double sigma);
double gsl_ran_gaussian_ratio_method (const gsl_rng * r, const double sigma);
double gsl_ran_gaussian_ziggurat (const gsl_rng * r, const double sigma);
double gsl_ran_gaussian_pdf (const double x, const double sigma);

double gsl_ran_ugaussian (const gsl_rng * r);
double gsl_ran_ugaussian_ratio_method (const gsl_rng * r);
double gsl_ran_ugaussian_pdf (const double x);

double gsl_ran_gaussian_tail (const gsl_rng * r, const double a, const double sigma);
double gsl_ran_gaussian_tail_pdf (const double x, const double a, const double sigma);

double gsl_ran_ugaussian_tail (const gsl_rng * r, const double a);
double gsl_ran_ugaussian_tail_pdf (const double x, const double a);

void gsl_ran_bivariate_gaussian (const gsl_rng * r, double sigma_x, double sigma_y, double rho, double *x, double *y);
double gsl_ran_bivariate_gaussian_pdf (const double x, const double y, const double sigma_x, const double sigma_y, const double rho);

double gsl_ran_landau (const gsl_rng * r);
double gsl_ran_landau_pdf (const double x);

unsigned int gsl_ran_geometric (const gsl_rng * r, const double p);
double gsl_ran_geometric_pdf (const unsigned int k, const double p);

unsigned int gsl_ran_hypergeometric (const gsl_rng * r, unsigned int n1, unsigned int n2, unsigned int t);
double gsl_ran_hypergeometric_pdf (const unsigned int k, const unsigned int n1, const unsigned int n2, unsigned int t);

double gsl_ran_gumbel1 (const gsl_rng * r, const double a, const double b);
double gsl_ran_gumbel1_pdf (const double x, const double a, const double b);

double gsl_ran_gumbel2 (const gsl_rng * r, const double a, const double b);
double gsl_ran_gumbel2_pdf (const double x, const double a, const double b);

double gsl_ran_logistic (const gsl_rng * r, const double a);
double gsl_ran_logistic_pdf (const double x, const double a);

double gsl_ran_lognormal (const gsl_rng * r, const double zeta, const double sigma);
double gsl_ran_lognormal_pdf (const double x, const double zeta, const double sigma);

unsigned int gsl_ran_logarithmic (const gsl_rng * r, const double p);
double gsl_ran_logarithmic_pdf (const unsigned int k, const double p);

void gsl_ran_multinomial (const gsl_rng * r, const size_t K,
                          const unsigned int N, const double p[],
                          unsigned int n[] );
double gsl_ran_multinomial_pdf (const size_t K,
                                const double p[], const unsigned int n[] );
double gsl_ran_multinomial_lnpdf (const size_t K,
                           const double p[], const unsigned int n[] );

unsigned int gsl_ran_negative_binomial (const gsl_rng * r, double p, double n);
double gsl_ran_negative_binomial_pdf (const unsigned int k, const double p, double n);

unsigned int gsl_ran_pascal (const gsl_rng * r, double p, unsigned int n);
double gsl_ran_pascal_pdf (const unsigned int k, const double p, unsigned int n);

double gsl_ran_pareto (const gsl_rng * r, double a, const double b);
double gsl_ran_pareto_pdf (const double x, const double a, const double b);

unsigned int gsl_ran_poisson (const gsl_rng * r, double mu);
void gsl_ran_poisson_array (const gsl_rng * r, size_t n, unsigned int array[],
                            double mu);
double gsl_ran_poisson_pdf (const unsigned int k, const double mu);

double gsl_ran_rayleigh (const gsl_rng * r, const double sigma);
double gsl_ran_rayleigh_pdf (const double x, const double sigma);

double gsl_ran_rayleigh_tail (const gsl_rng * r, const double a, const double sigma);
double gsl_ran_rayleigh_tail_pdf (const double x, const double a, const double sigma);

double gsl_ran_tdist (const gsl_rng * r, const double nu);
double gsl_ran_tdist_pdf (const double x, const double nu);

double gsl_ran_laplace (const gsl_rng * r, const double a);
double gsl_ran_laplace_pdf (const double x, const double a);

double gsl_ran_levy (const gsl_rng * r, const double c, const double alpha);
double gsl_ran_levy_skew (const gsl_rng * r, const double c, const double alpha, const double beta);

double gsl_ran_weibull (const gsl_rng * r, const double a, const double b);
double gsl_ran_weibull_pdf (const double x, const double a, const double b);

void gsl_ran_dir_2d (const gsl_rng * r, double * x, double * y);
void gsl_ran_dir_2d_trig_method (const gsl_rng * r, double * x, double * y);
void gsl_ran_dir_3d (const gsl_rng * r, double * x, double * y, double * z);
void gsl_ran_dir_nd (const gsl_rng * r, size_t n, double * x);

void gsl_ran_shuffle (const gsl_rng * r, void * base, size_t nmembm, size_t size);
int gsl_ran_choose (const gsl_rng * r, void * dest, size_t k, void * src, size_t n, size_t size) ;
void gsl_ran_sample (const gsl_rng * r, void * dest, size_t k, void * src, size_t n, size_t size) ;

typedef struct {                /* struct for Walker algorithm */
    size_t K;
    size_t *A;
    double *F;
} gsl_ran_discrete_t;

gsl_ran_discrete_t * gsl_ran_discrete_preproc (size_t K, const double *P);
void gsl_ran_discrete_free(gsl_ran_discrete_t *g);
size_t gsl_ran_discrete (const gsl_rng *r, const gsl_ran_discrete_t *g);
double gsl_ran_discrete_pdf (size_t k, const gsl_ran_discrete_t *g);

__END_DECLS

#endif /* __GSL_RANDIST_H__ */


#include <gsl/gsl_test.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_math.h>
#include "gsl_cblas.h"

#include "tests.h"

void
test_nrm2 (void) {
const double flteps = 1e-4, dbleps = 1e-6;
  {
   int N = 1;
   float X[] = { 0.317f };
   int incX = -1;
   float expected = 0.0f;
   float f;
   f = cblas_snrm2(N, X, incX);
   gsl_test_rel(f, expected, flteps, "snrm2(case 28)");
  };

  {
   int N = 1;
   double X[] = { 0.071 };
   int incX = -1;
   double expected = 0;
   double f;
   f = cblas_dnrm2(N, X, incX);
   gsl_test_rel(f, expected, dbleps, "dnrm2(case 29)");
  };

  {
   int N = 1;
   float X[] = { 0.776f, 0.983f };
   int incX = -1;
   float expected = 0.0f;
   float f;
   f = cblas_scnrm2(N, X, incX);
   gsl_test_rel(f, expected, flteps, "scnrm2(case 30)");
  };

  {
   int N = 1;
   double X[] = { 0.549, -0.354 };
   int incX = -1;
   double expected = 0;
   double f;
   f = cblas_dznrm2(N, X, incX);
   gsl_test_rel(f, expected, dbleps, "dznrm2(case 31)");
  };

  {
   int N = 2;
   float X[] = { 0.14f, -0.632f };
   int incX = 1;
   float expected = 0.647320631527f;
   float f;
   f = cblas_snrm2(N, X, incX);
   gsl_test_rel(f, expected, flteps, "snrm2(case 32)");
  };

  {
   int N = 2;
   double X[] = { 0.696, -0.804 };
   int incX = 1;
   double expected = 1.06340584915;
   double f;
   f = cblas_dnrm2(N, X, incX);
   gsl_test_rel(f, expected, dbleps, "dnrm2(case 33)");
  };

  {
   int N = 2;
   float X[] = { 0.281f, -0.063f, 0.367f, 0.232f };
   int incX = 1;
   float expected = 0.521001919382f;
   float f;
   f = cblas_scnrm2(N, X, incX);
   gsl_test_rel(f, expected, flteps, "scnrm2(case 34)");
  };

  {
   int N = 2;
   double X[] = { -0.359, -0.76, -0.906, -0.108 };
   int incX = 1;
   double expected = 1.24055672986;
   double f;
   f = cblas_dznrm2(N, X, incX);
   gsl_test_rel(f, expected, dbleps, "dznrm2(case 35)");
  };

  {
   int N = 2;
   float X[] = { 0.918f, -0.126f };
   int incX = -1;
   float expected = 0.0f;
   float f;
   f = cblas_snrm2(N, X, incX);
   gsl_test_rel(f, expected, flteps, "snrm2(case 36)");
  };

  {
   int N = 2;
   double X[] = { 0.217, -0.588 };
   int incX = -1;
   double expected = 0;
   double f;
   f = cblas_dnrm2(N, X, incX);
   gsl_test_rel(f, expected, dbleps, "dnrm2(case 37)");
  };

  {
   int N = 2;
   float X[] = { 0.31f, 0.059f, -0.442f, 0.987f };
   int incX = -1;
   float expected = 0.0f;
   float f;
   f = cblas_scnrm2(N, X, incX);
   gsl_test_rel(f, expected, flteps, "scnrm2(case 38)");
  };

  {
   int N = 2;
   double X[] = { 0.609, 0.615, -0.143, -0.957 };
   int incX = -1;
   double expected = 0;
   double f;
   f = cblas_dznrm2(N, X, incX);
   gsl_test_rel(f, expected, dbleps, "dznrm2(case 39)");
  };

}


/* specfunc/bessel_Jnu.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, 2017 Konrad Griessinger
 *
 * 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_sf_bessel.h>
#include <gsl/gsl_sf_sincos_pi.h>

#include "error.h"

#include "bessel.h"
#include "bessel_olver.h"
#include "bessel_temme.h"

/* Evaluate at large enough nu to apply asymptotic
 * results and apply backward recurrence.
 */
#if 0
static
int
bessel_J_recur_asymp(const double nu, const double x,
                     gsl_sf_result * Jnu, gsl_sf_result * Jnup1)
{
  const double nu_cut = 25.0;
  int n;
  int steps = ceil(nu_cut - nu) + 1;

  gsl_sf_result r_Jnp1;
  gsl_sf_result r_Jn;
  int stat_O1 = gsl_sf_bessel_Jnu_asymp_Olver_e(nu + steps + 1.0, x, &r_Jnp1);
  int stat_O2 = gsl_sf_bessel_Jnu_asymp_Olver_e(nu + steps,       x, &r_Jn);
  double r_fe = fabs(r_Jnp1.err/r_Jnp1.val) + fabs(r_Jn.err/r_Jn.val);
  double Jnp1 = r_Jnp1.val;
  double Jn   = r_Jn.val;
  double Jnm1;
  double Jnp1_save;

  for(n=steps; n>0; n--) {
    Jnm1 = 2.0*(nu+n)/x * Jn - Jnp1;
    Jnp1 = Jn;
    Jnp1_save = Jn;
    Jn   = Jnm1;
  }

  Jnu->val = Jn;
  Jnu->err = (r_fe + GSL_DBL_EPSILON * (steps + 1.0)) * fabs(Jn);
  Jnup1->val = Jnp1_save;
  Jnup1->err = (r_fe + GSL_DBL_EPSILON * (steps + 1.0)) * fabs(Jnp1_save);

  return GSL_ERROR_SELECT_2(stat_O1, stat_O2);
}
#endif

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

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

  if(x == 0.0) {
    if(nu == 0.0) {
      result->val = 1.0;
      result->err = 0.0;
    }
    else {
      result->val = 0.0;
      result->err = 0.0;
    }
    return GSL_SUCCESS;
  }
  else if(x*x < 10.0*(nu+1.0)) {
    return gsl_sf_bessel_IJ_taylor_e(nu, x, -1, 100, GSL_DBL_EPSILON, result);
  }
  else if(nu > 50.0) {
    return gsl_sf_bessel_Jnu_asymp_Olver_e(nu, x, result);
  }
  else if(x > 1000.0)
  {
    /* We need this to avoid feeding large x to CF1; note that
     * due to the above check, we know that n <= 50. See similar
     * block in bessel_Jn.c.
     */
    return gsl_sf_bessel_Jnu_asympx_e(nu, x, result);
  }
  else {
    /* -1/2 <= mu <= 1/2 */
    int N = (int)(nu + 0.5);
    double mu = nu - N;

    /* Determine the J ratio at nu.
     */
    double Jnup1_Jnu;
    double sgn_Jnu;
    const int stat_CF1 = gsl_sf_bessel_J_CF1(nu, x, &Jnup1_Jnu, &sgn_Jnu);

    if(x < 2.0) {
      /* Determine Y_mu, Y_mup1 directly and recurse forward to nu.
       * Then use the CF1 information to solve for J_nu and J_nup1.
       */
      gsl_sf_result Y_mu, Y_mup1;
      const int stat_mu = gsl_sf_bessel_Y_temme(mu, x, &Y_mu, &Y_mup1);

      double Ynm1 = Y_mu.val;
      double Yn   = Y_mup1.val;
      double Ynp1 = 0.0;
      int n;
      for(n=1; n<N; n++) {
        Ynp1 = 2.0*(mu+n)/x * Yn - Ynm1;
        Ynm1 = Yn;
        Yn   = Ynp1;
      }

      result->val = 2.0/(M_PI*x) / (Jnup1_Jnu*Yn - Ynp1);
      result->err = GSL_DBL_EPSILON * (N + 2.0) * fabs(result->val);
      return GSL_ERROR_SELECT_2(stat_mu, stat_CF1);
    }
    else {
      /* Recurse backward from nu to mu, determining the J ratio
       * at mu. Use this together with a Steed method CF2 to
       * determine the actual J_mu, and thus obtain the normalization.
       */
      double Jmu;
      double Jmup1_Jmu;
      double sgn_Jmu;
      double Jmuprime_Jmu;
      double P, Q;
      const int stat_CF2 = gsl_sf_bessel_JY_steed_CF2(mu, x, &P, &Q);
      double gamma;

      double Jnp1 = sgn_Jnu * GSL_SQRT_DBL_MIN * Jnup1_Jnu;
      double Jn   = sgn_Jnu * GSL_SQRT_DBL_MIN;
      double Jnm1;
      int n;
      for(n=N; n>0; n--) {
        Jnm1 = 2.0*(mu+n)/x * Jn - Jnp1;
        Jnp1 = Jn;
        Jn   = Jnm1;
      }
      Jmup1_Jmu = Jnp1/Jn;
      sgn_Jmu   = GSL_SIGN(Jn);
      Jmuprime_Jmu = mu/x - Jmup1_Jmu;

      gamma = (P - Jmuprime_Jmu)/Q;
      Jmu   = sgn_Jmu * sqrt(2.0/(M_PI*x) / (Q + gamma*(P-Jmuprime_Jmu)));

      result->val = Jmu * (sgn_Jnu * GSL_SQRT_DBL_MIN) / Jn;
      result->err = 2.0 * GSL_DBL_EPSILON * (N + 2.0) * fabs(result->val);

      return GSL_ERROR_SELECT_2(stat_CF2, stat_CF1);
    }
  }
}

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

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if (nu < 0.0) {
    int Jstatus = gsl_sf_bessel_Jnupos_e(-nu, x, result);
    double Jval = result->val;
    double Jerr = result->err;
    int Ystatus = gsl_sf_bessel_Ynupos_e(-nu, x, result);
    double Yval = result->val;
    double Yerr = result->err;
    /* double s = sin(M_PI*nu), c = cos(M_PI*nu); */
    int sinstatus = gsl_sf_sin_pi_e(nu, result);
    double s = result->val;
    double serr = result->err;
    int cosstatus = gsl_sf_cos_pi_e(nu, result);
    double c = result->val;
    double cerr = result->err;
    result->val = s*Yval + c*Jval;
    result->err = fabs(c*Yerr) + fabs(s*Jerr) + fabs(cerr*Yval) + fabs(serr*Jval);
    return GSL_ERROR_SELECT_4(Jstatus, Ystatus, sinstatus, cosstatus);
  }
  else return gsl_sf_bessel_Jnupos_e(nu, x, result);
}

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

#include "eval.h"

double gsl_sf_bessel_Jnu(const double nu, const double x)
{
  EVAL_RESULT(gsl_sf_bessel_Jnu_e(nu, x, &result));
}


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C code for the conversion between time scales.
 *
 * Formulas are taken from the UNITED STATES NAVAL OBSERVATORY CIRCULAR NO. 179 by George H. Kaplan
 * See http://aa.usno.navy.mil/publications/docs/Circular_179.php
 *
 */

#define _XOPEN_SOURCE 500

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "timeconversion.h"

/***********************************************************/
/********* Core functions to compute overlaps **************/

/* Table of leap seconds - from LAL XLALLeapSeconds.h */
static const struct leaps_table { double jd; int gpssec; int taiutc; } leaps[] =
{
  {2444239.5,    -43200, 19},  /* 1980-Jan-01 */
  {2444786.5,  46828800, 20},  /* 1981-Jul-01 */
  {2445151.5,  78364801, 21},  /* 1982-Jul-01 */
  {2445516.5, 109900802, 22},  /* 1983-Jul-01 */
  {2446247.5, 173059203, 23},  /* 1985-Jul-01 */
  {2447161.5, 252028804, 24},  /* 1988-Jan-01 */
  {2447892.5, 315187205, 25},  /* 1990-Jan-01 */
  {2448257.5, 346723206, 26},  /* 1991-Jan-01 */
  {2448804.5, 393984007, 27},  /* 1992-Jul-01 */
  {2449169.5, 425520008, 28},  /* 1993-Jul-01 */
  {2449534.5, 457056009, 29},  /* 1994-Jul-01 */
  {2450083.5, 504489610, 30},  /* 1996-Jan-01 */
  {2450630.5, 551750411, 31},  /* 1997-Jul-01 */
  {2451179.5, 599184012, 32},  /* 1999-Jan-01 */
  {2453736.5, 820108813, 33},  /* 2006-Jan-01 */
  {2454832.5, 914803214, 34},  /* 2009-Jan-01 */
  {2456109.5, 1025136015, 35}, /* 2012-Jul-01 */
  {2457204.5, 1119744016, 36}  /* 2015-Jul-01 */
};
static const int numleaps = sizeof( leaps ) / sizeof( *leaps );

/* Returns the leap seconds TAI-UTC at a given GPS second */
static int leapseconds(const double gpstime)
{
  int leap;

  if ( gpstime < leaps[0].gpssec )
  {
    printf( "Error - Don't know leap seconds before GPS time %d\n", leaps[0].gpssec );
    exit(1);
  }

  /* scan leap second table and locate the appropriate interval */
  for ( leap = 1; leap < numleaps; ++leap )
    if ( gpstime < leaps[leap].gpssec )
      break;

  return leaps[leap-1].taiutc;
}

/***********************************************************/
/********* Core functions to compute overlaps **************/

/* Function computing the correction from the earth rotation angle to the gmst angle */
/* Formula (2.12) of the USNO circular */
static double gmst_correction(const double gpstime) /* gpstime in seconds */
{
  /* T is the number of julian centuries since J2000.0 */
  double T = (gpstime - EPOCH_J2000_0_GPS)/(86400.0*36525.0);
  double correction = 2*PI*(0.014506 + 4612.156534*T + 1.3915817*T*T - 0.00000044*T*T*T - 0.000029956*T*T*T*T - 0.0000000368*T*T*T*T*T)/15.0/86400.0;
  return correction;
}

/* Earth rotation angle from the UT1 seconds elapsed since J2000.0 */
/* Formula (2.11) of the USNO circular */
static double era_angle_from_ut1(const double ut1_j2000) /* ut1_j2000 in seconds */
{
  double DU = ut1_j2000/86400.0;
  double fracDU = DU - trunc(DU);
  double era_angle = 2*PI*(0.7790572732640 + 0.00273781191135448*DU + fracDU);
  return era_angle;
}

/* Function computing the gmst angle from the gps time */
/* Formula (2.12) of the USNO circular */
double gmst_angle_from_gpstime(const double gpstime) /* gpstime in seconds */
{
  /* UTC time - UTC at J2000, computed from the GPS time */
  double utc_j2000 = (gpstime - EPOCH_J2000_0_GPS) - (leapseconds(gpstime) - EPOCH_J2000_0_TAI_UTC);

  /* Earth rotation angle */
  /* BEWARE: here we assimilate UTC and UT1 times - by construction, |UTC-UT1| < 1sec */
  double era_angle = era_angle_from_ut1(utc_j2000);

  /* GMST angle */
  double gmst_angle = era_angle + gmst_correction(gpstime);
  return gmst_angle;
}


/*
 * PARTICLE SWARM OPTIMIZATION
 * ===========================
 *
 * Author: Gabriel E Leventhal, 2011-12-02
 *
 * adapted from
 *   Author: Tomas V. Arredondo
 *   SimPSOLib: A simple yet flexible PSO implementation in C++.
 *
 */

#ifndef __PSO_SWARM__
#define __PSO_SWARM__

#include <iostream>
#include <iomanip>
#include <sstream>
#include <vector>
#include <ctime>
#include <queue>
using namespace std;

#include "GSLRng.h"
#include <gsl/gsl_math.h>

#include "Particle.h"
#include "Point.h"
#include "Parameters.h"
#include "Network.h"

#ifdef USE_MPI
#include "mpi.h"
#endif

namespace PSO {
  class Swarm {
    public:
      Swarm() {}

      Swarm(int size, int np, Parameters* pars)
        : swarmSize(size), numParams(np), numEvals(0), curIt(0),
          numInform(3),
          phiPi(np,2.5), phiPf(np,0.5), phiGi(np,0.5),phiGf(np,2.5),
          omegai(np,0.721), omegaf(np,0.721), p(pars),
          bestVal(-INFINITY), bestParticle(-1),
          mpi_type(0), mpi_rank(0), mpi_ntasks(1)
      {
        bestPos = Point(np,0.0);
        setInformants(numInform);
      }

      virtual ~Swarm() { destroy(); }

      inline void setMPI(int type) { mpi_type = (type >= 0) ? type : 0; }

      inline void setVars(const Point& pp, const Point& pg, const Point& o) {
        phiPi = pp; phiGi = pg; omegai = o;
        phiPf = pp; phiGf = pg; omegaf = o;
        phiP = pp; phiG = pg; omega = o;
      }

      inline void setInformants(int K) {
        numInform = (K > swarmSize-1) ? swarmSize-1 : K;
        links.resize(swarmSize);
        init_network();
      }

      double maximum_swarm_radius() const;
      double search_space_diameter() const;

      // get everything ready to go
      void begin(int argc, char* argv[]);
      // finish up stuff
      void end();

      void initialize(int type = 0);
      void display(ostream* out = &cout, string prefix = "");

      virtual void evaluate(int vflag = 0, ostream* out = NULL, ostream* hist = NULL);
      virtual double evaluateParticle(int j);

      void updateVelocity(int j, bool bestVals = true);
      void updateVelocity(bool bestVals = true);

      void updatePosition(int j);
      void updatePosition();

      void randomizeInf(int maxTries = 1000);
      void randomPosition(int j);

      virtual void run(int numIt, int slowdown = 0, int vflag = 0,
                       ostream* out = NULL, ostream* hist = NULL);

      inline bool is_master() const { return (mpi_rank == 0); }

      inline void seed_rng(unsigned long seed) { rng.set(seed); }

      inline const Particle& at(size_t i) const { return *swarm[i]; }
      inline Particle best() const { return *swarm[bestParticle]; }

      inline double fitness(int i) const { return swarm.at(i)->fitness(); }

      inline int size() const { return swarmSize; }

      inline int nextIt() { return ++curIt; }
      inline void setIt(int i) { curIt = i; }

      double mean(int i) const;
      double var(int i) const;
      Point mean() const;
      Point var() const;

#ifdef USE_MPI
      virtual void evaluate_slave();
      void run_mpi(int numInt, int vflag = 0, ostream* out = NULL, ostream* hist = NULL);
      virtual void run_master(int numInt, int vflag, ostream* out, ostream* hist);
      inline void evaluate_master(int vflag = 0) { run_master(-1,vflag,NULL,NULL); }
#endif

    protected:
      void create();
      void destroy();

      void init_network();
      int find_best();

      int swarmSize;                            /* swarm size */
      vector<Particle*> swarm;                  /* swarm */
      int numParams;                            /* number of parameters */
      int numEvals;                             /* current number of function evaluations */
      int curIt;                                /* current swarm iteration */

      int numInform;                            /* number of informants */
      Network links;                            /* information network */

      Point phiP;
      Point phiG;
      Point omega;
      Point phiPi;                              /* phiP initial */
      Point phiPf;                              /* phiP final */
      Point phiGi;                              /* phiG initial */
      Point phiGf;                              /* phiG final */
      Point omegai;                             /* omega initial */
      Point omegaf;                             /* omega final */

      Parameters* p;

      double bestVal;                           /* current best value */
      Point bestPos;                            /* current best position */
      int bestParticle;

      myGSL::Rng rng;

      int mpi_type;
      int mpi_rank;
      int mpi_ntasks;
  };
}

#endif // __PSO_SWARM__


/* -*- mode: C; c-basic-offset: 4 -*- */
/* ex: set shiftwidth=4 tabstop=4 expandtab: */
/*
 * Copyright (c) 2011, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s): Neil T. Dantam <ntd@gatech.edu>
 * Georgia Tech Humanoid Robotics Lab
 * Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
 *
 *
 * This file is provided under the following "BSD-style" License:
 *
 *
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <amino.h>
#include <cblas.h>
#include "reflex.h"

#define SYMM_PART CblasUpper
#define SYMM_PARTC "U"

AA_API void rfx_lqg_kf_predict_cov( size_t n, const double *A, const double *V, double *P )
{
    // P = A * P * A**T + V
    int ni = (int)n;
    double *T = (double*)aa_mem_region_local_tmpalloc( 2 * n*n * sizeof(P[0]) );

    // T := A*P
    cblas_dsymm( CblasColMajor, CblasRight, SYMM_PART,
                 ni, ni,
                 1.0, P, ni,
                 A, ni,
                 0.0, T, ni );

    // P := (A*P) * A**T + V
    dlacpy_( SYMM_PARTC, &ni, &ni,
             V, &ni,
             P, &ni );
    cblas_dgemm( CblasColMajor, CblasNoTrans, CblasTrans,
                 ni, ni, ni,
                 1.0, T, ni,
                 A, ni,
                 1.0, P, ni );
}

int rfx_lqg_kf_correct_gain
( size_t n_x, size_t n_z, const double *C, const double *P, const double *W, double *K )
{
    int nxi = (int)n_x;
    int nzi = (int)n_z;

    double *ptr = (double*) aa_mem_region_local_alloc( sizeof(ptr[0]) *
                                                       (n_z*n_x + n_z*n_z) );

    double *CP = ptr;
    double *Kp = &ptr[n_z*n_x];

    // P is symmetric, so P*C^T == (C*P^T)^T == (C*P)^T

    // Kp := C * P * C**T + W = C*(C*P)^T
    // Kp^T := C*(C * P)^T
    cblas_dsymm( CblasColMajor, CblasRight, SYMM_PART,
                 nzi, nxi,
                 1.0, P, nxi,
                 C, nzi,
                 0.0, CP, nzi );

    dlacpy_( SYMM_PARTC, &nzi, &nzi,
             W, &nzi,
             Kp, &nzi );
    cblas_dgemm( CblasColMajor, CblasNoTrans, CblasTrans,
                 nzi, nzi, nxi,
                 1.0, C, nzi,
                 CP, nzi,
                 1.0, Kp, nzi );

    // K := P * C**T * Kp^-1
    // K Kp := P * C**T
    // CP = Kp^T K^T
    // Kp is symmetric
    // CP = Kp K^T
    int info;

    // Cholesky
    dpotrf_( SYMM_PARTC, &nzi,
             Kp, &nzi,
             &info );

    // Solve it!
    dpotrs_( SYMM_PARTC, &nzi, &nxi,
             Kp, &nzi,
             CP, &nzi,
             &info );

    // Transpose
    for( size_t j = 0; j < n_z; j ++ )
        for( size_t i = 0; i < n_x; i ++ )
            K[j*n_x+i] = CP[i*n_z+j];

    int i = 0;

    return i;
}

void rfx_lqg_kf_correct_cov
( size_t n_x, size_t n_z, const double *C, double *P, double *K )
{
    int nxi = (int)n_x;
    int nzi = (int)n_z;

    double *ptr = (double*)aa_mem_region_local_tmpalloc( 2 * n_x*n_x * sizeof(P[0]) );
    double *KC = ptr;
    double *P1 = &ptr[n_x*n_x];

    // P := (I - K*C) * P
    cblas_dgemm( CblasColMajor, CblasNoTrans, CblasNoTrans,
                 nxi, nxi, nzi,
                 -1.0, K, nxi,
                 C, nzi,
                 0.0, KC, nxi );
    // KC += I
    for( size_t i = 0; i < n_x*n_x; i = i + n_x + 1 )
        KC[i] += 1;

    cblas_dsymm( CblasColMajor, CblasRight, SYMM_PART,
                 nxi, nxi,
                 1.0, P, nxi,
                 KC, nxi,
                 0.0, P1, nxi );
    dlacpy_( SYMM_PARTC, &nxi, &nxi, P1, &nxi, P, &nxi );
}

int rfx_lqg_ekf_predict
( void *cx, size_t n_x, double *x, const double *u, double *P, const double *V,
  rfx_lqg_ekf_process_fun process )
{
    double F[n_x*n_x];
    int i = process( cx, x, u, F );
    rfx_lqg_kf_predict_cov( n_x, F, V, P );
    return i;
}

int rfx_lqg_ekf_correct
( void *cx, size_t n_x, size_t n_z, double *x, const double *z, double *P, const double *W,
  rfx_lqg_ekf_measure_fun measure, rfx_lqg_ekf_innovate_fun innovate, rfx_lqg_ekf_update_fun update )
{
    int i;

    double H[n_z*n_x];
    double K[n_x*n_z];
    {
        double y[n_z];
        i = measure(cx, x, y, H);
        if(i) return i;

        i = rfx_lqg_kf_correct_gain( n_x, n_z, H, P, W, K );
        if(i) return i;

        i = innovate(cx, x, z, y);
        if(i) return i;

        {
            double Ky[n_x];
            cblas_dgemv( CblasColMajor, CblasNoTrans,
                         (int)n_x, (int)n_z,
                         1.0, K, (int)n_x,
                         y, 1,
                         0.0, Ky, 1 );

            i = update(cx, x, Ky);
        }
    }

    rfx_lqg_kf_correct_cov( n_x, n_z, H, P, K );

    return i;
}

AA_API void rfx_lqg_init( rfx_lqg_t *lqg, size_t n_x, size_t n_u, size_t n_z ) {
    memset( lqg, 0, sizeof(*lqg) );
    lqg->n_x = n_x;
    lqg->n_u = n_u;
    lqg->n_z = n_z;

    lqg->x = AA_NEW0_AR( double, lqg->n_x );
    lqg->u = AA_NEW0_AR( double, lqg->n_u );
    lqg->z = AA_NEW0_AR( double, lqg->n_z );

    lqg->A = AA_NEW0_AR( double, lqg->n_x * lqg->n_x );
    lqg->B = AA_NEW0_AR( double, lqg->n_x * lqg->n_u );
    lqg->C = AA_NEW0_AR( double, lqg->n_z * lqg->n_x );

    lqg->P = AA_NEW0_AR( double, lqg->n_x * lqg->n_x );
    lqg->V = AA_NEW0_AR( double, lqg->n_x * lqg->n_x );
    lqg->W = AA_NEW0_AR( double, lqg->n_z * lqg->n_z );

    lqg->Q = AA_NEW0_AR( double, lqg->n_x * lqg->n_x );
    lqg->R = AA_NEW0_AR( double, lqg->n_u * lqg->n_u );

    lqg->K = AA_NEW0_AR( double, lqg->n_z * lqg->n_x );
    lqg->L = AA_NEW0_AR( double, lqg->n_u * lqg->n_x );

}
AA_API void rfx_lqg_destroy( rfx_lqg_t *lqg ) {

    free(lqg->x);
    free(lqg->u);
    free(lqg->z);

    free(lqg->A);
    free(lqg->B);
    free(lqg->C);

    free(lqg->Q);
    free(lqg->R);

    free(lqg->L);
    free(lqg->K);

}

/*
 * X = op(A)*op(B)*op(C) + beta*X
 *
 * m*n = (m*k) * (k*n)
 *
 * X:     m*n
 * op(A): m*k
 * op(B): k*p
 * op(C): p*n
 */

static inline void matmul3( size_t m, size_t n, size_t k, size_t p,
                            enum CBLAS_TRANSPOSE transA,
                            enum CBLAS_TRANSPOSE transB,
                            enum CBLAS_TRANSPOSE transC,
                            const double *A, size_t lda,
                            const double *B, size_t ldb,
                            const double *C, size_t ldc,
                            double beta, double *X, size_t ldx) {
    double T[m*p];
    // T := A*B
    cblas_dgemm( CblasColMajor, transA, transB,
                 (int)m, (int)p, (int)k,
                 1.0, A, (int)lda,
                 B, (int)ldb,
                 0.0, T, (int)m );

    // X := (A*B) * C + beta*X
    cblas_dgemm( CblasColMajor, CblasNoTrans, transC,
                 (int)m, (int)n, (int)p,
                 1.0, T, (int)m,
                 C, (int)ldc,
                 beta, X, (int)ldx );
}

static void kf_innovate( rfx_lqg_t *lqg, double *xh ) {

    // xh = x + K * (z - C*x)

    // T := z - C*x
    double T[lqg->n_z];
    memcpy(T, lqg->z, sizeof(T));
    cblas_dgemv( CblasColMajor, CblasNoTrans,
                 (int)lqg->n_z, (int)lqg->n_x,
                 -1.0, lqg->C, (int)lqg->n_z,
                 lqg->x, 1,
                 1.0, T, 1 );
    // xh = K * (z - C*x) + xh
    cblas_dgemv( CblasColMajor, CblasNoTrans,
                 (int)lqg->n_x, (int)lqg->n_z,
                 1.0, lqg->K, (int)lqg->n_x,
                 T, 1,
                 1.0, xh, 1 );
}

AA_API void rfx_lqg_kf_predict( rfx_lqg_t *lqg ) {
    // x = A*x + B*u
    {
        double x[lqg->n_x];
        aa_lsim_dstep( lqg->n_x, lqg->n_u,
                       lqg->A, lqg->B,
                       lqg->x, lqg->u,
                       x );
        memcpy(lqg->x, x, sizeof(x));
    }
    // P = A * P * A**T + V
    rfx_lqg_kf_predict_cov( lqg->n_x, lqg->A, lqg->V, lqg->P );
}

AA_API void rfx_lqg_kf_correct( rfx_lqg_t *lqg ) {

    rfx_lqg_kf_correct_gain( lqg->n_x, lqg->n_z,
                             lqg->C, lqg->P, lqg->W, lqg->K );

    // x = x + K * (z - C*x)
    kf_innovate( lqg, lqg->x );

    // P = (I - K*C) * P
    rfx_lqg_kf_correct_cov( lqg->n_x, lqg->n_z,
                            lqg->C, lqg->P, lqg->K );
}

// kalman-bucy gain
void rfx_lqg_kbf_gain( rfx_lqg_t *lqg )
{
    rfx_lqg_kbf_gain_work( lqg->n_x, lqg->n_z,
                           lqg->A, lqg->C, lqg->V, lqg->W, lqg->P, lqg->K );
}

// kalman-bucy gain
void rfx_lqg_kbf_gain_work
( size_t n_x, size_t n_z,
  const double *A, const double *C, const double *V, const double *W, double *P, double *K )
{
    // dP = A*P + P*A' - P*C'*W^{-1}*C*P + V
    // K = P * C' * W^{-1}

    double Winv[n_z*n_z];
    memcpy(Winv, W, sizeof(Winv));
    aa_la_inv( n_z, Winv);

    {
        double ric_B[n_x*n_x];

        // ric_B := C' * W^{-1} * C
        matmul3( n_x, n_x, n_z, n_z,
                 CblasTrans, CblasNoTrans, CblasNoTrans,
                 C, n_z,
                 Winv, n_z,
                 C, n_z,
                 0.0, ric_B, n_x );

        double ric_A[n_x*n_x];
        aa_la_transpose2(n_x, n_x, A, ric_A );

        // solve ARE with dP = 0, result is P
        //aa_tick("laub: ");
        aa_la_care_laub( n_x, n_x, n_x,
                         ric_A, ric_B, V, P );
        //aa_tock();
    }

    // K = P * C' * W^{-1}
    matmul3( n_x, n_z, n_x, n_z,
             CblasNoTrans, CblasTrans, CblasNoTrans,
             P, n_x,
             C, n_z,
             Winv, n_z,
             0.0, K, n_x );

}

void rfx_lqg_lqr_gain( rfx_lqg_t *lqg ) {
    // A'*S + S*A - S*B*R^{-1}*B'*S + Q = 0

    double Rinv[lqg->n_u*lqg->n_u];
    memcpy(Rinv, lqg->R, sizeof(Rinv));
    aa_la_inv( lqg->n_u, Rinv);

    double S[lqg->n_x*lqg->n_x];
    {
        double ric_B[lqg->n_x*lqg->n_x];

        // ric_B := B * R^{-1} * B'
        matmul3( lqg->n_x, lqg->n_x, lqg->n_u, lqg->n_u,
                 CblasNoTrans, CblasNoTrans, CblasTrans,
                 lqg->B, lqg->n_x,
                 Rinv, lqg->n_u,
                 lqg->B, lqg->n_x,
                 0.0, ric_B, lqg->n_x );

        // solve ARE with dS = 0, result is S
        aa_la_care_laub( lqg->n_x, lqg->n_x, lqg->n_x,
                         lqg->A, ric_B, lqg->Q, S );
    }

    // L = R^{-1}*B'*S
    matmul3( lqg->n_u, lqg->n_x, lqg->n_u, lqg->n_x,
             CblasNoTrans, CblasTrans, CblasNoTrans,
             Rinv, lqg->n_u,
             lqg->B, lqg->n_x,
             S, lqg->n_x,
             0.0, lqg->L, lqg->n_u );
}

static void kbf_step_fun( const void *cx,
                          double t, const double *restrict x,
                          double *restrict dx ) {
    // dx = A*x + B*u + K(z-Cx)
    (void)t;
    rfx_lqg_t *lqg = (rfx_lqg_t*)cx;

    // dx := A*x + B*u
    aa_lsim_dstep( lqg->n_x, lqg->n_u,
                   lqg->A, lqg->B,
                   x, lqg->u,
                   dx );
    // dx := K * (z - C*x) + dx
    kf_innovate( lqg, dx );
}

void rfx_lqg_kbf_step1( rfx_lqg_t *lqg, double dt ) {
    // dx = A*x + B*u + K(z-Cx)
    double dx[lqg->n_x];
    kbf_step_fun( lqg, 0, lqg->x, dx );

    // x := dt*dx + x
    cblas_daxpy( (int)lqg->n_x, dt, dx, 1, lqg->x, 1 );
}

void rfx_lqg_kbf_step4( rfx_lqg_t *lqg, double dt ) {
    double x1[lqg->n_x];
    // runge-kutta 4
    aa_odestep_rk4( lqg->n_x, kbf_step_fun, lqg,
                    0, dt, lqg->x, x1 );
    memcpy( lqg->x, x1, sizeof(x1) );
}

AA_API void rfx_lqg_lqr_ctrl( rfx_lqg_t *lqg ) {
    cblas_dgemv( CblasColMajor, CblasNoTrans,
                 (int)lqg->n_u, (int)lqg->n_x,
                 -1.0, lqg->L, (int)lqg->n_u,
                 lqg->x, 1,
                 0.0, lqg->u, 1 );
}

void rfx_lqg_sys( const void *cx,
                  double t, const double *restrict x,
                  double *restrict dx ) {
    rfx_lqg_t * lqg = (rfx_lqg_t*)cx;
    (void)t;
    aa_lsim_dstep( lqg->n_x, lqg->n_u,
                   lqg->A, lqg->B,
                   x, lqg->u,
                   dx );
}

//AA_API void rfx_lqg_observe_euler( rfx_lqg_t *lqg, double dt, aa_mem_region_t *reg ) {
    // --- calculate kalman gain ---

    // --- predict from previous input ---
    // dx = A*x + B*u + K * ( z - C*xh )
    //double *dx = (double*)aa_mem_region_alloc(reg, sizeof(double)*lqg->n_x);
    // zz := z
    //double *zz = (double*)aa_mem_region_alloc(reg, sizeof(double)*lqg->n_z);

    // x = x + dx * dt
    //aa_la_axpy( lqg->n_x, dt, dx, lqg->x );
//}

//AA_API void rfx_lqg_ctrl( rfx_lqg_t *lqg, double dt, aa_mem_region_t *reg ) {
    // compute optimal gain
    //  -dS = A'*S + S*A - S*B*R^{-1}*B' + Q
    // solve ARE, result is S
    // L = R^{-1} * B' * S  : (nu*nu) * (nu*nx) * (nx*nx)

    // compute current input
    // u = -Lx
//}

/* int rfx_lqg_duqu_predict */
/* ( double dt, double *S, double *dx, double *P, const double *V ) */
/* { */
/*     return 0; */
/* } */


/* AUTORIGHTS
Copyright (C) 2007 Princeton University

This file is part of Ferret Toolkit.

Ferret Toolkit 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, 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.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_rng.h>

#include <cass.h>
#include <hash_table.h>
#include <arena.h>

#define	MAX_INT	2147483647

#define DEFAULT_PRIME	1017881
//#define DEFAULT_PRIME	 507217
//#define DEFAULT_PRIME	 432251
//#define DEFAULT_PRIME	 300301
//#define DEFAULT_PRIME	 128311
//#define DEFAULT_PRIME	  10007

MemArena	*hasharena;

typedef struct nodeT {
	int				val;
	struct nodeT	*next;
} nodeT;

typedef struct bucketT {
	int		cnt, idx;
	nodeT	*nodes;
} bucketT;

struct hashTable_t {
	int		size;
	int		cnt_item; 	// number of items
	int		cnt_bucket;	// number of non-empty buckets
	int		key_size;	// size of hash key (number of integers)
	hashT	rndVec;
	bucketT	**buckets;
};

void
hashTable_init(void)
{
	hasharena = mkmemarena(NULL, NULL, NULL, 0);
}

void
hashT_print(hashT key, int key_size, const char *tag, FILE *fp)
{
	int		i;

	fprintf(fp, "[%d", key[0]);
	for (i=1; i<key_size; i++)
		fprintf(fp, ",%d", key[i]);
	fprintf(fp, "]%s", tag);
}

nodeT *
nodeT_new(int val)
{
	nodeT	*ret = (nodeT *)memarenamalloc(hasharena, sizeof(nodeT));

	if (ret) {
		ret->val = val;
		ret->next = NULL;
	}

	return ret;
}

bucketT *
bucketT_new(int idx)
{
	bucketT	*ret = (bucketT *)memarenamalloc(hasharena, sizeof(bucketT));

	if (ret) {
		ret->idx = idx;
		ret->cnt = 0;
		ret->nodes = NULL;
	}

	return ret;
}

void
bucketT_insert(bucketT *bucket, int val)
{
	nodeT	*node = nodeT_new(val);
	if (bucket->cnt > 0)
		node->next = bucket->nodes;

	bucket->nodes = node;
	bucket->cnt++;
}

void
bucketT_free(bucketT *bucket)
{
#ifdef NOTDEF  // arena takes care of freeing the bucket
	nodeT	*p, *q;

	if (bucket) {
		p = bucket->nodes;
		while (p != NULL) {
			q = p->next;
			free(p);
			p = q;
		}

		free(bucket);
	}
#endif
}

int
bucketT_dump(bucketT *bucket, CASS_FILE *out)
{
	int		ret;
	nodeT	*p = bucket->nodes;

	ret = cass_write_uint32(&(bucket->cnt), 1, out);
	assert(ret == 1);

	ret = 0;
	while (p != NULL) {
		ret += cass_write_uint32(&(p->val), 1, out);
		p = p->next;
	}

	assert(ret == bucket->cnt);
	return 0;
}

bucketT *
bucketT_load(int idx, CASS_FILE *in)
{
	int			i, ret, cnt, val;
	bucketT		*bucket = bucketT_new(idx);

	ret = cass_read_uint32(&cnt, 1, in);
	assert(ret == 1);

	ret = 0;
	for (i=0; i<cnt; i++) {
		ret += cass_read_uint32(&val, 1, in);
		bucketT_insert(bucket, val);
	}

	assert(ret == cnt);

	return bucket;
}

hashTable_t *
hashTable_new(int key_size)
{
	int				i;
	hashTable_t		*ret = (hashTable_t *)malloc(sizeof(hashTable_t));

	const gsl_rng_type	*T;
	gsl_rng				*r;

	ret->key_size = key_size;
	ret->rndVec = (int *)calloc(key_size, sizeof(int));

	gsl_rng_env_setup();
	T = gsl_rng_default;
	r = gsl_rng_alloc(T);

	for (i=0; i<key_size; i++)
		ret->rndVec[i] = (int)(gsl_rng_uniform(r) * MAX_INT);

	gsl_rng_free(r);

	ret->size = DEFAULT_PRIME;
	ret->buckets = (bucketT **)calloc(ret->size, sizeof(bucketT *));
	for (i=0; i<ret->size; i++)
		ret->buckets[i] = NULL;

	ret->cnt_item = 0;
	ret->cnt_bucket = 0;

	return ret;
}

int
hashTable_free(hashTable_t *hash_table)
{
	int		i;

	if (hash_table != NULL) {
		for (i=0; i<hash_table->size; i++) {
			if (hash_table->buckets[i] != NULL)
				bucketT_free(hash_table->buckets[i]);
		}

		free(hash_table->buckets);
		free(hash_table->rndVec);
		free(hash_table);
	}

	return 0;
}

int
hashTable_dump(hashTable_t *hash_table, CASS_FILE *out)
{
	int			i, ret;

	ret = cass_write_uint32(&(hash_table->key_size), 1, out);
	ret += cass_write_uint32(&(hash_table->cnt_item), 1, out);
	ret += cass_write_uint32(&(hash_table->cnt_bucket), 1, out);
	assert(ret == 3);

	ret = cass_write_uint32(hash_table->rndVec,
		hash_table->key_size, out);
	assert(ret == hash_table->key_size);

	for (i=0; i<hash_table->size; i++)
		if (hash_table->buckets[i] != NULL) {
			ret = cass_write_uint32(&i, 1, out);
			assert(ret == 1);

			ret = bucketT_dump(hash_table->buckets[i], out);
			if (ret != 0)
				return ret;
		}

	return 0;
}

hashTable_t *
hashTable_load(CASS_FILE *in)
{
	int				i, x, ret;
	int				 key_size, cnt_item, cnt_bucket;
	hashTable_t		*hash_table = type_calloc(hashTable_t, 1);

	ret = cass_read_uint32(&key_size, 1, in);
	ret += cass_read_uint32(&cnt_item, 1, in);
	ret += cass_read_uint32(&cnt_bucket, 1, in);
	assert(ret == 3);

	hash_table->size = DEFAULT_PRIME;
	hash_table->key_size = key_size;
	hash_table->cnt_item = cnt_item;
	hash_table->cnt_bucket = cnt_bucket;

	hash_table->rndVec = (int *)calloc(key_size, sizeof(int));
	ret = cass_read_uint32(hash_table->rndVec, key_size, in);
	assert(ret == key_size);

	hash_table->buckets = (bucketT **)calloc(hash_table->size,
			sizeof(bucketT *));
	for (i=0; i<cnt_bucket; i++) {
		ret = cass_read_uint32(&x, 1, in);
		assert(ret == 1);

		hash_table->buckets[x] = bucketT_load(x, in);
		if (hash_table->buckets[x] == NULL)
			return NULL;
	}

	return hash_table;
}

int
hashTable_hash(hashTable_t *table, hashT key)
{
	int		i, k, sum = 0;

	for (i=0; i<table->key_size; i++) {
		//printf("%d\t", key[i]);
		sum += table->rndVec[i] * key[i];
	}

	k = abs(sum % (table->size));
	//printf("%d\n", k);

	return k;
}

void
hashTable_insert(hashTable_t *table, hashT key, int val)
{
	int			idx;
	bucketT		*bucket;

	idx = hashTable_hash(table, key);
	bucket = table->buckets[idx];
	if (bucket == NULL) {
		bucket = bucketT_new(idx);
		table->buckets[idx] = bucket;
		table->cnt_bucket++;
	}
	bucketT_insert(bucket, val);
	table->cnt_item++;
}

bucketT *
hashTable_get_bucket(hashTable_t *table, hashT key)
{
	int		idx = hashTable_hash(table, key);

	return table->buckets[idx];
}

int
hashTable_search(hashTable_t *table, bitmap_t *map, hashT key)
{
	int			i;
	nodeT		*p;
	int			idx = hashTable_hash(table, key);
	bucketT		*bucket = table->buckets[idx];

	if (bucket) {
		p = bucket->nodes;
		for (i=0; i<bucket->cnt; i++) {
			bitmap_insert(map, p->val);
			p = p->next;
		}
	}

	return 0;
}