#include "asf_sar.h"
#include <asf.h>
#include <asf_meta.h>
#include <asf_raster.h>

#include <glib.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_statistics_double.h>
#include <assert.h>

static double min2(double a, double b)
{
    return a<b ? a : b;
}

static double min4(double a, double b, double c, double d)
{
    return min2(min2(a,b), min2(c,d));
}

static double max2(double a, double b)
{
    return a>b ? a : b;
}

static double max4(double a, double b, double c, double d)
{
    return max2(max2(a,b), max2(c,d));
}

static void ts2ls(meta_parameters *meta, double time, double slant,
                  double *pLine, double *pSamp)
{
  double lat, lon;

  meta_timeSlantDop2latLon(meta, time, slant, 0, 0, &lat, &lon);
  meta_get_lineSamp(meta, lat, lon, 0, pLine, pSamp);

  //printf("ts2ls: %f,%f -> %f,%f -> %f,%f\n",
  //       time,slant, lat,lon, *pLine,*pSamp);
}

static int
proj_to_sr(const char *infile, const char *outfile, double pixel_size)
{
    int ii, jj, kk;
    const float_image_sample_method_t sampling_method =
        FLOAT_IMAGE_SAMPLE_METHOD_BILINEAR;

    // overall algorithm:
    // 1. find extents in time/slant space
    // 2. for each pixel in output, resample in input space

    meta_parameters *inMeta = meta_read(infile);
    int nl = inMeta->general->line_count;
    int ns = inMeta->general->sample_count;

    if (!inMeta->projection && !inMeta->transform)
        asfPrintError("Expected a projection/transform block!\n");
    if (!inMeta->state_vectors)
        asfPrintError("Input data does not have state vectors!\n");

    //asfPrintStatus("Converting %s to slant range...\n", infile);

    // first, find extents in time/slant space
    // do this by projecting image corners to time/slant
    int tl_x=0,    tl_y=0;
    int tr_x=ns-1, tr_y=0;
    int bl_x=0,    bl_y=nl-1;
    int br_x=ns-1, br_y=nl-1;

    // we have to find the "real" corners of the image
    // do this using the first band of the input image as a reference
    if (inMeta->general->band_count == 1)
        asfPrintStatus("Tiling the input image...\n");
    else
        asfPrintStatus("Tiling the reference band of the input image...\n");

    FloatImage *in = float_image_new_from_metadata(inMeta, infile);

    // find top left pixel -- TOP-most non-no-data pixel in the image
    for (ii=0; ii<nl; ++ii)
        for (jj=0; jj<ns; ++jj) {
            double val = float_image_get_pixel(in, jj, ii);
            if (val != inMeta->general->no_data && val != 0.0) {
                tl_x = jj; tl_y = ii;
                goto found_tl;
            }
        }

    asfPrintError("Couldn't find top-left pixel! Entire image no data?\n");

  found_tl:

    // find top right pixel -- RIGHT-most non-no-data pixel in the image
    for (jj=ns-1; jj>=0; --jj)
        for (ii=0; ii<nl; ++ii) {
            double val = float_image_get_pixel(in, jj, ii);
            if (val != inMeta->general->no_data && val != 0.0) {
                tr_x = jj; tr_y = ii;
                goto found_tr;
            }
        }

    asfPrintError("Couldn't find top-right pixel! Entire image no data?\n");

  found_tr:

    // find bottom left pixel -- LEFT-most non-no-data pixel in the image
    for (jj=0; jj<ns; ++jj)
        for (ii=nl-1; ii>=0; --ii) {
            double val = float_image_get_pixel(in, jj, ii);
            if (val != inMeta->general->no_data && val != 0.0) {
                bl_x = jj; bl_y = ii;
                goto found_bl;
            }
        }

    asfPrintError("Couldn't find bottom-left pixel! Entire image no data?\n");

  found_bl:

    // find bottom right pixel -- BOTTOM-most non-no-data pixel in the image
    for (ii=nl-1; ii>=0; --ii)
        for (jj=ns-1; jj>=0; --jj) {
            double val = float_image_get_pixel(in, jj, ii);
            if (val != inMeta->general->no_data && val != 0.0) {
                br_x = jj; br_y = ii;
                goto found_br;
            }
        }

    asfPrintError("Couldn't find bottom-right pixel! Entire image no data?\n");

  found_br:

    asfPrintStatus("Determining image extents in time/slant coordinates.\n");
    //asfPrintStatus("Corners are at: TL (%d,%d)\n", tl_y, tl_x);
    //asfPrintStatus(" (line,sample)  TR (%d,%d)\n", tr_y, tr_x);
    //asfPrintStatus("                BL (%d,%d)\n", bl_y, bl_x);
    //asfPrintStatus("                BR (%d,%d)\n", br_y, br_x);

    double tl_time, tl_slant;
    double tr_time, tr_slant;
    double bl_time, bl_slant;
    double br_time, br_slant;

    meta_get_timeSlantDop(inMeta, tl_y, tl_x, &tl_time, &tl_slant, NULL);
    meta_get_timeSlantDop(inMeta, tr_y, tr_x, &tr_time, &tr_slant, NULL);
    meta_get_timeSlantDop(inMeta, bl_y, bl_x, &bl_time, &bl_slant, NULL);
    meta_get_timeSlantDop(inMeta, br_y, br_x, &br_time, &br_slant, NULL);

    //asfPrintStatus("Corners are at: TL (%f,%f)\n", tl_time, tl_slant);
    //asfPrintStatus(" (time,slant)   TR (%f,%f)\n", tr_time, tr_slant);
    //asfPrintStatus("                BL (%f,%f)\n", bl_time, bl_slant);
    //asfPrintStatus("                BR (%f,%f)\n", br_time, br_slant);

    double slant_start = min4(tl_slant, tr_slant, bl_slant, br_slant);
    double slant_end = max4(tl_slant, tr_slant, bl_slant, br_slant);

    double time_min = min4(tl_time, tr_time, bl_time, br_time);
    double time_max = max4(tl_time, tr_time, bl_time, br_time);

    double slant_incr;
    double time_start, time_end, time_incr;
    int onl, ons;

    if (pixel_size > 0) {
        slant_incr = pixel_size;
        ons = (slant_end - slant_start) / slant_incr;

        if (inMeta->sar) {
            // in this case, the original data has a SAR block, we will use the
            // same azimuth time per pixel.
            time_incr = inMeta->sar->azimuth_time_per_pixel;

            // we always want to be DECREASING in time
            // latest time is on top (line 1), earliest on bottom (line ONL)
            if (time_incr > 0) {
                time_incr = -time_incr;
                inMeta->sar->azimuth_time_per_pixel =
                    -inMeta->sar->azimuth_time_per_pixel;
            }

            time_start = time_max;
            time_end = time_min;

            onl = (time_end - time_start) / time_incr;
        }
        else {
            // here, no sar block in the original data, just make a square
            // image with decreasing time
            onl = ons;
            time_incr = (time_min - time_max) / (double)onl;
            time_start = time_max;
            time_end = time_min;
        }
    }
    else {
        // not provided a slant range pixel size, we'll figure something out

        if (inMeta->sar) {
            // use the same azimuth time per pixel.
            time_incr = inMeta->sar->azimuth_time_per_pixel;

            // we always want to be DECREASING in time
            // latest time is on top (line 1), earliest on bottom (line ONL)
            if (time_incr > 0) {
                time_incr = -time_incr;
                inMeta->sar->azimuth_time_per_pixel =
                    -inMeta->sar->azimuth_time_per_pixel;
            }

            time_start = time_max;
            time_end = time_min;

            onl = (time_end - time_start) / time_incr;
        }
        else {
            // no info... determine azimuth time per pixel by keeping
            // the height the same as in the original image
            onl = nl;
            time_incr = (time_min - time_max) / (double)onl;
            time_start = time_max;
            time_end = time_min;
        }

        // make it square, to get the slant range pixel size
        ons = onl;
        pixel_size = slant_incr = (slant_end - slant_start) / (double)ons;
    }

    asfRequire(onl > 0, "Internal Error: Invalid output line count: %d\n", onl);
    asfRequire(ons > 0, "Internal Error: Invalid output sample count: %d\n", ons);

    asfPrintStatus("  Slant range values: %f -> %f\n", slant_start, slant_end);
    asfPrintStatus("  Slant range pixel size: %f\n", pixel_size);
    asfPrintStatus("  Time values: %f -> %f\n", time_start, time_end);
    asfPrintStatus("  Output Image will be %5d x %5d LxS\n", onl, ons);
    asfPrintStatus("      (Input Image was %5d x %5d LxS)\n", nl, ns);

    // generate a grid over the image, to generate our splines
    // this grid size seems to work pretty well...
    int n = 120;

    asfPrintStatus("Creating %dx%d mapping grid...\n", n, n);

    double time_grid_incr = ((double)(onl))/((double)(n)) * time_incr;
    double slant_grid_incr = ((double)(ons))/((double)(n)) * slant_incr;

    // allocating memory for the splines, and the arrays to generate them
    gsl_interp_accel **samp_accels = MALLOC(sizeof(gsl_interp_accel *) * n);
    gsl_spline **samp_splines = MALLOC(sizeof(gsl_spline *) * n);

    gsl_interp_accel **line_accels = MALLOC(sizeof(gsl_interp_accel *) * n);
    gsl_spline **line_splines = MALLOC(sizeof(gsl_spline *) * n);

    double *slant_in = MALLOC(sizeof(double)*n);
    double *line_out = MALLOC(sizeof(double)*n);
    double *samp_out = MALLOC(sizeof(double)*n);

    // an alias -- use the same array (to save memory -- these are not used
    // at the same time), but create an alias for it, so it is not so confusing
    double *time_in = slant_in;
    //double max_err = 0;

    // set up the vertical splines
    for (jj=0; jj<n; ++jj) {
        double slant = slant_start + jj*slant_grid_incr;

        for (ii=0; ii<n; ++ii) {
            // splines need strictly increasing range variables
            if (time_grid_incr > 0)
                time_in[ii] = time_start + ii*time_grid_incr;
            else
                time_in[ii] = time_end - ii*time_grid_incr;

            ts2ls(inMeta, time_in[ii], slant, &line_out[ii], &samp_out[ii]);
        }

        samp_accels[jj] = gsl_interp_accel_alloc();
        samp_splines[jj] = gsl_spline_alloc(gsl_interp_cspline, n);
        gsl_spline_init(samp_splines[jj], time_in, samp_out, n);

        line_accels[jj] = gsl_interp_accel_alloc();
        line_splines[jj] = gsl_spline_alloc(gsl_interp_cspline, n);
        gsl_spline_init(line_splines[jj], time_in, line_out, n);
    }

    // now, we're on to the resampling stage.. loop through output pixels
    asfPrintStatus("Generating slant range image...\n");

    double no_data_value = meta_is_valid_double(inMeta->general->no_data) ?
        inMeta->general->no_data : 0;

    // keep track of error sizes
    double max_error = 0;
    double avg_error = 0;
    int count = 0;

    // these stride values allow us to track when we're in between grid points
    int ii_n = onl/n;
    int jj_n = ons/n;
    int ii_n2 = ii_n/2;
    int jj_n2 = jj_n/2;

    // set up output metadata
    meta_parameters *outMeta = meta_read(infile);

    if (outMeta->transform) {
        FREE(outMeta->transform);
        outMeta->transform = NULL;
    }

    if (outMeta->projection) {
        FREE(outMeta->projection);
        outMeta->projection = NULL;
    }

    outMeta->general->line_count = onl;
    outMeta->general->sample_count = ons;

    if (!outMeta->sar)
        outMeta->sar = meta_sar_init();

    outMeta->sar->image_type = 'S';

    outMeta->sar->azimuth_time_per_pixel = time_incr;
    assert(outMeta->sar->azimuth_time_per_pixel < 0);

    outMeta->sar->time_shift = time_start;

    outMeta->general->y_pixel_size = inMeta->sar->azimuth_time_per_pixel /
                                     time_incr * inMeta->general->y_pixel_size;
    assert(outMeta->general->y_pixel_size > 0);

    outMeta->sar->slant_range_first_pixel = slant_start;
    outMeta->general->x_pixel_size = slant_incr;

    outMeta->sar->line_increment = outMeta->sar->sample_increment = 1;
    outMeta->general->start_sample = outMeta->general->start_line = 0;

    outMeta->general->no_data = no_data_value;

    char **band_name = extract_band_names(inMeta->general->bands,
                                          inMeta->general->band_count);

    // now generate output image
    char *img_file = appendExt(outfile, ".img");
    float *out = MALLOC(sizeof(float) * ons);

    for (kk=0; kk<inMeta->general->band_count; ++kk) {
        if (inMeta->general->band_count != 1)
            asfPrintStatus("Working on band: %s\n", band_name[kk]);

        // for the 2nd and higher bands, free the band from the previous iteration,
        // and read in the next band from the input image
        if (kk>0) {
            float_image_free(in);
            asfPrintStatus("Loading input...\n");
            in = float_image_band_new_from_metadata(inMeta, kk, infile);
        }

        FILE *ofp = FOPEN(img_file, kk==0 ? "wb" : "ab");
        asfPrintStatus("Generating output...\n");
        for (ii=0; ii<onl; ++ii) {
            asfLineMeter(ii,onl);
            double time = time_start + ii * time_incr;

            // set up horizontal splines for this row
            gsl_interp_accel *samp_accel = gsl_interp_accel_alloc();
            gsl_spline *samp_spline = gsl_spline_alloc(gsl_interp_cspline, n);

            gsl_interp_accel *line_accel = gsl_interp_accel_alloc();
            gsl_spline *line_spline = gsl_spline_alloc(gsl_interp_cspline, n);

            //printf("time: %f slant: %f\n", time, slant_start);
            for (jj=0; jj<n; ++jj) {
                slant_in[jj] = slant_start + jj * slant_grid_incr;
                //printf("time: %f slant: %f\n", time, slant_in[jj]);
                samp_out[jj] = gsl_spline_eval(samp_splines[jj], time,
                                               samp_accels[jj]);
                line_out[jj] = gsl_spline_eval(line_splines[jj], time,
                                               line_accels[jj]);
                //printf("samp_out: %f line_out: %f\n", samp_out[jj], line_out[jj]);
            }

            gsl_spline_init(samp_spline, slant_in, samp_out, n);
            gsl_spline_init(line_spline, slant_in, line_out, n);

            // use the splines to produce output pixels
            for (jj=0; jj<ons; ++jj) {
                double slant = slant_start + jj * slant_incr;
                double samp = gsl_spline_eval(samp_spline, slant, samp_accel);
                double line = gsl_spline_eval(line_spline, slant, line_accel);

                // check the spline every so often (halfway between grid points)
                // only do this on band #1 (the reference band)
                if (kk==0 && ii%ii_n2==0 &&
                    ii%ii_n!=0 && jj%jj_n2==0 && jj%jj_n!=0)
                {
                    double samp_real, line_real;
                    ts2ls(inMeta, time, slant, &line_real, &samp_real);

                    double err = (line-line_real)*(line-line_real) +
                                 (samp-samp_real)*(samp-samp_real);

                    //printf("(%d,%d) -- Actual: (%f,%f) Splined: (%f,%f)\n",
                    //       ii, jj, line_real, samp_real, line, samp);

                    if (err > max_error) max_error = err;
                    avg_error += err;
                    ++count;
                }

                // now interpolate within the original image
                // if we are outside, use "no_data" from metadata
                double val = no_data_value;
                if (line > 0 && line < nl-1 && samp > 0 && samp < ns-1)
                    val = float_image_sample(in, samp, line, sampling_method);

                out[jj] = (float)val;
            }

            gsl_interp_accel_free(samp_accel);
            gsl_spline_free(samp_spline);

            gsl_interp_accel_free(line_accel);
            gsl_spline_free(line_spline);

            put_float_line(ofp, outMeta, ii, out);
        }

        fclose(ofp);
    }

    // free the last band of the input
    float_image_free(in);

    FREE(slant_in);
    FREE(line_out);
    FREE(samp_out);

    for (ii=0; ii<n; ++ii) {
        gsl_interp_accel_free(samp_accels[ii]);
        gsl_spline_free(samp_splines[ii]);
        gsl_interp_accel_free(line_accels[ii]);
        gsl_spline_free(line_splines[ii]);
    }

    FREE(samp_accels);
    FREE(samp_splines);
    FREE(line_accels);
    FREE(line_splines);

    FREE(out);

    for (kk=0; kk<inMeta->general->band_count; ++kk)
        FREE(band_name[kk]);
    FREE(band_name);

    // see how bad our errors were
    avg_error /= (double)count;
    asfPrintStatus("Model max error: %f, avg: %f\n",
                   max_error, avg_error);

    double thresh = 0.1;
    if (max_error > 100*thresh)
        asfPrintError("Maximum error exceeded threshold: %f > %f\n",
                      max_error, 100*thresh);
    else if (avg_error > 10*thresh)
        asfPrintError("Average error exceeded threshold: %f > %f\n",
                      avg_error, 10*thresh);
    if (max_error > 10*thresh)
        asfPrintWarning("Maximum error exceeds threshold: %f > %f\n",
                        max_error, 10*thresh);
    if (avg_error > thresh)
        asfPrintWarning("Average error exceeds threshold: %f > %f\n",
                        avg_error, thresh);

    char *meta_file = appendExt(outfile, ".meta");
    asfPrintStatus("Writing %s\n", meta_file);
    meta_write(outMeta, meta_file);
    free(meta_file);
    free(img_file);

    meta_free(outMeta);
    meta_free(inMeta);

    return 0; //success
}

