/* -*- mode: C; tab-width: 8; indent-tabs-mode: nil; c-basic-offset: 4 -*-
 *      vim: sw=4 ts=8 et tw=80
 */

/* filter.c
 *
 * This file contains a few time-domain processing routines, copied
 * without much modification from moby.
 *
 * These are mostly needed for filtering during mapping, and for
 * various pathologies processing.
 */

#include "pyactpol.h"
#include "myassert.h"

#include <stdbool.h>

#include <fftw3.h>
#include <gsl/gsl_cblas.h>

static PyObject *remove_mean(PyObject *self, PyObject *args)
{
    PyArrayObject *tod_array;
    PyArrayObject *dets_array;
    PyArrayObject *means_array;

    if (!PyArg_ParseTuple(args, "O!O!",
                          &PyArray_Type, &tod_array,
                          &PyArray_Type, &dets_array
            ))
        po_raise("invalid arguments");

    ASSERT_CARRAY_TYPE_NDIM(tod_array, NPY_FLOAT32, 2);
    ASSERT_CARRAY_TYPE_NDIM(dets_array, NPY_INT64, 1);

    long int ndata = PyArray_DIMS(tod_array)[1];
    int ndet = PyArray_DIMS(dets_array)[0];
    long int *dets = PyArray_DATA(dets_array);
    float *data = PyArray_DATA(tod_array);

    npy_intp dims[1];
    dims[0] = ndet;
    means_array = (PyArrayObject *)PyArray_SimpleNew(1, dims, NPY_DOUBLE);
    double *means = (double *)PyArray_DATA(means_array);

#pragma omp parallel for shared(data,ndata,dets,ndet)
  for (int d=0; d<ndet; d++)
  {
    double m = 0.0;
    for (long int i = dets[d]*ndata; i < (dets[d]+1)*ndata; i++)
      m += (double)data[i];
    m /= ndata;
    means[d] = m;
    for (long int i = dets[d]*ndata; i < (dets[d]+1)*ndata; i++)
      data[i] -= m;
  }
  return PyArray_Return(means_array);
}

static void filter_one_detector(
    fftwf_complex *data,
    const float *real, const float *imag,
    int ndata,
    fftwf_plan p_forward, fftwf_plan p_back,
    int detrend, int retrend);

static void mbTODFourierFilter(
    float *tod,        ///  2d data (nd,nsamps) to filter
    int nsamps,        ///< data size, fast dimension
    const int *dets,  ///< tod detector numbers -- designates
                       ///< which detectors to filter
    int ndets,         ///< length of the dets vector
    const float *freq, ///> frequency vector (nsamps)
    const float *real, ///< real component of filter, (nsamps)
    const float *imag, ///< imaginary component of filter, (nsamps)
    const float *tau,  ///< time constants to deconvolve (ndets)
    int tau_do_inverse,///< true if deconvolving time constants
    int detrend,       ///< whether to detrend the data before filtering
    int retrend)       ///< whether to retrend the detrended data after filtering
{
    //float *freqs;
    //freqs = genFreqs(tod->ndata, tod->sampleTime);

    int n = nsamps;
    fftwf_complex *data = fftwf_malloc(n * sizeof(fftwf_complex));
    fftwf_plan p_forward;
    fftwf_plan p_back;
    p_forward = fftwf_plan_dft_1d(n, data, data, FFTW_FORWARD, FFTW_ESTIMATE);
    p_back    = fftwf_plan_dft_1d(n, data, data, FFTW_BACKWARD, FFTW_ESTIMATE);
    fftwf_free(data);

#pragma omp parallel shared(tod,real,imag,dets,ndets,freq,tau,tau_do_inverse, \
    detrend,retrend,n,p_forward,p_back) default (none)
    {
        fftwf_complex *fftdata = fftwf_malloc(n * sizeof(fftwf_complex));
            // Customize the filter with a time constant per-detector
        float *tmp_real = malloc(n * sizeof(float));
        float *tmp_imag = malloc(n * sizeof(float));
        if (tau == NULL) {
            for (int j=0; j<n; j++) {
                tmp_real[j] = real[j];
                tmp_imag[j] = imag[j];
            }
        }

#pragma omp for
        for (int i = 0; i < ndets; i++) {
            for (int j = 0; j < n; j++) {
                if (tau != NULL) {
                    float gain, dtc;
                    dtc = 2.0*tau[i]*freq[j]*M_PI;
                    if (tau_do_inverse) {
                        gain = 1.0;
                    } else {
                        gain = 1.0 / (1.0 + dtc*dtc);
                        dtc = -dtc;
                    }
                    tmp_real[j] = gain * (real[j] - imag[j]*dtc);
                    tmp_imag[j] = gain * (real[j]*dtc + imag[j]);
                }
                fftdata[j][0] = tod[dets[i]*n+j];
                fftdata[j][1] = 0.0;
            }
            filter_one_detector(fftdata, tmp_real, tmp_imag, n, p_forward, p_back, detrend, retrend);
            for (int j = 0; j < n; j++) tod[dets[i]*n+j] = fftdata[j][0];
        }

        fftwf_free(fftdata);
        free(tmp_real);
        free(tmp_imag);
    }
#pragma omp critical
    {
        fftwf_destroy_plan(p_forward);
        fftwf_destroy_plan(p_back);
    }
}

/*--------------------------------------------------------------------------------*/
/*!
 * Filter a detector from a TOD with a give Fourier-domain filter.
 * \param data      The data vector to be filtered.  Modified and returns the filtered result.
 * \param real      The real part of the filter.
 * \param imag      The imaginary part of the filter.
 * \param p_forward The FFTW plan for going to the frequency domain.
 * \param p_back    The FFTW plan for going back to the time domain.
 * \param ndata     The length of the data set.
 */
static void filter_one_detector(fftwf_complex *data, const float *real, const float *imag,
                                int ndata, fftwf_plan p_forward, fftwf_plan p_back,
                                int detrend, int retrend)
{
  float tmp;
  double x0 = 0.0, x1 = 0.0, m = 0, trendSlope = 0;
  int win = 1000;
  if (win > ndata/2) win = ndata/2;
  if (detrend) {
    for (int i = 0; i < win; i++) {
      x0 += data[i][0];
      x1 += data[ndata-1 - i][0];
    }
    x0 /= (double)win;
    x1 /= (double)win;

    m = (x0+x1)/2.0;
    trendSlope = (x1-x0)/(double)(ndata-1.0-win);

    for (int j = 0; j < ndata; j++)
      data[j][0] -= (x0 - m + trendSlope*((double) (j - win/2)));
  }

  fftwf_execute_dft(p_forward, data, data);
  for (int j = 0; j < ndata; j++) {
    // Multiply by complex filter
    tmp = data[j][0]*real[j] - data[j][1]*imag[j];
    data[j][1] = (data[j][0]*imag[j] + data[j][1]*real[j]) / (float)ndata;
    data[j][0] = tmp / (float)ndata;
  }
  fftwf_execute_dft(p_back, data, data);

  if (retrend && detrend)
    for (int j = 0; j < ndata; j++)
      data[j][0] += (x0 - m + trendSlope*((double) (j - win/2)));

}

/*--------------------------------------------------------------------------------*/
/*!
 * Apply a pre-calculated smoothing filter to a data vector.  Then do one of
 * (1) Just smooth the data if (do_smooth).
 * (2) Preserve the data, replacing with smoothed data only during large glitches if (apply_glitch).
 * (3) Fill a vector cutvec saying whether to cut each point due to glitches if (do_cuts).
 * You can call any combination that doesn't involve both #1 and #2.
 * If apply_glitch is true, then replace anything flagged as over the cut threshold (defined as a
 * smoothed value differing from raw by at least nsig*median absolute deviation of smoothed values)
 * with the smoothed value.  Since the glitch filter should have zero in the middle, height of
 * cosmic ray doesn't matter.
 */

typedef float actData;

static void glitch_one_detector(
  fftwf_complex *fftdata, ///< Real data vector to filter.  Modified and returned.
  const float *real,      ///< The pre-calculated smoothing filter to use (real part).
  char *cutvec,           ///< Pre-allocated vector of length n.  Returns cut list if (do_cuts).
  int n,                  ///< Length of the input data vector mydat.
  fftwf_plan p_forward,   ///< The FFTW plan for going to the frequency domain.
  fftwf_plan p_back,      ///< The FFTW plan for going back to the time domain.
  actData nsig,           ///< The cut threshold is this factor times the median abs deviation of smooths.
  int minSep)             ///< Min separation between cut detections to consider them different cuts.
{
  // Save a copy of the input data, then apply the smoothing filter only.
  actData *mydat = malloc(sizeof(actData)*n);
  float *imag = malloc(n * sizeof(float));
  for (int i = 0; i < n; i++) {
    mydat[i] = fftdata[i][0];
    imag[i] = 0.0;
  }
  filter_one_detector(fftdata, real, imag, n, p_forward, p_back, true, false);
  free(imag);

  // tmpclean is the absolute difference between smooth and raw vectors.
  // Find its median (for use in cuts).
  float *tmpclean = malloc(n*sizeof(actData));
  for (int j = 0; j < n; j++)
      tmpclean[j] = fabs(fftdata[j][0]);
  actData thresh = pyactpol_sselect((n*3)/4, n, tmpclean);
  thresh -= pyactpol_sselect(n/4, n, tmpclean);
  thresh *= 0.741*nsig;

  // Find cuts and reset raw data
  int lind = 0;
  memset(cutvec,0,sizeof(*cutvec)*n);
  for (int j = 0; j < n; j++) {
      if (fabs(fftdata[j][0]) > thresh) {
          cutvec[j] = 1;
          if (j-lind < minSep)
              for (int k = lind+1; k < j; k++) cutvec[k] = 1;
          lind = j;
      }
      fftdata[j][0] = mydat[j];
  }

  free(mydat);
  free(tmpclean);
}

/*--------------------------------------------------------------------------------*/
/*!
 * Remove glitches from multiple detector data vectors in a TOD using a pre-computed smoothing (and
 * glitch removal) filter. Also, if (do_cuts), then also apply all glitches as new cuts in the cuts
 * object.
 */

static void mbGlitchC(
    float *tod_data,       ///< The TOD to modify.
    int nsamps,            ///< tod.shape[1]
    const float *filt,     ///< The pre-calculated smoothing filter to use.
    const int *dets,       ///< List of detectors (by index) to clean.
    int ndet,              ///< Number of detectors to clean (length of dets)
    PyObject **cuts,       ///< An array of pointers to PyObjects, where we should put the cuts.
    actData nsig,          ///< The cut threshold is this factor times the median abs deviation of smooths.
    int maxGlitch,         ///< Maximum allowed number of glitches.  If exceeded, cut whole detector.
    int minSep)            ///< Minimum separation between cuts to consider them different cuts.
{

    int **cuts_data = malloc(ndet * sizeof(*cuts_data));
    int *n_cuts = malloc(ndet * sizeof(*n_cuts));
    assert(cuts_data != NULL && n_cuts != NULL);

#pragma omp parallel shared(tod_data,filt,nsamps,dets,ndet,cuts_data,n_cuts,nsig,maxGlitch,minSep) default(none)
    {
        fftwf_plan p_forward;
        fftwf_plan p_back;

        const int n = nsamps;
        fftwf_complex *fftdata = fftwf_malloc(sizeof(fftwf_complex)*n);
        char *cutvec = malloc(n*sizeof(int));

#pragma omp critical
        {
            p_forward = fftwf_plan_dft_1d(n, fftdata, fftdata, FFTW_FORWARD, FFTW_ESTIMATE);
            p_back    = fftwf_plan_dft_1d(n, fftdata, fftdata, FFTW_BACKWARD, FFTW_ESTIMATE);
        }

#pragma omp for
        for (int i = 0; i < ndet; i++) {
            for (int j = 0; j < n; j++) {
                fftdata[j][0] = tod_data[dets[i]*nsamps+j];
                fftdata[j][1] = 0.0;
            }

            glitch_one_detector(fftdata, filt, cutvec ,n, p_forward, p_back, nsig, minSep);

            for (int j = 0; j < n; j++) tod_data[dets[i]*nsamps+j] = fftdata[j][0];

            /* Convert cuts vector start / stop positions */
            cuts_data[i] = pyactpol_cuts_vector_from_mask(cutvec, nsamps, &n_cuts[i]);
        }

#pragma omp critical
        {
            fftwf_destroy_plan(p_forward);
            fftwf_destroy_plan(p_back);
            fftwf_free(fftdata);
            free(cutvec);
        }
    }
    /* Non-parallel: create arrays for the resulting cuts vectors. */
    npy_intp dims[2] = {0, 2};
    for (int i=0; i<ndet; i++) {
        dims[0] = n_cuts[i];
        cuts[i] = PyArray_SimpleNew(2, dims, NPY_INT);
        if (n_cuts[i] > 0) {
            int *dest = PyArray_DATA((PyArrayObject *)cuts[i]);
            memcpy(dest, cuts_data[i], 2*n_cuts[i]*sizeof(*dest));
            free(cuts_data[i]);
        }
    }
}

