#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <limits.h>
#include <omp.h>
#include <stdbool.h>
#include <getopt.h>
#include <math.h>
#include <mpi.h>
#include "parmt_config.h"
#ifdef PARMT_USE_INTEL
#include <ippcore.h>
#include <mkl.h>
#include <mkl_cblas.h>
#else
#include <cblas.h>
#endif
#include "compearth.h"
#include "parmt_mtsearch.h"
#include "parmt_postProcess.h"
#include "parmt_utils.h"
#include "iscl/array/array.h"
#include "iscl/memory/memory.h"

#define PROGRAM_NAME "parmt"
static int parseArguments(int argc, char *argv[],
                          const int nprocs,
                          int *npInObsGroups,
                          int *npInLocGroups,
                          int *npInMTGroups,
                          char iniFile[PATH_MAX]);
static void printUsage(void);
int parmt_freeData(struct parmtData_struct *data);
int parmt_freeLocalMTs(struct localMT_struct *mts);
/*!
 *
 */
int main(int argc, char *argv[])
{
    char iniFile[PATH_MAX];
    double *betas, *h, *kappas, *gammas, *M0s, *sigmas, *thetas, *u, *v;
    double *deps, *luneMPDF, *phi, t0, t1, du, dv, dh, dk, ds;
    int64_t ngridSearch;
    double hLower, hUpper, uLower, uUpper, vLower, vUpper, xnorm;
    int *lags, i, ierr, iobs, myid, npInLocGroups, nmt, npInMTGroups,
        npInObsGroups, nprocs, provided;
    int ix, iy, k;
    MPI_Comm mtComm, locComm, obsComm;
    bool linMTComm, linLocComm, linObsComm;
    const int master = 0;
    const int ldm = 8;
struct parmtGeneralParms_struct parms;
struct parmtData_struct data;
struct parmtMtSearchParms_struct mtsearch;
    struct localMT_struct mtloc;
    //------------------------------------------------------------------------//
    //
    // initialize MPI, ISCL, and handle threading issues
    MPI_Init_thread(&argc, &argv, MPI_THREAD_FUNNELED, &provided);
    MPI_Comm_rank(MPI_COMM_WORLD, &myid);
    MPI_Comm_size(MPI_COMM_WORLD, &nprocs);
    iscl_init();
#ifdef PARMT_USE_INTEL
    //omp_set_num_threads(2);
    mkl_set_num_threads(1);
    ippSetNumThreads(1);
#endif
    // initialize
    phi = NULL;
    M0s = NULL;
    betas = NULL;
    gammas = NULL;
    sigmas = NULL;
    kappas = NULL;
    thetas = NULL;
    h = NULL;
    u = NULL;
    v = NULL;
    lags = NULL;
    memset(&parms, 0, sizeof(struct parmtGeneralParms_struct));
    memset(&data, 0, sizeof(struct parmtData_struct));
    memset(&mtsearch, 0, sizeof(struct parmtMtSearchParms_struct));
    memset(&mtloc, 0, sizeof(struct localMT_struct));
    if (myid == master)
    {
        ierr = parseArguments(argc, argv, nprocs,
                              &npInObsGroups, &npInLocGroups, &npInMTGroups,
                              iniFile);
        if (ierr != 0){goto INIT_ERROR;}
        // read the ini file
        printf("%s: Parsing ini file %s...\n", PROGRAM_NAME, iniFile);
        ierr = 0;
        ierr += parmt_utils_readGeneralParms(iniFile, &parms);
        strcpy(parms.programName, PROGRAM_NAME);
        parms.programID = PARMT_ID;
        ierr += parmt_utils_readMtSearch(iniFile, &mtsearch);
        if (ierr != 0)
        {
            printf("%s: Error reading ini file\n", PROGRAM_NAME);
            goto INIT_ERROR;
        }
        printf("%s: Process management:\n", PROGRAM_NAME);
        printf("        Number of processes in observation groups %d\n",
               npInObsGroups);
        printf("        Number of processes in location groups %d\n",
               npInLocGroups);
        printf("        Number of processes in moment tensor groups %d\n",
               npInMTGroups);
//        npInObsGroups = 1;
//        npInLocGroups = 1;
//        npInMTGroups = nprocs;
printf("verify greens fns are correct\n");
/*
        double pAxis[3], nAxis[3], tAxis[3], beta, gamma, u,v;
        u= 3.0*M_PI/8.0;
        v=-1.0/9.0;
        compearth_u2beta(1, 10, 2, &u, 1.e-10, &beta);
        compearth_v2gamma(1, &v, &gamma);
        postprocess_tt2tnp(beta, gamma, 4.0*M_PI/5.0, -M_PI/2.0, 0.723,
                           pAxis, nAxis, tAxis);
        int nxp = 51;
        double *xw1 = memory_calloc64f(nxp*nxp);
        double *yw1 = memory_calloc64f(nxp*nxp);
        int8_t *pn1 = (int8_t *) calloc(nxp*nxp, sizeof(int8_t));
        const double xc = 1.5; const double yc = 1.5; const double rad = 1.0;
        // Compute the corresponding moment tensor
        double M0loc = 1.0/sqrt(2.0), Muse[6], lamUse[9], Uuse[9];
        double gammaDeg = gamma*180.0/M_PI;
        double deltaDeg = (M_PI_2 - beta)*180.0/M_PI;
        double kappaDeg = (4.0*M_PI/5.0)*180.0/M_PI;
        double sigmaDeg = (-M_PI_2)*180.0/M_PI;
        double thetaDeg = 0.723*180.0/M_PI;
        compearth_tt2cmt(gammaDeg, deltaDeg, M0loc,
                         kappaDeg, thetaDeg,
                         sigmaDeg,
                         Muse, lamUse, Uuse);
        printf("mt = [%f,%f,%f,%f,%f,%f]\n", Muse[0], Muse[1], Muse[2], Muse[3], Muse[4], Muse[5]);
printf("drawing\n");
for (int i=0; i<10000; i++)
{
        postprocess_tnp2beachballPolarity(nxp, xc, yc, rad,
                                          pAxis, nAxis, tAxis,
                                          xw1, yw1, pn1);
}
        FILE *fwork = fopen("depmag/beachball.txt", "w");
        for (int iy=0; iy<nxp; iy++)
        {
            for (int ix=0; ix<nxp; ix++)
            {
                int k = iy*nxp + ix;
                fprintf(fwork, "%e %e %d\n", xw1[k], yw1[k], pn1[k]);
            }
            fprintf(fwork, "\n");
        }
        fclose(fwork);
        memory_free64f(&xw1);
        memory_free64f(&yw1);
        free(pn1);
printf("back\n");
//getchar();
*/
    }
INIT_ERROR:;
    MPI_Bcast(&ierr, 1, MPI_INT, master, MPI_COMM_WORLD);
    if (ierr != 0){goto FINISH;}
    MPI_Bcast(&npInObsGroups, 1, MPI_INT, master, MPI_COMM_WORLD);
    MPI_Bcast(&npInLocGroups, 1, MPI_INT, master, MPI_COMM_WORLD);
    MPI_Bcast(&npInMTGroups,  1, MPI_INT, master, MPI_COMM_WORLD);
    // split the communicators
    ierr = parmt_splitComm(MPI_COMM_WORLD,
                           npInObsGroups, npInLocGroups, npInMTGroups,
                           &linObsComm, &obsComm,
                           &linLocComm, &locComm,
                           &linMTComm,  &mtComm);
    if (ierr != 0)
    {
        printf("%s: Fatal error splitting communicators - have a nice day\n",
               PROGRAM_NAME);
        MPI_Abort(MPI_COMM_WORLD, 30);
    }
    // load the observations and greens functions then broadcast
    if (myid == master)
    {
        printf("%s: Loading data...\n", PROGRAM_NAME);
        memset(&data, 0, sizeof(struct parmtData_struct));
        ierr = utils_dataArchive_readAllWaveforms(parms.dataFile, &data);
        data.est = (struct sacData_struct *)
                   calloc((size_t) data.nobs, sizeof(struct sacData_struct));
        ngridSearch = (int64_t) (mtsearch.nm*mtsearch.nb*mtsearch.ng)
                     *(int64_t) (mtsearch.nk*mtsearch.ns*mtsearch.nt)
                     *(int64_t) data.nlocs;
        if (ngridSearch > INT_MAX)
        {
            printf("%s: Error - grid search too big - integer overflow",
                   PROGRAM_NAME);
            ierr = 1;
        }
    }
    MPI_Bcast(&ierr, 1, MPI_INT, master, MPI_COMM_WORLD);
    if (ierr != 0){goto FINISH;}
    // Send the inputs to everyone
    if (myid == master)
    {
        printf("%s: Broadcasting parameters...\n", PROGRAM_NAME);
    }
    ierr = parmt_broadcast_mtSearchParms(&mtsearch, master, MPI_COMM_WORLD);
    if (ierr != 0)
    {
        printf("%s: Fatal error broadcasting parameters\n", PROGRAM_NAME);
        MPI_Abort(MPI_COMM_WORLD, 30);
    }
    ierr = parmt_broadcast_generalParms(&parms, master, MPI_COMM_WORLD);
    if (ierr != 0)
    {
        printf("%s: Fatal error broadcasting general parameters\n",
               PROGRAM_NAME);
        MPI_Abort(MPI_COMM_WORLD, 30);
    }
    // Send the waveforms to everyone
    ierr = parmt_broadcast_data(&data, master, MPI_COMM_WORLD);
    if (ierr != 0)
    {
        printf("%s: Fatal error broadcasting waveforms\n", PROGRAM_NAME);
        MPI_Abort(MPI_COMM_WORLD, 30);
    }
    // Discretize the moment tensor space in (u, v, h) space
    if (myid == master)
    {
        printf("%s: Discretizing MT space...\n", PROGRAM_NAME);
    }
    // Discretize the moment tensor space in (u, v, h) space
    ierr = parmt_discretizeCells64f(
               mtsearch.nb, mtsearch.betaLower,  mtsearch.betaUpper,
               mtsearch.ng, mtsearch.gammaLower, mtsearch.gammaUpper,
               mtsearch.nk, mtsearch.kappaLower, mtsearch.kappaUpper,
               mtsearch.ns, mtsearch.sigmaLower, mtsearch.sigmaUpper,
               mtsearch.nt, mtsearch.thetaLower, mtsearch.thetaUpper,
               mtsearch.nm, mtsearch.m0Lower,    mtsearch.m0Upper,
               mtsearch.luseLog,
               &betas,  &gammas, &kappas,
               &sigmas, &thetas, &M0s);
    if (ierr != 0)
    {
        printf("%s: Error discretizing MT space\n", PROGRAM_NAME);
        MPI_Abort(MPI_COMM_WORLD, 30);
    }
/*
    compearth_beta2u(1, &mtsearch.betaLower, &uLower);
    compearth_beta2u(1, &mtsearch.betaUpper, &uUpper);
    compearth_gamma2v(1, &mtsearch.gammaLower, &vLower);
    compearth_gamma2v(1, &mtsearch.gammaUpper, &vUpper);
    compearth_theta2h(1, &mtsearch.thetaLower, &hLower);
    compearth_theta2h(1, &mtsearch.thetaUpper, &hUpper);
    du = (uUpper - uLower)/(double) mtsearch.nb;
    dv = (vUpper - vLower)/(double) mtsearch.ng;
    dh =-(hUpper - hLower)/(double) mtsearch.nt;
    dk = (mtsearch.kappaUpper - mtsearch.kappaLower)/(double) mtsearch.nk;
    ds = (mtsearch.sigmaUpper - mtsearch.sigmaLower)/(double) mtsearch.ns;
//if (myid == master){printf("%f %f %f\n", du, dv, dh);}
    u = array_linspace64f(uLower+du/2, uUpper-du/2, mtsearch.nb, &ierr);
    v = array_linspace64f(vLower+dv/2, vUpper-dv/2, mtsearch.ng, &ierr);
    h = array_linspace64f(hLower-dh/2, hUpper+dh/2, mtsearch.nt, &ierr);
    betas  = memory_calloc64f(mtsearch.nb);
    gammas = memory_calloc64f(mtsearch.ng);
    thetas = memory_calloc64f(mtsearch.nt);
    compearth_u2beta(mtsearch.nb, 20, 2, u, 1.e-6, betas);
    compearth_v2gamma(mtsearch.ng, v, gammas);
    compearth_h2theta(mtsearch.nt, h, thetas);
    M0s    = array_linspace64f(mtsearch.m0Lower,    mtsearch.m0Upper,
                               mtsearch.nm, &ierr);
    kappas = array_linspace64f(mtsearch.kappaLower+dk/2, mtsearch.kappaUpper-dk/2,
                               mtsearch.nk, &ierr);
    sigmas = array_linspace64f(mtsearch.sigmaLower+ds/2, mtsearch.sigmaUpper-ds/2,
                               mtsearch.ns, &ierr);
*/
//if (myid != master){for (int i=0; i<mtsearch.nb; i++){printf("%f %f\n", u[i],betas[i]);}}
    // avoid an annoying warning
    for (i=0; i<mtsearch.ns; i++)
    {
        if (fabs(sigmas[i]) < 1.e-7){sigmas[i] = 1.e-7;}
    }
    nmt = mtsearch.ng*mtsearch.nb*mtsearch.nk
         *mtsearch.ns*mtsearch.nt*mtsearch.nm;
    if (myid == master)
    {
        printf("%s: Discretizing moment tensor...\n", PROGRAM_NAME);
    }
    t0 = MPI_Wtime();
    ierr = parmt_discretizeMT64f_MPI(mtComm,
                                     mtsearch.ng, gammas,
                                     mtsearch.nb, betas,
                                     mtsearch.nm, M0s,
                                     mtsearch.nk, kappas,
                                     mtsearch.nt, thetas,
                                     mtsearch.ns, sigmas,
                                     ldm, &mtloc);
    if (ierr != 0)
    {
        printf("%s: Error discretizing moment tensor on process %d\n",
               PROGRAM_NAME, myid);
        MPI_Abort(MPI_COMM_WORLD, 30);
    }
    MPI_Barrier(MPI_COMM_WORLD);
    t1 = MPI_Wtime();
    if (myid == master)
    {
        printf("%s: Discretization time %6.2f (seconds)\n",
               PROGRAM_NAME, t1 - t0);
    }
    // Initialize the  output file
    t0 = MPI_Wtime();
    if (myid == master)
    {
        printf("%s: Initializing output file...\n", PROGRAM_NAME);
        deps = memory_calloc64f(data.nlocs);
        for (i=0; i<data.nlocs; i++)
        {
            deps[i] = data.sacGxx[i].header.evdp;
        }
        ierr = parmt_io_createObjfnArchive64f(//parms.resultsDir, parms.projnm,
                                              //parms.resultsFileSuffix,
                                              PROGRAM_NAME, parms.parmtArchive,
                                              data.nobs,
                                              data.nlocs, deps,
                                              mtsearch.nm, M0s,
                                              mtsearch.nb, betas,
                                              mtsearch.ng, gammas,
                                              mtsearch.nk, kappas,
                                              mtsearch.ns, sigmas,
                                              mtsearch.nt, thetas);
        memory_free64f(&deps);
    }
    MPI_Barrier(MPI_COMM_WORLD);
    // Compute the objective functions
    t0 = MPI_Wtime();
    phi = NULL;
    lags = NULL;
/*
    nlags = 0;
double lagTime;
bool ldefault, lwantLags;
int iobs = 0;
int nlags = 0;
    phi = NULL;
    lags = NULL;
    nlags = 0;
    if (parms.lwantLags)
    {
        //lagTime = data.data[iobs].header.user0;
        //if (lagTime < 0.0){lagTime = parms.defaultMaxLagTime;}
        lagTime = parmt_utils_getLagTime(data.data[iobs],
                                         parms.defaultMaxLagTime,
                                         &ldefault);
        nlags = (int) (lagTime/data.data[iobs].header.delta + 0.5);
    }
    lags = NULL;
    lwantLags = false;
    if (nlags > 0){lwantLags = true;}
*/
    if (myid == master)
    {
        phi = memory_calloc64f(data.nlocs*mtloc.nmtAll);
    }
    else
    {
        phi = memory_calloc64f(1);
    }
    // Perform the grid search
    MPI_Wtime();
    ierr = parmt_obsSearch64f(MPI_COMM_WORLD,
                              obsComm, locComm,
                              linObsComm, linLocComm,
                              parms, data, mtloc, phi);
    if (ierr != 0)
    {
        printf("%s: Error calling obsSearch64f\n", PROGRAM_NAME);
        MPI_Abort(MPI_COMM_WORLD, 30);
    }
    MPI_Barrier(MPI_COMM_WORLD);
    if (myid == master)
    {
        MPI_Wtime();
        printf("%s: Objective function computation time: %f\n",
               PROGRAM_NAME, MPI_Wtime() - t0);
    }
    if (ierr != 0)
    {
        printf("%s: Error calling locSearchXC64f\n", PROGRAM_NAME);
        MPI_Abort(MPI_COMM_WORLD, 30);
    }
    if (myid == master)
    {
        printf("%s: Writing results...\n", PROGRAM_NAME);
        ierr = parmt_io_writeObjectiveFunction64f(
                   //parms.resultsDir, parms.projnm, parms.resultsFileSuffix,
                   parms.parmtArchive, nmt, phi);
        if (ierr != 0)
        {
            printf("%s: Error writing objective function\n", PROGRAM_NAME);
            goto FINISH;
        }
        int jloc, jm, jb, jg, jk, js, jt;
        int imtopt = array_argmax64f(data.nlocs*mtloc.nmtAll, phi, &ierr);
        marginal_getOptimum(data.nlocs, mtsearch.nm, mtsearch.nb,
                            mtsearch.ng, mtsearch.nk, mtsearch.ns,
                            mtsearch.nt, phi,
                            &jloc, &jm, &jb, &jg,
                            &jk, &js, &jt);
        printf("%d %f %f %f %f %f %f\n", jloc,
               gammas[jg]*180.0/M_PI, 90.0-betas[jb]*180.0/M_PI, 1.0,
               kappas[jk]*180.0/M_PI, thetas[jt]*180.0/M_PI,
               sigmas[js]*180.0/M_PI);
        double Muse[6], Mned[6], lam[3], U[9];
        printf("phi opt: %d %e\n", imtopt, phi[imtopt]);
        printf("%d %d %d %d %d %d %d\n", jloc, jg, jb, jm, jk, jt, js);
        double gammaOpt = gammas[jg]*180.0/M_PI;
        double deltaOpt = 90.0-betas[jb]*180.0/M_PI;
        double kappaOpt = kappas[jk]*180.0/M_PI;
        double thetaOpt = thetas[jt]*180.0/M_PI;
        double sigmaOpt = sigmas[js]*180.0/M_PI;
        compearth_TT2CMT(1, &gammaOpt,
                         &deltaOpt,
                         &M0s[jm],
                         &kappaOpt,
                         &thetaOpt,
                         &sigmaOpt,
                         Muse, lam, U);
/*
        compearth_tt2cmt(gammas[jg]*180.0/M_PI,
                         90.0-betas[jb]*180.0/M_PI,
                         M0s[jm],
                         kappas[jk]*180.0/M_PI,
                         thetas[jt]*180.0/M_PI,
                         sigmas[js]*180.0/M_PI,
                         Muse, lam, U);
*/
        parmt_discretizeMT64f(1, &gammas[jg],
                              1, &betas[jb],
                              1, &M0s[jm],
                              1, &kappas[jk],
                              1, &thetas[jt],
                              1, &sigmas[js],
                              6, 1, Mned);
        //printf("%e %e %e\n", phi[0], phi[1000], phi[500000]);
        printf("mtUSE =[%f,%f,%f,%f,%f,%f]\n",
               Muse[0], Muse[1], Muse[2], Muse[3], Muse[4], Muse[5]);
        printf("mtNED =[%f,%f,%f,%f,%f,%f]\n",
               Mned[0], Mned[1], Mned[2], Mned[3], Mned[4], Mned[5]);
    }
FINISH:;
    if (linObsComm){MPI_Comm_free(&obsComm);}
    if (linLocComm){MPI_Comm_free(&locComm);}
    if (linMTComm){MPI_Comm_free(&mtComm);}
    memory_free64f(&M0s);
    memory_free64f(&h);
    memory_free64f(&u);
    memory_free64f(&v);
    memory_free64f(&betas);
    memory_free64f(&gammas);
    memory_free64f(&kappas);
    memory_free64f(&sigmas);
    memory_free64f(&thetas);
    memory_free64f(&phi);
    memory_free32i(&lags);
    parmt_freeData(&data);
    parmt_freeLocalMTs(&mtloc);
    iscl_finalize();
    MPI_Finalize();
    return EXIT_SUCCESS;
}