int to_sr_pixsiz(const char *infile, const char *outfile, double pixel_size)
{
   meta_parameters *inMeta = meta_read(infile);
   int ret;

   if (inMeta->sar && inMeta->sar->image_type == 'G') {
       // ground range image
       ret = gr2sr_pixsiz(infile, outfile, pixel_size);
   }
   else if (inMeta->sar && inMeta->sar->image_type == 'S') {
       // already a slant range image, just copy it
       copyImgAndMeta(infile, outfile);
       ret = 0; // success
   }
   else if ((inMeta->sar && inMeta->sar->image_type == 'P') ||
            (inMeta->projection)) {
       // projected image
       ret = proj_to_sr(infile, outfile, pixel_size);
   }
   else if (inMeta->sar && inMeta->transform &&
            inMeta->sar->image_type == 'R') {
       // georeferenced, ALOS most likely
       ret = proj_to_sr(infile, outfile, pixel_size);
   }
   else {
       asfPrintError("Couldn't figure out what kind of image this is.\n");
       ret = 1;
   }

   meta_free(inMeta);
   return ret;
}

int to_sr(const char *infile, const char *outfile)
{
    char *img_in = MALLOC(sizeof(char)*(strlen(infile)+10));
    char *img_out = MALLOC(sizeof(char)*(strlen(outfile)+10));
    create_name(img_in, infile, ".img");
    create_name(img_out, outfile, ".img");
    int ret = to_sr_pixsiz(img_in, img_out, -1);
    FREE(img_in);
    FREE(img_out);
    return ret;
}


/*
** libproj -- library of cartographic projections
**
** Copyright (c) 2003, 2006   Gerald I. Evenden
*/
static const char
LIBPROJ_ID[] = "Id";
/*
** Permission is hereby granted, free of charge, to any person obtaining
** a copy of this software and associated documentation files (the
** "Software"), to deal in the Software without restriction, including
** without limitation the rights to use, copy, modify, merge, publish,
** distribute, sublicense, and/or sell copies of the Software, and to
** permit persons to whom the Software is furnished to do so, subject to
** the following conditions:
**
** The above copyright notice and this permission notice shall be
** included in all copies or substantial portions of the Software.
**
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
** EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
** MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
** IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
** CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
** TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
** SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#define N_TOL 1e-6
#define PROJ_LIB__
#ifdef PROJ_HAVE_GSL
#include <gsl/gsl_integration.h>
#define GSL_WORK 1000
#define PROJ_PARMS__ \
  double n[2]; \
  gsl_function func; \
  gsl_integration_workspace *work; \
  int mode;
#include  <lib_proj.h>
  double
mayr_kernel(double x, void * n) {
  return (pow(cos(x), *(double*)n));
}
#else
#define PROJ_PARMS__ \
  int mode;
#include  <lib_proj.h>
#endif
PROJ_HEAD(mayr, "Mayr (Tobler Meridian  Geometric Mean)") "\n\tPCyl., Sph., NoInv.";
/* definition of segment bounds for <1e-7 error (unit sphere)  */
#define SEG1 1.4
#define SEG2 1.55
#define SEG3 1.57
#define BASE1 1.151132004484049
#define BASE2 1.196140916241303
#define BASE3 1.19812525384759
#define HALFN 4
/* Gauss Legendre weights/abscissa */
double X[] = {
    0.96028985649753618,
    0.79666647741362673,
    0.52553240991632899,
    0.18343464249564981 };
double W[] = {
    0.10122853629037638,
    0.22238103445337443,
    0.31370664587788744,
    0.36268378337836199 };
/* Mayr kernel */
#define func(v) sqrt(cos(v))
  static
double gauss_legendre(double x0, double x1) {
  int i = 0;
  double s = 0., *x = X,  *w = W, xsize, xmean, arg;

  xmean = 0.5 * (x1 + x0);
  xsize = 0.5 * (x1 - x0);
  while ( i++ < HALFN ) {
    arg = xsize * *x++;
    s += *w++ * (func(xmean - arg) + func(xmean + arg));
  };
  return xsize * s;
}
  static
double integrate(double val) {
  double out;

  if (val <= SEG1)
    out = gauss_legendre(0., val);
  else if (val <= SEG2)
    out = BASE1 + gauss_legendre(SEG1, val);
  else if (val <= SEG3)
    out = BASE2 + gauss_legendre(SEG2, val);
  else
    out = BASE3 + gauss_legendre(SEG3, val);
  return out;
}
FORWARD(s_forward); /* spheroid */
  (void) P; /* avoid warning */

  xy.x = lp.lam * func(lp.phi);
  xy.y = integrate(fabs(lp.phi));
  if (lp.phi < 0.)
    xy.y = -xy.y;
  return (xy);
}
#ifdef PROJ_HAVE_GSL
FORWARD(s_forwardg); /* numerical integration n <> 0.5 */
  double error;

  gsl_integration_qags(&P->func, 0., fabs(lp.phi), 1e-7, 1e-8, GSL_WORK,
    P->work, &xy.y, &error);
  xy.x = lp.lam * pow(cos(lp.phi), P->n[1]);
  if (lp.phi < 0.)
    xy.y = -xy.y;
  return (xy);
}
#endif
FREEUP;
  if (P) {
#if PROJ_HAVE_GSL
    if (P->mode)
      gsl_integration_workspace_free(P->work);
#endif
    free(P);
  }
}
ENTRY0(mayr)
  P->es = 0;
  if (proj_param(P->params, "tn").i) {
#if PROJ_HAVE_GSL
    P->n[0] = proj_param(P->params, "dn").f;
    if ((P->n[0] < N_TOL) || (P->n[0] > 1.-N_TOL))
      E_ERROR(-40)
    P->fwd = s_forwardg;
    P->n[1] = 1. - P->n[0];
    P->func.function = &mayr_kernel;
    P->func.params = P->n;
    P->work = gsl_integration_workspace_alloc (GSL_WORK);
    P->mode = 1;
#else
    E_ERROR(-47)
#endif
  } else {
    P->mode = 0;
    P->fwd = s_forward;
  }
ENDENTRY(P)
/*
** Log: proj_mayr.c
** Revision 3.2  2006/01/19 01:52:59  gie
** correct some casting ^)*^&^
**
** Revision 3.1  2006/01/11 01:38:18  gie
** Initial
**
*/


/* gmres.c
 *
 * Copyright (C) 2014 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.
 */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_spmatrix.h>
#include <gsl/gsl_spblas.h>
#include <gsl/gsl_splinalg.h>

/*
 * The code in this module is based on the Householder GMRES
 * algorithm described in
 *
 * [1] H. F. Walker, Implementation of the GMRES method using
 *     Householder transformations, SIAM J. Sci. Stat. Comput.
 *     9(1), 1988.
 *
 * [2] Y. Saad, Iterative methods for sparse linear systems,
 *     2nd edition, SIAM, 2003.
 */

typedef struct
{
  size_t n;        /* size of linear system */
  size_t m;        /* dimension of Krylov subspace K_m */
  gsl_vector *r;   /* residual vector r = b - A*x */
  gsl_matrix *H;   /* Hessenberg matrix n-by-(m+1) */
  gsl_vector *tau; /* householder scalars */
  gsl_vector *y;   /* least squares rhs and solution vector */

  double *c;       /* Givens rotations */
  double *s;

  double normr;    /* residual norm ||r|| */
} gmres_state_t;

static void gmres_free(void *vstate);
static int gmres_iterate(const gsl_spmatrix *A, const gsl_vector *b,
                         const double tol, gsl_vector *x, void *vstate);

/*
gmres_alloc()
  Allocate a GMRES workspace for solving an n-by-n system A x = b

Inputs: n        - size of system
        krylov_m - size of Krylov subspace (ie: number of inner iterations)
                   if this parameter is 0, the value GSL_MIN(n,10) is
                   used

Return: pointer to workspace
*/