/* filter_tod_data
 *
 * Apply an arbitrary Fourier filter to time ordered data.  Possibly
 * also convolve or deconvolve the effects of a time constant.
 *
 * Input:
 *   data: (ndet,nsamps) time-ordered data to filter (modified in place).
 *   dets: (ndet_used) index of data vectors to actually filter.
 *   filter: (nsamps) complex filter to apply to the data
 *   taus: (ndet_used) time constants, in seconds, to convolve/deconvolve.
 *   dt: the sample separation, in seconds.
 *   tau_gain: boolean, set to true for time constant deconvolution.
 *   detrend: boolean, set to true to remove a trend prior to filtering.
 *   retrend: boolean, set to true to restore the removed trend after filtering.
 */

PyDoc_STRVAR(filter_tod_data__doc__,
	     "filter_tod_data(data, dets, filter, time_const)\n"
	     "\n"
	     "The dets array gives indices into the first dimension of data, "
	     "so the largest value in dets must be smaller than data.shape[0].\n"
	     "\n"
	     "Applies\n"
	     "        data[dets,:] *= cal[:,None]\n"
    );

static PyObject *filter_tod_data(PyObject *self, PyObject *args)
{
    PyArrayObject *tod_array;
    PyArrayObject *det_idx_array;
    PyArrayObject *filter_array;
    PyObject *taus_object;
    double dt;
    int tau_do_inverse;
    int detrend;
    int retrend;

    if (!PyArg_ParseTuple(args, "O!O!O!Odiii",
                          &PyArray_Type, &tod_array,
                          &PyArray_Type, &det_idx_array,
                          &PyArray_Type, &filter_array,
                          &taus_object,
                          &dt, &tau_do_inverse, &detrend, &retrend
            ))
        po_raise("invalid arguments");

    // Make a frequency vector
    int nsamps = PyArray_DIMS(tod_array)[1];
    float *freq = malloc(nsamps*sizeof(*freq));
    float df = 1.0/nsamps/dt;
    int i=0;
    for (; i<nsamps/2; i++)
        freq[i] = i*df;
    for (; i<nsamps; i++)
        freq[i] = (i-nsamps)*df;

    // Copy filter to real and imaginary pieces
    ASSERT_CARRAY_TYPE_NDIM(filter_array, NPY_COMPLEX64, 1);
    po_assert(PyArray_DIMS(filter_array)[0] == nsamps);
    float *filter = PyArray_DATA(filter_array);
    float *real = malloc(nsamps*sizeof(*real));
    float *imag = malloc(nsamps*sizeof(*imag));
    for (i=0; i<nsamps; i++) {
        real[i] = filter[i*2];
        imag[i] = filter[i*2+1];
    }

    // Those other vectors
    po_assert(PyArray_TYPE(tod_array) == NPY_FLOAT32);
    float *tod = PyArray_DATA(tod_array);
    ASSERT_CARRAY_TYPE_NDIM(det_idx_array, NPY_INT32, 1);
    int *det_idx = PyArray_DATA(det_idx_array);
    float *tau = NULL;
    if (taus_object != Py_None) {
        PyArrayObject *taus_array = (PyArrayObject*)taus_object;
        ASSERT_CARRAY_TYPE_NDIM(taus_array, NPY_FLOAT32, 1);
        po_assert(
                  PyArray_DIMS(det_idx_array)[0]
                  == PyArray_DIMS(taus_array)[0]);
        tau = PyArray_DATA(taus_array);
    }

    // Go
    mbTODFourierFilter(tod, nsamps, det_idx, PyArray_SIZE(det_idx_array),
                       freq, real, imag, tau,
                       tau_do_inverse, detrend, retrend);

    free(freq);
    free(real);
    free(imag);
    Py_RETURN_NONE;
}

/* deglitch_tod_data
 *
 * Input:
 *   data: (ndet,nsamps) time-ordered data to filter (modified in place).
 *   dets: (ndet_used) index of data vectors to actually filter.
 *   filter: (real!) low-pass filter that will be used to remove low
 *     frequency signal
 *   nsig: float that controls strength of glitch rejection.
 *   maxGlitch: maximum number of allowed glitches; when limit is reached
 *     the whole detector will be cut.
 *   minSep: integer, minimum separation cuts must have to not get
 *     auto-joined.
 *
 * Returns:
 *   cuts: list of cuts vectors, indicating which samples were deglitched.
 *
 */

static PyObject *get_glitch_cuts(PyObject *self, PyObject *args)
{
    PyArrayObject *tod_array;
    PyArrayObject *det_idx_array;
    PyArrayObject *filter_array;
    double nsig;
    int maxGlitch;
    int minSep;

    if (!PyArg_ParseTuple(args, "O!O!O!dii",
                          &PyArray_Type, &tod_array,
                          &PyArray_Type, &det_idx_array,
                          &PyArray_Type, &filter_array,
                          &nsig, &maxGlitch, &minSep
            ))
        po_raise("invalid arguments.");

    int nsamps = PyArray_DIMS(tod_array)[1];
    ASSERT_CARRAY_TYPE_NDIM(filter_array, NPY_FLOAT32, 1);
    float *filter = PyArray_DATA(filter_array);

    // Those other vectors
    po_assert(PyArray_TYPE(tod_array) == NPY_FLOAT32);
    float *tod = PyArray_DATA(tod_array);
    ASSERT_CARRAY_TYPE_NDIM(det_idx_array, NPY_INT32, 1);
    int *det_idx = PyArray_DATA(det_idx_array);

    // Deglitcher will return some cuts information
    int ndets = PyArray_DIMS(det_idx_array)[0];
    PyObject **cuts = malloc(ndets*sizeof(PyObject*));
    assert(cuts != NULL);

    // Go
    mbGlitchC(tod, nsamps, filter, det_idx, PyArray_SIZE(det_idx_array),
              cuts,
              nsig, maxGlitch, minSep);

    // Turn the cuts into a list.
    PyObject *cuts_list = PyList_New(ndets);
    for (int i=0; i<ndets; i++)
        PyList_SET_ITEM(cuts_list, i, cuts[i]);

    free(cuts);

    return cuts_list;
}

// --------------------------------------------------------------------
// Analyzes the scan to determine scan frequency, amplitude, speed, and
// identify sections of good and bad scan
/* Error codes:
   0: Good scan
   1: No scan
   2: Bad scan ending
   3: Bad scan beginning
   4: Multiple scan sections
   5: Too many scan sections
*/

typedef struct {
    int min_chunk;
    float min_scan;
    float max_scan;
    float freq;
    float speed;
    float az_max;
    float az_min;
    int *section_limits;
    int nsection;
    int max_sections;
} mbScan;

static int mbAnalyzeScan(double *az, double *ctime, int ndata,
                         mbScan *scan)
{
    // Hack the downsampleLevel to none...
    int downsampleLevel = 1;

    double sampleTime = ctime[1] - ctime[0];

    // Consider N_chunk segments of length chunck
    int N_chunk = (ndata*downsampleLevel)/scan->min_chunk;
    if (N_chunk == 0) N_chunk = 1;
    int chunk = ndata/N_chunk;
    int i, k, nSamp;
    double y, Sy, Sxy;
    double az_Hi, az_Low;

    float *az_max = (float *)malloc(N_chunk * sizeof(float));
    float *az_min = (float *)malloc(N_chunk * sizeof(float));
    float *freq = (float *)malloc(N_chunk * sizeof(float));
    float *speeds = (float *)malloc(N_chunk * sizeof(float));

    // For each chunk
    for (int c = 0; c < N_chunk; c++) {
        i = c*chunk + 1;
        /* Determine az_min and az_max by finding the first two
         * turn-arounds in this chunk, if possible. */
        if (az[i] > az[i-1]) {
            while ((az[i] >= az[i-1]) && (i < (c+1)*chunk)) i++;
            az_max[c] = az[i-1];
            if (i > c*chunk + 1 + chunk/2) {
                freq[c] = -1;
                continue;
            }
            i += 100;
            while ((az[i] <= az[i-1]) && (i < (c+1)*chunk)) i++;
            az_min[c] = az[i-1];
        }
        else {
            while ((az[i] <= az[i-1]) && (i < (c+1)*chunk)) i++;
            az_min[c] = az[i-1];
            if (i > c*chunk + 1 + chunk/2) {
                freq[c] = -1;
                continue;
            }
            i += 100;
            while ((az[i] >= az[i-1]) && (i < (c+1)*chunk)) i++;
            az_max[c] = az[i-1];
        }
        /* Reject weird scan amplitudes */
        if ((az_max[c] - az_min[c] < scan->min_scan) ||
            (az_max[c] - az_min[c] > scan->max_scan) ||
            (i == (c+1)*chunk)) {
            freq[c] = -1;
            continue;
        }
        /* Now go through chunk again, getting speed measurements at
         * every sample that lies in the central 20% of the az range.
         * Also get the mean time of each such segment, and basically
         * fit a line to those times to get the scan frequency.  */
        az_Hi = az_max[c]*0.6 + az_min[c]*0.4;
        az_Low = az_max[c]*0.4 + az_min[c]*0.6;
        nSamp = 0;
        Sy = 0;
        Sxy = 0;
        speeds[c] = 0.0;
        i = c*chunk;
        /* Skip any initial segment that is in the central range */
        while ((az[i] > az_Low) && (az[i] < az_Hi) && (i < (c+1)*chunk)) i++;
        while (i < (c+1)*chunk) {
            if ((az[i] > az_Low) && (az[i] < az_Hi)) {
                nSamp++;
                k = 0;
                y = 0.0;
                while ((az[i] > az_Low) && (az[i] < az_Hi) && (i < (c+1)*chunk)) {
                    y += ctime[i] - ctime[0];
                    k++;
                    i++;
                }
                speeds[c] += fabs(az[i-1]-az[i-k-1])/k;
                y /= k;
                Sy += y;
                Sxy += nSamp*y;
            }
            else i++;
        }
        speeds[c] /= nSamp*sampleTime;
        if (nSamp > 1) freq[c] = nSamp*(nSamp-1) / 12.0 / (2*Sxy/(nSamp+1) - Sy);
        else {
            freq[c] = -1;
        }
    }

    /* Now just throw out any bad chunks, collapsing the various
     * arrays. */
    k = 0;
    for (int c = 0; c < N_chunk; c++) {
        /* printf("freq[%d] = %f; speed[%d] = %f; az_max[%d] = %f; az_min[%d] = %f\n", */
        /*        c,freq[c], c, speeds[c], c, az_max[c], c, az_min[c]); */
        if (freq[c] != -1) {
            az_max[k] = az_max[c];
            az_min[k] = az_min[c];
            freq[k] = freq[c];
            speeds[k] = speeds[c];
            k++;
        }
    }
    /* You either of zero, many, or a small number of good chunks.  So
     * compute and store nothing, the median, or the mean,
     * respectively. */
    if (k == 0) { // This TOD seems to have no scanning
        return 1;
    }
    if (k > 2) {
        scan->az_max = compute_median(k, az_max);
        scan->az_min = compute_median(k, az_min);
        scan->freq = compute_median(k, freq);
        scan->speed = compute_median(k,speeds);
    }
    else {
        scan->az_max = scan->az_min = scan->freq = scan->speed = 0.0;
        for (i = 0; i < k; i++) {
            scan->az_max += az_max[i]/k;
            scan->az_min += az_min[i]/k;
            scan->freq += freq[i]/k;
            scan->speed += speeds[i]/k;
        }
    }

    free(az_max);
    free(az_min);
    free(freq);
    free(speeds);
    //printf("freq_m = %f; speed_m = %f\n", scan->freq, scan->speed);

/*
 * ACT 2
 *
 * Look for bad turn-arounds.  Construct list of segments over which
 * scanning looks ok.  Return 0 only if the whole scan looks like one
 * good scan session.
 */

    int turn = 0, pivot = 0;
    int hperiod = (int)(1./2/scan->freq/sampleTime);
    float speed, speed300;
    float delta = scan->az_max - scan->az_min;
    float margin1 = delta*0.08;
    float margin2 = delta*0.01;

    scan->nsection = 0;
    i = 3;
    /* Loop over half-scans... hopefully */
    while (i+hperiod < ndata) {
        while (turn == 0) {
            if (scan->nsection == scan->max_sections) {
                return 5; // Too many scan sections
            }
            speed = (az[i] - az[i-3])/sampleTime/3;
            // Case in middle of the scan
            if ((fabs(fabs(speed) - scan->speed)/scan->speed <  0.1) &&
                (az[i] <= scan->az_max - margin1) &&
                (az[i] >= scan->az_min + margin1)) {  // good speed
                if (speed > 0) turn = 1;
                else turn = -1;
                scan->nsection++;
                scan->section_limits[(scan->nsection-1)*2] = i-3;
                i++;
                // Search for pivot
                while ((speed*turn > 0) && (i < ndata)) {
                    speed = (az[i] - az[i-1])/sampleTime;
                    i++;
                }
                pivot = i-1;
                turn *= -1;
                //printf("Add1: az_pivot = %f, turn = %d\n", az[pivot], turn);
                break;
            }
            // Case in the lower or upper turnarounds
            else if (((az[i] > scan->az_max - margin1) &&
                      (az[i] < scan->az_max + margin2)) ||
                     ((az[i] < scan->az_min + margin1) &&
                      (az[i] > scan->az_min - margin2))) {
                if (i >= ndata - 300) {
                    return 2; // Bad scan end
                }
                speed300 = (az[i+300] - az[i+297])/sampleTime/3;
                if ((((speed300 < 0) && (az[i] > (scan->az_max + scan->az_min)/2)) ||
                     ((speed300 > 0) && (az[i] < (scan->az_max + scan->az_min)/2))) &&
                    (fabs(fabs(speed300) - scan->speed)/scan->speed < 0.1)) { //good turnaround
                    while (speed == 0) {
                        i++;
                        speed = (az[i] - az[i-1])/sampleTime;
                    }
                    scan->nsection++;
                    scan->section_limits[(scan->nsection-1)*2] = i-3;
                    if (az[i] > (scan->az_max + scan->az_min)/2) turn = -1;
                    else turn = 1;

                    // Search for pivot
                    while ((speed*turn < 0) && (i < ndata-300)) {
                        i++;
                        speed = (az[i] - az[i-1])/sampleTime;
                    }
                    pivot = i-1;
                    if (((turn == -1) &&
                         (fabs(az[pivot] - scan->az_max) > margin2)) ||
                        ((turn == 1) &&
                         (fabs(az[pivot] - scan->az_min) > margin2))) {
                        while ((speed*turn > 0) && (i < ndata-300)) {
                            i++;
                            speed = (az[i] - az[i-1])/sampleTime;
                        }
                        turn *= -1;
                    }
                    pivot = i-1;
                    //printf("Add2: speed300 = %f; az = %f; az_m = %f\n",
                    //     speed300, az[i], (scan->az_max + scan->az_min)/2);
                    break;
                }
                else i++;
            }
            else {
                i++;
            }
            if (i > ndata - hperiod) {
                if (scan->nsection > 1)
                    return 4; //multiple scan sections
                else
                    return 2; // Bad scan end
            }
        }

        //printf("pivot = %d; turn = %d; az = %f\n",pivot, turn, az[pivot+hperiod]);
        i = pivot;
        // Check if we are on a turnarond
        if (((turn == 1) &&
             (fabs(az[pivot+hperiod] - scan->az_max) > margin2)) ||
            ((turn == -1) &&
             (fabs(az[pivot+hperiod] - scan->az_min) > margin2))) { // not good
            //printf("az = %f\n",fabs(az[pivot+hperiod]));
            //printf("turn = %d\n",turn);
            turn = 0;
            scan->section_limits[(scan->nsection-1)*2+1] = i;
            i += hperiod;
        }
        else {
            // Reset the pivot every time to avoid numerical errors
            speed = (az[i] - az[i-1])/sampleTime;
            if (speed*turn < 0) {
                while (speed*turn < 0) {
                    i++;
                    speed = (az[i] - az[i-1])/sampleTime;
                }
                pivot = i-1;
            }
            else {
                while (speed*turn > 0) {
                    i--;
                    speed = (az[i] - az[i-1])/sampleTime;
                }
                pivot = i+1;
            }
            pivot += hperiod;
            i = pivot;
            turn *= -1;
        }
    }
    if (turn != 0)
        scan->section_limits[(scan->nsection-1)*2+1] = ndata-1;

    if (scan->nsection > 1)
        return 4; //multiple scan sections
    else if ((scan->section_limits[0] == 0) && (scan->section_limits[1] == ndata-1))
        return 0;  // Good scan
    else
        return 3;  // Bad beginning of scan
}