static int parseArguments(int argc, char *argv[],
                          const int nprocs,
                          int *npInObsGroups,
                          int *npInLocGroups,
                          int *npInMTGroups,
                          char iniFile[PATH_MAX])
{
    bool linFile;
    int prod;
    linFile = false;
    *npInObsGroups = 1;
    *npInLocGroups = 1;
    *npInMTGroups = 1;
    memset(iniFile, 0, PATH_MAX*sizeof(char));
    while (true)
    {
        static struct option longOptions[] =
        {
            {"help", no_argument, 0, '?'},
            {"help", no_argument, 0, 'h'},
            {"ini_file", required_argument, 0, 'i'},
            {"obsGroupSize", required_argument, 0, 'w'},
            {"mtGroupSize",  required_argument, 0, 'm'},
            {"locGroupSize", required_argument, 0, 'l'},
            {0, 0, 0, 0}
        };
        int c, optionIndex;
        c = getopt_long(argc, argv, "?hi:w:m:l:",
                        longOptions, &optionIndex);
        if (c ==-1){break;}
        if (c == 'i')
        {
            strcpy(iniFile, (const char *) optarg);
            linFile = true;
        }
        else if (c == 'w' && nprocs > 1)
        {
            *npInObsGroups = atoi(optarg);
        }
        else if (c == 'l' && nprocs > 1)
        {
            *npInLocGroups = atoi(optarg);
        }
        else if (c == 'm' && nprocs > 1)
        {
            *npInMTGroups = atoi(optarg);
        }
        else if (c == 'h' || c == '?')
        {
            printUsage();
            return -2;
        }
        else
        {
            printf("%s: Unknown options: %s\n",
                   PROGRAM_NAME, argv[optionIndex]);
        }
    }
    if (!linFile)
    {
        printf("%s: Error must specify ini file\n\n", PROGRAM_NAME);
        printUsage();
        return -1;
    }
    prod = (*npInObsGroups)*(*npInLocGroups)*(*npInMTGroups);
    if (prod != nprocs)
    {
        if (prod != 1)
        {
            printf("%s: Invalid process layout - defaulting to (%d,%d,%d)\n",
                   PROGRAM_NAME, 1, 1, nprocs);
        }
        *npInObsGroups = 1;
        *npInLocGroups = 1;
        *npInMTGroups = nprocs;
    }
    return 0;
}

static void printUsage(void)
{
    printf("Usage:\n   parmt -i input_file\n\n");
    printf("Required arguments:\n");
    printf("   -i input_file specifies the initialization file\n");
    printf("\n");
    printf("Optional arguments:\n");
    printf("   -m number of processes in moment-tensor groups\n");
    printf("   -w number of processes in waveform (observation) groups\n");
    printf("   -l number of processes in location groups\n");
    printf("   -h displays this message\n");
    printf("   Note if m*w*l must equal the number of processes\n");
    printf("   If they are not set the program will set -m=nprocs\n");
    return;
}

int parmt_freeData(struct parmtData_struct *data)
{
    int i;
    if (data->nobs > 0 && data->nlocs > 0 &&
        data->sacGxx != NULL && data->sacGyy != NULL && data->sacGzz != NULL &&
        data->sacGxy != NULL && data->sacGxz != NULL && data->sacGyz != NULL)
    {
        for (i=0; i<data->nobs*data->nlocs; i++)
        {
            sacio_freeData(&data->sacGxx[i]);
            sacio_freeData(&data->sacGyy[i]);
            sacio_freeData(&data->sacGzz[i]);
            sacio_freeData(&data->sacGxy[i]);
            sacio_freeData(&data->sacGxz[i]);
            sacio_freeData(&data->sacGyz[i]);
        }
        free(data->sacGxx);
        free(data->sacGyy);
        free(data->sacGzz);
        free(data->sacGxy);
        free(data->sacGxz);
        free(data->sacGyz);
    }
    if (data->nobs > 0 && data->data != NULL)
    {
        for (i=0; i<data->nobs; i++)
        {
            sacio_freeData(&data->data[i]);
        }
        free(data->data);
    }
    if (data->nobs > 0 && data->est != NULL)
    {
        free(data->est);
    }
    memset(data, 0, sizeof(struct parmtData_struct));
    return 0;
}