static void *
gmres_alloc(const size_t n, const size_t m)
{
  gmres_state_t *state;

  if (n == 0)
    {
      GSL_ERROR_NULL("matrix dimension n must be a positive integer",
                     GSL_EINVAL);
    }

  state = calloc(1, sizeof(gmres_state_t));
  if (!state)
    {
      GSL_ERROR_NULL("failed to allocate gmres state", GSL_ENOMEM);
    }

  state->n = n;

  /* compute size of Krylov subspace */
  if (m == 0)
    state->m = GSL_MIN(n, 10);
  else
    state->m = GSL_MIN(n, m);

  state->r = gsl_vector_alloc(n);
  if (!state->r)
    {
      gmres_free(state);
      GSL_ERROR_NULL("failed to allocate r vector", GSL_ENOMEM);
    }

  state->H = gsl_matrix_alloc(n, state->m + 1);
  if (!state->H)
    {
      gmres_free(state);
      GSL_ERROR_NULL("failed to allocate H matrix", GSL_ENOMEM);
    }

  state->tau = gsl_vector_alloc(state->m + 1);
  if (!state->tau)
    {
      gmres_free(state);
      GSL_ERROR_NULL("failed to allocate tau vector", GSL_ENOMEM);
    }

  state->y = gsl_vector_alloc(state->m + 1);
  if (!state->y)
    {
      gmres_free(state);
      GSL_ERROR_NULL("failed to allocate y vector", GSL_ENOMEM);
    }

  state->c = malloc(state->m * sizeof(double));
  state->s = malloc(state->m * sizeof(double));
  if (!state->c || !state->s)
    {
      gmres_free(state);
      GSL_ERROR_NULL("failed to allocate Givens vectors", GSL_ENOMEM);
    }

  state->normr = 0.0;

  return state;
} /* gmres_alloc() */

static void
gmres_free(void *vstate)
{
  gmres_state_t *state = (gmres_state_t *) vstate;

  if (state->r)
    gsl_vector_free(state->r);

  if (state->H)
    gsl_matrix_free(state->H);

  if (state->tau)
    gsl_vector_free(state->tau);

  if (state->y)
    gsl_vector_free(state->y);

  if (state->c)
    free(state->c);

  if (state->s)
    free(state->s);

  free(state);
} /* gmres_free() */

/*
gmres_iterate()
  Solve A*x = b using GMRES algorithm

Inputs: A    - sparse square matrix
        b    - right hand side vector
        tol  - stopping tolerance (see below)
        x    - (input/output) on input, initial estimate x_0;
               on output, solution vector
        work - workspace

Return:
GSL_SUCCESS if converged to solution (solution stored in x). In
this case the following will be true:

||b - A*x|| <= tol * ||b||

GSL_CONTINUE if not yet converged; in this case x contains the
most recent solution vector and calling this function more times
with the input x could result in convergence (ie: restarted GMRES)

Notes:
1) Based on algorithm 2.2 of (Walker, 1998 [1]) and algorithm 6.10 of
(Saad, 2003 [2])

2) On output, work->normr contains ||b - A*x||
*/