PyDoc_STRVAR(analyze_scan__doc__,
             "analyze_scan(az, times)\n"
             "\n"
             "Determine basic properties of a triangle-wave azimuth scan. "
             "Each vector should be a simple ndarray of doubles, with az "
             "in radians and times in seconds.\n"
             "\n"
             "Returns a tuple of items:\n"
             "  (status, speed, scan_freq, az_max, az_min, sections_array)\n"
             "\n"
             "Units are combinations of radians and seconds.  status=0 is "
             "good; otherwise there is something weird with the scan, which "
             "can you can figure out from sections_array.\n");

static PyObject *analyze_scan(PyObject *self, PyObject *args)
{
    PyArrayObject *az_array;
    PyArrayObject *ctime_array;

    if (!PyArg_ParseTuple(args, "O!O!",
                          &PyArray_Type, &az_array,
                          &PyArray_Type, &ctime_array
            ))
        po_raise("invalid arguments.");

    double *az = PyArray_DATA(az_array);
    ASSERT_CARRAY_TYPE_NDIM(az_array, NPY_FLOAT64, 1);
    double *ctime = PyArray_DATA(ctime_array);
    ASSERT_CARRAY_TYPE_NDIM(ctime_array, NPY_FLOAT64, 1);
    int nsamps = PyArray_DIMS(ctime_array)[0];

    mbScan scan;
    scan.min_chunk = 24000;
    scan.min_scan = 0.00873; // Half a degree in azimuth
    scan.max_scan = 0.34907; // 20 degrees in azimuth
    scan.freq = 0.0;
    scan.speed = 0.0;
    scan.az_max = 0.0;
    scan.az_min = 0.0;
    scan.nsection = 0;
    scan.max_sections = 5;
    scan.section_limits = (int*)malloc(2*scan.max_sections *
                                       sizeof(int));
    for (int i = 0; i < 2*scan.max_sections; i++)
        scan.section_limits[i] = -1;

    int scan_err = mbAnalyzeScan(az, ctime, nsamps, &scan);

    // Pack up the scan sections in a little array...
    npy_intp dims[2] = {scan.nsection, 2};
    PyArrayObject* sections_array = (PyArrayObject*)
        PyArray_SimpleNew(2, dims, NPY_INT32);
    for (int i=0; i<scan.nsection*2; i++)
        ((int*)PyArray_DATA(sections_array))[i] = scan.section_limits[i];
    free(scan.section_limits);

    /* Return everything in a tuple.  Update the docstring ^^^ if you
     * alter this. */
    return Py_BuildValue("iffffN",
                         scan_err,
                         scan.speed,
                         scan.freq,
                         scan.az_max,
                         scan.az_min,
                         sections_array);
}

/// ------------------------------------------------------------------------
/// Simple search for TOD jumps
static PyObject *find_jumps(PyObject *self, PyObject *args)
{
    PyArrayObject *data_array;
    int dsStep, window;

    if (!PyArg_ParseTuple(args, "O!ii",
                          &PyArray_Type, &data_array,
                          &dsStep, &window
            ))
        po_raise("invalid arguments.");

    // Types and ordering... the ordering is stricter than we really need.
    ASSERT_CARRAY_TYPE_NDIM(data_array, NPY_FLOAT32, 2);
    int ndet = PyArray_DIMS(data_array)[0];
    int nsamps = PyArray_DIMS(data_array)[1];
    int ndata = nsamps/dsStep;

    float *tod_data = PyArray_DATA(data_array);

    npy_intp dims[1];
    dims[0] = ndet;
    PyObject *jump_array = PyArray_SimpleNew(1, dims, NPY_FLOAT32);
    float *jump = PyArray_DATA((PyArrayObject*) jump_array);

    // Downsample data
    float *data = (float *)malloc(ndet * ndata * sizeof(float));
    for (int i = 0; i < ndet; i++) {
        float *this_data = tod_data + nsamps * i;
        for (int j = 0; j < ndata; j++) {
            data[i*ndata + j] = this_data[dsStep * j];
        }
    }

    // Find jumps
    int w = window;
    int nj = ndata-2*w+1;
    float jump_j;
    for (int i = 0; i < ndet; i++) {
        int d = i*ndata;
        jump[i] = 0;
        for (int j = 0; j < nj; j++) {
            jump_j = 0;
            for (int k=0; k<w; k++) jump_j += data[d+j+k] - data[d+j+k+w];
            jump_j /= w;
            if (fabs(jump_j) > jump[i]) jump[i] = fabs(jump_j);
        }
    }
    free(data);
    return jump_array;
}

/// ------------------------------------------------------------------------
/// Perform preselection of detectors depending on correlation with common
/// mode and gain with respect to the common mode. The common mode is
/// obtained using the median of the initial set of candidates.
static PyObject *preselect_dets(PyObject *self, PyObject *args)
{
    PyArrayObject *data_array;
    PyArrayObject *sel_array;
    double min_corr, norm_factor;
    int dsStep;

    if (!PyArg_ParseTuple(args, "O!O!ddi",
                          &PyArray_Type, &data_array,
                          &PyArray_Type, &sel_array,
                          &min_corr, &norm_factor, &dsStep
            ))
        po_raise("invalid arguments.");

    // Types and ordering... the ordering is stricter than we really need.
    ASSERT_CARRAY_TYPE_NDIM(data_array, NPY_FLOAT32, 2);
    int ndet = PyArray_DIMS(data_array)[0];
    int nsamps = PyArray_DIMS(data_array)[1];
    int ndata = nsamps/dsStep;

    // Selections array
    ASSERT_CARRAY_TYPE_NDIM(sel_array, NPY_INT32, 1);
    po_assert(PyArray_DIMS(sel_array)[0] == ndet);

    float *tod_data = PyArray_DATA(data_array);
    int *sel = PyArray_DATA(sel_array);

    // Produce list of detector candidates
    int N = 0;
    for (int i = 0; i < ndet; i++) if (sel[i]) N++;
    int *dets = (int *)malloc(N * sizeof(int));
    int j = 0;
    for (int i = 0; i < ndet; i++)
        if (sel[i]) {dets[j] = i; j++;}

    // Downsample data and obtain norm of every detector TOD
    int NN = N;
    float *norm = (float *)malloc(N * sizeof(float));
    float *norm2 = (float *)malloc(N * sizeof(float));
    float *data = (float *)malloc(N * ndata * sizeof(float));
    for (int i = 0; i < N; i++) {
        float *this_data = tod_data + nsamps * dets[i];
        norm[i] = 0.0;
        for (int j = 0; j < ndata; j++) {
            data[i*ndata + j] = this_data[dsStep * j];
            norm[i] += this_data[dsStep * j]*this_data[dsStep * j];
        }
        norm[i] = sqrt(norm[i]);
        norm2[i] = norm[i];
    }
    float norm_m = compute_median(N,norm2);
    for (int i = 0; i < N; i++) {
        if ((norm[i]/norm_m > norm_factor) || (norm[i]/norm_m < 1./norm_factor)) {
            sel[dets[i]] = 0;
            NN--;
        }
    }

    // Obtain median common mode
    float cm2 = 0.0;
    float *cm = (float *)malloc(ndata * sizeof(float));
    float *temp = (float *)malloc(N * sizeof(int));
    int k;
    for (int s = 0; s < ndata; s++) {
        k = 0;
        for (int j = 0; j < N; j++) {
            if (sel[dets[j]]) {
                temp[k] = data[j*ndata + s]/norm[j];
                k++;
            }
        }
        cm[s] = compute_median(NN, temp);
        cm2 += cm[s] * cm[s];
    }

    // Select according to correlation and gain
    float *dot = (float *)malloc(N * sizeof(float));
    cblas_sgemv(CblasRowMajor, CblasNoTrans,
                N, ndata, 1.0, data, ndata, cm, 1, 0.0, dot, 1);

    // Select according to correlation first
    double c;
    for (int i = 0; i < N; i++) {
        if (sel[dets[i]]) {
            if (norm[i] != 0.0) c = dot[i]/norm[i]/sqrt(cm2);
            else c = 0.0;
            if (c < min_corr) {
                sel[dets[i]] = 0;
                NN--;
            }
        }
    }

    free(norm);
    free(data);
    free(dets);
    free(cm);
    free(temp);
    free(dot);
    Py_RETURN_NONE;
}

PyDoc_STRVAR(preselect_dets__doc__,
	     "preselect_dets(data, sel, min_corr, norm_factor, dsStep)\n"
    );

PyDoc_STRVAR( get_drift_tests__doc__,
              "get_drift_tests(...)\n"
    );

////////////////////////////////////////////////////////////////////////////
//                            CLAPACK WRAPPER                             //
////////////////////////////////////////////////////////////////////////////
static int sgesdd(char JOBZ, int M, int N, float *A, int LDA, float *S, float *U,
           int LDU, float *VT, int LDVT, float *WORK, int LWORK, int *IWORK)
{
    extern void sgesdd_(char *JOBZp, int *Mp, int *Np, float *A, int *LDAp,
                        float *S, float *U, int *LDUp, float *VT, int *LDVTp,
                        float *WORK, int *LWORKp, int *IWORK, int *INFOp);
    int INFO;
    sgesdd_(&JOBZ, &M, &N, A, &LDA, S, U, &LDU, VT, &LDVT, WORK, &LWORK, IWORK,
            &INFO);
    return INFO;
}