int parmt_freeLocalMTs(struct localMT_struct *mtloc)
{
    memory_free64f(&mtloc->mts);
    //memory_free32i(&mtloc->l2g); // TODO remove
    memory_free32i(&mtloc->offset);
    memset(mtloc, 0, sizeof(struct localMT_struct));
    return 0;
}


#include <string.h>
#include <stdint.h>
#include <mpi.h>

#include <gsl/gsl_rng.h>
#include <fastpm/libfastpm.h>
#include <fastpm/logging.h>
#include <fastpm/string.h>
#include <fastpm/transfer.h>
#include "chealpix.h"
#include <mpsort.h>

#include "pmpfft.h"
#include "pmghosts.h"

static double RNDTABLE[8192];

gsl_rng * random_generator;
void fastpm_utils_init_randtable() {
    random_generator = gsl_rng_alloc(gsl_rng_ranlxd1);
    int rank;
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
    gsl_rng_set(random_generator, 37 * (rank + 1) + 1);  /* start-up seed */
    int i;
    for(i = 0; i < 8192; i ++) {
        RNDTABLE[i] = gsl_rng_uniform(random_generator);
    }
    //gsl_rng_free(random_generator);
}

double
fastpm_utils_get_random(uint64_t id)
{
    return gsl_rng_uniform(random_generator);
    uint64_t ind = 0;
    ind = id;

    while(id != 0) {
        ind += id;
        id /= 8192;
    }
    ind %= 8192;
    return RNDTABLE[ind];
}

void
fastpm_utils_dump(PM * pm , const char * filename, FastPMFloat *data)
{
    char fn1[1024];
    char fn2[1024];
    if(pm->NTask > 1) {
        sprintf(fn1, "%s.%03d.geometry", filename, pm->ThisTask);
        sprintf(fn2, "%s.%03d", filename, pm->ThisTask);
    } else {
        sprintf(fn1, "%s.geometry", filename);
        sprintf(fn2, "%s", filename);
    }

    fastpm_path_ensure_dirname(filename);

    FILE * fp;
    fp = fopen(fn2, "w");
    if(pm->allocsize != fwrite(data, sizeof(FastPMFloat), pm->allocsize, fp)) {
        fastpm_raise(-1, "IO failed.\n");
    }
    fclose(fp);
    fp = fopen(fn1, "w");
    fprintf(fp, "# real\n");
    fprintf(fp, "start: %td %td %td\n",
                    pm->IRegion.start[0],
                    pm->IRegion.start[1],
                    pm->IRegion.start[2]);
    fprintf(fp, "size: %td %td %td\n",
                    pm->IRegion.size[0],
                    pm->IRegion.size[1],
                    pm->IRegion.size[2]);
    fprintf(fp, "strides: %td %td %td\n",
                    pm->IRegion.strides[0],
                    pm->IRegion.strides[1],
                    pm->IRegion.strides[2]);

    fprintf(fp, "# complex\n");
    fprintf(fp, "start: %td %td %td\n",
                    pm->ORegion.start[0],
                    pm->ORegion.start[1],
                    pm->ORegion.start[2]);
    fprintf(fp, "size: %td %td %td\n",
                    pm->ORegion.size[0],
                    pm->ORegion.size[1],
                    pm->ORegion.size[2]);
    fprintf(fp, "strides: %td %td %td\n",
                    pm->ORegion.strides[0],
                    pm->ORegion.strides[1],
                    pm->ORegion.strides[2]);

    fclose(fp);
}

void
fastpm_utils_load(PM * pm , const char * filename, FastPMFloat *data)
{
    char fn1[1024];
    char fn2[1024];
    if(pm->NTask > 1) {
        sprintf(fn1, "%s.%03d.geometry", filename, pm->ThisTask);
        sprintf(fn2, "%s.%03d", filename, pm->ThisTask);
    } else {
        sprintf(fn1, "%s.geometry", filename);
        sprintf(fn2, "%s", filename);
    }
    FILE * fp;
    fp = fopen(fn2, "r");
    if(pm->allocsize != fread(data, sizeof(FastPMFloat), pm->allocsize, fp)) {
        fastpm_raise(-1, "File was bad\n");
    };
    fclose(fp);
}

static double
tk_eh(double k, struct fastpm_powerspec_eh_params * params)		/* from Martin White */
{
    double q, theta, ommh2, a, s, gamma, L0, C0;
    double tmp;
    double omegam, ombh2, hubble;

    /* other input parameters */
    hubble = params->hubble_param;
    omegam = params->omegam;
    ombh2 = params->omegab * hubble * hubble;

    theta = 2.728 / 2.7;
    ommh2 = omegam * hubble * hubble;
    s = 44.5 * log(9.83 / ommh2) / sqrt(1. + 10. * exp(0.75 * log(ombh2))) * hubble;
    a = 1. - 0.328 * log(431. * ommh2) * ombh2 / ommh2
        + 0.380 * log(22.3 * ommh2) * (ombh2 / ommh2) * (ombh2 / ommh2);
    gamma = a + (1. - a) / (1. + exp(4 * log(0.43 * k * s)));
    gamma *= omegam * hubble;
    q = k * theta * theta / gamma;
    L0 = log(2. * exp(1.) + 1.8 * q);
    C0 = 14.2 + 731. / (1. + 62.5 * q);
    tmp = L0 / (L0 + C0 * q * q);
    return (tmp);
}

double
fastpm_utils_powerspec_eh(double k, struct fastpm_powerspec_eh_params * param)	/* Eisenstein & Hu */
{
    return param->Norm * k * pow(tk_eh(k, param), 2);
}

double
fastpm_utils_powerspec_white(double k, double * amplitude) 	/* White Noise*/
{
    return *amplitude;
}

static void
_sort_pix(const void * ptr, void * radix, void * arg)
{
    memcpy(radix, ptr, 8);
}

/* generate evenly balanced healpix positions that are inside the lightcone. */
void
fastpm_utils_healpix_ra_dec (
                int nside,
                double **ra,
                double **dec,
                uint64_t **pix,
                size_t * n,
                FastPMLightCone * lc,
                MPI_Comm comm
            )
{
    const double rad_to_degree = 180./M_PI;

    size_t npix = nside2npix (nside);
    int ThisTask, NTask;

    MPI_Comm_rank(comm, &ThisTask);
    MPI_Comm_size(comm, &NTask);

    //fastpm_info("healpix npix %ld \n",*npix);
    size_t i = 0;

    size_t pix_start = ThisTask * npix / NTask;
    size_t pix_end = (ThisTask + 1) * npix / NTask;

    uint64_t local_npix = 0;

    uint64_t * pixels = NULL;

    /* two iterations; estimate and fill */
    while(1) {
        size_t j = 0;

        for (i = pix_start; i < pix_end; i++)
        {
            double vec[3];

            pix2vec_ring(nside, i, vec);

            if(!fastpm_lc_inside(lc, vec)) continue;

            if(pixels != NULL) {
                pixels[j] = i;
            }

            j ++;
        }
        if(pixels == NULL) {
            local_npix = j;
            pixels = malloc(sizeof(uint64_t) * local_npix);

        } else {
            break;
        }
    }

    /* count total */
    uint64_t valid_npix = local_npix;
    MPI_Allreduce(MPI_IN_PLACE, &valid_npix, 1, MPI_LONG, MPI_SUM, comm);

    /* redistribute / balance */

    size_t localsize =
        (ThisTask + 1) * valid_npix / NTask
    -
        (ThisTask    ) * valid_npix / NTask;

    uint64_t * recv_buffer = malloc(sizeof(uint64_t) * localsize);

    mpsort_mpi_newarray(pixels, local_npix, recv_buffer, localsize, sizeof(uint64_t), _sort_pix, 8, NULL, comm);

    free(pixels);

    *ra = malloc(sizeof(double) * localsize);
    *dec = malloc(sizeof(double) * localsize);

    for(i = 0; i < localsize; i ++) {
        double phi, theta;

        pix2ang_ring(nside, recv_buffer[i], &theta, &phi);

        phi *= rad_to_degree;
        theta*= rad_to_degree;
        (*ra)[i] = phi;
        (*dec)[i]= 90 - theta;
        recv_buffer[i] += (((uint64_t)nside) << 48);
    }
    *n = localsize;
    *pix = recv_buffer;
}

void
fastpm_gldot(double glmatrix[4][4], double xi[4], double xo[4])
{
    int i, j;
    for(i = 0; i < 4; i ++) {
        xo[i] = 0;
        for(j = 0; j < 4; j ++) {
            xo[i] += glmatrix[i][j] * xi[j];
        }
    }
}

void
fastpm_gldotf(double glmatrix[4][4], float vi[4], float vo[4])
{
    int i, j;
    for(i = 0; i < 4; i ++) {
        vo[i] = 0;
        for(j = 0; j < 4; j ++) {
            vo[i] += glmatrix[i][j] * vi[j];
        }
    }
}

/* returns MPI_LOR reduction */
int
MPIU_Any(MPI_Comm comm, int value)
{
    MPI_Allreduce(MPI_IN_PLACE, &value, 1, MPI_INT, MPI_LOR, comm);
    return value;
}

/* returns MPI_LAND reduction */
int
MPIU_All(MPI_Comm comm, int value)
{
    MPI_Allreduce(MPI_IN_PLACE, &value, 1, MPI_INT, MPI_LAND, comm);
    return value;
}

/*
 * Compute the standard statistics of a value over communicator.
 * fmt is a string, describing the list of stats to return:
 *  < : minimum  (double)
 *  , : rank of minimum (int)
 *  > : maximum  (double)
 *  . : rank of maximum (int)
 *  - : mean  (double)
 *  s : std (biased standard derivation)
 *  v : variance (biased variance)
 *  S : unbiased std
 *  V : unbiased variance
 *
 * e.g.
 *
 *    double min, max;
 *    MPIU_stats(comm, r, "<>", &min, &max);
 *
 * returns number of items converted;
 * if a fmt char is unknown, the input variable is unmodified.
 *
 * */
int
MPIU_stats(MPI_Comm comm,
    const double value,
    const char * fmt, ...)
{

    va_list va;
    va_start(va, fmt);

    int NTask;
    MPI_Comm_size(comm, &NTask);
    int ThisTask;
    MPI_Comm_rank(comm, &ThisTask);

    double sum = value;
    struct {
        double value;
        int rank;
    } min = {value, ThisTask},
      max = {value, ThisTask};

    double sumsq = value * value;

    MPI_Allreduce(MPI_IN_PLACE, &sum, 1, MPI_DOUBLE, MPI_SUM, comm);
    MPI_Allreduce(MPI_IN_PLACE, &sumsq, 1, MPI_DOUBLE, MPI_SUM, comm);
    MPI_Allreduce(MPI_IN_PLACE, &min, 1, MPI_DOUBLE_INT, MPI_MINLOC, comm);
    MPI_Allreduce(MPI_IN_PLACE, &max, 1, MPI_DOUBLE_INT, MPI_MAXLOC, comm);

    double avg = (sum) / NTask;
    double avg_sq = (sumsq) / NTask;
    double var = avg_sq - avg * avg;
    double std = sqrt(avg_sq - avg * avg);

    int i;
    int c = 0;
    for(i = 0; i < strlen(fmt); i ++) {
        void * r = va_arg(va, void *);
        int * ir = (int * ) r;
        double * dr = (double * ) r;

        c++;
        switch(fmt[i]) {
            case '-':
                *dr = avg;
                break;
            case '<':
                *dr = min.value;
                break;
            case '>':
                *dr = max.value;
                break;
            case ',':
                *ir = min.rank;
                break;
            case '.':
                *ir = max.rank;
                break;
            case 's':
                *dr = std;
                break;
            case 'S':
                *dr = std * sqrt(1.0 * NTask / (NTask - 1.0));
                break;
            case 'v':
                *dr = var;
                break;
            case 'V':
                *dr = var * 1.0 * NTask / (NTask - 1.0);
                break;
            default:
                c--;
        }
    }
    va_end(va);

    return c;
}