static int
gmres_iterate(const gsl_spmatrix *A, const gsl_vector *b,
              const double tol, gsl_vector *x,
              void *vstate)
{
  const size_t N = A->size1;
  gmres_state_t *state = (gmres_state_t *) vstate;

  if (N != A->size2)
    {
      GSL_ERROR("matrix must be square", GSL_ENOTSQR);
    }
  else if (N != b->size)
    {
      GSL_ERROR("matrix does not match right hand side", GSL_EBADLEN);
    }
  else if (N != x->size)
    {
      GSL_ERROR("matrix does not match solution vector", GSL_EBADLEN);
    }
  else if (N != state->n)
    {
      GSL_ERROR("matrix does not match workspace", GSL_EBADLEN);
    }
  else
    {
      int status = GSL_SUCCESS;
      const size_t maxit = state->m;
      const double normb = gsl_blas_dnrm2(b); /* ||b|| */
      const double reltol = tol * normb;      /* tol*||b|| */
      double normr;                           /* ||r|| */
      size_t m, k;
      double tau;                             /* householder scalar */
      gsl_matrix *H = state->H;               /* Hessenberg matrix */
      gsl_vector *r = state->r;               /* residual vector */
      gsl_vector *w = state->y;               /* least squares RHS */
      gsl_matrix_view Rm;                     /* R_m = H(1:m,2:m+1) */
      gsl_vector_view ym;                     /* y(1:m) */
      gsl_vector_view h0 = gsl_matrix_column(H, 0);

      /*
       * The Hessenberg matrix will have the following structure:
       *
       * H = [ ||r_0|| | v_1 v_2 ... v_m     ]
       *     [   u_1   | u_2 u_3 ... u_{m+1} ]
       *
       * where v_j are the orthonormal vectors spanning the Krylov
       * subpsace of length j + 1 and u_{j+1} are the householder
       * vectors of length n - j - 1.
       * In fact, u_{j+1} has length n - j since u_{j+1}[0] = 1,
       * but this 1 is not stored.
       */
      gsl_matrix_set_zero(H);

      /* Step 1a: compute r = b - A*x_0 */
      gsl_vector_memcpy(r, b);
      gsl_spblas_dgemv(CblasNoTrans, -1.0, A, x, 1.0, r);

      /* Step 1b */
      gsl_vector_memcpy(&h0.vector, r);
      tau = gsl_linalg_householder_transform(&h0.vector);

      /* store tau_1 */
      gsl_vector_set(state->tau, 0, tau);

      /* initialize w (stored in state->y) */
      gsl_vector_set_zero(w);
      gsl_vector_set(w, 0, gsl_vector_get(&h0.vector, 0));

      for (m = 1; m <= maxit; ++m)
        {
          size_t j = m - 1; /* C indexing */
          double c, s;      /* Givens rotation */

          /* v_m */
          gsl_vector_view vm = gsl_matrix_column(H, m);

          /* v_m(m:end) */
          gsl_vector_view vv = gsl_vector_subvector(&vm.vector, j, N - j);

          /* householder vector u_m for projection P_m */
          gsl_vector_view um = gsl_matrix_subcolumn(H, j, j, N - j);

          /* Step 2a: form v_m = P_m e_m = e_m - tau_m w_m */
          gsl_vector_set_zero(&vm.vector);
          gsl_vector_memcpy(&vv.vector, &um.vector);
          tau = gsl_vector_get(state->tau, j); /* tau_m */
          gsl_vector_scale(&vv.vector, -tau);
          gsl_vector_set(&vv.vector, 0, 1.0 - tau);

          /* Step 2a: v_m <- P_1 P_2 ... P_{m-1} v_m */
          for (k = j; k > 0 && k--; )
            {
              gsl_vector_view uk =
                gsl_matrix_subcolumn(H, k, k, N - k);
              gsl_vector_view vk =
                gsl_vector_subvector(&vm.vector, k, N - k);
              tau = gsl_vector_get(state->tau, k);
              gsl_linalg_householder_hv(tau, &uk.vector, &vk.vector);
            }

          /* Step 2a: v_m <- A*v_m */
          gsl_spblas_dgemv(CblasNoTrans, 1.0, A, &vm.vector, 0.0, r);
          gsl_vector_memcpy(&vm.vector, r);

          /* Step 2a: v_m <- P_m ... P_1 v_m */
          for (k = 0; k <= j; ++k)
            {
              gsl_vector_view uk = gsl_matrix_subcolumn(H, k, k, N - k);
              gsl_vector_view vk = gsl_vector_subvector(&vm.vector, k, N - k);
              tau = gsl_vector_get(state->tau, k);
              gsl_linalg_householder_hv(tau, &uk.vector, &vk.vector);
            }

          /* Steps 2c,2d: find P_{m+1} and set v_m <- P_{m+1} v_m */
          if (m < N)
            {
              /* householder vector u_{m+1} for projection P_{m+1} */
              gsl_vector_view ump1 = gsl_matrix_subcolumn(H, m, m, N - m);

              tau = gsl_linalg_householder_transform(&ump1.vector);
              gsl_vector_set(state->tau, j + 1, tau);
            }

          /* Step 2e: v_m <- J_{m-1} ... J_1 v_m */
          for (k = 0; k < j; ++k)
            {
              gsl_linalg_givens_gv(&vm.vector, k, k + 1,
                                   state->c[k], state->s[k]);
            }

          if (m < N)
            {
              /* Step 2g: find givens rotation J_m for v_m(m:m+1) */
              gsl_linalg_givens(gsl_vector_get(&vm.vector, j),
                                gsl_vector_get(&vm.vector, j + 1),
                                &c, &s);

              /* store givens rotation for later use */
              state->c[j] = c;
              state->s[j] = s;

              /* Step 2h: v_m <- J_m v_m */
              gsl_linalg_givens_gv(&vm.vector, j, j + 1, c, s);

              /* Step 2h: w <- J_m w */
              gsl_linalg_givens_gv(w, j, j + 1, c, s);
            }

          /*
           * Step 2i: R_m = [ R_{m-1}, v_m ] - already taken care
           * of due to our memory storage scheme
           */

          /* Step 2j: check residual w_{m+1} for convergence */
          normr = fabs(gsl_vector_get(w, j + 1));
          if (normr <= reltol)
            {
              /*
               * method has converged, break out of loop to compute
               * update to solution vector x
               */
              break;
            }
        }

      /*
       * At this point, we have either converged to a solution or
       * completed all maxit iterations. In either case, compute
       * an update to the solution vector x and test again for
       * convergence.
       */

      /* rewind m if we exceeded maxit iterations */
      if (m > maxit)
        m--;

      /* Step 3a: solve triangular system R_m y_m = w, in place */
      Rm = gsl_matrix_submatrix(H, 0, 1, m, m);
      ym = gsl_vector_subvector(w, 0, m);
      gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit,
                     &Rm.matrix, &ym.vector);

      /*
       * Step 3b: update solution vector x; the loop below
       * uses a different but equivalent formulation from
       * Saad, algorithm 6.10, step 14; store Krylov projection
       * V_m y_m in 'r'
       */
      gsl_vector_set_zero(r);
      for (k = m; k > 0 && k--; )
        {
          double ymk = gsl_vector_get(&ym.vector, k);
          gsl_vector_view uk = gsl_matrix_subcolumn(H, k, k, N - k);
          gsl_vector_view rk = gsl_vector_subvector(r, k, N - k);

          /* r <- n_k e_k + r */
          gsl_vector_set(r, k, gsl_vector_get(r, k) + ymk);

          /* r <- P_k r */
          tau = gsl_vector_get(state->tau, k);
          gsl_linalg_householder_hv(tau, &uk.vector, &rk.vector);
        }

      /* x <- x + V_m y_m */
      gsl_vector_add(x, r);

      /* compute new residual r = b - A*x */
      gsl_vector_memcpy(r, b);
      gsl_spblas_dgemv(CblasNoTrans, -1.0, A, x, 1.0, r);
      normr = gsl_blas_dnrm2(r);

      if (normr <= reltol)
        status = GSL_SUCCESS;  /* converged */
      else
        status = GSL_CONTINUE; /* not yet converged */

      /* store residual norm */
      state->normr = normr;

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

static double
gmres_normr(const void *vstate)
{
  const gmres_state_t *state = (const gmres_state_t *) vstate;
  return state->normr;
} /* gmres_normr() */

static const gsl_splinalg_itersolve_type gmres_type =
{
  "gmres",
  &gmres_alloc,
  &gmres_iterate,
  &gmres_normr,
  &gmres_free
};

const gsl_splinalg_itersolve_type * gsl_splinalg_itersolve_gmres =
  &gmres_type;


#ifndef FUNCTIONS_H
#define FUNCTIONS_H

#include <point2d.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_multimin.h>
#include <QVector>

#define PI 3.1415926535
#define R2 33000
#define PARAM_COUNT 6

struct param_struct {
    QVector<Point2D> *data;
    int resi;
    int aresi;
    double f_min;
    double f_max;
    double RmU,LmU,CmU,C0U,UU,R0U;
    double Rm,Lm,Cm,C0,U,R0;
};

double If(const gsl_vector *v, void *params, double f); //I(f)

double StDev(const gsl_vector *v, void *params); //sum from 0 to N-1 (I_exp(f)-I(f))**2

Point2D find_extremum(QVector<Point2D> dat, bool max, int* index=0);

#endif // FUNCTIONS_H


// xi2bandpow.c
// 31.08.2018
// converts xi_+/-, xi_g+/x, and xi_gg to bandpowers

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_sf_bessel.h>
#include "bjutils.h"

#define xi_pm_balance 0.5 // default weight for xi_+/- in P_E

void xi_to_derived2pt();
double K_bandpow_ee();
double K_bandpow_ne();
double K_bandpow_nn();
double apod_upper();
double apod_lower();

int main(int argc, char *argv[])
{
  int i,j;
  double dr,xip,xim,scale_terms,kernel;
  FILE *dat;
  char name[400],path[200],ident[200],outident[200];

  if (argc!=14) {
    printf("Syntax: %s\n1: <working directory>\n2: <input file identifier>\n3: <output file identifier>\n4: <number of input angular bins>\n5: <min input separation to use in conversion [arcmin] (xi_+ in case of ee)>\n6: <max input separation to use in conversion [arcmin] (xi_+ in case of ee)>\n7: <min input separation to use in conversion [arcmin] (xi_- in case of ee; otherwise unused)>\n8: <max input separation to use in conversion [arcmin] (xi_- in case of ee; otherwise unused)>\n9: <number of output angular bins>\n10: <min output angular frequency>\n11: <max output angular frequency>\n12: <correlation type (1: ee; 2: ne; 3: gg)>\n13: <log width of apodisation window [total width of apodised range is tmax/tmin=exp(width) in arcmin; <0 for no apodisation]>\n\n",argv[0]);
    exit(-1);
  }
  sprintf(path,"%s",argv[1]);
  sprintf(ident,"%s",argv[2]);
  sprintf(outident,"%s",argv[3]);
  const int NR=atoi(argv[4]);
  const double angmin1=atof(argv[5]);
  const double angmax1=atof(argv[6]);
  const double angmin2=atof(argv[7]);
  const double angmax2=atof(argv[8]);
  const int NOUT=atoi(argv[9]);
  const double ellmin=atof(argv[10]);
  const double ellmax=atof(argv[11]);
  const int corrtype=atoi(argv[12]);
  const double logwidth=atof(argv[13]);

  // allocations
  if (corrtype>3) {
    printf("Error: incorrect input for correlation type!\n");
    exit(-1);
  }

  double *tin=bj_alloc(NR);
  double *ell_centre=bj_alloc(NOUT);
  double *ell_bound=bj_alloc(NOUT+1);
  double *weight=bj_alloc(NR);
  double *weight_xim=bj_alloc(NR);
  double *pfrac=bj_alloc(NOUT);

  gsl_vector *xi=gsl_vector_calloc(2*NR);
  gsl_vector *derived2pt=gsl_vector_calloc(2*NOUT);
  gsl_matrix *trans=gsl_matrix_calloc(2*NOUT,2*NR);
  gsl_matrix *xierr=gsl_matrix_calloc(2*NR,2*NR);
  gsl_matrix *derived2pterr=gsl_matrix_calloc(2*NOUT,2*NOUT);

  double logbinwidth=log(ellmax/ellmin)/(1.*NOUT);  // output binning
  for (i=0;i<NOUT;i++) {
    ell_centre[i]=ellmin*exp((i+0.5)*logbinwidth);   // use log centre of bin
    ell_bound[i]=ellmin*exp(i*logbinwidth);
  }
  ell_bound[NOUT]=ellmin*exp(NOUT*logbinwidth);

  // read input data
  sprintf(name,"%s/xi2bandpow_input_%s.dat",path,ident);
  dat=bj_fileopen("r",name);
  for (i=0;i<NR;i++) {
    if (corrtype==3) {
      fscanf(dat,"%lf  %lf",&tin[i],&xip);  //theta (bin centres; in arcmin), w_gg
      xim=0.0;   // use only first half of vector
    }
    else {
      fscanf(dat,"%lf  %lf  %lf",&tin[i],&xip,&xim);  //theta (bin centres), xi_+, xi_- for ee; theta (bin centres), xi_g+, xi_gx for ne
    }

    gsl_vector_set(xi,i,xip);
    gsl_vector_set(xi,NR+i,xim);
    gsl_matrix_set(xierr,i,i,xip*xip);       // treat xi as noise, uncorrelated so diagonal
    gsl_matrix_set(xierr,NR+i,NR+i,xim*xim);

    if (logwidth<0) {   // no apodisation
      if ((tin[i]>angmin1)&&(tin[i]<angmax1)) {
	weight[i]=1.0;
      }
      else {
	weight[i]=0.0;
      }
    }
    else {
      weight[i]=apod_lower(tin[i],angmin1,logwidth)*apod_upper(tin[i],angmax1,logwidth);
    }

    if (corrtype==1) { //ee
      if (logwidth<0) {   // no apodisation
	if ((tin[i]>angmin2)&&(tin[i]<angmax2)) {
	  weight_xim[i]=1.0;
	}
	else {
	  weight_xim[i]=0.0;
	}
      }
      else {
	weight_xim[i]=apod_lower(tin[i],angmin2,logwidth)*apod_upper(tin[i],angmax2,logwidth);
      }
    }

    tin[i]*=PI/10800.;   // convert from arcmin to radian
  }  // end of i loop
  fclose(dat);

  if ((tin[NR-1]<angmax1*PI/10800.*exp(logwidth/2.))||(corrtype==1 && tin[NR-1]<angmax2*PI/10800.*exp(logwidth/2.))) {
    printf("Error: correlation function not available out to max. range requested: %g < %g ; %g\n",tin[NR-1]/PI*10800.,angmax1*exp(logwidth/2.),angmax2*PI/10800.*exp(logwidth/2.));
    exit(-1);
  }
  if ((tin[0]>angmin1*PI/10800.*exp(-logwidth/2.))||(corrtype==1 && tin[0]>angmin2*PI/10800.*exp(-logwidth/2.))) {
    printf("Error: correlation function not available out to min. range requested: %g > %g ; %g\n",tin[0]/PI*10800.,angmin1*exp(-logwidth/2.),angmin2*PI/10800.*exp(-logwidth/2.));
    exit(-1);
  }

  sprintf(name,"%s/xi2bandpow_pmweights_%s.dat",path,ident);
  if ((dat=fopen(name,"r"))==NULL) {
    printf("No file %s - use pmweight of %g instead.\n",name,xi_pm_balance);
    for (i=0;i<NOUT;i++) {
      pfrac[i]=xi_pm_balance;
    }
  }
  else {
    printf("Using pmweights from file %s\n",name);
    for (i=0;i<NOUT;i++) {
      fscanf(dat,"%lf",&pfrac[i]);  // xi_+/- weight in P_E; one value per output ell bin
      printf("K_+ for Bin %i: %g\n",i+1,pfrac[i]);
    }
    fclose(dat);
  }

  // construct transformation matrix
  logbinwidth=log(tin[NR-1]/tin[0])/(1.*NR-1.);
  for (j=0;j<NR;j++) {
    dr=tin[j]*(exp(0.5*logbinwidth)-exp(-0.5*logbinwidth));  // input bin width
    for (i=0;i<NOUT;i++) {
      scale_terms=2.*PI*dr/(tin[j]*log(ell_bound[i+1]/ell_bound[i]));

      kernel=0.0;
      if (corrtype==1) {
	kernel=K_bandpow_ee(tin[j]*ell_bound[i+1],0)-K_bandpow_ee(tin[j]*ell_bound[i],0);
	gsl_matrix_set(trans,i,j,pfrac[i]*weight[j]*kernel*scale_terms);
	gsl_matrix_set(trans,NOUT+i,j,0.5*weight[j]*kernel*scale_terms);

	kernel=K_bandpow_ee(tin[j]*ell_bound[i+1],1)-K_bandpow_ee(tin[j]*ell_bound[i],1);
	gsl_matrix_set(trans,i,NR+j,(1.-pfrac[i])*weight_xim[j]*kernel*scale_terms);
	gsl_matrix_set(trans,NOUT+i,NR+j,-0.5*weight_xim[j]*kernel*scale_terms);
      }
      if (corrtype==2) {
	kernel=K_bandpow_ne(tin[j]*ell_bound[i+1])-K_bandpow_ne(tin[j]*ell_bound[i]);
	gsl_matrix_set(trans,i,j,weight[j]*kernel*scale_terms);
	gsl_matrix_set(trans,NOUT+i,NR+j,weight[j]*kernel*scale_terms);
      }
      if (corrtype==3) {
	kernel=K_bandpow_nn(tin[j]*ell_bound[i+1])-K_bandpow_nn(tin[j]*ell_bound[i]);
	gsl_matrix_set(trans,i,j,weight[j]*kernel*scale_terms);
      }
    }
  }

  // convert signals
  xi_to_derived2pt(xi,xierr,trans,derived2pt,derived2pterr,2*NR,2*NOUT);

  // output band powers
  sprintf(name,"%s/xi2bandpow_output_%s.dat",path,outident);
  dat=bj_fileopen("w",name);
  fprintf(dat,"#   ell         PEE             PEEerr          PBB                PBBerr\n");
  for (i=0;i<NOUT;i++) {
    if (corrtype==3) {
      fprintf(dat,"%15.10g\t%15.10g\t%15.10g\n",ell_centre[i],gsl_vector_get(derived2pt,i),sqrt(gsl_matrix_get(derived2pterr,i,i)));   // ell, bandpow, err
    }
    else {
      fprintf(dat,"%15.10g\t%15.10g\t%15.10g\t%15.10g\t%15.10g\n",ell_centre[i],gsl_vector_get(derived2pt,i),sqrt(gsl_matrix_get(derived2pterr,i,i)),gsl_vector_get(derived2pt,NOUT+i),sqrt(gsl_matrix_get(derived2pterr,NOUT+i,NOUT+i)));   // ell, E/+, err, B/x, err
    }
  }
  fclose(dat);

  sprintf(name,"%s/xi2bandpow_kernels_%s.dat",path,outident);  // write kernels
  dat=bj_fileopen("w",name);
  for (j=0;j<NR;j++) {
    fprintf(dat,"%15.10g\t",tin[j]/PI*10800.);
    for (i=0;i<NOUT;i++) {
      fprintf(dat,"%15.10g\t",gsl_matrix_get(trans,i,j));   // E-mode, xi_+
    }
    for (i=0;i<NOUT;i++) {
      fprintf(dat,"%15.10g\t",gsl_matrix_get(trans,i,NR+j));   // E-mode, xi_-
    }
    for (i=0;i<NOUT;i++) {
      fprintf(dat,"%15.10g\t",gsl_matrix_get(trans,NOUT+i,j));   // B-mode, xi_+
    }
    for (i=0;i<NOUT;i++) {
      fprintf(dat,"%15.10g\t",gsl_matrix_get(trans,NOUT+i,NR+j));   // B-mode, xi_-
    }
    fprintf(dat,"\n");
  }
  fclose(dat);

  // clean up
  gsl_vector_free(xi);
  gsl_vector_free(derived2pt);
  gsl_matrix_free(trans);
  gsl_matrix_free(xierr);
  gsl_matrix_free(derived2pterr);
  free(tin);
  free(ell_centre);
  free(ell_bound);
  free(weight);
  free(weight_xim);
  free(pfrac);
  return 0;
}

void xi_to_derived2pt(gsl_vector *xi,gsl_matrix *xierr,gsl_matrix *trans,gsl_vector *res,gsl_matrix *reserr,int nin,int nout)
{
  gsl_matrix *tmp=gsl_matrix_calloc(nout,nin);
  gsl_matrix *transt=gsl_matrix_calloc(nin,nout);

  // signal conversion
  mattimesvec_gen(trans,xi,res,nout,nin);

  // error conversion
  mattimesmat_gen(trans,xierr,tmp,nout,nin,nin);
  matrix_transpose(trans,transt,nout,nin);
  mattimesmat_gen(tmp,transt,reserr,nout,nin,nout);

  gsl_matrix_free(tmp);
  gsl_matrix_free(transt);
  return;
}

double K_bandpow_ee(double x,int pm)
{
  double res=0.0;

  if (pm==0) {
    res=x*gsl_sf_bessel_Jn(1,x);
  }
  else if (pm==1) {
    res=(x-8./x)*gsl_sf_bessel_Jn(1,x)-8.*gsl_sf_bessel_Jn(2,x);
  }
  else {
    printf("Error in routine K_bandpow_ee: invalid option for 'pm'.\n");
    exit(-1);
  }

  return(res);
}

double K_bandpow_ne(double x)
{
  return((-1.)*x*gsl_sf_bessel_Jn(1,x)-2.*gsl_sf_bessel_Jn(0,x));
}

double K_bandpow_nn(double x)
{
  return(x*gsl_sf_bessel_Jn(1,x));
}

double apod_upper(double x,double scale,double logwidth)
{
  double res=0.0;
  double logx=log(x);
  double logxmin=log(scale)-logwidth/2.;
  double logxmax=log(scale)+logwidth/2.;

  if (logx<=logxmin) {
    res=1.;
  }
  else if (logx>logxmax) {
    res=0.;
  }
  else {
    res=cos(PI/2.*(logx-logxmin)/(logxmax-logxmin));
    res=res*res;  // cos^2
  }
  return(res);
}

double apod_lower(double x,double scale,double logwidth)
{
  double res=0.0;
  double logx=log(x);
  double logxmin=log(scale)-logwidth/2.;
  double logxmax=log(scale)+logwidth/2.;

  if (logx<=logxmin) {
    res=0.;
  }
  else if (logx>logxmax) {
    res=1.;
  }
  else {
    res=cos(PI/2.*(logx-logxmax)/(logxmax-logxmin));
    res=res*res;  // cos^2
  }
  return(res);
}


// Copyright (c) 2021 Stig Rune Sellevag
//
// This file is distributed under the MIT License. See the accompanying file
// LICENSE.txt or http://www.opensource.org/licenses/mit-license.php for terms
// and conditions.

#ifndef SCILIB_LINALG_LSTSQ_H
#define SCILIB_LINALG_LSTSQ_H

#ifdef USE_MKL
#include <mkl.h>
#else
#include <lapacke.h>
#endif

#include <scilib/mdarray.h>
#include <scilib/linalg_impl/lapack_types.h>
#include <experimental/mdspan>
#include <exception>
#include <algorithm>
#include <type_traits>

namespace Sci {
namespace Linalg {

// Compute the minimum norm-solution to a real linear least squares problem.
template <class Layout>
inline void lstsq(Sci::Matrix_view<double, Layout> a,
                  Sci::Matrix_view<double, Layout> b)
{
    namespace stdex = std::experimental;

    static_assert(a.is_contiguous());
    static_assert(b.is_contiguous());

    BLAS_INT m = static_cast<BLAS_INT>(a.extent(0));
    BLAS_INT n = static_cast<BLAS_INT>(a.extent(1));
    BLAS_INT nrhs = static_cast<BLAS_INT>(b.extent(1));
    BLAS_INT rank;

    double rcond = -1.0;                           // use machine epsilon
    Sci::Vector<double, Layout> s(std::min(m, n)); // singular values of a

    auto matrix_layout = LAPACK_ROW_MAJOR;
    BLAS_INT lda = n;
    BLAS_INT ldb = nrhs;

    if constexpr (std::is_same_v<Layout, stdex::layout_left>) {
        matrix_layout = LAPACK_COL_MAJOR;
        lda = m;
        ldb = n;
    }
    BLAS_INT info = LAPACKE_dgelsd(matrix_layout, m, n, nrhs, a.data(), lda,
                                   b.data(), ldb, s.data(), rcond, &rank);
    if (info != 0) {
        throw std::runtime_error("dgelsd failed");
    }
}

template <class Layout, class Allocator>
inline void lstsq(Sci::Matrix<double, Layout, Allocator>& a,
                  Sci::Matrix<double, Layout, Allocator>& b)
{
    lstsq(a.view(), b.view());
}

} // namespace Linalg
} // namespace Sci

#endif // SCILIB_LINALG_LSTSQ_H


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

/* Author:  G. Jungman */

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

#include "error.h"

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

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

/* based on SLATEC besk1(), besk1e() */

/* chebyshev expansions

 series for bk1        on the interval  0.          to  4.00000d+00
                                        with weighted error   7.02e-18
                                         log weighted error  17.15
                               significant figures required  16.73
                                    decimal places required  17.67

 series for ak1        on the interval  1.25000d-01 to  5.00000d-01
                                        with weighted error   6.06e-17
                                         log weighted error  16.22
                               significant figures required  15.41
                                    decimal places required  16.83

 series for ak12       on the interval  0.          to  1.25000d-01
                                        with weighted error   2.58e-17
                                         log weighted error  16.59
                               significant figures required  15.22
                                    decimal places required  17.16
*/

static double bk1_data[11] = {
   0.0253002273389477705,
  -0.3531559607765448760,
  -0.1226111808226571480,
  -0.0069757238596398643,
  -0.0001730288957513052,
  -0.0000024334061415659,
  -0.0000000221338763073,
  -0.0000000001411488392,
  -0.0000000000006666901,
  -0.0000000000000024274,
  -0.0000000000000000070
};

static cheb_series bk1_cs = {
  bk1_data,
  10,
  -1, 1,
  8
};

static double ak1_data[17] = {
   0.27443134069738830,
   0.07571989953199368,
  -0.00144105155647540,
   0.00006650116955125,
  -0.00000436998470952,
   0.00000035402774997,
  -0.00000003311163779,
   0.00000000344597758,
  -0.00000000038989323,
   0.00000000004720819,
  -0.00000000000604783,
   0.00000000000081284,
  -0.00000000000011386,
   0.00000000000001654,
  -0.00000000000000248,
   0.00000000000000038,
  -0.00000000000000006
};
static cheb_series ak1_cs = {
  ak1_data,
  16,
  -1, 1,
  9
};

static double ak12_data[14] = {
   0.06379308343739001,
   0.02832887813049721,
  -0.00024753706739052,
   0.00000577197245160,
  -0.00000020689392195,
   0.00000000973998344,
  -0.00000000055853361,
   0.00000000003732996,
  -0.00000000000282505,
   0.00000000000023720,
  -0.00000000000002176,
   0.00000000000000215,
  -0.00000000000000022,
   0.00000000000000002
};
static cheb_series ak12_cs = {
  ak12_data,
  13,
  -1, 1,
  7
};

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

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

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 2.0*GSL_DBL_MIN) {
    OVERFLOW_ERROR(result);
  }
  else if(x <= 2.0) {
    const double lx = log(x);
    const double ex = exp(x);
    int stat_I1;
    gsl_sf_result I1;
    gsl_sf_result c;
    cheb_eval_e(&bk1_cs, 0.5*x*x-1.0, &c);
    stat_I1 = gsl_sf_bessel_I1_e(x, &I1);
    result->val  = ex * ((lx-M_LN2)*I1.val + (0.75 + c.val)/x);
    result->err  = ex * (c.err/x + fabs(lx)*I1.err);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_I1;
  }
  else if(x <= 8.0) {
    const double sx = sqrt(x);
    gsl_sf_result c;
    cheb_eval_e(&ak1_cs, (16.0/x-5.0)/3.0, &c);
    result->val  = (1.25 + c.val) / sx;
    result->err  = c.err / sx;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    const double sx = sqrt(x);
    gsl_sf_result c;
    cheb_eval_e(&ak12_cs, 16.0/x-1.0, &c);
    result->val  = (1.25 + c.val) / sx;
    result->err  = c.err / sx;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

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

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 2.0*GSL_DBL_MIN) {
    OVERFLOW_ERROR(result);
  }
  else if(x <= 2.0) {
    const double lx = log(x);
    int stat_I1;
    gsl_sf_result I1;
    gsl_sf_result c;
    cheb_eval_e(&bk1_cs, 0.5*x*x-1.0, &c);
    stat_I1 = gsl_sf_bessel_I1_e(x, &I1);
    result->val  = (lx-M_LN2)*I1.val + (0.75 + c.val)/x;
    result->err  = c.err/x + fabs(lx)*I1.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_I1;
  }
  else {
    gsl_sf_result K1_scaled;
    int stat_K1 = gsl_sf_bessel_K1_scaled_e(x, &K1_scaled);
    int stat_e  = gsl_sf_exp_mult_err_e(-x, 0.0,
                                           K1_scaled.val, K1_scaled.err,
                                           result);
    result->err = fabs(result->val) * (GSL_DBL_EPSILON*fabs(x) + K1_scaled.err/K1_scaled.val);
    return GSL_ERROR_SELECT_2(stat_e, stat_K1);
  }
}

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