static PyObject *get_drift_tests(PyObject *self, PyObject *args)
{
    PyArrayObject *tod_array;
    PyArrayObject *row_array;
    PyArrayObject *col_array;
    PyArrayObject *ldet_array;
    PyArrayObject *ddet_array;
    PyArrayObject *temp_array;

    int dsStep, nBlock, nModes, removeDarkCM;

    if (!PyArg_ParseTuple(args, "O!O!O!O!O!O!iiii",
                          &PyArray_Type, &tod_array,
                          &PyArray_Type, &row_array,
                          &PyArray_Type, &col_array,
                          &PyArray_Type, &ldet_array,
                          &PyArray_Type, &ddet_array,
                          &PyArray_Type, &temp_array,
                          &dsStep,
                          &nBlock,
                          &nModes,
                          &removeDarkCM
            ))
        po_raise("invalid arguments.");

    // Check TOD data...
    ASSERT_CARRAY_TYPE_NDIM(tod_array, NPY_FLOAT32, 2);
    int ndet = PyArray_DIMS(tod_array)[0];
    int tod_ndata = PyArray_DIMS(tod_array)[1];
    float *tod_data = PyArray_DATA(tod_array);

    // Check row and col
    ASSERT_CARRAY_TYPE_NDIM(row_array, NPY_INT32, 1);
    ASSERT_CARRAY_TYPE_NDIM(col_array, NPY_INT32, 1);
    int *tod_rows = PyArray_DATA(row_array);
    int *tod_cols = PyArray_DATA(col_array);
    po_assert(PyArray_DIMS(row_array)[0] == ndet);
    po_assert(PyArray_DIMS(col_array)[0] == ndet);

    // Check det lists...
    ASSERT_CARRAY_TYPE_NDIM(ldet_array, NPY_INT32, 1);
    ASSERT_CARRAY_TYPE_NDIM(ddet_array, NPY_INT32, 1);
    int *ldet = PyArray_DATA(ldet_array);
    int nldet = PyArray_DIMS(ldet_array)[0];
    int *ddet = PyArray_DATA(ddet_array);
    int nddet = PyArray_DIMS(ddet_array)[0];

    // Check temperature vector
    ASSERT_CARRAY_TYPE_NDIM(temp_array, NPY_FLOAT32, 1);
    po_assert(PyArray_DIMS(temp_array)[0] == tod_ndata);
    int ndata = tod_ndata / dsStep;
    float *temp = PyArray_DATA(temp_array);
    bool valid_temp = false;
    if ((temp[0] != temp[ndata/2]) || (temp[0] != temp[ndata/3])) valid_temp = true;

    // Downsampling -- ndata is the downsampled size.

    // Sanity checks
    assert((nBlock == 1) || (nBlock == 4) || (nBlock = 9) || (nBlock == 16));
    assert(ndet > 0);
    assert(ndata > 0);
    if (nModes > nBlock)
        nModes = nBlock;

    // Ready output arrays.
    npy_intp dims[2];
    /* Time-like... */
    dims[0] = ndata;
    PyObject *cm_array = PyArray_SimpleNew(1, dims, NPY_FLOAT32);
    PyObject *dcm_array = PyArray_SimpleNew(1, dims, NPY_FLOAT32);
    float *cm = PyArray_DATA((PyArrayObject*) cm_array);
    float *dcm = PyArray_DATA((PyArrayObject*) dcm_array);
    /* Det-like... */
    dims[0] = ndet;
    /* Template:
       PyObject *X_array = PyArray_SimpleNew(1, dims, NPY_FLOAT64);
       double *X         = PyArray_DATA((PyArrayObject*) X_array);
    */
    PyObject *norm_array     = PyArray_SimpleNew(1, dims, NPY_FLOAT64);
    double   *norm           = PyArray_DATA((PyArrayObject*) norm_array);
    PyObject *corrLive_array = PyArray_SimpleNew(1, dims, NPY_FLOAT64);
    double   *corrLive       = PyArray_DATA((PyArrayObject*) corrLive_array);
    PyObject *DELive_array   = PyArray_SimpleNew(1, dims, NPY_FLOAT64);
    double   *DELive         = PyArray_DATA((PyArrayObject*) DELive_array);
    PyObject *gainLive_array = PyArray_SimpleNew(1, dims, NPY_FLOAT64);
    double   *gainLive       = PyArray_DATA((PyArrayObject*) gainLive_array);
    PyObject *corrDark_array = PyArray_SimpleNew(1, dims, NPY_FLOAT64);
    double   *corrDark       = PyArray_DATA((PyArrayObject*) corrDark_array);
    PyObject *DEDark_array   = PyArray_SimpleNew(1, dims, NPY_FLOAT64);
    double   *DEDark         = PyArray_DATA((PyArrayObject*) DEDark_array);
    PyObject *gainDark_array = PyArray_SimpleNew(1, dims, NPY_FLOAT64);
    double   *gainDark       = PyArray_DATA((PyArrayObject*) gainDark_array);

    /* Resume original moby code... */

  int side = (int)sqrt((float)nBlock);
  int nSide = (int)ceil(32.0/side);
  int rmin, cmin, N, zeros = 0;

  int *bdet = (int *)malloc(ndet*sizeof(int));
  int *rows = (int *)malloc(nldet*sizeof(int));
  int *cols = (int *)malloc(nldet*sizeof(int));
  for (int i = 0; i < nldet; i++) {
    rows[i] = tod_rows[ldet[i]];
    cols[i] = tod_cols[ldet[i]];
  }

  // Downsample data for every detector TOD
  float *data = (float *)malloc(ndet * ndata * sizeof(float));
  for (int i = 0; i < ndet; i++) {
    double mean = 0.;
    for (int j = 0; j < ndata; j++) {
      data[i*ndata + j] = tod_data[i*tod_ndata + dsStep * j];
      mean += data[i*ndata+j];
    }
    mean /= ndata;
    for (int j = 0; j < ndata; j++) {
      data[i*ndata + j] -= mean;
    }
  }

  float *dot = (float *)malloc(ndet * sizeof(float));
  if (valid_temp) {
    // Downsample temperature
    float *dstemp = (float *)malloc(ndata * sizeof(float));
    double mean = 0;
    double temp2 = 0;
    for (int j = 0; j < ndata; j++) {
      dstemp[j] = temp[dsStep * j];
      mean += dstemp[j];
    }
    // Remove mean and obtain norm^2 of temp
    mean /= ndata;
    for (int j = 0; j < ndata; j++) {
      dstemp[j] -= mean;
      temp2 += dstemp[j] * dstemp[j];
    }
    if (temp2==0.)
        temp2 = 1.;

    // Obtain fit temperature to data
    cblas_sgemv(CblasRowMajor, CblasNoTrans, ndet, ndata, 1.0, data, ndata,
                dstemp, 1, 0.0, dot, 1);

    // Remove temperature signal
//#pragma omp parallel shared(ndet, ndata, data, dot, dstemp)
    {
//#pragma omp for
      for (int d = 0; d < ndet; d++) {
        for (int i = 0; i < ndata; i++) data[d*ndata + i] -= dot[d]*dstemp[i]/temp2;
      }
    }
    free(dstemp);
  }

  // Obtain dark common mode
  float cm2 = 0;
  for (int s = 0; s < ndata; s++) {
    dcm[s] = 0;
    for (int j = 0; j < nddet; j++) dcm[s] += data[ddet[j]*ndata + s]/nddet;
    cm2 += dcm[s] * dcm[s];
  }

  // Obtain dark correlation and gain
  cblas_sgemv(CblasRowMajor, CblasNoTrans, ndet, ndata, 1.0, data, ndata,
              dcm, 1, 0.0, dot, 1);
  for (int i = 0; i < ndet; i++) {
    norm[i] = 0.0;
    for (int j = 0; j < ndata; j++)  norm[i] += data[i*ndata+j]*data[i*ndata+j];
    norm[i] = sqrt(norm[i]/ndata);
    if (norm[i] != 0.0) corrDark[i] = dot[i]/norm[i]/sqrt(cm2*ndata);
    else corrDark[i] = 0.0;
    gainDark[i] = dot[i]/cm2;
  }

  // Obtain dark drift error
//#pragma omp parallel shared(DEDark, ndet, ndata, data, gainDark)
  {
//#pragma omp for
    for (int d = 0; d < ndet; d++) {
      float dat;
      // Remove dark common mode
      DEDark[d] = 0.0;
      for (int i = 0; i < ndata; i++) {
        dat = data[d*ndata + i] - gainDark[d]*dcm[i];
        DEDark[d] += dat * dat / ndata;
        if (removeDarkCM==1) data[d*ndata + i] = dat;
      }
      DEDark[d] = sqrt(DEDark[d]);
      if (norm[d] != 0.0) DEDark[d] /= norm[d];
    }
  }
  //Note that DEDark here constitutes, for live detectors, the norm of the
  //data minus the dark common mode

  // Remove slope
  int Ns = ((ndata+1)/2)*2 - 1;
  float Ns2 = (float)(Ns/2);
  long int sx2 = (long int)(Ns2*Ns2*Ns2/3 + Ns2*Ns2/2 + Ns2/6)*2;
  double a, A;
  A = 0;
  for (int j = 0; j < ndet; j++) {
    a = 0;
    for (int s = 0; s < Ns; s++) a += (s - Ns2)*data[j*ndata + s];
    for (int d = 0; d < nldet; d++) if (j == ldet[d]) A += a;
    for (int s = 0; s < ndata; s++) data[j*ndata + s] -= (s - Ns2)*a/sx2;
  }
  A /= nldet;

  // Obtain live common mode
  cm2 = 0;
  for (int s = 0; s < ndata; s++) {
    cm[s] = 0;
    for (int j = 0; j < nldet; j++) cm[s] += data[ldet[j]*ndata + s]/nldet;
    cm2 += cm[s] * cm[s];
  }
  // Obtain live correlation and gain
  cblas_sgemv(CblasRowMajor, CblasNoTrans, ndet, ndata, 1.0, data, ndata,
              cm, 1, 0.0, dot, 1);
  for (int d = 0; d < ndet; d++) {
    norm[d] = 0.0;
    for (int j = 0; j < ndata; j++)  norm[d] += data[d*ndata+j]*data[d*ndata+j];
    norm[d] = sqrt(norm[d]/ndata);
    if (norm[d] != 0.0) corrLive[d] = dot[d]/norm[d]/sqrt(cm2*ndata);
    else corrLive[d] = 0.0;
    gainLive[d] = dot[d]/cm2;
  }

  // Obtain block common modes, weighted by the number of detectors in
  // each block.
  float *cmodes = (float *)malloc(nBlock * ndata * sizeof(float));
  for (int i = 0; i < nBlock; i++) {
    rmin = nSide * (i/side);
    cmin = nSide*i - rmin*side;
    N = 0;
    for (int j = 0; j < nldet; j++) {
      if (rows[j] >= rmin && rows[j] < rmin + nSide &&
          cols[j] >= cmin && cols[j] < cmin + nSide) {
        bdet[N] = ldet[j];
        N++;
      }
    }
    if (N > 0) {
      for (int s = 0; s < ndata; s++) {
        cmodes[(i-zeros)*ndata + s] = 0;
        for (int j = 0; j < N; j++)
          cmodes[(i-zeros)*ndata + s] += data[bdet[j]*ndata + s];
      }
    }
    else {
      zeros++;
    }
  }
  nBlock -= zeros;
  if (nModes > nBlock) nModes = nBlock;

  // Obtain the SVD
    // A (M-by-N) with M rows and N columns
    // JOBZ = 'S'
    // M = ndata
    // N = nBlock
    // LDA = ndata
  float *S = (float *)malloc(nBlock * sizeof(float));
  float *U = (float *)malloc(nBlock * ndata * sizeof(float));
  float *VT = (float *)malloc(nBlock * nBlock * sizeof(float));
  int LWORK = nBlock * ndata;
  float *WORK = (float *)malloc(LWORK * sizeof(float));
  int *IWORK = (int *)malloc(8 * nBlock * sizeof(int));
  // Apply Lapack SVD
  sgesdd('S', ndata, nBlock, cmodes, ndata, S, U, ndata, VT, nBlock,
         WORK, LWORK, IWORK);

  // Find fit coefficients (mT A).
  // Note that matrices are transposed in this definition
  float *coeff = (float *)malloc(nBlock * ndet * sizeof(float));
  cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, ndet, nBlock, ndata, 1.0,
              data, ndata, U, ndata, 0.0, coeff, nBlock);

  // Rearrange coeff matrix to contain only nModes modes
  for (int i = 0; i < ndet; i++) {
    for (int j = 0; j < nModes; j ++) {
      coeff[i*nModes + j] = coeff[i*nBlock + j];
    }
  }

  // Remove modes (A - m mT A)
  cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, ndet, ndata, nModes, -1.0,
              coeff, nModes, U, ndata, 1.0, data, ndata);

  // Obtain drift error
//#pragma omp parallel shared(DELive, ndet, ndata, data)
  {
//#pragma omp for
    for (int d = 0; d < ndet; d++) {
      DELive[d] = 0.0;
      for (int i = 0; i < ndata; i++) {
        DELive[d] += data[d*ndata + i] * data[d*ndata + i] / ndata;
      }
      if (norm[d] != 0.0) DELive[d] = sqrt(DELive[d])/norm[d];
    }
  }
  //Note that we have normalized the DriftError to cancel the effect of the gain in
  //the value, because we want to represent differences in shape with respect to the
  //common mode

  //Re-add the slope to the common-mode
  for (int s = 0; s < ndata; s++) cm[s] += (s - Ns2)*A/sx2;

  free(bdet);
  free(rows);
  free(cols);
  free(data);
  free(dot);
  free(cmodes);
  free(S);
  free(U);
  free(VT);
  free(WORK);
  free(IWORK);
  free(coeff);

    return Py_BuildValue("NNNNNNNNN",
                         cm_array,
                         dcm_array,
                         norm_array,
                         corrLive_array,
                         DELive_array,
                         gainLive_array,
                         corrDark_array,
                         DEDark_array,
                         gainDark_array);

}