/*
 *
 * create a copy of string src and broadcast it to all ranks.
 *
 * Every rank receives a copy of src as the return value, including the root.
 *
 * if free is not NULL, the function is called as free(src)
 * */
char *
MPIU_Bcast_string(MPI_Comm comm, char * src, int root, void(*free)(void * ptr))
{
    int srcNULL = src == NULL;
    MPI_Bcast(&srcNULL, 1, MPI_INT, root, comm);

    if(srcNULL) return NULL;

    int rank;
    int N;
    MPI_Comm_rank(comm, &rank);

    if(rank == root) {
        N = strlen(src) + 1;
    }
    MPI_Bcast(&N, 1, MPI_INT, root, comm);

    char * ret = malloc(N);
    if(rank == root) {
        strcpy(ret, src);
    }

    if(free && rank == root) free(src);
    MPI_Bcast(ret, N, MPI_BYTE, root, comm);

    return ret;
}


// clang -DHAVE_INLINE -O3 mussel.c -o mussel -lgsl -lgslcblas

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

int main(int argc, char *argv[])
{
    // Setup the random number generator:
    gsl_rng_env_setup();
    gsl_rng *rng = gsl_rng_alloc(gsl_rng_taus2);
    const unsigned int seed = (argc == 2)? atof(argv[1]) : time(NULL);
    gsl_rng_set(rng, seed);

    // Parameters:
    const int T = 1000;
    const double A0 = 0.5;
    const double A1 = 0.025;
    const double A2 = 0.2;
    const int N = 1000;
    const int TWO_N = N * N;

    // Allocate memory for the matrix:
    int **m = (int**)malloc(N * sizeof(int*));
    int i;
    for (i = 0; i < N; ++i)
    {
        m[i] = (int*)malloc(N * sizeof(int));
    }

    // Fill the matrix:
    int x0, y0;
    for (x0 = 0; x0 < N; ++x0)
    {
        for (y0 = 0; y0 < N; ++y0)
        {
            m[x0][y0] = (gsl_rng_uniform(rng) < 0.5) ? 0 : 2;
        }
    }

    // Main loop
    int n, x, y, x1, y1, t, cell;
    for (t = 0; t < T; ++t)
    {
        // Loop around the lattice at random to update cells;
        for (cell = 0; cell < TWO_N; ++cell)
        {
            // Select at random the cell to update
            x0 = (int)(gsl_rng_uniform(rng) * N);
            y0 = (int)(gsl_rng_uniform(rng) * N);

            // Update the mussels
            // Disturbed cells automatically transformed into empty cells
            if (m[x0][y0] == 0)
            {
                m[x0][y0] = 1;
            }
            else if (m[x0][y0] == 1) // If the cell is empty, test if there is colonization
            {
                // First calculate the number of neighbours
                n = 0;
                for (x = -1; x <= 1; ++x)
                {
                    for (y = -1; y <= 1; ++y)
                    {
                        if (x0 + x == -1)
                        {
                            x1 = N - 1;
                        }
                        else if (x0 + x == N)
                        {
                            x1 = 0;
                        }
                        else
                        {
                            x1 = x0 + x;
                        }

                        if (y0 + y == -1)
                        {
                            y1 = N - 1;
                        }
                        else if (y0 + y == N)
                        {
                            y1 = 0;
                        }
                        else
                        {
                            y1 = y0 + y;
                        }

                        if (x != 0 && y != 0 && m[x1][y1] == 2)
                        {
                            ++n;
                        }
                    }
                }
                // Calculate if the status if the cell is changed
                if (gsl_rng_uniform(rng) < A2 * n * 0.125)
                {
                    m[x0][y0] = 2;
                }
            }
            else if (m[x0][y0] == 2)
            { // If the cell is occupied, test if there is disturbance
                n = 0;
                for (x = -1; x <= 1; x++)
                {
                    for (y = -1; y <= 1; y++)
                    {
                        if (x0 + x == -1)
                        {
                            x1 = N - 1;
                        }
                        else if (x0 + x == N)
                        {
                            x1 = 0;
                        }
                        else
                        {
                            x1 = x0 + x;
                        }

                        if (y0 + y == -1)
                        {
                            y1 = N - 1;
                        }
                        else if (y0 + y == N)
                        {
                            y1 = 0;
                        }
                        else
                        {
                            y1 = y0 + y;
                        }

                        if (x != 0 && y != 0 && m[x1][y1] == 0)
                        {
                            n = 1;
                        }
                    }
                }
                // Calculate if the status of the cell is changed
                if (gsl_rng_uniform(rng) < A0 * n * 0.125 + A1)
                {
                    m[x0][y0] = 0;
                }
            }
        }
    }

    char buffer[100];
    sprintf(buffer, "mussel-%u.txt", seed);
    FILE *out = fopen(buffer, "w");

    for (x0 = 0; x0 < N; ++x0)
    {
        for (y0 = 0; y0 < N; ++y0)
        {
            fprintf(out, "%d ", m[x0][y0]);
        }
        fprintf(out, "\n");
    }
    fclose(out);

    gsl_rng_free(rng);
    for (i = 0; i < N; ++i)
    {
        free(m[i]);
    }
    free(m);
    return EXIT_SUCCESS;
}


/* multifit/multireg.c
 *
 * Copyright (C) 2015 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/*
 * References:
 *
 * [1] P. C. Hansen & D. P. O'Leary, "The use of the L-curve in
 * the regularization of discrete ill-posed problems",  SIAM J. Sci.
 * Comput. 14 (1993), pp. 1487-1503.
 *
 * [2] P. C. Hansen, "Discrete Inverse Problems: Insight and Algorithms,"
 * SIAM Press, 2010.
 */

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

#include "linear_common.c"

int
gsl_multifit_linear_solve (const double lambda,
                           const gsl_matrix * X,
                           const gsl_vector * y,
                           gsl_vector * c,
                           double *rnorm,
                           double *snorm,
                           gsl_multifit_linear_workspace * work)
{
  size_t rank;
  int status;

  status = multifit_linear_solve(X, y, GSL_DBL_EPSILON, lambda, &rank, c,
                                 rnorm, snorm, work);

  return status;
} /* gsl_multifit_linear_solve() */

/*
gsl_multifit_linear_applyW()
  Apply weight matrix to (X,y) LS system

Inputs: X    - least squares matrix n-by-p
        w    - weight vector n-by-1 or NULL for W = I
        y    - right hand side n-by-1
        WX   - (output) sqrt(W) X, n-by-p
        Wy   - (output) sqrt(W) y, n-by-1

Notes:
1) If w = NULL, on output WX = X and Wy = y
2) It is allowed for WX = X and Wy = y for in-place transform
*/