#include "eval.h"

double gsl_sf_bessel_K1_scaled(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_K1_scaled_e(x, &result));
}

double gsl_sf_bessel_K1(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_K1_e(x, &result));
}


/* multifit/fsolver.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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.
 */

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_multifit_nlin.h>

gsl_multifit_fsolver *
gsl_multifit_fsolver_alloc (const gsl_multifit_fsolver_type * T,
                            size_t n, size_t p)
{
  int status;

  gsl_multifit_fsolver * s;

  if (n < p)
    {
      GSL_ERROR_VAL ("insufficient data points, n < p", GSL_EINVAL, 0);
    }

  s = (gsl_multifit_fsolver *) malloc (sizeof (gsl_multifit_fsolver));

  if (s == 0)
    {
      GSL_ERROR_VAL ("failed to allocate space for multifit solver struct",
                        GSL_ENOMEM, 0);
    }

  s->x = gsl_vector_calloc (p);

  if (s->x == 0)
    {
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for x", GSL_ENOMEM, 0);
    }

  s->f = gsl_vector_calloc (n);

  if (s->f == 0)
    {
      gsl_vector_free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for f", GSL_ENOMEM, 0);
    }

  s->dx = gsl_vector_calloc (p);

  if (s->dx == 0)
    {
      gsl_vector_free (s->x);
      gsl_vector_free (s->f);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for dx", GSL_ENOMEM, 0);
    }

  s->state = malloc (T->size);

  if (s->state == 0)
    {
      gsl_vector_free (s->dx);
      gsl_vector_free (s->x);
      gsl_vector_free (s->f);
      free (s);         /* exception in constructor, avoid memory leak */

      GSL_ERROR_VAL ("failed to allocate space for multifit solver state",
                        GSL_ENOMEM, 0);
    }

  s->type = T ;

  status = (s->type->alloc)(s->state, n, p);

  if (status != GSL_SUCCESS)
    {
      (s->type->free)(s->state);
      free (s->state);
      gsl_vector_free (s->dx);
      gsl_vector_free (s->x);
      gsl_vector_free (s->f);
      free (s);         /* exception in constructor, avoid memory leak */

      GSL_ERROR_VAL ("failed to set solver", status, 0);
    }

  s->function = NULL;

  return s;
}

int
gsl_multifit_fsolver_set (gsl_multifit_fsolver * s,
                          gsl_multifit_function * f,
                          const gsl_vector * x)
{
  if (s->f->size != f->n)
    {
      GSL_ERROR ("function size does not match solver", GSL_EBADLEN);
    }

  if (s->x->size != x->size)
    {
      GSL_ERROR ("vector length does not match solver", GSL_EBADLEN);
    }

  s->function = f;
  gsl_vector_memcpy(s->x,x);

  return (s->type->set) (s->state, s->function, s->x, s->f, s->dx);
}

int
gsl_multifit_fsolver_iterate (gsl_multifit_fsolver * s)
{
  return (s->type->iterate) (s->state, s->function, s->x, s->f, s->dx);
}

void
gsl_multifit_fsolver_free (gsl_multifit_fsolver * s)
{
  RETURN_IF_NULL (s);
  (s->type->free) (s->state);
  free (s->state);
  gsl_vector_free (s->dx);
  gsl_vector_free (s->x);
  gsl_vector_free (s->f);
  free (s);
}

const char *
gsl_multifit_fsolver_name (const gsl_multifit_fsolver * s)
{
  return s->type->name;
}

gsl_vector *
gsl_multifit_fsolver_position (const gsl_multifit_fsolver * s)
{
  return s->x;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen 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 Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#ifndef EIGEN_GSL_HELPER
#define EIGEN_GSL_HELPER

#include <Eigen/Core>

#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_poly.h>

namespace Eigen {

template<typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex> struct GslTraits
{
  typedef gsl_matrix* Matrix;
  typedef gsl_vector* Vector;
  static Matrix createMatrix(int rows, int cols) { return gsl_matrix_alloc(rows,cols); }
  static Vector createVector(int size) { return gsl_vector_alloc(size); }
  static void free(Matrix& m) { gsl_matrix_free(m); m=0; }
  static void free(Vector& m) { gsl_vector_free(m); m=0; }
  static void prod(const Matrix& m, const Vector& v, Vector& x) { gsl_blas_dgemv(CblasNoTrans,1,m,v,0,x); }
  static void cholesky(Matrix& m) { gsl_linalg_cholesky_decomp(m); }
  static void cholesky_solve(const Matrix& m, const Vector& b, Vector& x) { gsl_linalg_cholesky_solve(m,b,x); }
  static void eigen_symm(const Matrix& m, Vector& eval, Matrix& evec)
  {
    gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc(m->size1);
    Matrix a = createMatrix(m->size1, m->size2);
    gsl_matrix_memcpy(a, m);
    gsl_eigen_symmv(a,eval,evec,w);
    gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_VAL_ASC);
    gsl_eigen_symmv_free(w);
    free(a);
  }
  static void eigen_symm_gen(const Matrix& m, const Matrix& _b, Vector& eval, Matrix& evec)
  {
    gsl_eigen_gensymmv_workspace * w = gsl_eigen_gensymmv_alloc(m->size1);
    Matrix a = createMatrix(m->size1, m->size2);
    Matrix b = createMatrix(_b->size1, _b->size2);
    gsl_matrix_memcpy(a, m);
    gsl_matrix_memcpy(b, _b);
    gsl_eigen_gensymmv(a,b,eval,evec,w);
    gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_VAL_ASC);
    gsl_eigen_gensymmv_free(w);
    free(a);
  }

  template<class EIGEN_VECTOR, class EIGEN_ROOTS>
  static void eigen_poly_solve(const EIGEN_VECTOR& poly, EIGEN_ROOTS& roots )
  {
    const int deg = poly.size()-1;
    double *z = new double[2*deg];
    double *a = new double[poly.size()];
    for( int i=0; i<poly.size(); ++i ){ a[i] = poly[i]; }

    gsl_poly_complex_workspace * w = gsl_poly_complex_workspace_alloc (poly.size());
    gsl_poly_complex_solve(a, poly.size(), w, z);
    gsl_poly_complex_workspace_free (w);
    for( int i=0; i<deg; ++i )
    {
      roots[i].real() = z[2*i];
      roots[i].imag() = z[2*i+1];
    }

    delete[] a;
    delete[] z;
  }
};