PyMethodDef pyactpol_filter_methods[] = {
    {"remove_mean", remove_mean, METH_VARARGS, ""},
    {"filter_tod_data", filter_tod_data, METH_VARARGS,
    filter_tod_data__doc__},
    {"get_glitch_cuts", get_glitch_cuts, METH_VARARGS, ""},
    {"analyze_scan", analyze_scan, METH_VARARGS,
     analyze_scan__doc__},
    {"preselect_dets", preselect_dets, METH_VARARGS,
     preselect_dets__doc__},
    {"find_jumps", find_jumps, METH_VARARGS, ""},
    {"get_drift_tests", get_drift_tests, METH_VARARGS,
     get_drift_tests__doc__},
    {NULL, NULL, 0, NULL}        /* Sentinel */
};


/* ode-initval/rk4imp.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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Runge-Kutta 4, Gaussian implicit */

/* Author:  G. Jungman */

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

#include "odeiv_util.h"

typedef struct
{
  double *k1nu;
  double *k2nu;
  double *ytmp1;
  double *ytmp2;
}
rk4imp_state_t;

static void *
rk4imp_alloc (size_t dim)
{
  rk4imp_state_t *state = (rk4imp_state_t *) malloc (sizeof (rk4imp_state_t));

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

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

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

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

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

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

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

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

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

  return state;
}

static int
rk4imp_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)
{
  rk4imp_state_t *state = (rk4imp_state_t *) vstate;

  const double ir3 = 1.0 / M_SQRT3;
  const int iter_steps = 3;
  int status = 0;
  int nu;
  size_t i;

  double *const k1nu = state->k1nu;
  double *const k2nu = state->k2nu;
  double *const ytmp1 = state->ytmp1;
  double *const ytmp2 = state->ytmp2;

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

  DBL_MEMCPY (k2nu, k1nu, dim);

  /* iterative solution */
  for (nu = 0; nu < iter_steps; nu++)
    {
      for (i = 0; i < dim; i++)
        {
          ytmp1[i] =
            y[i] + h * (0.25 * k1nu[i] + 0.5 * (0.5 - ir3) * k2nu[i]);
          ytmp2[i] =
            y[i] + h * (0.25 * k2nu[i] + 0.5 * (0.5 + ir3) * k1nu[i]);
        }
      {
        int s =
          GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h * (1.0 - ir3), ytmp1, k1nu);
        GSL_STATUS_UPDATE (&status, s);
      }
      {
        int s =
          GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h * (1.0 + ir3), ytmp2, k2nu);
        GSL_STATUS_UPDATE (&status, s);
      }
    }

  /* assignment */
  for (i = 0; i < dim; i++)
    {
      const double d_i = 0.5 * (k1nu[i] + k2nu[i]);
      if (dydt_out != NULL)
        dydt_out[i] = d_i;
      y[i] += h * d_i;
      yerr[i] = h * h * d_i;    /* FIXME: is this an overestimate ? */
    }

  return status;
}