int
gsl_multifit_linear_applyW(const gsl_matrix * X,
                           const gsl_vector * w,
                           const gsl_vector * y,
                           gsl_matrix * WX,
                           gsl_vector * Wy)
{
  const size_t n = X->size1;
  const size_t p = X->size2;

  if (n != y->size)
    {
      GSL_ERROR("y vector does not match X", GSL_EBADLEN);
    }
  else if (w != NULL && n != w->size)
    {
      GSL_ERROR("weight vector does not match X", GSL_EBADLEN);
    }
  else if (n != WX->size1 || p != WX->size2)
    {
      GSL_ERROR("WX matrix dimensions do not match X", GSL_EBADLEN);
    }
  else if (n != Wy->size)
    {
      GSL_ERROR("Wy vector must be length n", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      /* copy WX = X; Wy = y if distinct pointers */
      if (WX != X)
        gsl_matrix_memcpy(WX, X);
      if (Wy != y)
        gsl_vector_memcpy(Wy, y);

      if (w != NULL)
        {
          /* construct WX = sqrt(W) X and Wy = sqrt(W) y */
          for (i = 0; i < n; ++i)
            {
              double wi = gsl_vector_get(w, i);
              double swi;
              gsl_vector_view row = gsl_matrix_row(WX, i);
              double *yi = gsl_vector_ptr(Wy, i);

              if (wi < 0.0)
                wi = 0.0;

              swi = sqrt(wi);
              gsl_vector_scale(&row.vector, swi);
              *yi *= swi;
            }
        }

      return GSL_SUCCESS;
    }
}

/*
gsl_multifit_linear_wstdform1()
  Using regularization matrix
L = diag(l_1,l_2,...,l_p), transform to Tikhonov standard form:

X~ = sqrt(W) X L^{-1}
y~ = sqrt(W) y
c~ = L c

Inputs: L    - Tikhonov matrix as a vector of diagonal elements p-by-1;
               or NULL for L = I
        X    - least squares matrix n-by-p
        y    - right hand side vector n-by-1
        w    - weight vector n-by-1; or NULL for W = I
        Xs   - least squares matrix in standard form X~ n-by-p
        ys   - right hand side vector in standard form y~ n-by-1
        work - workspace

Return: success/error

Notes:
1) It is allowed for X = Xs and y = ys
*/

int
gsl_multifit_linear_wstdform1 (const gsl_vector * L,
                               const gsl_matrix * X,
                               const gsl_vector * w,
                               const gsl_vector * y,
                               gsl_matrix * Xs,
                               gsl_vector * ys,
                               gsl_multifit_linear_workspace * work)
{
  const size_t n = X->size1;
  const size_t p = X->size2;

  if (n > work->nmax || p > work->pmax)
    {
      GSL_ERROR("observation matrix larger than workspace", GSL_EBADLEN);
    }
  else if (L != NULL && p != L->size)
    {
      GSL_ERROR("L vector does not match X", GSL_EBADLEN);
    }
  else if (n != y->size)
    {
      GSL_ERROR("y vector does not match X", GSL_EBADLEN);
    }
  else if (w != NULL && n != w->size)
    {
      GSL_ERROR("weight vector does not match X", GSL_EBADLEN);
    }
  else if (n != Xs->size1 || p != Xs->size2)
    {
      GSL_ERROR("Xs matrix dimensions do not match X", GSL_EBADLEN);
    }
  else if (n != ys->size)
    {
      GSL_ERROR("ys vector must be length n", GSL_EBADLEN);
    }
  else
    {
      int status = GSL_SUCCESS;

      /* compute Xs = sqrt(W) X and ys = sqrt(W) y */
      status = gsl_multifit_linear_applyW(X, w, y, Xs, ys);
      if (status)
        return status;

      if (L != NULL)
        {
          size_t j;

          /* construct X~ = sqrt(W) X * L^{-1} matrix */
          for (j = 0; j < p; ++j)
            {
              gsl_vector_view Xj = gsl_matrix_column(Xs, j);
              double lj = gsl_vector_get(L, j);

              if (lj == 0.0)
                {
                  GSL_ERROR("L matrix is singular", GSL_EDOM);
                }

              gsl_vector_scale(&Xj.vector, 1.0 / lj);
            }
        }

      return status;
    }
}

/*
gsl_multifit_linear_stdform1()
  Using regularization matrix L = diag(l_1,l_2,...,l_p),
and W = I, transform to Tikhonov standard form:

X~ = X L^{-1}
y~ = y
c~ = L c

Inputs: L    - Tikhonov matrix as a vector of diagonal elements p-by-1
        X    - least squares matrix n-by-p
        y    - right hand side vector n-by-1
        Xs   - least squares matrix in standard form X~ n-by-p
        ys   - right hand side vector in standard form y~ n-by-1
        work - workspace

Return: success/error

Notes:
1) It is allowed for X = Xs
*/

int
gsl_multifit_linear_stdform1 (const gsl_vector * L,
                              const gsl_matrix * X,
                              const gsl_vector * y,
                              gsl_matrix * Xs,
                              gsl_vector * ys,
                              gsl_multifit_linear_workspace * work)
{
  int status;

  status = gsl_multifit_linear_wstdform1(L, X, NULL, y, Xs, ys, work);

  return status;
}

int
gsl_multifit_linear_L_decomp (gsl_matrix * L, gsl_vector * tau)
{
  const size_t m = L->size1;
  const size_t p = L->size2;
  int status;

  if (tau->size != GSL_MIN(m, p))
    {
      GSL_ERROR("tau vector must be min(m,p)", GSL_EBADLEN);
    }
  else if (m >= p)
    {
      /* square or tall L matrix */
      status = gsl_linalg_QR_decomp(L, tau);
      return status;
    }
  else
    {
      /* more columns than rows, compute qr(L^T) */
      gsl_matrix_view LTQR = gsl_matrix_view_array(L->data, p, m);
      gsl_matrix *LT = gsl_matrix_alloc(p, m);

      /* XXX: use temporary storage due to difficulties in transforming
       * a rectangular matrix in-place */
      gsl_matrix_transpose_memcpy(LT, L);
      gsl_matrix_memcpy(&LTQR.matrix, LT);
      gsl_matrix_free(LT);

      status = gsl_linalg_QR_decomp(&LTQR.matrix, tau);

      return status;
    }
}

/*
gsl_multifit_linear_wstdform2()
  Using regularization matrix L which is m-by-p, transform to Tikhonov
standard form. This routine is separated into two cases:

Case 1: m >= p, here we can use the QR decomposition of L = QR, and note
that ||L c|| = ||R c|| where R is p-by-p. Therefore,

X~ = X R^{-1} is n-by-p
y~ = y is n-by-1
c~ is p-by-1
M is not used

Case 2: m < p

X~ is (n - p + m)-by-m
y~ is (n - p + m)-by-1
c~ is m-by-1
M is n-by-p (workspace)

Inputs: LQR  - output from gsl_multifit_linear_L_decomp()
        Ltau - output from gsl_multifit_linear_L_decomp()
        X    - least squares matrix n-by-p
        w    - weight vector n-by-1; or NULL for W = I
        y    - right hand side vector n-by-1
        Xs   - (output) least squares matrix in standard form
               case 1: n-by-p
               case 2: (n - p + m)-by-m
        ys   - (output) right hand side vector in standard form
               case 1: n-by-1
               case 2: (n - p + m)-by-1
        M    - (output) workspace matrix needed to reconstruct solution vector
               case 1: not used
               case 2: n-by-p
        work - workspace

Return: success/error

Notes:
1) If m >= p, on output:
   Xs = X R^{-1}
   ys = y

2) If m < p, on output:
   M(:,1:pm) contains QR decomposition of A * K_o, needed to reconstruct
   solution vector, where pm = p - m; M(:,p) contains Householder scalars
*/

int
gsl_multifit_linear_wstdform2 (const gsl_matrix * LQR,
                               const gsl_vector * Ltau,
                               const gsl_matrix * X,
                               const gsl_vector * w,
                               const gsl_vector * y,
                               gsl_matrix * Xs,
                               gsl_vector * ys,
                               gsl_matrix * M,
                               gsl_multifit_linear_workspace * work)
{
  const size_t m = LQR->size1;
  const size_t n = X->size1;
  const size_t p = X->size2;

  if (n > work->nmax || p > work->pmax)
    {
      GSL_ERROR("observation matrix larger than workspace", GSL_EBADLEN);
    }
  else if (p != LQR->size2)
    {
      GSL_ERROR("LQR and X matrices have different numbers of columns", GSL_EBADLEN);
    }
  else if (n != y->size)
    {
      GSL_ERROR("y vector does not match X", GSL_EBADLEN);
    }
  else if (w != NULL && n != w->size)
    {
      GSL_ERROR("weights vector must be length n", GSL_EBADLEN);
    }
  else if (m >= p) /* square or tall L matrix */
    {
      /* the sizes of Xs and ys depend on whether m >= p or m < p */
      if (n != Xs->size1 || p != Xs->size2)
        {
          GSL_ERROR("Xs matrix must be n-by-p", GSL_EBADLEN);
        }
      else if (n != ys->size)
        {
          GSL_ERROR("ys vector must have length n", GSL_EBADLEN);
        }
      else
        {
          int status;
          size_t i;
          gsl_matrix_const_view R = gsl_matrix_const_submatrix(LQR, 0, 0, p, p);

          /* compute Xs = sqrt(W) X and ys = sqrt(W) y */
          status = gsl_multifit_linear_applyW(X, w, y, Xs, ys);
          if (status)
            return status;

          /* compute X~ = X R^{-1} using QR decomposition of L */
          for (i = 0; i < n; ++i)
            {
              gsl_vector_view v = gsl_matrix_row(Xs, i);

              /* solve: R^T y = X_i */
              gsl_blas_dtrsv(CblasUpper, CblasTrans, CblasNonUnit, &R.matrix, &v.vector);
            }

          return GSL_SUCCESS;
        }
    }
  else /* L matrix with m < p */
    {
      const size_t pm = p - m;
      const size_t npm = n - pm;

      /*
       * This code closely follows section 2.6.1 of Hansen's
       * "Regularization Tools" manual
       */

      if (npm != Xs->size1 || m != Xs->size2)
        {
          GSL_ERROR("Xs matrix must be (n-p+m)-by-m", GSL_EBADLEN);
        }
      else if (npm != ys->size)
        {
          GSL_ERROR("ys vector must be of length (n-p+m)", GSL_EBADLEN);
        }
      else if (n != M->size1 || p != M->size2)
        {
          GSL_ERROR("M matrix must be n-by-p", GSL_EBADLEN);
        }
      else
        {
          int status;
          gsl_matrix_view A = gsl_matrix_submatrix(work->A, 0, 0, n, p);
          gsl_vector_view b = gsl_vector_subvector(work->t, 0, n);

          gsl_matrix_view LTQR = gsl_matrix_view_array(LQR->data, p, m);           /* qr(L^T) */
          gsl_matrix_view Rp = gsl_matrix_view_array(LQR->data, m, m);             /* R factor of L^T */
          gsl_vector_const_view LTtau = gsl_vector_const_subvector(Ltau, 0, m);

          /*
           * M(:,1:p-m) will hold QR decomposition of A K_o; M(:,p) will hold
           * Householder scalars
           */
          gsl_matrix_view MQR = gsl_matrix_submatrix(M, 0, 0, n, pm);
          gsl_vector_view Mtau = gsl_matrix_subcolumn(M, p - 1, 0, GSL_MIN(n, pm));

          gsl_matrix_view AKo, AKp, HqTAKp;
          gsl_vector_view v;
          size_t i;

          /* compute A = sqrt(W) X and b = sqrt(W) y */
          status = gsl_multifit_linear_applyW(X, w, y, &A.matrix, &b.vector);
          if (status)
            return status;

          /* compute: A <- A K = [ A K_p ; A K_o ] */
          gsl_linalg_QR_matQ(&LTQR.matrix, &LTtau.vector, &A.matrix);
          AKp = gsl_matrix_submatrix(&A.matrix, 0, 0, n, m);
          AKo = gsl_matrix_submatrix(&A.matrix, 0, m, n, pm);

          /* compute QR decomposition [H,T] = qr(A * K_o) and store in M */
          gsl_matrix_memcpy(&MQR.matrix, &AKo.matrix);
          gsl_linalg_QR_decomp(&MQR.matrix, &Mtau.vector);

          /* AKp currently contains A K_p; apply H^T from the left to get H^T A K_p */
          gsl_linalg_QR_QTmat(&MQR.matrix, &Mtau.vector, &AKp.matrix);

          /* the last npm rows correspond to H_q^T A K_p */
          HqTAKp = gsl_matrix_submatrix(&AKp.matrix, pm, 0, npm, m);

          /* solve: Xs R_p^T = H_q^T A K_p for Xs */
          gsl_matrix_memcpy(Xs, &HqTAKp.matrix);
          for (i = 0; i < npm; ++i)
            {
              gsl_vector_view x = gsl_matrix_row(Xs, i);
              gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, &Rp.matrix, &x.vector);
            }

          /*
           * compute: ys = H_q^T b; this is equivalent to computing
           * the last q elements of H^T b (q = npm)
           */
          v = gsl_vector_subvector(&b.vector, pm, npm);
          gsl_linalg_QR_QTvec(&MQR.matrix, &Mtau.vector, &b.vector);
          gsl_vector_memcpy(ys, &v.vector);

          return GSL_SUCCESS;
        }
    }
}

int
gsl_multifit_linear_stdform2 (const gsl_matrix * LQR,
                              const gsl_vector * Ltau,
                              const gsl_matrix * X,
                              const gsl_vector * y,
                              gsl_matrix * Xs,
                              gsl_vector * ys,
                              gsl_matrix * M,
                              gsl_multifit_linear_workspace * work)
{
  int status;

  status = gsl_multifit_linear_wstdform2(LQR, Ltau, X, NULL, y, Xs, ys, M, work);

  return status;
}

/*
gsl_multifit_linear_genform1()
  Backtransform regularized solution vector using matrix
L = diag(L)
*/

int
gsl_multifit_linear_genform1 (const gsl_vector * L,
                              const gsl_vector * cs,
                              gsl_vector * c,
                              gsl_multifit_linear_workspace * work)
{
  if (L->size > work->pmax)
    {
      GSL_ERROR("L vector does not match workspace", GSL_EBADLEN);
    }
  else if (L->size != cs->size)
    {
      GSL_ERROR("cs vector does not match L", GSL_EBADLEN);
    }
  else if (L->size != c->size)
    {
      GSL_ERROR("c vector does not match L", GSL_EBADLEN);
    }
  else
    {
      /* compute true solution vector c = L^{-1} c~ */
      gsl_vector_memcpy(c, cs);
      gsl_vector_div(c, L);

      return GSL_SUCCESS;
    }
}

/*
gsl_multifit_linear_wgenform2()
  Backtransform regularized solution vector in standard form to recover
original vector

Inputs: LQR  - output from gsl_multifit_linear_L_decomp()
        Ltau - output from gsl_multifit_linear_L_decomp()
        X    - original least squares matrix n-by-p
        w    - original weight vector n-by-1 or NULL for W = I
        y    - original rhs vector n-by-1
        cs   - standard form solution vector
        c    - (output) original solution vector p-by-1
        M    - matrix computed by gsl_multifit_linear_wstdform2()
        work - workspace
*/

int
gsl_multifit_linear_wgenform2 (const gsl_matrix * LQR,
                               const gsl_vector * Ltau,
                               const gsl_matrix * X,
                               const gsl_vector * w,
                               const gsl_vector * y,
                               const gsl_vector * cs,
                               const gsl_matrix * M,
                               gsl_vector * c,
                               gsl_multifit_linear_workspace * work)
{
  const size_t m = LQR->size1;
  const size_t n = X->size1;
  const size_t p = X->size2;

  if (n > work->nmax || p > work->pmax)
    {
      GSL_ERROR("X matrix does not match workspace", GSL_EBADLEN);
    }
  else if (p != LQR->size2)
    {
      GSL_ERROR("LQR matrix does not match X", GSL_EBADLEN);
    }
  else if (p != c->size)
    {
      GSL_ERROR("c vector does not match X", GSL_EBADLEN);
    }
  else if (w != NULL && n != w->size)
    {
      GSL_ERROR("w vector does not match X", GSL_EBADLEN);
    }
  else if (n != y->size)
    {
      GSL_ERROR("y vector does not match X", GSL_EBADLEN);
    }
  else if (m >= p)                    /* square or tall L matrix */
    {
      if (p != cs->size)
        {
          GSL_ERROR("cs vector must be length p", GSL_EBADLEN);
        }
      else
        {
          int s;
          gsl_matrix_const_view R = gsl_matrix_const_submatrix(LQR, 0, 0, p, p); /* R factor of L */

          /* solve R c = cs for true solution c, using QR decomposition of L */
          gsl_vector_memcpy(c, cs);
          s = gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, &R.matrix, c);

          return s;
        }
    }
  else                                /* rectangular L matrix with m < p */
    {
      if (m != cs->size)
        {
          GSL_ERROR("cs vector must be length m", GSL_EBADLEN);
        }
      else if (n != M->size1 || p != M->size2)
        {
          GSL_ERROR("M matrix must be size n-by-p", GSL_EBADLEN);
        }
      else
        {
          int status;
          const size_t pm = p - m;
          gsl_matrix_view A = gsl_matrix_submatrix(work->A, 0, 0, n, p);
          gsl_vector_view b = gsl_vector_subvector(work->t, 0, n);
          gsl_matrix_view Rp = gsl_matrix_view_array(LQR->data, m, m); /* R_p */
          gsl_matrix_view LTQR = gsl_matrix_view_array(LQR->data, p, m);
          gsl_vector_const_view LTtau = gsl_vector_const_subvector(Ltau, 0, m);
          gsl_matrix_const_view MQR = gsl_matrix_const_submatrix(M, 0, 0, n, pm);
          gsl_vector_const_view Mtau = gsl_matrix_const_subcolumn(M, p - 1, 0, GSL_MIN(n, pm));
          gsl_matrix_const_view To = gsl_matrix_const_submatrix(&MQR.matrix, 0, 0, pm, pm);
          gsl_vector_view workp = gsl_vector_subvector(work->xt, 0, p);
          gsl_vector_view v1, v2;

          /* compute A = sqrt(W) X and b = sqrt(W) y */
          status = gsl_multifit_linear_applyW(X, w, y, &A.matrix, &b.vector);
          if (status)
            return status;

          /* initialize c to zero */
          gsl_vector_set_zero(c);

          /* compute c = L_inv cs = K_p R_p^{-T} cs */

          /* set c(1:m) = R_p^{-T} cs */
          v1 = gsl_vector_subvector(c, 0, m);
          gsl_vector_memcpy(&v1.vector, cs);
          gsl_blas_dtrsv(CblasUpper, CblasTrans, CblasNonUnit, &Rp.matrix, &v1.vector);

          /* c <- K R_p^{-T} cs = [ K_p R_p^{_T} cs ; 0 ] */
          gsl_linalg_QR_Qvec(&LTQR.matrix, &LTtau.vector, c);

          /* compute: b1 = b - A L_inv cs */
          gsl_blas_dgemv(CblasNoTrans, -1.0, &A.matrix, c, 1.0, &b.vector);

          /* compute: b2 = H^T b1 */
          gsl_linalg_QR_QTvec(&MQR.matrix, &Mtau.vector, &b.vector);

          /* compute: b3 = T_o^{-1} b2 */
          v1 = gsl_vector_subvector(&b.vector, 0, pm);
          gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, &To.matrix, &v1.vector);

          /* compute: b4 = K_o b3 */
          gsl_vector_set_zero(&workp.vector);
          v2 = gsl_vector_subvector(&workp.vector, m, pm);
          gsl_vector_memcpy(&v2.vector, &v1.vector);
          gsl_linalg_QR_Qvec(&LTQR.matrix, &LTtau.vector, &workp.vector);

          /* final solution vector */
          gsl_vector_add(c, &workp.vector);

          return GSL_SUCCESS;
        }
    }
}