template<typename Scalar> struct GslTraits<Scalar,true>
{
  typedef gsl_matrix_complex* Matrix;
  typedef gsl_vector_complex* Vector;
  static Matrix createMatrix(int rows, int cols) { return gsl_matrix_complex_alloc(rows,cols); }
  static Vector createVector(int size) { return gsl_vector_complex_alloc(size); }
  static void free(Matrix& m) { gsl_matrix_complex_free(m); m=0; }
  static void free(Vector& m) { gsl_vector_complex_free(m); m=0; }
  static void cholesky(Matrix& m) { gsl_linalg_complex_cholesky_decomp(m); }
  static void cholesky_solve(const Matrix& m, const Vector& b, Vector& x) { gsl_linalg_complex_cholesky_solve(m,b,x); }
  static void prod(const Matrix& m, const Vector& v, Vector& x)
  { gsl_blas_zgemv(CblasNoTrans,gsl_complex_rect(1,0),m,v,gsl_complex_rect(0,0),x); }
  static void eigen_symm(const Matrix& m, gsl_vector* &eval, Matrix& evec)
  {
    gsl_eigen_hermv_workspace * w = gsl_eigen_hermv_alloc(m->size1);
    Matrix a = createMatrix(m->size1, m->size2);
    gsl_matrix_complex_memcpy(a, m);
    gsl_eigen_hermv(a,eval,evec,w);
    gsl_eigen_hermv_sort(eval, evec, GSL_EIGEN_SORT_VAL_ASC);
    gsl_eigen_hermv_free(w);
    free(a);
  }
  static void eigen_symm_gen(const Matrix& m, const Matrix& _b, gsl_vector* &eval, Matrix& evec)
  {
    gsl_eigen_genhermv_workspace * w = gsl_eigen_genhermv_alloc(m->size1);
    Matrix a = createMatrix(m->size1, m->size2);
    Matrix b = createMatrix(_b->size1, _b->size2);
    gsl_matrix_complex_memcpy(a, m);
    gsl_matrix_complex_memcpy(b, _b);
    gsl_eigen_genhermv(a,b,eval,evec,w);
    gsl_eigen_hermv_sort(eval, evec, GSL_EIGEN_SORT_VAL_ASC);
    gsl_eigen_genhermv_free(w);
    free(a);
  }
};

template<typename MatrixType>
void convert(const MatrixType& m, gsl_matrix* &res)
{
//   if (res)
//     gsl_matrix_free(res);
  res = gsl_matrix_alloc(m.rows(), m.cols());
  for (int i=0 ; i<m.rows() ; ++i)
    for (int j=0 ; j<m.cols(); ++j)
      gsl_matrix_set(res, i, j, m(i,j));
}

template<typename MatrixType>
void convert(const gsl_matrix* m, MatrixType& res)
{
  res.resize(int(m->size1), int(m->size2));
  for (int i=0 ; i<res.rows() ; ++i)
    for (int j=0 ; j<res.cols(); ++j)
      res(i,j) = gsl_matrix_get(m,i,j);
}

template<typename VectorType>
void convert(const VectorType& m, gsl_vector* &res)
{
  if (res) gsl_vector_free(res);
  res = gsl_vector_alloc(m.size());
  for (int i=0 ; i<m.size() ; ++i)
      gsl_vector_set(res, i, m[i]);
}

template<typename VectorType>
void convert(const gsl_vector* m, VectorType& res)
{
  res.resize (m->size);
  for (int i=0 ; i<res.rows() ; ++i)
    res[i] = gsl_vector_get(m, i);
}

template<typename MatrixType>
void convert(const MatrixType& m, gsl_matrix_complex* &res)
{
  res = gsl_matrix_complex_alloc(m.rows(), m.cols());
  for (int i=0 ; i<m.rows() ; ++i)
    for (int j=0 ; j<m.cols(); ++j)
    {
      gsl_matrix_complex_set(res, i, j,
        gsl_complex_rect(m(i,j).real(), m(i,j).imag()));
    }
}

template<typename MatrixType>
void convert(const gsl_matrix_complex* m, MatrixType& res)
{
  res.resize(int(m->size1), int(m->size2));
  for (int i=0 ; i<res.rows() ; ++i)
    for (int j=0 ; j<res.cols(); ++j)
      res(i,j) = typename MatrixType::Scalar(
        GSL_REAL(gsl_matrix_complex_get(m,i,j)),
        GSL_IMAG(gsl_matrix_complex_get(m,i,j)));
}

template<typename VectorType>
void convert(const VectorType& m, gsl_vector_complex* &res)
{
  res = gsl_vector_complex_alloc(m.size());
  for (int i=0 ; i<m.size() ; ++i)
      gsl_vector_complex_set(res, i, gsl_complex_rect(m[i].real(), m[i].imag()));
}

template<typename VectorType>
void convert(const gsl_vector_complex* m, VectorType& res)
{
  res.resize(m->size);
  for (int i=0 ; i<res.rows() ; ++i)
    res[i] = typename VectorType::Scalar(
        GSL_REAL(gsl_vector_complex_get(m, i)),
        GSL_IMAG(gsl_vector_complex_get(m, i)));
}

}

#endif // EIGEN_GSL_HELPER


#ifndef GMM_MODEL_H
#define GMM_MODEL_H

#include "DoubleVector.h"
#include "GMM/GmmAggregateNewComp.h"
#include "Handle.h"
#include "LambdaCreationFunctions.h"
#include "Object.h"
#include "PDBVector.h"

#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_vector.h>
#include <math.h>
#include <vector>

// By Tania, October 2017

using namespace pdb;

/* This class contains our current GMM model, that is:
 * - num of dimensions;
 * - weights: set of (prior) weights for each component
 * - means: mean for each component
 * - covars: covariance for each component
 * - inv_covars: inverse covariance for each component
 * - dets: determinant of each covariance matrix
 */
class GmmModel : public Object {

private:
  int ndim = 0;
  int k = 0;
  Handle<DoubleVector> weights;
  Handle<Vector<Handle<DoubleVector>>> means;      // k*ndim
  Handle<Vector<Handle<DoubleVector>>> covars;     // k*ndim*ndim
  Handle<Vector<Handle<DoubleVector>>> inv_covars; // k*ndim*ndim
  Handle<DoubleVector> dets;

public:
  ENABLE_DEEP_COPY

  GmmModel() {}

  GmmModel(int k, int ndim) {
    std::cout << "K= " << k << std::endl;
    std::cout << "ndim= " << ndim << std::endl;

    this->ndim = ndim;
    this->k = k;

    this->weights = pdb::makeObject<DoubleVector>(k);
    this->dets = pdb::makeObject<DoubleVector>(k);

    this->means = pdb::makeObject<Vector<Handle<DoubleVector>>>();
    this->covars = pdb::makeObject<Vector<Handle<DoubleVector>>>();
    this->inv_covars = pdb::makeObject<Vector<Handle<DoubleVector>>>();

    for (int i = 0; i < k; i++) {
      this->weights->setDouble(i, 1.0 / k);
      this->means->push_back(makeObject<DoubleVector>(ndim));
      this->covars->push_back(makeObject<DoubleVector>(ndim * ndim));
      this->inv_covars->push_back(makeObject<DoubleVector>(ndim * ndim));
    }

    std::cout << "Model created!" << std::endl;
  }

  // this prints the first 10 elements of a DoubleVector
  void print_vector(Handle<DoubleVector> v) {
    int MAX = 10;
    if (v->size < MAX)
      MAX = v->size;
    for (int i = 0; i < MAX; i++) {
      std::cout << i << ": " << v->getDouble(i) << "; ";
    }
    std::cout << std::endl;
  }

  // Print current model
  void print() {
    // Print mean
    std::cout << "**Current model**" << std::endl;
    std::cout << "Weights: ";
    print_vector(this->weights);
    for (int i = 0; i < k; i++) {
      std::cout << "**Component " << i << std::endl;
      std::cout << "Mean: ";
      print_vector((*this->means)[i]);
      std::cout << "Covar: ";
      print_vector((*this->covars)[i]);
      std::cout << "Inv covars: ";
      print_vector((*this->inv_covars)[i]);
    }
  }

  int getNumK() { return k; }

  int getNDim() { return ndim; }

  double getWeight(int i) { return this->weights->getDouble(i); }

  void updateWeights(std::vector<double> w) {
    for (int i = 0; i < k; i++) {
      this->weights->setDouble(i, w[i]);
    }
    std::cout << "updated weights";
    print_vector(this->weights);
  }

  void updateMeans(std::vector<std::vector<double>> &m) {
    for (int i = 0; i < k; i++) {
      std::cout << "updated mean for comp " << i << std::endl;

      double *mean = (*this->means)[i]->getRawData();
      for (int j = 0; j < ndim; j++) {
        mean[j] = m[i][j];
      }
      print_vector((*this->means)[i]);
    }
  }

  void updateCovars(std::vector<std::vector<double>> &c) {
    for (int i = 0; i < k; i++) {
      std::cout << "updated covar for comp " << i << std::endl;

      double *covar = (*this->covars)[i]->getRawData();

      for (int j = 0; j < (ndim * ndim); j++) {
        covar[j] = c[i][j];
      }
      print_vector((*this->covars)[i]);
    }

    // calcInvCovars();
  }

  // It calculates determinants and inverse covariances based on Cholesky
  // decomposition
  void calcInvCovars() {

    for (int i = 0; i < this->getNumK(); i++) {

      gsl_matrix_view covar =
          gsl_matrix_view_array((*covars)[i]->data->c_ptr(), ndim, ndim);
      gsl_matrix_view inv_covar =
          gsl_matrix_view_array((*inv_covars)[i]->data->c_ptr(), ndim, ndim);

      gsl_matrix_memcpy(&inv_covar.matrix, &covar.matrix);

      // gsl_permutation *p = gsl_permutation_alloc(ndim);

      gsl_linalg_cholesky_decomp(&inv_covar.matrix);

      // Calculate determinant -> logdet
      double ax = 0.0;
      for (int i = 0; i < ndim; i++) {
        ax += log(gsl_matrix_get(&inv_covar.matrix, i, i));
      }

      // Check for
      gsl_linalg_cholesky_invert(&inv_covar.matrix);

      dets->setDouble(i, ax);
    }
  }

  // It returns the rvalue, responsability, posterior probability of the
  // datapoint
  // for GMM component i
  double log_normpdf(int i, Handle<DoubleVector> inputData) {

    gsl_vector_view data =
        gsl_vector_view_array(inputData->data->c_ptr(), ndim);
    gsl_vector_view mean =
        gsl_vector_view_array((*means)[i]->data->c_ptr(), ndim);
    gsl_matrix_view covar =
        gsl_matrix_view_array((*covars)[i]->data->c_ptr(), ndim, ndim);
    gsl_matrix_view inv_covar =
        gsl_matrix_view_array((*inv_covars)[i]->data->c_ptr(), ndim, ndim);

    double ax, ay;
    int s;
    gsl_vector *ym;
    gsl_vector *datan = gsl_vector_alloc(ndim);

    ax = dets->getDouble(i);

    gsl_vector_memcpy(datan, &data.vector);
    gsl_vector_sub(datan, &mean.vector);

    ym = gsl_vector_alloc(ndim);
    gsl_blas_dsymv(CblasUpper, 1.0, &inv_covar.matrix, datan, 0.0, ym);
    gsl_blas_ddot(datan, ym, &ay);

    gsl_vector_free(ym);
    gsl_vector_free(datan);

    ay = -ax - 0.5 * ay;

    return ay;
  }

  // It calculates the sum of elements in vector v in log space
  // To do this, we extract the maximum factor (maxVal) from
  // all the values
  double logSumExp(double *v, size_t size) {

    double maxVal = v[0];
    double sum = 0;

    for (int i = 1; i < size; i++) {
      if (v[i] > maxVal) {
        maxVal = v[i];
      }
    }

    for (int i = 0; i < size; i++) {
      sum += exp(v[i] - maxVal);
    }

    return log(sum) + maxVal;
  }

  double logSumExp(DoubleVector &vector) {
    return logSumExp(vector.getRawData(), vector.size);
  }

  double logSumExp(Vector<double> &vector) {
    return logSumExp(vector.c_ptr(), vector.size());
  }

  // It uses the partial results from the Aggregate to update the model