static int
rk4imp_reset (void *vstate, size_t dim)
{
  rk4imp_state_t *state = (rk4imp_state_t *) vstate;

  DBL_ZERO_MEMSET (state->k1nu, dim);
  DBL_ZERO_MEMSET (state->k2nu, dim);
  DBL_ZERO_MEMSET (state->ytmp1, dim);
  DBL_ZERO_MEMSET (state->ytmp2, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk4imp_order (void *vstate)
{
  rk4imp_state_t *state = (rk4imp_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 4;
}

static void
rk4imp_free (void *vstate)
{
  rk4imp_state_t *state = (rk4imp_state_t *) vstate;
  free (state->k1nu);
  free (state->k2nu);
  free (state->ytmp1);
  free (state->ytmp2);
  free (state);
}

static const gsl_odeiv_step_type rk4imp_type = { "rk4imp",      /* name */
  1,                            /* can use dydt_in */
  0,                            /* gives exact dydt_out */
  &rk4imp_alloc,
  &rk4imp_apply,
  &rk4imp_reset,
  &rk4imp_order,
  &rk4imp_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rk4imp = &rk4imp_type;


#include <math.h>
#include <string.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_sf_erf.h>
#include "prob_functions.h"
#include "data.h"

static const double ALPHA_PRIOR_MEAN = 1, BETA_PRIOR_MEAN = 1;

double my_f (const gsl_vector *x, void *params)
{
	Dataset *data = (Dataset *) params;

	unpackX(x, data);

	return - computeQ(data);
}

void my_df (const gsl_vector *x, void *params, gsl_vector *g)
{
	int i, j;

	Dataset *data = (Dataset *) params;
	double *dQdAlpha = (double *) malloc(sizeof(double) * data->numLabelers * data->numWorkflows);
	double *dQdBeta = (double *) malloc(sizeof(double) * data->numImages * data->numWorkflows);

	unpackX(x, data);

	gradientQ(data, dQdAlpha, dQdBeta);

	/* Pack dQdAlpha and dQdBeta into gsl_vector */
	for (i = 0; i < data->numLabelers * data->numWorkflows; i++) {
		gsl_vector_set(g, i, - dQdAlpha[i]);  /* Flip the sign since we want to minimize */
	}
	for (j = 0; j < data->numImages * data->numWorkflows; j++) {
	  gsl_vector_set(g, (data->numLabelers * data->numWorkflows) + j, - dQdBeta[j]);  /* Flip the sign since we want to minimize */
	}

	free(dQdAlpha);
	free(dQdBeta);
}

void my_fdf (const gsl_vector *x, void *params, double *f, gsl_vector *g)
{
	*f = my_f(x, params);
	my_df(x, params, g);
}

void gradientQ (Dataset *data, double *dQdAlpha, double *dQdBeta)
{
	int i, j;
	int idx;

	/* This comes from the priors */
	/*RandoPriors*/
	/*
	for (i = 0; i < data->numLabelers; i++) {
		dQdAlpha[i] = - 1 * (data->alpha[i] - data->priorAlpha[i]);
	}

	for (j = 0; j < data->numImages; j++) {
		dQdBeta[j] = - 5 * (data->beta[j] - data->priorBeta[j]);
	}
	*/
	/*Here's where the real shit starts*/
	for (idx = 0; idx < data->numLabels; idx++) {
		int i = data->labels[idx].labelerId;
		int j = data->labels[idx].imageIdx;
		int k = data->labels[idx].workflowId;
		int alphaidx = k * data->numLabelers + i;
		int betaidx = k * data->numImages + j;
		int lij = data->labels[idx].label;
		double sigma = prob(data->alpha[alphaidx], data->beta[betaidx]);

		double increa, increb;
		double weight = data->probZ1[j] * (lij - sigma) + data->probZ0[j] * (1 - lij - sigma);
		//double weight = data->probZ1[j] * (2 * lij - 1) + data->probZ0[j] * (-2 * lij + 1);
		increa = .5 * weight * log(1-data->beta[betaidx]) * pow( 1-data->beta[betaidx],
									 data->alpha[alphaidx] );
		increb = -.5 * weight * data->alpha[alphaidx] * pow( 1-data->beta[betaidx],
								     data->alpha[alphaidx]-1 );
		dQdAlpha[alphaidx] += increa;
		dQdBeta[betaidx] += increb;
		//printf("Alpha: %f Beta: %f", data->alpha[alphaidx], data->beta[betaidx]);
		//printf("Blah%f %f %f\n", log(1-data->beta[j]), 1-data->beta[j], data->alpha[i]);
		//printf("%d %d %d %f %f %f %f\n", i, j, lij, sigma, increa, increb, weight);
		//printf("%f %f\n", data->probZ1[j], data->probZ0[j]);
		/*
		if ( isnan(prob(data->alpha[i], data->beta[j])) )
{
printf("%d %d\n", i, j);
printf("%f %f\n", data->probZ1[j], data->probZ0[j]);
printf("alpha %f beta %f\n", data->alpha[i], data->beta[j]);
printf("sigma %f\n", sigma);
printf("label %d\n", lij);
printf("prob: %f\n", prob);
printf("%f\n", pow( 1-data->beta[j], data->alpha[i]-1 ));
printf("%f\n", data->alpha[i]);
printf("%f %f\n", increa, increb);
}
fflush(0);

	for (i = 0; i < data->numLabelers; i++) {
		printf("i %d %f %f\n", i, data->alpha[i], dQdAlpha[i]);
	}

	for (j = 0; j < data->numImages; j++) {
		printf("j %d %f %f\n", j, data->beta[j], dQdBeta[j]);
	}
		*/
	  }

}
/*
double computeLikelihood (Dataset *data)
{
	int i, j;
	int idx;
	double L = 0;

	double *alpha = data->alpha, *beta = data->beta;

	for (j = 0; j < data->numImages; j++) {
		double P1 = data->priorZ1[j];
		double P0 = 1 - data->priorZ1[j];
		for (idx = 0; idx < data->numLabels; idx++) {
			if (data->labels[idx].imageIdx == j) {
				int i = data->labels[idx].labelerId;
				int lij = data->labels[idx].label;
		double sigma = prob(alpha[i], beta[j]);
				P1 *= pow(sigma, lij) * pow(1 - sigma, 1 - lij);
				P0 *= pow(sigma, 1 - lij) * pow(1 - sigma, lij);
			}
		}
		L += log(P1 + P0);
	}

	/// Add Gaussian (standard normal) prior for alpha
	for (i = 0; i < data->numLabelers; i++) {
		L += log(gsl_sf_erf_Z(alpha[i] - data->priorAlpha[i]));
	}

	/// Add Gaussian (standard normal) prior for beta
	for (j = 0; j < data->numImages; j++) {
		L += log(gsl_sf_erf_Z(beta[j] - data->priorBeta[j]));
	}

	return L;
}*/

double computeQ (Dataset *data)
{
	int i, j;
	int idx;
	double Q = 0;

	double *alpha = data->alpha, *beta = data->beta;

	/* Start with the expectation of the sum of priors over all images */
	for (j = 0; j < data->numImages; j++) {
		Q += data->probZ1[j] * log(data->priorZ1[j]);
		Q += data->probZ0[j] * log(1 - data->priorZ1[j]);
	}

	for (idx = 0; idx < data->numLabels; idx++) {
		int i = data->labels[idx].labelerId;
		int j = data->labels[idx].imageIdx;
		int k = data->labels[idx].workflowId;
		int alphaidx = k * data->numLabelers + i;
		int betaidx = k * data->numImages + j;
		int lij = data->labels[idx].label;
		/* Do some analytic manipulation first for numerical stability! */
		double logSigma = log( prob(alpha[alphaidx], beta[betaidx]) );
		double logOneMinusSigma = log( 1-prob(alpha[alphaidx], beta[betaidx]) );
		if (beta[betaidx] > 1 || beta[betaidx] < 0 || alpha[alphaidx] < 0) {
		  Q += -100000000000000000;
		} else {
		  Q += data->probZ1[j] * (lij * logSigma + (1 - lij) * logOneMinusSigma) +
		    data->probZ0[j] * ((1 - lij) * logSigma + lij * logOneMinusSigma);

		}
		if (isnan(Q)) {
		  printf("%d %d %d %f %f %f %f %f %f %f %f\n", i, j, lij, logSigma,
			 logOneMinusSigma,
			 prob(alpha[i], beta[j]),
			 data->probZ1[j],
			 data->probZ0[j],
			 alpha[alphaidx],
			 beta[betaidx],
			 Q);
		  abort();
		}
	}

	/* Add Gaussian (standard normal) prior for alpha */
	for (i = 0; i < data->numLabelers * data->numWorkflows; i++) {
		//Q += log(gsl_sf_erf_Z(alpha[i] - data->priorAlpha[i]));
	}

	/* Add Gaussian (standard normal) prior for beta */
	for (j = 0; j < data->numImages * data->numWorkflows; j++) {
		//Q += log(gsl_sf_erf_Z(beta[j] - data->priorBeta[j]));
	}

	// add penalty of beta not between 0 and 1
	for (j = 0; j < data->numImages * data->numWorkflows; j++) {
	  if ( data->beta[j] > 1 || data->beta[j] < 0 ) {
	    Q -= 100;
	  }
	}
	return Q;
}

double logProbL (int l, int z, double alphaI, double betaJ)
{
	double p;

	if (z == l) {
		p = log(prob(alphaI, betaJ));
	} else {
		p = log(1-prob(alphaI, betaJ));
	}
/*
  printf("p: %f\n", prob(alphaI, betaJ) );
  printf("1-p: %f\n", 1-prob(alphaI, betaJ) );
  printf("log: %f\n", p);
*/
	return p;
}

double prob(double alpha, double beta)
{
  return .5 + .5 * pow(1-beta, alpha);
}

void EStep (Dataset *data)
{
	int j;
	int idx;
/*
  printf("begin Estep\n");
  fflush(0);
*/
	for (j = 0; j < data->numImages; j++) {
		data->probZ1[j] = log(data->priorZ1[j]);
		data->probZ0[j] = log(1 - data->priorZ1[j]);
		//printf("%d %f %f\n", j, data->probZ1[j], data->probZ0[j]);
	}

	for (idx = 0; idx < data->numLabels; idx++) {
		int i = data->labels[idx].labelerId;
		int j = data->labels[idx].imageIdx;
		int k = data->labels[idx].workflowId;
		int alphaidx = k * data->numLabelers + i;
		int betaidx = k * data->numImages + j;
		int lij = data->labels[idx].label;
		//printf("JOE%d %d %d %f %f\n", k, alphaidx, betaidx, data->alpha[alphaidx], data->beta[betaidx]);
		data->probZ1[j] += logProbL(lij, 1, data->alpha[alphaidx],
					    data->beta[betaidx]);
		data->probZ0[j] += logProbL(lij, 0, data->alpha[alphaidx],
					    data->beta[betaidx]);
		//printf("%d %d %f %f\n", j, lij, data->probZ1[j], data->probZ0[j]);
	}

	/* Exponentiate and renormalize */
	for (j = 0; j < data->numImages; j++) {
	  //printf("Before: %f %f\n", data->probZ1[j], data->probZ0[j]);
	  /*Chris adding code here to do this better */
	  double normalizingDenominator = data->probZ1[j] +
	    log(1 + exp(data->probZ0[j] -
			data->probZ1[j]));
	  data->probZ1[j] = data->probZ1[j] - normalizingDenominator;
	  data->probZ0[j] = data->probZ0[j] - normalizingDenominator;
	  //printf("Hmm: %f %f %f\n", normalizingDenominator, data->probZ1[j], data->probZ0[j]);
	  data->probZ1[j] = exp(data->probZ1[j]);
	  data->probZ0[j] = exp(data->probZ0[j]);
	  /*
	    data->probZ1[j] = exp(data->probZ1[j]);
	    data->probZ0[j] = exp(data->probZ0[j]);
	    printf("%f %f\n", data->probZ1[j], data->probZ0[j]);
	    data->probZ1[j] = data->probZ1[j] / (data->probZ1[j] + data->probZ0[j]);
	    data->probZ0[j] = 1 - data->probZ1[j];
	  */
	  //printf("%d %f %f\n", j, data->probZ1[j], data->probZ0[j]);
	  if (isnan(data->probZ1[j])) {
	    printf("ABORTING?");
	    abort();
	  }
	}
/*
  printf("end Estep\n");
  fflush(0);
*/
}

void MStep (Dataset *data)
{
	double lastF;
	int iter, status;
	int i, j;

	int numLabelers = data->numLabelers;
	int numImages = data->numImages;
	int numWorkflows = data->numWorkflows;

	gsl_vector *x;
	gsl_multimin_fdfminimizer *s;
	const gsl_multimin_fdfminimizer_type *T;
	gsl_multimin_function_fdf my_func;

	x = gsl_vector_alloc((numLabelers * numWorkflows) + (numImages * numWorkflows));

	/* Pack alpha and beta into a gsl_vector */
	packX(x, data);

	/* Initialize objective function */
	my_func.n = (numLabelers * numWorkflows) + (numImages * numWorkflows);
	my_func.f = &my_f;
	my_func.df = &my_df;
	my_func.fdf = &my_fdf;
	my_func.params = data;

	/* Initialize minimizer */
	T = gsl_multimin_fdfminimizer_conjugate_pr;
	s = gsl_multimin_fdfminimizer_alloc(T, (numLabelers * numWorkflows) + (numImages * numWorkflows));

	gsl_multimin_fdfminimizer_set(s, &my_func, x, 0.01, 0.01);
	iter = 0;
	do {
		lastF = s->f;
		iter++;
		/*printf("iter=%d f=%f\n", iter, s->f);*/

		status = gsl_multimin_fdfminimizer_iterate(s);
		if (status) {
			break;
		}
		status = gsl_multimin_test_gradient(s->gradient, 1e-3);
//	} while (lastF - s->f > 0.01 && status == GSL_CONTINUE && iter < 25);
	} while (iter < 5);

	/* Unpack alpha and beta from gsl_vector */
	unpackX(s->x, data);

	gsl_multimin_fdfminimizer_free(s);
	gsl_vector_free(x);
}


/*
 * BIG_DATA.c
 *
 *  Created on: 3 Sep 2020
 *      Author: heine
 */

#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>

#include "kalman_filter.h"
#include "particle_filters.h"

#define N_SETTINGS 10
#define N_SCALES 11

void init_with_zeros(double* array, long length);
void scale_sample_sizes(double scaler, long sample_size_collection[N_SETTINGS][2], int n_settings);
void particle_allocation_time_matching(int bpf_sample_size, double std_sig, double obs_std, int Nobs, int data_length, double x0, double *y, double *Rinv1, double *Rinv0, gsl_matrix *R);

int main(void) {

  gsl_rng * rng = gsl_rng_alloc(gsl_rng_taus);
  gsl_rng_set(rng,clock());

  double scaler;
  int N_top = 50;
  int N_bpf = 250;
  int run_time_matching = 0;
  int data_length = 50;
  int Nobs = 500;
  double sig_std = .1;
  double obs_std = 1;
  double x0 = 0;
  double p0 = sig_std * sig_std;
  double sample_size_coeff = (double)68000 / (double) 250;

  /* -----------------------------------------------------------------------------------
   *
   * Create data
   *
   -----------------------------------------------------------------------------------*/
  double *x = (double*) malloc(data_length * sizeof(double));
  double *y = (double*) malloc(data_length * Nobs * sizeof(double));
  double *noise = (double*) malloc(Nobs * sizeof(double));
  gsl_matrix * A;

  double x_prev = 0;

  /* Create the measurement covariance square root */
  A = gsl_matrix_alloc(Nobs, Nobs);
  for (int i = 0; i < Nobs; i++) {
    for (int j = 0; j < Nobs; j++) {
      /* Uncomment for identity matrix */
      /*
       *       if (i == j) {
       *         gsl_matrix_set(A, i, j, 1);
       *       } else {
       *         gsl_matrix_set(A, i, j, 0);
       } */
//      gsl_matrix_set(A, i, j, gsl_ran_gaussian(rng, 1));
      gsl_matrix_set(A, i, j, gsl_rng_uniform(rng));
    }
  }

  /* Calculate the observation covariance */
  gsl_matrix * R = gsl_matrix_alloc(Nobs, Nobs);
  double tmp;
  double decay = 2; // Decay of correlations
  FILE * R_out = fopen("R_matrix.txt", "w");
  for (int i = 0; i < Nobs; i++) {
    for (int j = 0; j < Nobs; j++) {
      tmp = 0;
      for (int k = 0; k < Nobs; k++) {
        tmp += gsl_matrix_get(A, i, k) * gsl_matrix_get(A, j, k);
      }
      tmp = obs_std * obs_std * exp(-decay * (double) abs(i - j)) * tmp;
      gsl_matrix_set(R, i, j, tmp);
      fprintf(R_out, "%e ", tmp);
    }
    fprintf(R_out, "\n");
  }
  fclose(R_out);

  // Copy R to A
  for (int i = 0; i < Nobs; i++) {
    for (int j = 0; j < Nobs; j++) {
      gsl_matrix_set(A, i, j, gsl_matrix_get(R, i, j));
    }
  }
  gsl_linalg_cholesky_decomp1(A);

  printf("Obsevation covariance written in R_matrix.txt\n");

  /* Main loop for generating the signal and the data */
  FILE *data_file = fopen("data.txt", "w");
  for (int n = 0; n < data_length; n++) {

    x[n] = x_prev + gsl_ran_gaussian(rng, sig_std);
    x_prev = x[n];
    fprintf(data_file, "%i %e", n, x[n]);

    /* Uncorrelated observation noise */
    for (int i = 0; i < Nobs; i++) {
      noise[i] = gsl_ran_gaussian(rng, obs_std);
    }
    /* Make the noise correlated by multiplying by A */
    for (int i = 0; i < Nobs; i++) {
      y[n * Nobs + i] = x[n];
      for (int j = 0; j <= i; j++)
        y[n * Nobs + i] += gsl_matrix_get(A, i, j) * noise[j];
      fprintf(data_file, " %e,", y[n * Nobs + i]);
    }
    fprintf(data_file, "\n");

  }
  fclose(data_file);
  printf("Signal and data of length %i generated and written in data.txt\n", data_length);

  /*
   * Calculate the inverse observation covariances
   */
  printf("Computing the observation covariance inverse...\n");
  fflush(stdout);
  clock_t start = clock();

  /* Observation noise covariance */
  gsl_vector * eval = gsl_vector_alloc(Nobs);
  gsl_matrix * evec = gsl_matrix_alloc(Nobs, Nobs);
  double * Rinv1 = (double*) malloc(Nobs * Nobs * sizeof(double));
  gsl_eigen_symmv_workspace * workspace = gsl_eigen_symmv_alloc(Nobs);

  /* Compute the eigen decomposition of R to get the inverse */
  gsl_eigen_symmv(R, eval, evec, workspace);
  FILE* rinv_out = fopen("R_inv.txt", "w");
  for (int i = 0; i < Nobs; i++) {
    for (int k = 0; k < Nobs; k++) {
      tmp = 0;
      for (int j = 0; j < Nobs; j++) {
        tmp += gsl_matrix_get(evec, i, j) * gsl_matrix_get(evec, k, j)
        / gsl_vector_get(eval, j);

      }
      Rinv1[i * Nobs + k] = tmp;
      fprintf(rinv_out, "%e, ", tmp);
    }
    fprintf(rinv_out, "\n");
  }
  fclose(rinv_out);
  printf("Observation covariance inverse computed and written in R_inv.txt\n");

  gsl_matrix_free(evec);
  gsl_vector_free(eval);
  gsl_eigen_symmv_free(workspace);

  /* Limit the observation correlation to a band and ... */
  int band = 0; // Note we only consider diagonal, i.e. band = 0.
  gsl_matrix * R_low = gsl_matrix_alloc(Nobs, Nobs);
  for (int i = 0; i < Nobs; i++) {
    for (int j = 0; j < Nobs; j++) {
      tmp = abs(i - j) <= band ? gsl_matrix_get(R, i, j) : 0;
      gsl_matrix_set(R_low, i, j, tmp);
    }
  }
  /* ...take inverse */
  gsl_vector * eval_low = gsl_vector_alloc(Nobs);
  gsl_matrix * evec_low = gsl_matrix_alloc(Nobs, Nobs);
  gsl_eigen_symmv_workspace * workspace_low = gsl_eigen_symmv_alloc(Nobs);
  double * Rinv0 = (double*) malloc(Nobs * Nobs * sizeof(double));

  gsl_eigen_symmv(R_low, eval_low, evec_low, workspace_low);

  FILE * band_R_out = fopen("R_band_inv.txt", "w");
  for (int i = 0; i < Nobs; i++) {
    for (int k = 0; k < Nobs; k++) {
      tmp = 0;
      for (int j = 0; j < Nobs; j++) {
        tmp += gsl_matrix_get(evec_low, i, j) * gsl_matrix_get(evec_low, k, j)
        / gsl_vector_get(eval_low, j);

      }
      Rinv0[i * Nobs + k] = tmp;
      fprintf(band_R_out, "%e ", tmp);
    }
    fprintf(band_R_out, "\n");
  }
  fclose(band_R_out);
  printf(
         "Inverse of the diagonal covariance approximation computed and written in R_band_inv.txt\n");

  gsl_matrix_free(evec_low);
  gsl_vector_free(eval_low);
  gsl_eigen_symmv_free(workspace_low);

  printf("elapsed %5.2f sec\n", (double) (clock() - start) / CLOCKS_PER_SEC);

  if (run_time_matching == 1) {
    /*
     * Particle allocation time matching
     *
     * This part of the code should be run when trying to find the correct N0 sample sizes to match
     * the BPF running time
     */

    particle_allocation_time_matching(N_bpf, sig_std, obs_std, Nobs, data_length, x0, y, Rinv1, Rinv0, R);

    printf("Particle allocation time matching done! Exiting...\n");
    return 0; // the code exits
  }

  /* ------------------------------------------------------------
   * Kalman filter
   * --------------------------------------------------------- */
  double *x_hat = (double*) malloc(data_length * sizeof(double));
  double *p_hat = (double*) malloc(data_length * sizeof(double));

  kalman_filter(x, y, Rinv1, sig_std, obs_std, x_hat, p_hat, data_length, x0, p0, Nobs);

  /* Write KF results in a file */
  FILE *kf_out = fopen("kf_out.txt", "w");
  for (int i = 0; i < data_length; i++)
    fprintf(kf_out, "%i %f %f %f\n", i, x[i], x_hat[i], p_hat[i]);
  fclose(kf_out);

  /*
   * Top level loop
   * */
  double scalers[N_SCALES] = {0.5,1,2,3,4,5,6,7,8,9,10};
  short N_levels = 2;
  long sample_sizes[2] = {0,0};
  long sample_size[1] = { N_bpf }; // This is for one level BPF

  double *x_hat_bpf = (double*) malloc(data_length * sizeof(double));
  double *errors = (double *) malloc((N_SETTINGS + 1) * sizeof(double));
  double *times = (double *) malloc((N_SETTINGS + 1) * sizeof(double));
  init_with_zeros(errors, N_SETTINGS + 1);
  init_with_zeros(times, N_SETTINGS + 1);

  double error;
  FILE * error_out = fopen("error.txt", "w");

  double filtering_time[4];
  double worst_case_sign_ratio[1] = { 100 }; // This is basically sustitute for "Inf"
  long unilevel_sample_size = N_bpf;
  long error_matched_unilevel_sample_size = 7 * N_bpf;
  int N1, N0;
  double step;

  /*
   * MAIN LOOP FOR TOP LEVEL MONTE CARLO
   */
  for (int i = 0; i < N_top; i++) {

    printf("Top level iteration %i\n", i);

    for (int k = 0; k < N_SCALES; k++) {

      scaler = scalers[k];

      printf("Scaler %f\n", scaler);

      /* Run the basic BPF (time matched) */
      sample_size[0] = (long) round(unilevel_sample_size * scaler);
      bootstrapfilter(y, sig_std, obs_std, data_length, x0, Nobs, sample_size[0],
                      x_hat_bpf, filtering_time, Rinv1, 1);

      /* Sum of squared errors over the whole signal */
      error = 0;
      for (int n = 0; n < data_length; n++) {
        error += (x_hat[n] - x_hat_bpf[n]) * (x_hat[n] - x_hat_bpf[n]);
      }

      fprintf(error_out, "%e %i %lu %f %i %e %e %f %e %f\n",
              error / (double) data_length, 0, sample_size[0], filtering_time[0], 0,
              filtering_time[1], filtering_time[2], worst_case_sign_ratio[0],
              filtering_time[3], scaler);
      fflush(error_out);

      if(scaler < 8) {
        /* Run the basic BPF (error matched) */
        sample_size[0] = (long) round(error_matched_unilevel_sample_size * scaler);
        bootstrapfilter(y, sig_std, obs_std, data_length, x0, Nobs, sample_size[0],
                        x_hat_bpf, filtering_time, Rinv1, 1);

        /* Sum of squared errors over the whole signal */
        error = 0;
        for (int n = 0; n < data_length; n++) {
          error += (x_hat[n] - x_hat_bpf[n]) * (x_hat[n] - x_hat_bpf[n]);
        }

        fprintf(error_out, "%e %i %lu %f %i %e %e %f %e %f\n",
                error / (double) data_length, 0, sample_size[0], filtering_time[0], -1,
                filtering_time[1], filtering_time[2], worst_case_sign_ratio[0],
                filtering_time[3], scaler);
        fflush(error_out);
      }

      /* Iterate over different configurations */
      N1 = 0;
      for (int j = 0; j < N_SETTINGS; j++) {

        step = (double)(N_bpf - 5) / (double)(N_SETTINGS-1);
        N1 = (int) j * step;
        N0 = (int) floor((N_bpf-N1)*sample_size_coeff);
        worst_case_sign_ratio[0] = 100;

        sample_sizes[0] = N0 * scaler;
        sample_sizes[1] = N1 * scaler;

        printf("N0 = %lu, N1 = %lu, N = %lu\n", sample_sizes[0], sample_sizes[1], sample_sizes[0]+sample_sizes[1]);

        MLbootstrapfilter(y, sig_std, obs_std, data_length, x0, Nobs, N_levels,
                          sample_sizes, x_hat_bpf, filtering_time, Rinv0, Rinv1,
                          worst_case_sign_ratio);

        /* Sum of squared errors over the whole signal */
        error = 0;
        for (int n = 0; n < data_length; n++) {
          error += (x_hat[n] - x_hat_bpf[n]) * (x_hat[n] - x_hat_bpf[n]);
        }

        fprintf(error_out, "%e %lu %lu %f %i %e %e %f %e %f\n",
                error / (double) data_length, sample_sizes[0],
                sample_sizes[1], filtering_time[0], j+1, filtering_time[1],
                filtering_time[2], worst_case_sign_ratio[0], filtering_time[3], scaler);

      }
      fflush(error_out);
    }

  }

  fclose(error_out);

  printf("Results of all BPF runs written in error.txt\n");
  printf("Error summary written in error_summary.txt\n");

  return 0;
}

void init_with_zeros(double *array, long length) {
  for (int i = 0; i < length; i++)
    array[i] = 0;
}

void scale_sample_sizes(double scaler, long sample_size_collection[N_SETTINGS][2], int n_settings) {

  for (int i = 0; i < n_settings; i++)
    for (int j = 0; j < 1; j++)
      sample_size_collection[i][j] = (long) round(scaler * sample_size_collection[i][j]);

}

void particle_allocation_time_matching(int bpf_sample_size, double std_sig, double obs_std, int Nobs, int data_length, double x0, double *y, double *Rinv1, double *Rinv0, gsl_matrix *R) {

  int N1_step = 20;
  int N0_step = 10000;
  int N0_init = 10;
  int N_top = 5;
  double time_per_iteration;
  double time_tolerance = 1.00; //
  double filtering_time[1];
  double worst_case_sign_ratio[1] = { 100 };

  double *x_hat_bpf = (double*) malloc(data_length * sizeof(double));

  /* Find the reference time per iteration for BPF */
  double bpf_time_per_iteration = 0;

  for(int nmc = 0; nmc < N_top; nmc++) {
    bootstrapfilter(y, std_sig, obs_std, data_length, x0, Nobs, bpf_sample_size,
                    x_hat_bpf, filtering_time, Rinv1, 1);
    bpf_time_per_iteration += filtering_time[0] / (double) N_top;
  }

  printf("Time per iteration (BPF): %0.5f\n",bpf_time_per_iteration);
  fflush(stdout);

  long sample_sizes[2] = {0,0};
  int N0;
  short direction;

  FILE* out = fopen("time_matched_sample_sizes.txt","w");

  /* For given L0 mesh, iterate over N1 */
  for(int N1 = 0; N1 <= bpf_sample_size ; N1 = N1 + N1_step) {

    N0 = N0_init;
    N0_step = 10000;

    /* For given L0 mesh and N1 iterate over increasing N0 until MLBPF time per iteration exceeds
     BPF time per iteration */
    time_per_iteration = 0;

    direction = 1; // up
    while(1) {

      sample_sizes[0] = N0;
      sample_sizes[1] = N1;

      /* Estimate the time per iteration by averaging N_top runs */
      time_per_iteration = 0;
      for(int nmc = 0; nmc < N_top; nmc++) {

        MLbootstrapfilter(y, std_sig, obs_std, data_length, x0, Nobs, 2,
                          sample_sizes, x_hat_bpf, filtering_time, Rinv0, Rinv1,
                          worst_case_sign_ratio);

        time_per_iteration += filtering_time[0] / (double) N_top;
      }

      if (direction == 1 && time_per_iteration > bpf_time_per_iteration * time_tolerance && N0_step > 10) {
        direction = -1; // start going down
        N0_step /= 2; // halve the step

      } else if (direction == -1 && time_per_iteration < bpf_time_per_iteration * 1.00 && N0_step > 10) {
        direction = 1;
        N0_step /= 2;
      } else if (N0_step < 10) {
        break;
      }
      N0 = N0 + direction * N0_step;

      if(N0 < 1) { // Make sure N0 remains positive and go up if this happens
        N0 = 1;
        direction = 1;
      }

    }
    fprintf(out,"%i %i %i %i\n",N0-N0_step,N1,0,0);
    fflush(out);
  }

  fclose(out);
  free(x_hat_bpf);
}


/* integration/qaws.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 <math.h>
#include <float.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>

#include "initialise.c"
#include "append.c"
#include "qpsrt.c"
#include "util.c"
#include "qc25s.c"

int
gsl_integration_qaws (gsl_function * f,
                      const double a, const double b,
                      gsl_integration_qaws_table * t,
                      const double epsabs, const double epsrel,
                      const size_t limit,
                      gsl_integration_workspace * workspace,
                      double *result, double *abserr)
{
  double area, errsum;
  double result0, abserr0;
  double tolerance;
  size_t iteration = 0;
  int roundoff_type1 = 0, roundoff_type2 = 0, error_type = 0;

  /* Initialize results */

  initialise (workspace, a, b);

  *result = 0;
  *abserr = 0;

  if (limit > workspace->limit)
    {
      GSL_ERROR ("iteration limit exceeds available workspace", GSL_EINVAL) ;
    }

  if (b <= a)
    {
      GSL_ERROR ("limits must form an ascending sequence, a < b", GSL_EINVAL) ;
    }

  if (epsabs <= 0 && (epsrel < 50 * GSL_DBL_EPSILON || epsrel < 0.5e-28))
    {
      GSL_ERROR ("tolerance cannot be acheived with given epsabs and epsrel",
                 GSL_EBADTOL);
    }

  /* perform the first integration */

  {
    double area1, area2;
    double error1, error2;
    int err_reliable1, err_reliable2;
    double a1 = a;
    double b1 = 0.5 * (a + b);
    double a2 = b1;
    double b2 = b;

    qc25s (f, a, b, a1, b1, t, &area1, &error1, &err_reliable1);
    qc25s (f, a, b, a2, b2, t, &area2, &error2, &err_reliable2);

    if (error1 > error2)
      {
        append_interval (workspace, a1, b1, area1, error1);
        append_interval (workspace, a2, b2, area2, error2);
      }
    else
      {
        append_interval (workspace, a2, b2, area2, error2);
        append_interval (workspace, a1, b1, area1, error1);
      }

    result0 = area1 + area2;
    abserr0 = error1 + error2;
  }

  /* Test on accuracy */

  tolerance = GSL_MAX_DBL (epsabs, epsrel * fabs (result0));

  /* Test on accuracy, use 0.01 relative error as an extra safety
     margin on the first iteration (ignored for subsequent iterations) */

  if (abserr0 < tolerance && abserr0 < 0.01 * fabs(result0))
    {
      *result = result0;
      *abserr = abserr0;

      return GSL_SUCCESS;
    }
  else if (limit == 1)
    {
      *result = result0;
      *abserr = abserr0;

      GSL_ERROR ("a maximum of one iteration was insufficient", GSL_EMAXITER);
    }

  area = result0;
  errsum = abserr0;

  iteration = 2;

  do
    {
      double a1, b1, a2, b2;
      double a_i, b_i, r_i, e_i;
      double area1 = 0, area2 = 0, area12 = 0;
      double error1 = 0, error2 = 0, error12 = 0;
      int err_reliable1, err_reliable2;

      /* Bisect the subinterval with the largest error estimate */

      retrieve (workspace, &a_i, &b_i, &r_i, &e_i);

      a1 = a_i;
      b1 = 0.5 * (a_i + b_i);
      a2 = b1;
      b2 = b_i;

      qc25s (f, a, b, a1, b1, t, &area1, &error1, &err_reliable1);
      qc25s (f, a, b, a2, b2, t, &area2, &error2, &err_reliable2);

      area12 = area1 + area2;
      error12 = error1 + error2;

      errsum += (error12 - e_i);
      area += area12 - r_i;

      if (err_reliable1 && err_reliable2)
        {
          double delta = r_i - area12;

          if (fabs (delta) <= 1.0e-5 * fabs (area12) && error12 >= 0.99 * e_i)
            {
              roundoff_type1++;
            }
          if (iteration >= 10 && error12 > e_i)
            {
              roundoff_type2++;
            }
        }

      tolerance = GSL_MAX_DBL (epsabs, epsrel * fabs (area));

      if (errsum > tolerance)
        {
          if (roundoff_type1 >= 6 || roundoff_type2 >= 20)
            {
              error_type = 2;   /* round off error */
            }

          /* set error flag in the case of bad integrand behaviour at
             a point of the integration range */

          if (subinterval_too_small (a1, a2, b2))
            {
              error_type = 3;
            }
        }

      update (workspace, a1, b1, area1, error1, a2, b2, area2, error2);

      retrieve (workspace, &a_i, &b_i, &r_i, &e_i);

      iteration++;

    }
  while (iteration < limit && !error_type && errsum > tolerance);

  *result = sum_results (workspace);
  *abserr = errsum;

  if (errsum <= tolerance)
    {
      return GSL_SUCCESS;
    }
  else if (error_type == 2)
    {
      GSL_ERROR ("roundoff error prevents tolerance from being achieved",
                 GSL_EROUND);
    }
  else if (error_type == 3)
    {
      GSL_ERROR ("bad integrand behavior found in the integration interval",
                 GSL_ESING);
    }
  else if (iteration == limit)
    {
      GSL_ERROR ("maximum number of subdivisions reached", GSL_EMAXITER);
    }
  else
    {
      GSL_ERROR ("could not integrate function", GSL_EFAILED);
    }

}


/*************************************************************************************

	Copyright 2010 Philip Waldron

    This file is part of BayRate.

    BayRate 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.

    BayRate 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 Foobar.  If not, see <http://www.gnu.org/licenses/>.

***************************************************************************************/

#pragma once
#include <string>
#include <vector>
#include <map>
#include <gsl/gsl_math.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_chebyshev.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <boost/date_time/gregorian/gregorian.hpp>
#include "player.h"
#include "game.h"
#include "tdListEntry.h"

using namespace std;

class collection
{
public:
	collection(void);
	~collection(void);
	string tournamentCode;
	map<int, player> playerHash;
	vector<game> gameList;
	std::string tournamentName;
	boost::gregorian::date tournamentDate;

	double calc_pt(const gsl_vector *v);
	double calc_pt_df(const gsl_vector *x, gsl_vector *df);
	void calc_sigma();
	void calc_sigma2();
	int calc_ratings();
	int calc_ratings_fdf();

	void reset();
	void initSeeding(map<int, tdListEntry> &tdList);
	void findImprobables(map<int, tdListEntry> &tdList);
	void setQuiet(bool q);
	int  getFdfIterations();
	int  getSimplexIterations();

private:
	double PI;
	const gsl_rng_type *T;
	gsl_rng *r;
	bool quiet;
	int  fdfiterations;
	int  simplexiterations;
};


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

#ifndef __GSL_EIGEN_H__
#define __GSL_EIGEN_H__

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>

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

__BEGIN_DECLS

typedef struct {
  size_t size;
  double * d;
  double * sd;
} gsl_eigen_symm_workspace;

gsl_eigen_symm_workspace * gsl_eigen_symm_alloc (const size_t n);
void gsl_eigen_symm_free (gsl_eigen_symm_workspace * w);
int gsl_eigen_symm (gsl_matrix * A, gsl_vector * eval, gsl_eigen_symm_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * gc;
  double * gs;
} gsl_eigen_symmv_workspace;

gsl_eigen_symmv_workspace * gsl_eigen_symmv_alloc (const size_t n);
void gsl_eigen_symmv_free (gsl_eigen_symmv_workspace * w);
int gsl_eigen_symmv (gsl_matrix * A, gsl_vector * eval, gsl_matrix * evec, gsl_eigen_symmv_workspace * w);

typedef struct {
  size_t size;           /* matrix size */
  size_t max_iterations; /* max iterations since last eigenvalue found */
  size_t n_iter;         /* number of iterations since last eigenvalue found */
  size_t n_evals;        /* number of eigenvalues found so far */

  gsl_vector *hv2;       /* temporary 2-by-1 householder vector */
  gsl_vector *hv3;       /* temporary 3-by-1 householder vector */

  int compute_t;         /* compute Schur form T = Z^t A Z */

  gsl_matrix *H;         /* pointer to Hessenberg matrix */

  gsl_matrix *Z;         /* pointer to Schur vector matrix */
} gsl_eigen_francis_workspace;

gsl_eigen_francis_workspace * gsl_eigen_francis_alloc (void);
void gsl_eigen_francis_free (gsl_eigen_francis_workspace * w);
void gsl_eigen_francis_T (const int compute_t,
                          gsl_eigen_francis_workspace * w);
int gsl_eigen_francis (gsl_matrix * H, gsl_vector_complex * eval,
                       gsl_eigen_francis_workspace * w);
int gsl_eigen_francis_Z (gsl_matrix * H, gsl_vector_complex * eval,
                         gsl_matrix * Z,
                         gsl_eigen_francis_workspace * w);

typedef struct {
  size_t size;                 /* size of matrices */
  gsl_vector *diag;            /* diagonal matrix elements from balancing */
  gsl_vector *tau;             /* Householder coefficients */
  gsl_matrix *Z;               /* pointer to Z matrix */
  int do_balance;              /* perform balancing transformation? */
  size_t n_evals;              /* number of eigenvalues found */

  gsl_eigen_francis_workspace *francis_workspace_p;
} gsl_eigen_nonsymm_workspace;

gsl_eigen_nonsymm_workspace * gsl_eigen_nonsymm_alloc (const size_t n);
void gsl_eigen_nonsymm_free (gsl_eigen_nonsymm_workspace * w);
void gsl_eigen_nonsymm_params (const int compute_t, const int balance,
                               gsl_eigen_nonsymm_workspace *w);
int gsl_eigen_nonsymm (gsl_matrix * A, gsl_vector_complex * eval,
                       gsl_eigen_nonsymm_workspace * w);
int gsl_eigen_nonsymm_Z (gsl_matrix * A, gsl_vector_complex * eval,
                         gsl_matrix * Z, gsl_eigen_nonsymm_workspace * w);

typedef struct {
  size_t size;                 /* size of matrices */
  gsl_vector *work;            /* scratch workspace */
  gsl_vector *work2;           /* scratch workspace */
  gsl_vector *work3;           /* scratch workspace */

  gsl_matrix *Z;               /* pointer to Schur vectors */

  gsl_eigen_nonsymm_workspace *nonsymm_workspace_p;
} gsl_eigen_nonsymmv_workspace;

gsl_eigen_nonsymmv_workspace * gsl_eigen_nonsymmv_alloc (const size_t n);
void gsl_eigen_nonsymmv_free (gsl_eigen_nonsymmv_workspace * w);
int gsl_eigen_nonsymmv (gsl_matrix * A, gsl_vector_complex * eval,
                        gsl_matrix_complex * evec,
                        gsl_eigen_nonsymmv_workspace * w);
int gsl_eigen_nonsymmv_Z (gsl_matrix * A, gsl_vector_complex * eval,
                          gsl_matrix_complex * evec, gsl_matrix * Z,
                          gsl_eigen_nonsymmv_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * tau;
} gsl_eigen_herm_workspace;

gsl_eigen_herm_workspace * gsl_eigen_herm_alloc (const size_t n);
void gsl_eigen_herm_free (gsl_eigen_herm_workspace * w);
int gsl_eigen_herm (gsl_matrix_complex * A, gsl_vector * eval,
                         gsl_eigen_herm_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * tau;
  double * gc;
  double * gs;
} gsl_eigen_hermv_workspace;

gsl_eigen_hermv_workspace * gsl_eigen_hermv_alloc (const size_t n);
void gsl_eigen_hermv_free (gsl_eigen_hermv_workspace * w);
int gsl_eigen_hermv (gsl_matrix_complex * A, gsl_vector * eval,
                           gsl_matrix_complex * evec,
                           gsl_eigen_hermv_workspace * w);

typedef enum {
  GSL_EIGEN_SORT_VAL_ASC,
  GSL_EIGEN_SORT_VAL_DESC,
  GSL_EIGEN_SORT_ABS_ASC,
  GSL_EIGEN_SORT_ABS_DESC
}
gsl_eigen_sort_t;

/* Sort eigensystem results based on eigenvalues.
 * Sorts in order of increasing value or increasing
 * absolute value.
 *
 * exceptions: GSL_EBADLEN
 */

int gsl_eigen_symmv_sort(gsl_vector * eval, gsl_matrix * evec,
                         gsl_eigen_sort_t sort_type);

int gsl_eigen_hermv_sort(gsl_vector * eval, gsl_matrix_complex * evec,
                         gsl_eigen_sort_t sort_type);

int gsl_eigen_nonsymmv_sort(gsl_vector_complex * eval,
                            gsl_matrix_complex * evec,
                            gsl_eigen_sort_t sort_type);

/* The following functions are obsolete: */

/* Eigensolve by Jacobi Method
 *
 * The data in the matrix input is destroyed.
 *
 * exceptions:
 */
int
gsl_eigen_jacobi(gsl_matrix * matrix,
                      gsl_vector * eval,
                      gsl_matrix * evec,
                      unsigned int max_rot,
                      unsigned int * nrot);

/* Invert by Jacobi Method
 *
 * exceptions:
 */
int
gsl_eigen_invert_jacobi(const gsl_matrix * matrix,
                             gsl_matrix * ainv,
                             unsigned int max_rot);

__END_DECLS

#endif /* __GSL_EIGEN_H__ */


#ifdef HAVE_GSL
#include <gsl/gsl_sf_fermi_dirac.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_sf_result.h>
#include <gsl/gsl_errno.h>
#else
#include <stddef.h>
#ifdef __cplusplus
extern "C"
{
#endif
   typedef  double gsl_multifit_linear_workspace ;
   typedef  struct { size_t size; double *v;}  gsl_vector ;
   typedef  struct { size_t size_rows; size_t size_columns; double* m;}  gsl_matrix ;
   double gsl_sf_fermi_dirac_mhalf(double x);
   double gsl_sf_fermi_dirac_half(double x);
   double gsl_sf_bessel_Kn(const int n, const double x);

   double gsl_vector_get (const gsl_vector * v, size_t i);
   gsl_matrix *gsl_matrix_alloc (size_t i, size_t j);
   gsl_vector *gsl_vector_alloc (size_t i);
   void gsl_matrix_set (gsl_matrix *m, size_t i, size_t j, double x);
   void gsl_vector_set (gsl_vector *v, size_t i, double x);
   void gsl_vector_free (gsl_vector * v);
   void gsl_matrix_free (gsl_matrix * v);
   gsl_multifit_linear_workspace *gsl_multifit_linear_alloc (size_t m,size_t n);
   void gsl_multifit_linear_free (gsl_multifit_linear_workspace *);
   int gsl_multifit_wlinear (const gsl_matrix * X, const gsl_vector * w, const gsl_vector * y, gsl_vector * c, gsl_matrix * cov, double * chisq, gsl_multifit_linear_workspace * work);
#ifdef __cplusplus
}
#endif
#endif


/* filter/gsl_filter.h
 *
 * Copyright (C) 2018 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.
 */

#ifndef __GSL_FILTER_H__
#define __GSL_FILTER_H__

#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_movstat.h>

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

__BEGIN_DECLS

/* end point handling methods */
typedef enum
{
  GSL_FILTER_END_PADZERO = GSL_MOVSTAT_END_PADZERO,
  GSL_FILTER_END_PADVALUE = GSL_MOVSTAT_END_PADVALUE,
  GSL_FILTER_END_TRUNCATE = GSL_MOVSTAT_END_TRUNCATE
} gsl_filter_end_t;

/* robust scale estimates */
typedef enum
{
  GSL_FILTER_SCALE_MAD, /* median absolute deviation */
  GSL_FILTER_SCALE_IQR, /* interquartile range */
  GSL_FILTER_SCALE_SN,  /* S_n scale statistic */
  GSL_FILTER_SCALE_QN   /* Q_n scale statistic */
} gsl_filter_scale_t;

/* workspace for Gaussian filter */
typedef struct
{
  size_t K;        /* window size */
  double *kernel;  /* Gaussian kernel, size K */
  gsl_movstat_workspace *movstat_workspace_p;
} gsl_filter_gaussian_workspace;

gsl_filter_gaussian_workspace *gsl_filter_gaussian_alloc(const size_t K);
void gsl_filter_gaussian_free(gsl_filter_gaussian_workspace * w);
int gsl_filter_gaussian(const gsl_filter_end_t endtype, const double alpha, const size_t order, const gsl_vector * x,
                        gsl_vector * y, gsl_filter_gaussian_workspace * w);
int gsl_filter_gaussian_kernel(const double alpha, const size_t order, const int normalize, gsl_vector * kernel);

/* workspace for standard median filter */
typedef struct
{
  gsl_movstat_workspace *movstat_workspace_p;
} gsl_filter_median_workspace;

gsl_filter_median_workspace *gsl_filter_median_alloc(const size_t K);
void gsl_filter_median_free(gsl_filter_median_workspace * w);
int gsl_filter_median(const gsl_filter_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_filter_median_workspace * w);

/* workspace for recursive median filter */
typedef struct
{
  size_t H;                            /* window half-length (K / 2) */
  size_t K;                            /* window size */
  void *state;                         /* workspace for min/max accumulator */
  double *window;                      /* array holding first window */
  const gsl_movstat_accum * minmaxacc; /* minimum/maximum accumulator */
  gsl_movstat_workspace *movstat_workspace_p;
} gsl_filter_rmedian_workspace;

gsl_filter_rmedian_workspace *gsl_filter_rmedian_alloc(const size_t K);
void gsl_filter_rmedian_free(gsl_filter_rmedian_workspace * w);
int gsl_filter_rmedian(const gsl_filter_end_t, const gsl_vector * x, gsl_vector * y, gsl_filter_rmedian_workspace * w);
int gsl_filter_rmedian2(const gsl_vector * x, gsl_vector * y, gsl_filter_rmedian_workspace * w);

typedef struct
{
  gsl_movstat_workspace *movstat_workspace_p;
} gsl_filter_impulse_workspace;

gsl_filter_impulse_workspace *gsl_filter_impulse_alloc(const size_t K);
void gsl_filter_impulse_free(gsl_filter_impulse_workspace * w);
int gsl_filter_impulse(const gsl_filter_end_t endtype, const gsl_filter_scale_t scale_type, const double t,
                       const gsl_vector * x, gsl_vector * y, gsl_vector * xmedian, gsl_vector * xsigma, size_t * noutlier,
                       gsl_vector_int * ioutlier, gsl_filter_impulse_workspace * w);

__END_DECLS

#endif /* __GSL_FILTER_H__ */