/**
 *
 * @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__ */


/* 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; */
/* } */


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

Copyright 2011-2019, Javier Burguete Tolosa.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

	1. Redistributions of source code must retain the above copyright notice,
		this list of conditions and the following disclaimer.

	2. Redistributions in binary form must reproduce the above copyright notice,
		this list of conditions and the following disclaimer in the
		documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY Javier Burguete Tolosa ``AS IS'' AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL Javier Burguete Tolosa OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

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

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

/**
 * Function to obtain the coefficients of a 2 steps 2nd order Runge-Kutta
 * method.
 */
int
rk_tb_2_2 (Optimize * optimize) ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_tb_2_2: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t2 (tb) = 1.L;
  t1 (tb) = r[0];
  b21 (tb) = 0.5L / t1 (tb);
  rk_b_2 (tb);
#if DEBUG_RK_2_2
  rk_print_tb (optimize, "rk_tb_2_2", stderr);
  fprintf (stderr, "rk_tb_2_2: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 2 steps 2nd order, 3rd order in
 * equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_2_2t (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_tb_2_2t: start\n");
#endif
  tb = optimize->coefficient;
  t2 (tb) = 1.L;
  t1 (tb) = 2.L / 3.L;
  b21 (tb) = 0.5L / t1 (tb);
  rk_b_2 (tb);
#if DEBUG_RK_2_2
  rk_print_tb (optimize, "rk_tb_2_2t", stderr);
  fprintf (stderr, "rk_tb_2_2t: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 2 steps 1st-2nd order Runge-Kutta
 * pair.
 */
int
rk_tb_2_2p (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_tb_2_2p: start\n");
#endif
  if (!rk_tb_2_2 (optimize))
    return 0;
  tb = optimize->coefficient;
  e20 (tb) = 1.L;
#if DEBUG_RK_2_2
  rk_print_tb (optimize, "rk_tb_2_2p", stderr);
  fprintf (stderr, "rk_tb_2_2p: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 2 steps 1st-2nd order, 1st-3rd order
 * in equations depending only on time, Runge-Kutta pair.
 */
int
rk_tb_2_2tp (Optimize * optimize)       ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_tb_2_2tp: start\n");
#endif
  if (!rk_tb_2_2t (optimize))
    return 0;
  tb = optimize->coefficient;
  e20 (tb) = 1.L;
#if DEBUG_RK_2_2
  rk_print_tb (optimize, "rk_tb_2_2tp", stderr);
  fprintf (stderr, "rk_tb_2_2tp: end\n");
#endif
  return 1;
}

/**
 * Function to calculate the objective function of a 2 steps 2nd order
 * Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_2_2 (RK * rk)   ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_objective_tb_2_2: start\n");
#endif
  tb = rk->tb->coefficient;
  if (b20 (tb) < 0.L)
    {
      o = 40.L - b20 (tb);
      goto end;
    }
  o = 30.L + fmaxl (1.L, t1 (tb));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_objective_tb_2_2: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_2_2: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 2 steps 2nd order, 3rd
 * order in equations depending only in time, Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_2_2t (RK * rk)  ///< RK struct.
{
  long double o;
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_objective_tb_2_2t: start\n");
#endif
  o = 31.L;
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
#if DEBUG_RK_2_2
  fprintf (stderr, "rk_objective_tb_2_2t: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_2_2t: end\n");
#endif
  return o;
}


#ifndef libCV_ellipse
#define libCV_ellipse

#include <objc/Object.h>
#include "cvtypes.h"
#include "util.h"
#include "Array.h"
#include "math.h"
#include <gsl/gsl_math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_eigen.h>

@interface Ellipse : Object {

	f64 x_axis;
	f64 y_axis;
	f64 x_center;
	f64 y_center;
	f64 rotation;

	f64 general[6];

	f64 bounds[4];

}

+(id) newAtX:(f64)xc andY:(f64)yc withXAxis:(f64)xa andYAxis:(f64)ya rotatedBy:(f64)phi;
+(id) newWithA:(f64)a b:(f64)b c:(f64)c d:(f64)d e:(f64)e f:(f64)f;

+(id) newFromPoints:(ArrayP)points;

-(void) drawInArray:(ArrayP)array;

-(void) toGeneralConic;
-(void) toGeneralEllipse;
-(void) findBounds;

// Get and set general ellipse parameters

-(f64) x_axis;
-(f64) y_axis;
-(f64) major_axis;
-(f64) minor_axis;
-(f64) rotation;
-(f64) majorRotation;
-(f64) minorRotation;
-(f64) x_center;
-(f64) y_center;

-(void) setX_axis:(f64)newx;
-(void) setY_axis:(f64)newy;
-(void) setX_center:(f64)newx;
-(void) setY_center:(f64)newy;
-(void) setRotation:(f64)newr;
-(void) setX_axis:(f64)xa y_axis:(f64)ya x_center:(f64)xc y_center:(f64)yc rotation:(f64)r;

// Get and set general conic parameters

-(f64) A;
-(f64) B;
-(f64) C;
-(f64) D;
-(f64) E;
-(f64) F;

-(f64 *) general;

-(void) setA:(f64)A;
-(void) setB:(f64)B;
-(void) setC:(f64)C;
-(void) setD:(f64)D;
-(void) setE:(f64)E;
-(void) setF:(f64)F;
-(void) setGeneralA:(f64)A B:(f64)B C:(f64)C D:(f64)D E:(f64)E F:(f64)F;

-(void) printInfoTo:(FILE *)fp;

-(u08) isValid;
-(id) free;

@end

enum ellipseindices {
	AX,
	BX,
	CX,
	DX,
	EX,
	FX,
};

enum ellipsebounds {
	XHI,
	XLO,
	YHI,
	YLO,
};

typedef Ellipse * EllipseP;

void originCenteredLSQEllipse( f64 x_values[], f64 y_values[], u64 number_of_points, f64 ellipse[] );
void generateBoundEllipses( f64 e[], f64 b1[], f64 b2[], f64 treshold );
f64 ellipseCircumference( f64 e[] );
EllipseP ellipseRANSAC( ArrayP edges, f64 fit_treshold, f64 min_percent_inliers, f64 certainty_probability, u64 max_iterations );

#endif