  double updateModel(GmmAggregateNewComp update) {

    const double MIN_COVAR = 1e-6;

    double *sumWeights = update.getSumWeights().c_ptr();
    double totalSumR = 0.0;

    for (int i = 0; i < k; i++) {
      totalSumR += sumWeights[i];
    }

    // Update weights -> weights = sumR/totalSumR
    gsl_vector_view gSumWeights = gsl_vector_view_array(sumWeights, k);
    gsl_vector_view gweights =
        gsl_vector_view_array((*weights).data->c_ptr(), k);
    gsl_vector_memcpy(&gweights.vector, &gSumWeights.vector);
    gsl_vector_scale(&gweights.vector, 1.0 / totalSumR);

    // Update Mean and Covar for each component
    for (int i = 0; i < k; i++) {

      // Update means -> means = 1/sumR * weightedX
      gsl_vector_view mean =
          gsl_vector_view_array((*means)[i]->data->c_ptr(), ndim);
      gsl_vector_view gsumMean =
          gsl_vector_view_array(update.getSumMean(i).c_ptr(), ndim);
      gsl_vector_memcpy(&mean.vector, &gsumMean.vector);
      gsl_vector_scale(&mean.vector, 1.0 / sumWeights[i]);

      // Update covars -> gsumCovarv / sumR - mean*mean^T

      gsl_vector_view gsumCovarv =
          gsl_vector_view_array(update.getSumCovar(i).c_ptr(), ndim * ndim);
      gsl_vector_scale(&gsumCovarv.vector, 1.0 / sumWeights[i]);
      gsl_matrix_view gsumCovarm =
          gsl_matrix_view_array(update.getSumCovar(i).c_ptr(), ndim, ndim);
      gsl_blas_dsyr(CblasUpper, -1.0, &mean.vector, &gsumCovarm.matrix);

      // Add constant to diagonal and copy lower triangular
      double d;
      for (int row = 0; row < ndim; row++) {
        d = gsl_matrix_get(&gsumCovarm.matrix, row, row);
        gsl_matrix_set(&gsumCovarm.matrix, row, row, d + MIN_COVAR);

        for (int col = row + 1; col < ndim; col++) {
          // matrix[j][i] = matrix[i][j]
          d = gsl_matrix_get(&gsumCovarm.matrix, row, col);
          gsl_matrix_set(&gsumCovarm.matrix, col, row, d);
        }
      }

      gsl_vector_view covar =
          gsl_vector_view_array((*covars)[i]->data->c_ptr(), ndim * ndim);
      gsl_vector_memcpy(&covar.vector, &gsumCovarv.vector);
    }

    // Finally, calculate inverse covariances and determinants to be used in the
    // next iteration
    calcInvCovars();

    return update.getLogLikelihood();
  }

  ~GmmModel() {}
};

#endif


/**
 *
 * @file example_strsmpl.c
 *
 *  PLASMA testing routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @brief Example for solving a system of linear equations using
 *        LU factorization and PLASMA_strsmpl routine
 *
 * @version 2.6.0
 * @author Bilel Hadri
 * @date 2010-11-15
 * @generated s Tue Jan  7 11:45:21 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <core_blas.h>

int check_solution(int, int , float *, int, float *, float *, int);

int IONE=1;
int ISEED[4] = {0,0,0,1};   /* initial seed for slarnv() */

int main ()
{

    int cores = 2;
    int N     = 10;
    int LDA   = 10;
    int NRHS  = 5;
    int LDB   = 10;
    int info;
    int info_solution;
    int i,j;
    int LDAxN = LDA*N;
    int LDBxNRHS = LDB*NRHS;

    float *A1 = (float *)malloc(LDA*N*(sizeof*A1));
    float *A2 = (float *)malloc(LDA*N*(sizeof*A2));
    float *B1 = (float *)malloc(LDB*NRHS*(sizeof*B1));
    float *B2 = (float *)malloc(LDB*NRHS*(sizeof*B2));
    PLASMA_desc *L;
    int *IPIV;

    /* Check if unable to allocate memory */
    if ((!A1)||(!A2)||(!B1)||(!B2)){
        printf("Out of Memory \n ");
        return EXIT_SUCCESS;
    }

    /*Plasma Initialize*/
    PLASMA_Init(cores);
    printf("-- PLASMA is initialized to run on %d cores. \n",cores);

    /* Initialize A1 and A2 Matrix */
    LAPACKE_slarnv_work(IONE, ISEED, LDAxN, A1);
    for ( i = 0; i < N; i++)
        for (  j = 0; j < N; j++)
            A2[LDA*j+i] = A1[LDA*j+i];

    /* Initialize B1 and B2 */
    LAPACKE_slarnv_work(IONE, ISEED, LDBxNRHS, B1);
    for ( i = 0; i < N; i++)
        for ( j = 0; j < NRHS; j++)
            B2[LDB*j+i] = B1[LDB*j+i];

    /* Allocate L and IPIV */
    info = PLASMA_Alloc_Workspace_sgetrf_incpiv(N, N, &L, &IPIV);

    /* LU factorization of the matrix A */
    info = PLASMA_sgetrf_incpiv(N, N, A2, LDA, L, IPIV);

    /* Solve the problem */
    info = PLASMA_strsmpl(N, NRHS, A2, LDA, L, IPIV, B2, LDB);
    info = PLASMA_strsm(PlasmaLeft, PlasmaUpper, PlasmaNoTrans, PlasmaNonUnit,
                        N, NRHS, (float)1.0, A2, LDA, B2, LDB);

    /* Check the solution */
    info_solution = check_solution(N, NRHS, A1, LDA, B1, B2, LDB);

    if ((info_solution != 0)|(info != 0))
       printf("-- Error in SGETRS example ! \n");
    else
       printf("-- Run of SGETRS example successful ! \n");

    free(A1); free(A2); free(B1); free(B2); free(IPIV); free(L);

    PLASMA_Finalize();

    return EXIT_SUCCESS;
}

/*------------------------------------------------------------------------
 *  Check the accuracy of the solution of the linear system
 */

int check_solution(int N, int NRHS, float *A1, int LDA, float *B1, float *B2, int LDB)
{
    int info_solution;
    float Rnorm, Anorm, Xnorm, Bnorm;
    float alpha, beta;
    float *work = (float *)malloc(N*sizeof(float));
    float eps;

    eps = LAPACKE_slamch_work('e');

    alpha = 1.0;
    beta  = -1.0;

    Xnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, NRHS, B2, LDB, work);
    Anorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, N, A1, LDA, work);
    Bnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, NRHS, B1, LDB, work);

    cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, N, NRHS, N, (alpha), A1, LDA, B2, LDB, (beta), B1, LDB);
    Rnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, NRHS, B1, LDB, work);

    printf("============\n");
    printf("Checking the Residual of the solution \n");
    printf("-- ||Ax-B||_oo/((||A||_oo||x||_oo+||B||_oo).N.eps) = %e \n",Rnorm/((Anorm*Xnorm+Bnorm)*N*eps));

    if ( isnan(Rnorm/((Anorm*Xnorm+Bnorm)*N*eps)) || (Rnorm/((Anorm*Xnorm+Bnorm)*N*eps) > 10.0) ){
        printf("-- The solution is suspicious ! \n");
        info_solution = 1;
    }
    else{
        printf("-- The solution is CORRECT ! \n");
        info_solution = 0;
    }

    free(work);

    return info_solution;
}


#include <math.h>
#include <cblas.h>

#ifdef _MSC_VER
# define inline __inline
#endif

/*
 * General Cholesky Delete.
 * Remove an element from the cholesky factorization
 * m = columns
 * n = rows
 *
 * TODO: put transpose as an option
 */
static inline void cholesky_delete_dbl(int m, int n, double *L, int go_out)
{
    double c, s;

    /* delete row go_out */
    double *L1 = L + (go_out * m);
    int i;
    for (i = go_out; i < n - 1; ++i) {
        cblas_dcopy (i + 2, L1 + m , 1, L1,  1);
        L1 += m;
    }

    L1 = L + (go_out * m);
    for (i=go_out; i < n - 1; ++i) {

        cblas_drotg(L1 + i, L1 + i + 1, &c, &s);
        if (L1[i] < 0) {
            /* Diagonals cannot be negative */
            L1[i] = fabs(L1[i]);
            c = -c;
            s = -s;
        }
        L1[i+1] = 0.; /* just for cleanup */
        L1 += m;

        cblas_drot(n - (i + 2), L1 + i, m, L1 + i + 1,
                   m, c, s);
    }
}

static inline void cholesky_delete_flt(int m, int n, float *L, int go_out)
{
    float c, s;

    /* delete row go_out */
    float *L1 = L + (go_out * m);
    int i;
    for (i = go_out; i < n - 1; ++i) {
        cblas_scopy (i + 2, L1 + m , 1, L1,  1);
        L1 += m;
    }

    L1 = L + (go_out * m);
    for (i=go_out; i < n - 1; ++i) {

        cblas_srotg(L1 + i, L1 + i + 1, &c, &s);
        if (L1[i] < 0) {
            /* Diagonals cannot be negative */
            L1[i] = fabsf(L1[i]);
            c = -c;
            s = -s;
        }
        L1[i+1] = 0.; /* just for cleanup */
        L1 += m;

        cblas_srot(n - (i + 2), L1 + i, m, L1 + i + 1,
                   m, c, s);
    }
}


#include "stdio.h"
#include "gsl/gsl_integration.h"
#include "gsl/gsl_errno.h"
//include "limber.h"
#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_monte_vegas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_sf_bessel.h>

// The integrated C_ell are in general allowed to be zero or negative if
// they describe cross-correlations. We use these statuses to describe
// errors where a log was requested also.
// LIMBER_STATUS_NEGATIVE is probably always an error
// but LIMBER_STATUS_ZERO is not necessarily
#define LIMBER_STATUS_OK 0
#define LIMBER_STATUS_ZERO 1
#define LIMBER_STATUS_NEGATIVE 2
#define LIMBER_STATUS_ERROR 3

// These are the options you can set for
// the Limber integrator.
typedef struct cl_config{
	int n_ell;  // Number of ell values you want in the spline
	int * ell;  // The chosen ell values you want
	double prefactor; //Scaling prefactor
    int status; // did everything go okay?
    double delta_range_factor;
    double log_nu_range;
    double absolute_tolerance;
    double relative_tolerance;
} cl_config;

int cl_integral(cl_config * config, gsl_spline * WX_red,
			    gsl_spline * WY_red, gsl_spline2d * P, gsl_spline * D_chi,
			    double * cl_out, double * cl_err_out);


/* ode-initval/rkf45.c
 *
 * Copyright (C) 2001 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Runge-Kutta-Fehlberg 4(5)*/

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include "gsl_odeiv.h"

#include "odeiv_util.h"

/* Runge-Kutta-Fehlberg constants */
static const double ah[] = { 1.0/4.0, 3.0/8.0, 12.0/13.0, 1.0, 1.0/2.0 };

static const double b3[] = { 3.0/32.0, 9.0/32.0 };

static const double b4[] = { 1932.0/2197.0, -7200.0/2197.0, 7296.0/2197.0};

static const double b5[] = { 8341.0/4104.0, -32832.0/4104.0, 29440.0/4104.0, -845.0/4104.0};

static const double b6[] = { -6080.0/20520.0, 41040.0/20520.0, -28352.0/20520.0, 9295.0/20520.0, -5643.0/20520.0};

static const double c1 = 902880.0/7618050.0;
static const double c3 = 3953664.0/7618050.0;
static const double c4 = 3855735.0/7618050.0;
static const double c5 = -1371249.0/7618050.0;
static const double c6 = 277020.0/7618050.0;

static const double ec[] = { 0.0,
  1.0 / 360.0,
  0.0,
  -128.0 / 4275.0,
  -2197.0 / 75240.0,
  1.0 / 50.0,
  2.0 / 55.0
};

typedef struct
{
  double *k1;
  double *k2;
  double *k3;
  double *k4;
  double *k5;
  double *k6;
  double *y0;
  double *ytmp;
}
rkf45_state_t;