int
gsl_multifit_linear_genform2 (const gsl_matrix * LQR,
                              const gsl_vector * Ltau,
                              const gsl_matrix * X,
                              const gsl_vector * y,
                              const gsl_vector * cs,
                              const gsl_matrix * M,
                              gsl_vector * c,
                              gsl_multifit_linear_workspace * work)
{
  int status;

  status = gsl_multifit_linear_wgenform2(LQR, Ltau, X, NULL, y, cs, M, c, work);

  return status;
}

/*
gsl_multifit_linear_lreg()
  Calculate regularization parameters to use in L-curve
analysis

Inputs: smin      - smallest singular value of LS system
        smax      - largest singular value of LS system > 0
        reg_param - (output) vector of regularization parameters
                    derived from singular values

Return: success/error
*/

int
gsl_multifit_linear_lreg (const double smin, const double smax,
                          gsl_vector * reg_param)
{
  if (smax <= 0.0)
    {
      GSL_ERROR("smax must be positive", GSL_EINVAL);
    }
  else
    {
      const size_t N = reg_param->size;

      /* smallest regularization parameter */
      const double smin_ratio = 16.0 * GSL_DBL_EPSILON;
      const double new_smin = GSL_MAX(smin, smax*smin_ratio);
      double ratio;
      size_t i;

      gsl_vector_set(reg_param, N - 1, new_smin);

      /* ratio so that reg_param(1) = s(1) */
      ratio = pow(smax / new_smin, 1.0 / ((double)N - 1.0));

      /* calculate the regularization parameters */
      for (i = N - 1; i > 0 && i--; )
        {
          double rp1 = gsl_vector_get(reg_param, i + 1);
          gsl_vector_set(reg_param, i, ratio * rp1);
        }

      return GSL_SUCCESS;
    }
}

/*
gsl_multifit_linear_lcurve()
  Calculate L-curve using regularization parameters estimated
from singular values of least squares matrix

Inputs: y         - right hand side vector
        reg_param - (output) vector of regularization parameters
                    derived from singular values
        rho       - (output) vector of residual norms ||y - X c||
        eta       - (output) vector of solution norms ||lambda c||
        work      - workspace

Return: success/error

Notes:
1) SVD of X must be computed first by calling multifit_linear_svd();
   work->n and work->p are initialized by this function
*/

int
gsl_multifit_linear_lcurve (const gsl_vector * y,
                            gsl_vector * reg_param,
                            gsl_vector * rho, gsl_vector * eta,
                            gsl_multifit_linear_workspace * work)
{
  const size_t n = y->size;
  const size_t N = rho->size; /* number of points on L-curve */

  if (n != work->n)
    {
      GSL_ERROR("y vector does not match workspace", GSL_EBADLEN);
    }
  else if (N < 3)
    {
      GSL_ERROR ("at least 3 points are needed for L-curve analysis",
                 GSL_EBADLEN);
    }
  else if (N != eta->size)
    {
      GSL_ERROR ("size of rho and eta vectors do not match",
                 GSL_EBADLEN);
    }
  else if (reg_param->size != eta->size)
    {
      GSL_ERROR ("size of reg_param and eta vectors do not match",
                 GSL_EBADLEN);
    }
  else
    {
      int status = GSL_SUCCESS;
      const size_t p = work->p;

      size_t i, j;

      gsl_matrix_view A = gsl_matrix_submatrix(work->A, 0, 0, n, p);
      gsl_vector_view S = gsl_vector_subvector(work->S, 0, p);
      gsl_vector_view xt = gsl_vector_subvector(work->xt, 0, p);
      gsl_vector_view workp = gsl_matrix_subcolumn(work->QSI, 0, 0, p);
      gsl_vector_view workp2 = gsl_vector_subvector(work->D, 0, p); /* D isn't used for regularized problems */

      const double smax = gsl_vector_get(&S.vector, 0);
      const double smin = gsl_vector_get(&S.vector, p - 1);

      double dr; /* residual error from projection */
      double normy = gsl_blas_dnrm2(y);
      double normUTy;

      /* compute projection xt = U^T y */
      gsl_blas_dgemv (CblasTrans, 1.0, &A.matrix, y, 0.0, &xt.vector);

      normUTy = gsl_blas_dnrm2(&xt.vector);
      dr = normy*normy - normUTy*normUTy;

      /* calculate regularization parameters */
      gsl_multifit_linear_lreg(smin, smax, reg_param);

      for (i = 0; i < N; ++i)
        {
          double lambda = gsl_vector_get(reg_param, i);
          double lambda_sq = lambda * lambda;

          for (j = 0; j < p; ++j)
            {
              double sj = gsl_vector_get(&S.vector, j);
              double xtj = gsl_vector_get(&xt.vector, j);
              double f = sj / (sj*sj + lambda_sq);

              gsl_vector_set(&workp.vector, j, f * xtj);
              gsl_vector_set(&workp2.vector, j, (1.0 - sj*f) * xtj);
            }

          gsl_vector_set(eta, i, gsl_blas_dnrm2(&workp.vector));
          gsl_vector_set(rho, i, gsl_blas_dnrm2(&workp2.vector));
        }

      if (n > p && dr > 0.0)
        {
          /* add correction to residual norm (see eqs 6-7 of [1]) */
          for (i = 0; i < N; ++i)
            {
              double rhoi = gsl_vector_get(rho, i);
              double *ptr = gsl_vector_ptr(rho, i);

              *ptr = sqrt(rhoi*rhoi + dr);
            }
        }

      /* restore D to identity matrix */
      gsl_vector_set_all(work->D, 1.0);

      return status;
    }
} /* gsl_multifit_linear_lcurve() */

/*
gsl_multifit_linear_lcorner()
  Determine point on L-curve of maximum curvature. For each
set of 3 points on the L-curve, the circle which passes through
the 3 points is computed. The radius of the circle is then used
as an estimate of the curvature at the middle point. The point
with maximum curvature is then selected.

Inputs: rho - vector of residual norms ||A x - b||
        eta - vector of solution norms ||L x||
        idx - (output) index i such that
              (log(rho(i)),log(eta(i))) is the point of
              maximum curvature

Return: success/error
*/

int
gsl_multifit_linear_lcorner(const gsl_vector *rho,
                            const gsl_vector *eta,
                            size_t *idx)
{
  const size_t n = rho->size;

  if (n < 3)
    {
      GSL_ERROR ("at least 3 points are needed for L-curve analysis",
                 GSL_EBADLEN);
    }
  else if (n != eta->size)
    {
      GSL_ERROR ("size of rho and eta vectors do not match",
                 GSL_EBADLEN);
    }
  else
    {
      int s = GSL_SUCCESS;
      size_t i;
      double x1, y1;      /* first point of triangle on L-curve */
      double x2, y2;      /* second point of triangle on L-curve */
      double rmin = -1.0; /* minimum radius of curvature */

      /* initial values */
      x1 = log(gsl_vector_get(rho, 0));
      y1 = log(gsl_vector_get(eta, 0));

      x2 = log(gsl_vector_get(rho, 1));
      y2 = log(gsl_vector_get(eta, 1));

      for (i = 1; i < n - 1; ++i)
        {
          /*
           * The points (x1,y1), (x2,y2), (x3,y3) are the previous,
           * current, and next point on the L-curve. We will find
           * the circle which fits these 3 points and take its radius
           * as an estimate of the curvature at this point.
           */
          double x3 = log(gsl_vector_get(rho, i + 1));
          double y3 = log(gsl_vector_get(eta, i + 1));

          double x21 = x2 - x1;
          double y21 = y2 - y1;
          double x31 = x3 - x1;
          double y31 = y3 - y1;
          double h21 = x21*x21 + y21*y21;
          double h31 = x31*x31 + y31*y31;
          double d = fabs(2.0 * (x21*y31 - x31*y21));
          double r = sqrt(h21*h31*((x3-x2)*(x3-x2)+(y3-y2)*(y3-y2))) / d;

          /* if d =~ 0 then there are nearly colinear points */
          if (gsl_finite(r))
            {
              /* check for smallest radius of curvature */
              if (r < rmin || rmin < 0.0)
                {
                  rmin = r;
                  *idx = i;
                }
            }

          /* update previous/current L-curve values */
          x1 = x2;
          y1 = y2;
          x2 = x3;
          y2 = y3;
        }

      /* check if a minimum radius was found */
      if (rmin < 0.0)
        {
          /* possibly co-linear points */
          GSL_ERROR("failed to find minimum radius", GSL_EINVAL);
        }

      return s;
    }
} /* gsl_multifit_linear_lcorner() */

/*
gsl_multifit_linear_lcorner2()
  Determine point on L-curve (lambda^2, ||c||^2) of maximum curvature.
For each set of 3 points on the L-curve, the circle which passes through
the 3 points is computed. The radius of the circle is then used
as an estimate of the curvature at the middle point. The point
with maximum curvature is then selected.

This routine is based on the paper

M. Rezghi and S. M. Hosseini, "A new variant of L-curve for Tikhonov
regularization", J. Comp. App. Math., 231 (2009).

Inputs: reg_param - vector of regularization parameters
        eta       - vector of solution norms ||L x||
        idx       - (output) index i such that
                    (lambda(i)^2,eta(i)^2) is the point of
                    maximum curvature

Return: success/error
*/