static void *
rkf45_alloc (size_t dim)
{
  rkf45_state_t *state = (rkf45_state_t *) malloc (sizeof (rkf45_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for rkf45_state", GSL_ENOMEM);
    }

  state->k1 = (double *) malloc (dim * sizeof (double));

  if (state->k1 == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k1", GSL_ENOMEM);
    }

  state->k2 = (double *) malloc (dim * sizeof (double));

  if (state->k2 == 0)
    {
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k2", GSL_ENOMEM);
    }

  state->k3 = (double *) malloc (dim * sizeof (double));

  if (state->k3 == 0)
    {
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k3", GSL_ENOMEM);
    }

  state->k4 = (double *) malloc (dim * sizeof (double));

  if (state->k4 == 0)
    {
      free (state->k3);
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k4", GSL_ENOMEM);
    }

  state->k5 = (double *) malloc (dim * sizeof (double));

  if (state->k5 == 0)
    {
      free (state->k4);
      free (state->k3);
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k5", GSL_ENOMEM);
    }

  state->k6 = (double *) malloc (dim * sizeof (double));

  if (state->k6 == 0)
    {
      free (state->k5);
      free (state->k4);
      free (state->k3);
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k6", GSL_ENOMEM);
    }

  state->y0 = (double *) malloc (dim * sizeof (double));

  if (state->y0 == 0)
    {
      free (state->k6);
      free (state->k5);
      free (state->k4);
      free (state->k3);
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->y0);
      free (state->k6);
      free (state->k5);
      free (state->k4);
      free (state->k3);
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  return state;
}

static int
rkf45_apply (void *vstate,
	    size_t dim,
	    double t,
	    double h,
	    double y[],
	    double yerr[],
	    const double dydt_in[],
	    double dydt_out[], const gsl_odeiv_system * sys)
{
  rkf45_state_t *state = (rkf45_state_t *) vstate;

  size_t i;
  int status = 0;

  double *const k1 = state->k1;
  double *const k2 = state->k2;
  double *const k3 = state->k3;
  double *const k4 = state->k4;
  double *const k5 = state->k5;
  double *const k6 = state->k6;
  double *const ytmp = state->ytmp;

  /* k1 step */
  if (dydt_in != NULL)
    {
      DBL_MEMCPY (k1, dydt_in, dim);
    }
  else
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t, y, k1);
      GSL_STATUS_UPDATE (&status, s);
    }

  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] +  ah[0] * h * k1[i];

  /* k2 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[0] * h, ytmp, k2);
    GSL_STATUS_UPDATE (&status, s);
  }
  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + h * (b3[0] * k1[i] + b3[1] * k2[i]);

  /* k3 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[1] * h, ytmp, k3);
    GSL_STATUS_UPDATE (&status, s);
  }
  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + h * (b4[0] * k1[i] + b4[1] * k2[i] + b4[2] * k3[i]);

  /* k4 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[2] * h, ytmp, k4);
    GSL_STATUS_UPDATE (&status, s);
  }
  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b5[0] * k1[i] + b5[1] * k2[i] + b5[2] * k3[i] +
		  b5[3] * k4[i]);

  /* k5 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[3] * h, ytmp, k5);
    GSL_STATUS_UPDATE (&status, s);
  }
  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b6[0] * k1[i] + b6[1] * k2[i] + b6[2] * k3[i] +
		  b6[3] * k4[i] + b6[4] * k5[i]);

  /* k6 step and final sum */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[4] * h, ytmp, k6);
    GSL_STATUS_UPDATE (&status, s);
  }

  for (i = 0; i < dim; i++)
    {
      const double d_i = c1 * k1[i] + c3 * k3[i] + c4 * k4[i] + c5 * k5[i] + c6 * k6[i];
      y[i] += h * d_i;
      if (dydt_out != NULL)
	dydt_out[i] = d_i;
    }

  /* difference between 4th and 5th order */
  for (i = 0; i < dim; i++)
    yerr[i] =
      h * (ec[1] * k1[i] + ec[3] * k3[i] + ec[4] * k4[i] + ec[5] * k5[i] +
	   ec[6] * k6[i]);

  return status;
}

static int
rkf45_reset (void *vstate, size_t dim)
{
  rkf45_state_t *state = (rkf45_state_t *) vstate;

  DBL_ZERO_MEMSET (state->k1, dim);
  DBL_ZERO_MEMSET (state->k2, dim);
  DBL_ZERO_MEMSET (state->k3, dim);
  DBL_ZERO_MEMSET (state->k4, dim);
  DBL_ZERO_MEMSET (state->k5, dim);
  DBL_ZERO_MEMSET (state->k6, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);

  return GSL_SUCCESS;
}

static unsigned int
rkf45_order (void *vstate)
{
  rkf45_state_t *state = (rkf45_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 5;
}

static void
rkf45_free (void *vstate)
{
  rkf45_state_t *state = (rkf45_state_t *) vstate;

  free (state->ytmp);
  free (state->y0);
  free (state->k6);
  free (state->k5);
  free (state->k4);
  free (state->k3);
  free (state->k2);
  free (state->k1);
  free (state);
}

static const gsl_odeiv_step_type rkf45_type = { "rkf45",	/* name */
  1,				/* can use dydt_in */
  0,				/* gives exact dydt_out */
  &rkf45_alloc,
  &rkf45_apply,
  &rkf45_reset,
  &rkf45_order,
  &rkf45_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rkf45 = &rkf45_type;


//  SMCTC: rng.hh
//
//   Copyright Adam Johansen, 2008.
//
//   This file is part of SMCTC.
//
//   SMCTC 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.
//
//   SMCTC 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 SMCTC.  If not, see <http://www.gnu.org/licenses/>.
//

//! \file
//! \brief Random number generation.
//!
//! This file contains the definitions for the smc::rng and smc::rnginfo class.
//! It wraps the random number generation facilities provided by the GSL and provides a convenient interfaces to access several of its more commonly-used features.

#ifndef __SMC_RNG_HH
#define __SMC_RNG_HH 1.0

extern "C" {
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
}

namespace smc {
  ///A gsl-rng information handling class (not templated)
  class gslrnginfo {
  private:
    ///This is a null terminated array of the available random number generators.
    const gsl_rng_type** typePtArray;
    ///The number of available random number generators.
    int nNumber;

  protected:
    gslrnginfo();

  public:
    ///Returns a reference to the sole static instance of this class.
    static gslrnginfo* GetInstance();

    ///Returns the number of available random number generators.
    int GetNumber();
    ///Returns the name of random number generator number nIndex.
    const char* GetNameByIndex(int nIndex);
    ///Returns a pointer to random number generator nIndex.
    const gsl_rng_type* GetPointerByIndex(int nIndex);
    ///Returns a pointer to the random number generator with name szName (or null if it doesn't exist).
    const gsl_rng_type* GetPointerByName(const char* szName);
  };

  ///The global application instance of the gslrnginfo class:
  extern gslrnginfo rngset;

  ///A random number generator class.

  ///    At present this serves as a wrapper for the gsl random number generation code.
  class rng {
  private:
    ///This is the type of random number generator underlying the class.
    const gsl_rng_type* type;
    ///This is a pointer to the internal workspace of the rng including its current state.
    gsl_rng* pWorkspace;

  public:
    ///Initialise the random number generator using default settings
    rng();
    ///Initialise the random number generator using the default seed for the type
    rng(const gsl_rng_type* Type);
    ///Initialise the random number generator using specified type and seed
    rng(const gsl_rng_type* Type,unsigned long int lSeed);

    ///Free the workspace allocated for random number generation
    ~rng();

    ///Provide access to the raw random number generator
    gsl_rng* GetRaw(void);

    ///Generate a multinomial random vector with parameters (n,w[1:k]) and store it in X
    void Multinomial(unsigned n, unsigned k, const double* w, unsigned* X);
    ///Returns a random integer generated uniformly between the minimum and maximum values specified
    long UniformDiscrete(long lMin, long lMax);

    ///Returns a random number generated from a Beta distribution with the specified parameters.
    double Beta(double da, double db);
    ///Returns a random number generated from a Cauchy distribution with the specified scale parameter.
    double Cauchy(double dScale);
    ///Returns a random number generated from an exponential distribution with the specified mean.
    double Exponential(double dMean);
    ///Return a random number generated from a gamma distribution with shape alpha and scale beta.
    double Gamma(double dAlpha, double dBeta);
    ///Returns a random number generated from a Laplacian distribution with the specified scale.
    double Laplacian(double dScale);
    ///Returns a random number generated from a Lognormal distribution of location mu and scale sigma
    double Lognormal(double dMu, double dSigma);
    ///Return a random number generated from a normal distribution with a specified mean and standard deviation
    double Normal(double dMean, double dStd);
    ///Return a random number generated from a standard normal distribution
    double NormalS(void);
    ///Returns a random number from a normal distribution, conditional upon it exceeding the specified threshold.
    double NormalTruncated(double dMean, double dStd, double dThreshold);
    ///Return a student-t random number generated with a specified number of degrees of freedom
    double StudentT(double dDF);
    ///Return a random number generated uniformly between dMin and dMax
    double Uniform(double dMin, double dMax);
    ///Returns a random number generated from the standard uniform[0,1) distribution
    double UniformS(void);
  };
}

#endif


//
// Created by robin on 2018/9/1.
// Copyright (c) 2018 Robin. All rights reserved.
//

#pragma once

#include <ostream>
#include <algorithm>
#include <vector>
#include <gsl/gsl>
#include "arithmetic.h"

namespace satz::gf2::v2 {

/// @brief Polynomial over GF(2).
class Polynomial {
public:
  using int_type = int;
  using container_type = std::vector<int_type>;

  Polynomial () = default;

  template <typename T>
  Polynomial (std::initializer_list<T> degrees)
      : Polynomial(degrees.begin(), degrees.end()) { }

  template <typename InputIt>
  Polynomial (InputIt first, InputIt last) : deg_(first, last) {
    Expects(std::all_of(deg_.begin(),
                        deg_.end(),
                        [] (const auto& x) { return x >= 0; }));

    if (!std::is_sorted(deg_.begin(), deg_.end())) {
      std::sort(deg_.begin(), deg_.end());
    }
    deg_.erase(std::unique(deg_.begin(), deg_.end()), deg_.end());
  }

  static Polynomial from_ulong (unsigned long);
  static Polynomial from_bytes (gsl::span<uint8_t>);
  static Polynomial from_bytes (gsl::span<uint8_t>, int_type degree);
  static Polynomial make_random (int_type degree);
  static Polynomial make_irreducible (int_type degree);

  /**
   * @brief Return a bit representation of the coefficients.
   *
   *    When the degree of this polynomial is -1, return an empty vector.
   *    In other cases, return a bit representation with just enough bytes,
   *    i.e., the most significant byte should be non-zero.
   */
  [[nodiscard]] std::vector<uint8_t> to_bytes () const;

  explicit operator bool () const;

  friend bool operator == (const Polynomial& lhs, const Polynomial& rhs);
  friend bool operator != (const Polynomial& lhs, const Polynomial& rhs);
  friend bool operator < (const Polynomial& lhs, const Polynomial& rhs);
  friend bool operator > (const Polynomial& lhs, const Polynomial& rhs);
  friend bool operator <= (const Polynomial& lhs, const Polynomial& rhs);
  friend bool operator >= (const Polynomial& lhs, const Polynomial& rhs);
  Polynomial& operator ^= (const Polynomial& rhs);
  Polynomial& operator |= (const Polynomial& rhs);
  Polynomial& operator &= (const Polynomial& rhs);
  Polynomial& operator += (const Polynomial& rhs);
  Polynomial& operator -= (const Polynomial& rhs);
  Polynomial& operator *= (const Polynomial& rhs);
  Polynomial& operator %= (const Polynomial& rhs);
  Polynomial& operator <<= (int_type n);
  Polynomial& operator >>= (int_type n);

  friend Polynomial operator ^ (const Polynomial& lhs, const Polynomial& rhs);
  friend Polynomial operator | (const Polynomial& lhs, const Polynomial& rhs);
  friend Polynomial operator & (const Polynomial& lhs, const Polynomial& rhs);
  friend Polynomial operator + (const Polynomial& lhs, const Polynomial& rhs);
  friend Polynomial operator - (const Polynomial& lhs, const Polynomial& rhs);
  friend Polynomial operator * (const Polynomial& lhs, const Polynomial& rhs);
  friend Polynomial operator % (const Polynomial& lhs, const Polynomial& rhs);
  Polynomial operator << (int_type n) const;
  Polynomial operator >> (int_type n) const;

  friend std::ostream& operator << (std::ostream& os, const Polynomial& obj);

  /**
   * @brief Indicates whether it is a zero polynomial.
   */
  [[nodiscard]] bool empty () const;

  /**
   * @brief Returns the degree of the polynomial.
   */
  [[nodiscard]] int_type degree () const;

  /**
   * @brief Returns the number of non-zero coefficients.
   */
  [[nodiscard]] int_type nnz () const;

  /**
   * @brief Given an interger n, return whether this polynomial contains an
   *    x^n term.
   * @param n
   */
  [[nodiscard]] bool contains (int_type n) const;

protected:
  explicit Polynomial (container_type&& v);

private:
  container_type deg_; // `deg_` for degrees
};

}

namespace satz::gf2 {
using namespace v2;
}

namespace satz {

template <>
const gf2::v2::Polynomial zero<gf2::v2::Polynomial> = gf2::v2::Polynomial{};

template <>
const gf2::v2::Polynomial one<gf2::v2::Polynomial> = gf2::v2::Polynomial{0};

template <>
const gf2::v2::Polynomial X<gf2::v2::Polynomial> = gf2::v2::Polynomial{1};

}