int
gsl_multifit_linear_lcorner2(const gsl_vector *reg_param,
                             const gsl_vector *eta,
                             size_t *idx)
{
  const size_t n = reg_param->size;

  if (n < 3)
    {
      GSL_ERROR ("at least 3 points are needed for L-curve analysis",
                 GSL_EBADLEN);
    }
  else if (n != eta->size)
    {
      GSL_ERROR ("size of reg_param and eta vectors do not match",
                 GSL_EBADLEN);
    }
  else
    {
      int s = GSL_SUCCESS;
      size_t i;
      double x1, y1;      /* first point of triangle on L-curve */
      double x2, y2;      /* second point of triangle on L-curve */
      double rmin = -1.0; /* minimum radius of curvature */

      /* initial values */
      x1 = gsl_vector_get(reg_param, 0);
      x1 *= x1;
      y1 = gsl_vector_get(eta, 0);
      y1 *= y1;

      x2 = gsl_vector_get(reg_param, 1);
      x2 *= x2;
      y2 = gsl_vector_get(eta, 1);
      y2 *= y2;

      for (i = 1; i < n - 1; ++i)
        {
          /*
           * The points (x1,y1), (x2,y2), (x3,y3) are the previous,
           * current, and next point on the L-curve. We will find
           * the circle which fits these 3 points and take its radius
           * as an estimate of the curvature at this point.
           */
          double lamip1 = gsl_vector_get(reg_param, i + 1);
          double etaip1 = gsl_vector_get(eta, i + 1);
          double x3 = lamip1 * lamip1;
          double y3 = etaip1 * etaip1;

          double x21 = x2 - x1;
          double y21 = y2 - y1;
          double x31 = x3 - x1;
          double y31 = y3 - y1;
          double h21 = x21*x21 + y21*y21;
          double h31 = x31*x31 + y31*y31;
          double d = fabs(2.0 * (x21*y31 - x31*y21));
          double r = sqrt(h21*h31*((x3-x2)*(x3-x2)+(y3-y2)*(y3-y2))) / d;

          /* if d =~ 0 then there are nearly colinear points */
          if (gsl_finite(r))
            {
              /* check for smallest radius of curvature */
              if (r < rmin || rmin < 0.0)
                {
                  rmin = r;
                  *idx = i;
                }
            }

          /* update previous/current L-curve values */
          x1 = x2;
          y1 = y2;
          x2 = x3;
          y2 = y3;
        }

      /* check if a minimum radius was found */
      if (rmin < 0.0)
        {
          /* possibly co-linear points */
          GSL_ERROR("failed to find minimum radius", GSL_EINVAL);
        }

      return s;
    }
} /* gsl_multifit_linear_lcorner2() */

#define GSL_MULTIFIT_MAXK      100

/*
gsl_multifit_linear_L()
  Compute discrete approximation to derivative operator of order
k on a regular grid of p points, ie: L is (p-k)-by-p
*/

int
gsl_multifit_linear_Lk(const size_t p, const size_t k, gsl_matrix *L)
{
  if (p <= k)
    {
      GSL_ERROR("p must be larger than derivative order", GSL_EBADLEN);
    }
  else if (k >= GSL_MULTIFIT_MAXK - 1)
    {
      GSL_ERROR("derivative order k too large", GSL_EBADLEN);
    }
  else if (p - k != L->size1 || p != L->size2)
    {
      GSL_ERROR("L matrix must be (p-k)-by-p", GSL_EBADLEN);
    }
  else
    {
      double c_data[GSL_MULTIFIT_MAXK];
      gsl_vector_view cv = gsl_vector_view_array(c_data, k + 1);
      size_t i, j;

      /* zeroth derivative */
      if (k == 0)
        {
          gsl_matrix_set_identity(L);
          return GSL_SUCCESS;
        }

      gsl_matrix_set_zero(L);

      gsl_vector_set_zero(&cv.vector);
      gsl_vector_set(&cv.vector, 0, -1.0);
      gsl_vector_set(&cv.vector, 1, 1.0);

      for (i = 1; i < k; ++i)
        {
          double cjm1 = 0.0;

          for (j = 0; j < k + 1; ++j)
            {
              double cj = gsl_vector_get(&cv.vector, j);

              gsl_vector_set(&cv.vector, j, cjm1 - cj);
              cjm1 = cj;
            }
        }

      /* build L, the c_i are the entries on the diagonals */
      for (i = 0; i < k + 1; ++i)
        {
          gsl_vector_view v = gsl_matrix_superdiagonal(L, i);
          double ci = gsl_vector_get(&cv.vector, i);

          gsl_vector_set_all(&v.vector, ci);
        }

      return GSL_SUCCESS;
    }
} /* gsl_multifit_linear_Lk() */

/*
gsl_multifit_linear_Lsobolev()
  Construct Sobolev smoothing norm operator

L = [ a_0 I; a_1 L_1; a_2 L_2; ...; a_k L_k ]

by computing the Cholesky factor of L^T L

Inputs: p     - number of columns of L
        kmax  - maximum derivative order (< p)
        alpha - vector of weights; alpha_k multiplies L_k, size kmax + 1
        L     - (output) upper triangular Sobolev matrix p-by-p,
                stored in upper triangle
        work  - workspace

Notes:
1) work->Q is used to store intermediate L_k matrices
*/

int
gsl_multifit_linear_Lsobolev(const size_t p, const size_t kmax,
                             const gsl_vector *alpha, gsl_matrix *L,
                             gsl_multifit_linear_workspace *work)
{
  if (p > work->pmax)
    {
      GSL_ERROR("p is larger than workspace", GSL_EBADLEN);
    }
  else if (p <= kmax)
    {
      GSL_ERROR("p must be larger than derivative order", GSL_EBADLEN);
    }
  else if (kmax + 1 != alpha->size)
    {
      GSL_ERROR("alpha must be size kmax + 1", GSL_EBADLEN);
    }
  else if (p != L->size1)
    {
      GSL_ERROR("L matrix is wrong size", GSL_EBADLEN);
    }
  else if (L->size1 != L->size2)
    {
      GSL_ERROR("L matrix is not square", GSL_ENOTSQR);
    }
  else
    {
      int s;
      size_t j, k;
      gsl_vector_view d = gsl_matrix_diagonal(L);
      const double alpha0 = gsl_vector_get(alpha, 0);

      /* initialize L to alpha0^2 I */
      gsl_matrix_set_zero(L);
      gsl_vector_add_constant(&d.vector, alpha0 * alpha0);

      for (k = 1; k <= kmax; ++k)
        {
          gsl_matrix_view Lk = gsl_matrix_submatrix(work->Q, 0, 0, p - k, p);
          double ak = gsl_vector_get(alpha, k);

          /* compute a_k L_k */
          s = gsl_multifit_linear_Lk(p, k, &Lk.matrix);
          if (s)
            return s;

          gsl_matrix_scale(&Lk.matrix, ak);

          /* LTL += L_k^T L_k */
          gsl_blas_dsyrk(CblasLower, CblasTrans, 1.0, &Lk.matrix, 1.0, L);
        }

      s = gsl_linalg_cholesky_decomp(L);
      if (s)
        return s;

      /* copy Cholesky factor to upper triangle and zero out bottom */
      gsl_matrix_transpose_tricpy('L', 1, L, L);

      for (j = 0; j < p; ++j)
        {
          for (k = 0; k < j; ++k)
            gsl_matrix_set(L, j, k, 0.0);
        }

      return GSL_SUCCESS;
    }
}


/*
 * Copyright (c) 2011-2018 The University of Tennessee and The University
 *                         of Tennessee Research Foundation.  All rights
 *                         reserved.
 * Copyright (c) 2013      Inria. All rights reserved.
 *
 * @precisions normal z -> c d s
 *
 */

#include "dplasma.h"
#include "dplasmatypes.h"
#include "dplasmaaux.h"
#include "parsec/data_dist/matrix/two_dim_rectangle_cyclic.h"
#include "parsec/data_dist/matrix/vector_two_dim_cyclic.h"

#include "zpltmg_chebvand.h"
#include "zpltmg_fiedler.h"
#include "zpltmg_hankel.h"
#include "zpltmg_toeppd.h"

#include <string.h>
#include <lapacke.h>

/**
 *******************************************************************************
 *
 *  Generic case
 *
 *******************************************************************************
 */
struct zpltmg_args_s {
    PLASMA_enum            mtxtype;
    unsigned long long int seed;
    parsec_complex64_t     *W;
};
typedef struct zpltmg_args_s zpltmg_args_t;

static int
dplasma_zpltmg_generic_operator( parsec_execution_stream_t *es,
                                 const parsec_tiled_matrix_dc_t *descA,
                                 void *_A,
                                 PLASMA_enum uplo, int m, int n,
                                 void *op_data )
{
    int tempmm, tempnn, ldam;
    zpltmg_args_t     *args = (zpltmg_args_t*)op_data;
    parsec_complex64_t *A    = (parsec_complex64_t*)_A;
    (void)es;
    (void)uplo;

    tempmm = (m == (descA->mt-1)) ? (descA->m - m * descA->mb) : descA->mb;
    tempnn = (n == (descA->nt-1)) ? (descA->n - n * descA->nb) : descA->nb;
    ldam   = BLKLDD( descA, m );

    if ( args->mtxtype == PlasmaMatrixCircul ) {
        return CORE_zpltmg_circul(
            tempmm, tempnn, A, ldam,
            descA->m, m*descA->mb, n*descA->nb, args->W );
    } else {
        return CORE_zpltmg(
            args->mtxtype, tempmm, tempnn, A, ldam,
            descA->m, descA->n, m*descA->mb, n*descA->nb, args->seed );
    }
}

/**
 *******************************************************************************
 *
 * @ingroup dplasma_complex64
 *
 * dplasma_zpltmg_generic - Generic wrapper for cases that are based on the map
 * function. This is the default for many test matrices generation.
 *
 * See parsec_apply() for further information.
 *
 *******************************************************************************
 *
 * @param[in,out] parsec
 *          The parsec context of the application that will run the operation.
 *
 * @param[in] mtxtype
 *          Type of matrix to be generated.
 *
 * @param[in,out] A
 *          Descriptor of the distributed matrix A to generate. Any tiled matrix
 *          descriptor can be used.
 *
 * @param[out] W
 *          Workspace required by some generators.
 *
 * @param[in] seed
 *          The seed used in the random generation.
 *
 *******************************************************************************
 *
 * @return
 *          \retval -i if the ith parameters is incorrect.
 *          \retval 0 on success.
 *
 *******************************************************************************
 *
 * @sa dplasma_cpltmg_genvect
 * @sa dplasma_dpltmg_genvect
 * @sa dplasma_spltmg_genvect
 *
 ******************************************************************************/
static inline int
dplasma_zpltmg_generic( parsec_context_t *parsec,
                        PLASMA_enum mtxtype,
                        parsec_tiled_matrix_dc_t *A,
                        parsec_complex64_t *W,
                        unsigned long long int seed)
{
    parsec_taskpool_t *parsec_zpltmg = NULL;
    zpltmg_args_t *params = (zpltmg_args_t*)malloc(sizeof(zpltmg_args_t));

    params->mtxtype = mtxtype;
    params->seed    = seed;
    params->W       = W;

    parsec_zpltmg = parsec_apply_New( PlasmaUpperLower, A, dplasma_zpltmg_generic_operator, params );
    if ( parsec_zpltmg != NULL )
    {
        parsec_context_add_taskpool(parsec, (parsec_taskpool_t*)parsec_zpltmg);
        dplasma_wait_until_completion(parsec);
        parsec_apply_Destruct( parsec_zpltmg );
        return 0;
    }
    return -101;
}

/**
 *******************************************************************************
 *
 * @ingroup dplasma_complex64
 *
 * dplasma_zpltmg_genvect - Generic wrapper for cases that are using two
 * datatypes: the default one, and one describing a vector.
 *
 *******************************************************************************
 *
 * @param[in,out] parsec
 *          The parsec context of the application that will run the operation.
 *
 * @param[in] mtxtype
 *          Type of matrix to be generated.
 *
 * @param[in,out] A
 *          Descriptor of the distributed matrix A to generate. Any tiled matrix
 *          descriptor can be used.
 *
 * @param[in] seed
 *          The seed used in the random generation.
 *
 *******************************************************************************
 *
 * @return
 *          \retval -i if the ith parameters is incorrect.
 *          \retval 0 on success.
 *
 *******************************************************************************
 *
 * @sa dplasma_cpltmg_genvect
 * @sa dplasma_dpltmg_genvect
 * @sa dplasma_spltmg_genvect
 *
 ******************************************************************************/
static inline int
dplasma_zpltmg_genvect( parsec_context_t *parsec,
                        PLASMA_enum mtxtype,
                        parsec_tiled_matrix_dc_t *A,
                        unsigned long long int seed )
{
    size_t vectorsize = 0;
    parsec_taskpool_t* tp;

    switch( mtxtype ) {
    case PlasmaMatrixChebvand:
        tp = (parsec_taskpool_t*)parsec_zpltmg_chebvand_new( seed,
                                                            A );
        vectorsize = 2 * A->nb * sizeof(parsec_complex64_t);
        break;

    case PlasmaMatrixFiedler:
        tp = (parsec_taskpool_t*)parsec_zpltmg_fiedler_new( seed,
                                                            A );
        vectorsize = A->mb * sizeof(parsec_complex64_t);
        break;

    case PlasmaMatrixHankel:
        tp = (parsec_taskpool_t*)parsec_zpltmg_hankel_new( seed,
                                                           A );
        vectorsize = A->mb * sizeof(parsec_complex64_t);
        break;

    case PlasmaMatrixToeppd:
        tp = (parsec_taskpool_t*)parsec_zpltmg_toeppd_new( seed,
                                                           A );
        vectorsize = 2 * A->mb * sizeof(parsec_complex64_t);
        break;

    default:
        return -2;
    }

    if (tp != NULL) {
        parsec_zpltmg_hankel_taskpool_t *zpltmg_tp = (parsec_zpltmg_hankel_taskpool_t*)tp;

        /* Default type */
        dplasma_add2arena_tile( zpltmg_tp->arenas[PARSEC_zpltmg_hankel_DEFAULT_ARENA],
                                A->mb*A->nb*sizeof(parsec_complex64_t),
                                PARSEC_ARENA_ALIGNMENT_SSE,
                                parsec_datatype_double_complex_t, A->mb );

        /* Vector type */
        dplasma_add2arena_tile( zpltmg_tp->arenas[PARSEC_zpltmg_hankel_VECTOR_ARENA],
                                vectorsize,
                                PARSEC_ARENA_ALIGNMENT_SSE,
                                parsec_datatype_double_complex_t, A->mb );

        parsec_context_add_taskpool(parsec, tp);
        dplasma_wait_until_completion(parsec);

        parsec_matrix_del2arena( zpltmg_tp->arenas[PARSEC_zpltmg_hankel_DEFAULT_ARENA] );
        parsec_matrix_del2arena( zpltmg_tp->arenas[PARSEC_zpltmg_hankel_VECTOR_ARENA ] );
        parsec_taskpool_free(tp);
        return 0;
    }
    return -101;
}

/**
 *******************************************************************************
 *
 * @ingroup dplasma_complex64
 *
 * dplasma_zpltmg_circul - Generates a Circulant test matrix by tiles.
 *
 *******************************************************************************
 *
 * @param[in,out] parsec
 *          The parsec context of the application that will run the operation.
 *
 * @param[in,out] A
 *          Descriptor of the distributed matrix A to generate. Any tiled matrix
 *          descriptor can be used.
 *
 * @param[in] seed
 *          The seed used in the random generation.
 *
 *******************************************************************************
 *
 * @return
 *          \retval -i if the ith parameters is incorrect.
 *          \retval 0 on success.
 *
 *******************************************************************************
 *
 * @sa dplasma_cpltmg_circul
 * @sa dplasma_dpltmg_circul
 * @sa dplasma_spltmg_circul
 *
 ******************************************************************************/
static inline int
dplasma_zpltmg_circul( parsec_context_t *parsec,
                       parsec_tiled_matrix_dc_t *A,
                       unsigned long long int seed )
{
    int info;
    parsec_complex64_t *V = (parsec_complex64_t*) malloc( A->m * sizeof(parsec_complex64_t) );

    CORE_zplrnt( A->m, 1, V, A->m, A->m, 0, 0, seed );

    info = dplasma_zpltmg_generic(parsec, PlasmaMatrixCircul, A, V, seed);

    free(V);
    return info;
}

/**
 *******************************************************************************
 *
 * @ingroup dplasma_internal
 *
 * dplasma_zpltmg_condex - Generates a Condex test matrix by tiles.
 *
 *******************************************************************************
 *
 * @param[in,out] parsec
 *          The parsec context of the application that will run the operation.
 *
 * @param[in,out] A
 *          Descriptor of the distributed matrix A to generate. Any tiled matrix
 *          descriptor can be used.
 *
 *******************************************************************************
 *
 * @return
 *          \retval -i if the ith parameters is incorrect.
 *          \retval 0 on success.
 *
 *******************************************************************************
 *
 * @sa dplasma_cpltmg_condex
 * @sa dplasma_dpltmg_condex
 * @sa dplasma_spltmg_condex
 *
 ******************************************************************************/
static inline int
dplasma_zpltmg_condex( parsec_context_t *parsec,
                       parsec_tiled_matrix_dc_t *A )
{
    /* gallery('condex', A->m, 4, 100.) */
    parsec_complex64_t theta = 100.;
    two_dim_block_cyclic_t *twodA = (two_dim_block_cyclic_t *)A;
    two_dim_block_cyclic_t Q;
    two_dim_block_cyclic_init( &Q, matrix_ComplexDouble, matrix_Tile,
                               1, A->super.myrank,
                               A->mb, A->nb, A->mb*A->mt, 3, 0, 0, A->m, 3, twodA->grid.strows, twodA->grid.stcols, 1 );
    Q.mat = parsec_data_allocate((size_t)Q.super.nb_local_tiles *
                                (size_t)Q.super.bsiz *
                                (size_t)parsec_datadist_getsizeoftype(Q.super.mtype));
    parsec_data_collection_set_key((parsec_data_collection_t*)&Q, "Q");

    if (A->super.myrank == 0) {
        parsec_complex64_t *Qmat;

        Qmat = (parsec_complex64_t*)(Q.mat);

        /* Initialize the Q matrix */
        CORE_zpltmg_condexq( A->m, A->n, Qmat, Q.super.lm );

        /*
         * Conversion to tile layout
         */
        {
            parsec_complex64_t *W = (parsec_complex64_t*) malloc (A->mb * sizeof(parsec_complex64_t) );
            int *leaders = NULL;
            int i, nleaders;

            /* Get all the cycles leaders and length
             * They are the same for each independent problem (each panel) */
            GKK_getLeaderNbr( Q.super.lmt, A->nb, &nleaders, &leaders );

            /* shift cycles. */
            for(i=0; i<nleaders; i++) {

                /* cycle #i belongs to this thread, so shift it */
                memcpy(W, Qmat + leaders[i*3] * A->mb, A->mb * sizeof(parsec_complex64_t) );
                CORE_zshiftw(leaders[i*3], leaders[i*3+1], A->mt, A->nb, A->mb, Qmat, W);
            }

            free(leaders); free(W);
        }
    }

    dplasma_zlaset( parsec, PlasmaUpperLower, 0., 1. + theta, A );
    dplasma_zgemm( parsec, PlasmaNoTrans, PlasmaConjTrans,
                   -theta, (parsec_tiled_matrix_dc_t*)&Q,
                           (parsec_tiled_matrix_dc_t*)&Q,
                   1.,     A );

    parsec_data_free(Q.mat);
    parsec_tiled_matrix_dc_destroy((parsec_tiled_matrix_dc_t*)&Q);
    return 0;
}

/**
 *******************************************************************************
 *
 * @ingroup dplasma_complex64
 *
 * dplasma_zpltmg_house - Generates a Householder test matrix by tiles.
 *
 *******************************************************************************
 *
 * @param[in,out] parsec
 *          The parsec context of the application that will run the operation.
 *
 * @param[in,out] A
 *          Descriptor of the distributed matrix A to generate. Any tiled matrix
 *          descriptor can be used.
 *
 * @param[in] seed
 *          The seed used in the random generation.
 *
 *******************************************************************************
 *
 * @return
 *          \retval -i if the ith parameters is incorrect.
 *          \retval 0 on success.
 *
 *******************************************************************************
 *
 * @sa dplasma_cpltmg_house
 * @sa dplasma_dpltmg_house
 * @sa dplasma_spltmg_house
 *
 ******************************************************************************/
static inline int
dplasma_zpltmg_house( parsec_context_t *parsec,
                      parsec_tiled_matrix_dc_t *A,
                      unsigned long long int seed )
{
    /* gallery('house', random, 0 ) */
    vector_two_dim_cyclic_t V;
    parsec_complex64_t *Vmat, tau;

    vector_two_dim_cyclic_init( &V, matrix_ComplexDouble, PlasmaVectorDiag,
                                1, A->super.myrank,
                                A->mb, A->m, 0, A->m, 1 );
    V.mat = parsec_data_allocate((size_t)V.super.nb_local_tiles *
                                (size_t)V.super.bsiz *
                                (size_t)parsec_datadist_getsizeoftype(V.super.mtype));
    parsec_data_collection_set_key((parsec_data_collection_t*)&V, "V");
    Vmat = (parsec_complex64_t*)(V.mat);

    /* Initialize Householder vector */
    if (A->super.myrank == 0) {
        CORE_zplrnt( A->m, 1, Vmat, A->m, A->m, 0, 0, seed );
        LAPACKE_zlarfg_work( A->m, Vmat, Vmat+1, 1, &tau );
        Vmat[0] = 1.;
    }

#if defined(PARSEC_HAVE_MPI)
    /* If we don't need to broadcast, don't do it, this way we don't require MPI to be initialized */
    if( A->super.nodes > 1 )
        MPI_Bcast( &tau, 1, parsec_datatype_double_complex_t, 0, *(MPI_Comm*)dplasma_pcomm );
#endif

    /* Compute the Householder matrix I - tau v * v' */
    dplasma_zlaset( parsec, PlasmaUpperLower, 0., 1., A);
    dplasma_zgerc( parsec, -tau,
                   (parsec_tiled_matrix_dc_t*)&V,
                   (parsec_tiled_matrix_dc_t*)&V,
                   A );

    parsec_data_free(V.mat);
    parsec_tiled_matrix_dc_destroy((parsec_tiled_matrix_dc_t*)&V);

    return 0;
}

/**
 *******************************************************************************
 *
 * @ingroup dplasma_complex64
 *
 * dplasma_zpltmg - Generates a special test matrix by tiles.
 *
 *******************************************************************************
 *
 * @param[in,out] parsec
 *          The parsec context of the application that will run the operation.
 *
 * @param[in] mtxtype
 *          See PLASMA_zpltmg() for possible values and information on generated
 *          matrices.
 *
 * @param[in,out] A
 *          Descriptor of the distributed matrix A to generate. Any tiled matrix
 *          descriptor can be used.
 *          On exit, the matrix A generated.
 *
 * @param[in] seed
 *          The seed used in the random generation.
 *
 *******************************************************************************
 *
 * @return
 *          \retval -i if the ith parameters is incorrect.
 *          \retval 0 on success.
 *
 *******************************************************************************
 *
 * @sa dplasma_cpltmg
 * @sa dplasma_dpltmg
 * @sa dplasma_spltmg
 *
 ******************************************************************************/
int
dplasma_zpltmg( parsec_context_t *parsec,
                PLASMA_enum mtxtype,
                parsec_tiled_matrix_dc_t *A,
                unsigned long long int seed)
{

    switch( mtxtype ) {
    case PlasmaMatrixCircul:
        return dplasma_zpltmg_circul(parsec, A, seed);
        break;

    case PlasmaMatrixChebvand:
        return dplasma_zpltmg_genvect(parsec, mtxtype,
                                      A, seed);
        break;

    case PlasmaMatrixCondex:
        return dplasma_zpltmg_condex(parsec, A);
        break;

    case PlasmaMatrixFiedler:
        return dplasma_zpltmg_genvect(parsec, mtxtype,
                                      A, seed);
        break;

    case PlasmaMatrixHankel:
        return dplasma_zpltmg_genvect(parsec, mtxtype,
                                      A, seed);
        break;

    case PlasmaMatrixHouse:
        return dplasma_zpltmg_house(parsec, A, seed);
        break;

    case PlasmaMatrixToeppd:
        return dplasma_zpltmg_genvect(parsec, mtxtype,
                                      A, seed);
        break;

    case PlasmaMatrixCauchy:
    case PlasmaMatrixCompan:
    case PlasmaMatrixDemmel:
    case PlasmaMatrixDorr:
    case PlasmaMatrixFoster:
    case PlasmaMatrixHadamard:
    case PlasmaMatrixHilb:
    case PlasmaMatrixInvhess:
    case PlasmaMatrixKms:
    case PlasmaMatrixLangou:
    case PlasmaMatrixLehmer:
    case PlasmaMatrixLotkin:
    case PlasmaMatrixMinij:
    case PlasmaMatrixMoler:
    case PlasmaMatrixOrthog:
    case PlasmaMatrixParter:
    case PlasmaMatrixRandom:
    case PlasmaMatrixRiemann:
    case PlasmaMatrixRis:
    case PlasmaMatrixWilkinson:
    case PlasmaMatrixWright:
        return dplasma_zpltmg_generic(parsec, mtxtype, A, NULL, seed);
        break;
    default:
        return -2;
    }

    return 0;
}