//===--- Sudoku/Solver.h                                                ---===//
//
//	Solver functions acting on a Board<Options>&
// Declarations are in Solver.fwd.h
//===----------------------------------------------------------------------===//
#pragma once

#include "Solvers_find.h"
#include "Solvers_remove_option.h"
#include "Solvers_set_option.h"

#include "Sudoku/Board.h"
#include "Sudoku/Location.h"
#include "Sudoku/Location_Utilities.h"
#include "Sudoku/Options.h"
#include "Sudoku/Size.h"
#include "Sudoku/Value.h"
#include <gsl/gsl>

#include <vector>

#include <algorithm>   // minmax
#include <type_traits> // is_base_of

#include "Solver.fwd.h" // Forward declarations

#include <cassert>

namespace Sudoku
{
//	Check if only one option remaining
//	IF true: process answer
template<int N, typename Options>
inline int single_option(
	Board<Options, N>& board, // NOLINT(runtime/references)
	const Location<N> loc)
{
	assert(is_valid(loc));

	if (const Value answer{get_answer(board.at(loc))})
	{
		return single_option(board, loc, answer);
	}
	return 0;
}

//	Process answer from [loc], update board_ options
//	Remove option from rest of row, col and block
template<int N, typename Options>
inline int single_option(
	Board<Options, N>& board, // NOLINT(runtime/references)
	const Location<N> loc,
	const Value value)
{
	{
		assert(is_valid(loc));
		assert(is_valid<N>(value));
		assert(board.at(loc).test(value));
	}
	int changes{};
	changes += set_Value(board, loc, value);
	changes += remove_option_section(board, board.row(loc), loc, value);
	changes += remove_option_section(board, board.col(loc), loc, value);
	changes += remove_option_section(board, board.block(loc), loc, value);
	return changes;
}

// if 2 options in element:
// find exact pair in section:
// remove form other elements in section
template<int N, typename Options>
inline int dual_option(
	Board<Options, N>& board, // NOLINT(runtime/references)
	const Location<N> loc)
{
	using Location = Location<N>;
	{
		assert(is_valid(loc));
		assert(board.at(loc).count() == 2);
	}
	const auto sorted_loc = [loc](const Location L)
	{
		const auto result = std::minmax(loc, L);
		return std::vector<Location>{result.first, result.second};
	};

	int changes{};
	const Options& item{board.at(loc)};
	const auto mask = [](Options x) noexcept
	{
		x[Value{0}] = false;
		return x;
	}(item);

	for (int i{}; i < full_size<N>; ++i)
	{
		// find exact same in board
		if (board[Location(i)] == item && Location(i) != loc)
		{
			// Remove values for rest of shared elements
			if (is_same_row(loc, Location(i)))
			{
				changes += remove_option_section(
					board, board.row(loc), sorted_loc(Location(i)), mask);
			}
			else if (is_same_col(loc, Location(i)))
			{
				changes += remove_option_section(
					board, board.col(loc), sorted_loc(Location(i)), mask);
			}
			// run not if already answered
			if (is_same_block(loc, Location(i)) && !item.is_answer())
			{ // NOTE this is slow
				changes += remove_option_section(
					board, board.block(loc), sorted_loc(Location(i)), mask);
			}
		}
	}
	return changes;
}

// Find subsets in any related sections,
// and remove the values from rest of these sections.
template<int N, typename Options>
inline int multi_option(
	Board<Options, N>& board, // NOLINT(runtime/references)
	const Location<N> loc)
{
	assert(is_valid(loc));

	return multi_option(board, loc, board.at(loc).count());
}

// Find subsets in any related section,
// and remove the values from rest of these sections.
template<int N, typename Options>
constexpr int multi_option(
	Board<Options, N>& board, // NOLINT(runtime/references)
	const Location<N> loc,
	const size_t count)
{
	assert(is_valid(loc));

	// set execution domain & specializations
	switch (count)
	{
	case 0: [[fallthrough]];     // already answered
	case elem_size<N>: return 0; // no result anyway
	case 1: return single_option(board, loc);
	case 2: return dual_option(board, loc);
	default: assert(count < elem_size<N>); break;
	}

	int changes{};                      // performance counter
	const Options& item{board.at(loc)}; // input item, to match with
	assert(item.count() == count);
	const auto mask = [](Options x) noexcept
	{
		x[Value{0}] = false;
		return x;
	}(item);

	const auto list = list_where_subset(board, item);

	// Further select per section.
	// When (subset size == #options) then: all answers in this selection.
	// Therefore: Remove values for rest of section.
	if (const auto in_row{get_same_row(loc, list)}; in_row.size() == count)
	{
		changes += remove_option_section(board, board.row(loc), in_row, mask);
	}
	if (const auto in_col{get_same_col(loc, list)}; in_col.size() == count)
	{
		changes += remove_option_section(board, board.col(loc), in_col, mask);
	}
	if (const auto in_block{get_same_block(loc, list)};
		in_block.size() == count)
	{
		// NOTE this is slow
		changes +=
			remove_option_section(board, board.block(loc), in_block, mask);
	}
	return changes;
}

// Solver: Find options appearing only once in a section and set as answer
template<int N, typename Options, typename SectionT>
inline int unique_in_section(
	Board<Options, N>& board, // NOLINT(runtime/references)
	const SectionT section)
{
	{
		static_assert(Board_Section::traits::is_Section_v<SectionT>);
	}
	const auto worker = appearance_once<N>(section);
	return set_uniques(board, section, worker);
}

//	Solver: find and process values appearing 1 to base_size times in section:
//	[row/col] IF all in same block -> remove from rest of block
//	[block] IF all in same row/col -> remove from rest of row/col
template<int N, typename Options, typename SectionT>
inline int section_exclusive(
	Board<Options, N>& board, // NOLINT(runtime/references)
	const SectionT section)
{
	{
		static_assert(Board_Section::traits::is_Section_v<SectionT>);
		static_assert(std::is_same_v<typename SectionT::value_type, Options>);
	}
	int changes{}; // performance counter

	size_t i{2};
	auto appearing             = appearance_sets<N>(section);
	const auto renew_appearing = [&i, &a = appearing, &section]()
	{
		i = 2;
		a = appearance_sets<N>(section);
	};
	while (i < appearing.size()) // won't run if condition fails
	{
		// unique specialization
		if (appearing[1].count_all() > 0)
		{
			changes += set_uniques(board, section, appearing[1]);
			renew_appearing();
			continue;
		}
		if (appearing.at(i).count_all() > 0)
		{ // for [row/col]: if in same block: remove from rest block
			// for [block]: if in same row/col: remove from rest row/col
			if (const int tmp_ = set_section_locals(
					board, section, gsl::narrow_cast<int>(i), appearing.at(i)))
			{
				changes += tmp_;
				renew_appearing();
				continue;
			}
		}
		++i;
	}
	return changes;
}

// cell containing 2 options
//	find in board_	size() == 2
//		find exact same in rest board
//			IF pair in same row/col -> remove values from rest row/col
//			IF same block -> remove values from rest block
//		repeat until end of board_
//	repeat until end of board_

// cell containing 2 options
//	find in board_	size() == 2
//		find exact same in rest row
//			IF found:	remove values from rest row
//		find exact same in rest col
//		find exact same in rest block

} // namespace Sudoku


/** \file
 * Analyze CSA output traces responding to pulses for ENC evaluation.
 */
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include <string.h>
#include <unistd.h>

#include <gsl/gsl_histogram2d.h>

#include "common.h"
#include "filters.h"
#include "hdf5rawWaveformIo.h"
typedef SCOPE_DATA_TYPE IN_WFM_BASE_TYPE;
typedef SCOPE_DATA_TYPE OUT_WFM_BASE_TYPE;

/** Parameters settable from commandline */
typedef struct param
{
    size_t diffSep;  //!< rise-time in samples for pulse finding
    double riseFrac; //!< fraction of max pulse height for pulse edge location
    double fltM;     //!< exp decay constant for Trapezoidal filter
    size_t sPre;     //!< samples before rising edge
    size_t sLen;     //!< total number of samples of a pulse
    size_t sHscale;  //!< histogram sample scaling factor
} param_t;

param_t param_default = {
    .diffSep = 150,
    .riseFrac = 0.5,
    .fltM = -1.0,
    .sPre = 1000,
    .sLen = 4000,
    .sHscale = 4
};

void print_usage(const param_t *pm)
{
    printf("Usage:\n");
    printf("      -r rise time in samples[%zd]\n", pm->diffSep);
    printf("      -f riseFrac[%g]\n", pm->riseFrac);
    printf("      -m fltM[%g]\n", pm->fltM);
    printf("      -p sPre[%zd]\n", pm->sPre);
    printf("      -l sLen[%zd]\n", pm->sLen);
    printf("      -s iStart[0] -e iStop[-1] starting and stopping(+1) eventid\n");
    printf("      inFileName(.h5) outFileName\n");
}

/** Find pulses by their rising edges.
 *
 * @param[out] npulse number of pulses found
 * @param[in] fltH input waveform should be loaded into it already
 * @param[in] diffSep basically the rise time in samples
 * @param[in] riseFrac fraction of max pulse height to set pulse edge location
 * @param[in] M exp decay constant for Trapezoidal filter
 * @return the list of rising edge positions (times)
 */
size_t *find_pulses(size_t *npulse, filters_t *fltH, size_t diffSep, double riseFrac, double M)
{

    size_t n, nc = 1024;
    size_t *pulseRiseT = NULL, *pulseRiseT1;

    size_t flt_k = 2*diffSep, flt_l = 2*flt_k;
    double flt_M = M, max, dV;
    ssize_t i, j, sep, prev;

    if((pulseRiseT = calloc(nc, sizeof(size_t))) == NULL) {
        error_printf("calloc failed for pulseRiseT in %s()\n", __FUNCTION__);
        return NULL;
    }

    filters_trapezoidal(fltH, flt_k, flt_l, flt_M);

    max = 0.0;
    for(i=0; i<fltH->wavLen; i++)
        if(fltH->outWav[i] > max) max = fltH->outWav[i];

    sep = flt_k + flt_l;
    prev = 0;
    n = 0;
    for(i=0; i<fltH->wavLen; i++) {
        j = ((i+diffSep)>=fltH->wavLen)?(fltH->wavLen-1):(i+diffSep);
        dV = fltH->inWav[j] - fltH->inWav[i];
        if(dV > max * riseFrac) {
            if(i-prev > sep) {
                pulseRiseT[n] = j; n++;
                if(n >= nc) {
                    nc *= 2;
                    if((pulseRiseT1 = realloc(pulseRiseT, nc * sizeof(size_t))) == NULL) {
                        error_printf("realloc failed for pulseRiseT in %s()\n", __FUNCTION__);
                        *npulse = n;
                        return pulseRiseT;
                    }
                    pulseRiseT = pulseRiseT1;
                }
                prev = i;
            }
        }
    }
    *npulse = n;
    return pulseRiseT;
}

int main(int argc, char **argv)
{
    int optC = 0;
    param_t pm;

    char *inFileName, *outFileName;
    struct hdf5rawWaveformIo_waveform_file *inWfmFile;
    struct waveform_attribute inWfmAttr;
    struct hdf5rawWaveformIo_waveform_event inWfmEvent;
    IN_WFM_BASE_TYPE *inWfmBuf;
    // OUT_WFM_BASE_TYPE *outWfmBuf;
    filters_t *fltHdl;

    ssize_t iStart=0, iStop=-1, i, j, k, iCh;
    size_t nEventsInFile, chGrpLen, npulse, *pulseRiseT, nSep;
    unsigned int v, c;
    size_t chGrpIdx[SCOPE_NCH] = {0};
    double frameSize, val, sep, sepMu, sepSigma;

    gsl_histogram2d *wav2H, *flt2H;

    memcpy(&pm, &param_default, sizeof(pm));
    // parse switches
    while((optC = getopt(argc, argv, "e:f:l:m:p:r:s:")) != -1) {
        switch(optC) {
        case 'e':
            iStop = atoll(optarg);
            break;
        case 'f':
            pm.riseFrac = atof(optarg);
            break;
        case 'l':
            pm.sLen = atoll(optarg);
            break;
        case 'm':
            pm.fltM = atof(optarg);
            break;
        case 'p':
            pm.sPre = atoll(optarg);
            break;
        case 'r':
            pm.diffSep = atoll(optarg);
            break;
        case 's':
            iStart = atoll(optarg);
            break;
        default:
            print_usage(&pm);
            return EXIT_FAILURE;
            break;
        }
    }

    argc -= optind;
    argv += optind;
    if(argc<2 || argc>=3) {
        print_usage(&pm);
        return EXIT_FAILURE;
    }

    inFileName = argv[0];
    outFileName = argv[1];

    inWfmFile = hdf5rawWaveformIo_open_file_for_read(inFileName);
    hdf5rawWaveformIo_read_waveform_attribute_in_file_header(inWfmFile, &inWfmAttr);

    fprintf(stderr, "waveform_attribute:\n"
            "     chMask  = 0x%04x\n"
            "     nPt     = %zd\n"
            "     nFrames = %zd\n"
            "     dt      = %g\n"
            "     t0      = %g\n"
            "     ymult   = %g %g %g %g\n"
            "     yoff    = %g %g %g %g\n"
            "     yzero   = %g %g %g %g\n",
            inWfmAttr.chMask, inWfmAttr.nPt, inWfmAttr.nFrames, inWfmAttr.dt,
            inWfmAttr.t0, inWfmAttr.ymult[0], inWfmAttr.ymult[1], inWfmAttr.ymult[2],
            inWfmAttr.ymult[3], inWfmAttr.yoff[0], inWfmAttr.yoff[1],
            inWfmAttr.yoff[2], inWfmAttr.yoff[3], inWfmAttr.yzero[0],
            inWfmAttr.yzero[1], inWfmAttr.yzero[2], inWfmAttr.yzero[3]);

    nEventsInFile = hdf5rawWaveformIo_get_number_of_events(inWfmFile);
    fprintf(stderr, "Number of events in file: %zd\n", nEventsInFile);
    if(iStart < 0) iStart = 0;
    if(iStart >= nEventsInFile) iStart = nEventsInFile - 1;
    if(iStop < 0) iStop = nEventsInFile;
    if(iStop <= iStart) iStop = iStart + 1;

    if(inWfmAttr.nFrames > 0) {
        frameSize = inWfmAttr.nPt / (double)inWfmAttr.nFrames;
    } else {
        frameSize = (double)inWfmAttr.nPt;
    }

    v = inWfmAttr.chMask;
    for(c=0; v; c++) v &= v - 1;
    /* Brian Kernighan's way of counting bits */
    chGrpLen = inWfmFile->nCh / c;
    i=0;
    for(v=0; v<SCOPE_NCH; v++)
        if((inWfmAttr.chMask >> v) & 0x01) {
            chGrpIdx[i] = v;
            i++;
        }
    inWfmBuf = (IN_WFM_BASE_TYPE*)malloc(inWfmFile->nPt * inWfmFile->nCh * sizeof(IN_WFM_BASE_TYPE));
    inWfmEvent.wavBuf = inWfmBuf;

    fltHdl = filters_init(NULL, inWfmFile->nPt);
    wav2H = gsl_histogram2d_alloc(pm.sLen/pm.sHscale, 128);
    gsl_histogram2d_set_ranges_uniform(wav2H, -0.5 * inWfmAttr.dt, (pm.sLen+0.5) * inWfmAttr.dt,
        inWfmAttr.yzero[chGrpIdx[0]] + (-128.5 - inWfmAttr.yoff[chGrpIdx[0]]) * inWfmAttr.ymult[chGrpIdx[0]],
        inWfmAttr.yzero[chGrpIdx[0]] + ( 127.5 - inWfmAttr.yoff[chGrpIdx[0]]) * inWfmAttr.ymult[chGrpIdx[0]]);

    flt2H = gsl_histogram2d_alloc(pm.sLen/pm.sHscale, 256);
    gsl_histogram2d_set_ranges_uniform(flt2H, -0.5 * inWfmAttr.dt, (pm.sLen+0.5) * inWfmAttr.dt, -0.01, -0.01 + 256 * inWfmAttr.ymult[chGrpIdx[0]]);

    nSep = 0; sepMu = 0.0; sepSigma = 0.0;
    for(inWfmEvent.eventId = iStart; inWfmEvent.eventId < iStop; inWfmEvent.eventId++) {
        hdf5rawWaveformIo_read_event(inWfmFile, &inWfmEvent);
        for(iCh=0; iCh < 1 /* inWfmFile->nCh */; iCh++) {
            for(i=0; i<inWfmFile->nPt; i++) {
                val = (inWfmBuf[(size_t)(iCh * inWfmFile->nPt + i)]
                       - inWfmAttr.yoff[chGrpIdx[iCh]])
                    * inWfmAttr.ymult[chGrpIdx[iCh]]
                    + inWfmAttr.yzero[chGrpIdx[iCh]];

                fltHdl->inWav[i] = val;
            }
        }
        pulseRiseT = find_pulses(&npulse, fltHdl,
                                 pm.diffSep, pm.riseFrac, pm.fltM);
        fprintf(stderr, "eventId = %zd, npulse = %zd, first at %zd\n",
               inWfmEvent.eventId, npulse, pulseRiseT[0]);
        for(i=0; i<npulse-1; i++) {
            sep = pulseRiseT[i+1] - pulseRiseT[i];
            sepMu += sep;
            sepSigma += sep * sep;
            nSep++;
        }

        for(i=1; i<npulse-1; i++) {
            for(j=0; j<pm.sLen; j++) {
                k = pulseRiseT[i]-pm.sPre + j;
                gsl_histogram2d_increment(wav2H, j*inWfmAttr.dt, fltHdl->inWav[k]);
                gsl_histogram2d_increment(flt2H, j*inWfmAttr.dt, fltHdl->outWav[k]);
            }
        }
        free(pulseRiseT);
    }
    sepMu /= (double)nSep;
    sepSigma = sqrt(1.0/(double)(nSep-1) * (sepSigma - nSep * sepMu * sepMu));

    printf("nSep = %zd, sepMu = %g, sepSigma = %g\n", nSep, sepMu, sepSigma);

    FILE *ofp;
    if((ofp = fopen(outFileName, "w"))==NULL) {
        perror(outFileName);
        goto Exit;
    }
    gsl_histogram2d_fprintf(ofp, wav2H, "%24.16e", "%g");
    fprintf(ofp, "\n\n");
    gsl_histogram2d_fprintf(ofp, flt2H, "%24.16e", "%g");
    fprintf(ofp, "\n\n");

    fprintf(ofp, "# baseline distribution");
    double yl, yu;
    for(i=0; i<gsl_histogram2d_ny(wav2H); i++) {
        gsl_histogram2d_get_yrange(wav2H, i, &yl, &yu);
        fprintf(ofp, "%24.16e, %g\n", yl, gsl_histogram2d_get(wav2H, 0, i));
    }
    fprintf(ofp, "\n\n");

    fprintf(ofp, "# filtered distribution");
    for(i=0; i<gsl_histogram2d_ny(flt2H); i++) {
        gsl_histogram2d_get_yrange(flt2H, i, &yl, &yu);
        fprintf(ofp, "%24.16e, %g\n", yl,
                gsl_histogram2d_get(flt2H, (pm.sPre+3*pm.diffSep)/pm.sHscale, i));
    }

    fclose(ofp);

Exit:
    free(inWfmBuf); inWfmBuf = NULL;
    gsl_histogram2d_free(wav2H);
    gsl_histogram2d_free(flt2H);
    filters_close(fltHdl);
    hdf5rawWaveformIo_close_file(inWfmFile);

    return EXIT_SUCCESS;
}


/* this program is in the public domain */

#include "bench-user.h"
#include <math.h>

#ifdef HAVE_GSL_GSL_VERSION_H
#include <gsl/gsl_version.h>
#endif
static const char *mkvers(void)
{
#ifdef HAVE_GSL_GSL_VERSION_H
     return gsl_version;
#else
     return "unknown"
#endif
}

BEGIN_BENCH_DOC
BENCH_DOC("name", NAME)
BENCH_DOC("package", "GNU Scientific Library (GSL)")
BENCH_DOC("author", "Brian Gough")
BENCH_DOC("year", "2002")
BENCH_DOC("url", "http://sources.redhat.com/gsl")
BENCH_DOC("url-was-valid-on", "Thu Jul 26 07:48:45 EDT 2001")
BENCH_DOC("copyright",
  "Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough\n"
  "\n"
  "This program is free software; you can redistribute it and/or modify\n"
  "it under the terms of the GNU General Public License as published by\n"
  "the Free Software Foundation; either version 2 of the License, or (at\n"
  "your option) any later version.\n")
BENCH_DOC("language", "C")
BENCH_DOCF("version", mkvers)
END_BENCH_DOC

#define CAT0(a, b) a ## b
#define CAT(a, b) CAT0(a, b)

#ifdef BENCHFFT_SINGLE
#include <gsl/gsl_fft_complex_float.h>
#include <gsl/gsl_fft_real_float.h>
#include <gsl/gsl_fft_halfcomplex_float.h>
#define C(x) CAT(gsl_fft_complex_float_, P(x))
#define R(x) CAT(gsl_fft_real_float_radix2_, x)
#define H(x) CAT(gsl_fft_halfcomplex_float_radix2_, x)
#else
#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_halfcomplex.h>
#define C(x) CAT(gsl_fft_complex_, P(x))
#define R(x) CAT(gsl_fft_real_radix2_, x)
#define H(x) CAT(gsl_fft_halfcomplex_radix2_, x)
#endif

int can_do(struct problem *p)
{
     return (p->rank == 1 &&
#ifdef DO_COMPLEX
	     p->kind == PROBLEM_COMPLEX
#else
	     p->kind == PROBLEM_REAL
#endif
	     && problem_power_of_two(p, 1));
}

void copy_h2c(struct problem *p, bench_complex *out)
{
     copy_h2c_1d_halfcomplex(p, out, -1.0);
}

void copy_c2h(struct problem *p, bench_complex *in)
{
     copy_c2h_1d_halfcomplex(p, in, -1.0);
}

void setup(struct problem *p)
{
     BENCH_ASSERT(can_do(p));
     /* nothing to do */
}

void doit(int iter, struct problem *p)
{
     int i;
     int n = p->n[0];
     void *in = p->in;

     if (p->kind == PROBLEM_COMPLEX) {
#ifdef DO_COMPLEX
	  if (p->sign == -1) {
	    for (i = 0; i < iter; ++i)
	      C(forward)(in, 1, n);
	  } else {
	    for (i = 0; i < iter; ++i)
	      C(backward)(in, 1, n);
	  }
#endif
     } else {
#ifdef DO_REAL
	  if (p->sign == -1) {
	    for (i = 0; i < iter; ++i)
	      R(transform)(in, 1, n);
	  } else {
	    for (i = 0; i < iter; ++i)
	      H(transform)(in, 1, n);
	  }
#endif
     }
}

void done(struct problem *p)
{
     UNUSED(p);
}


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

/*
 * =====================================================================================
 *
 *       Filename:  Reg_s0.c
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  15/03/2014 21:10:30
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:
 *
 * =====================================================================================
 */

#include <gsl/gsl_integration.h>

#include "funcs.h"

/*
   We split the interval of integration (0,+inf) into three parts:
   1) (0, O/2)
   		and we use the diffeomorphism k -> 1/k to cope with the singularity
		in k = 0 ;
   2) (O/2, 3*O/2)
   		and we calculate the Cauchy principal value around k = O ;
   3) (3*O/2, +inf)
 */

/*
 * ===  FUNCTION  ======================================================================
 *         Name:  fu_inv
 *  Description:  To integrate on the 1) interval we make the change of variable
 *		  k -> 1/k
 * =====================================================================================
 */
double fu_inv ( double k, void* params )
{
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1, alpha ;
	assign_p ( pars, &o_c, &b, &O, &o_1 ) ;
	alpha = pars->alpha ;

	double temp = alpha*O*exp(-1/(k*o_c))/(1-(O*O*k*k)) ;
	double val = temp/(k*tanh(b/(k*2))) ;

	return val ;
}

/*
 * ===  FUNCTION  ======================================================================
 *         Name:  fu_coth
 *  Description:  The function to be integrated around the singularity O, by calculating
 *		  the Cauchy principal value. We use qawc and therefore we multiply by
 *		  (k-O).
 * =====================================================================================
 */
double fu_cau ( double k, void* params )
{
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1, alpha ;
	assign_p ( pars, &o_c, &b, &O, &o_1 ) ;
	alpha = pars->alpha ;

	double temp = alpha*O*k*exp(-k/o_c)/(k+O) ;
	double val = temp/tanh(b*k/2) ;

	return val ;
}

/*
 * ===  FUNCTION  ======================================================================
 *         Name:  fu_coth
 *  Description:  Function to be integrated on the tail (3*O/2 , +inf)
 * =====================================================================================
 */
double fu_coth ( double k, void* params )
{
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1, alpha ;
	assign_p ( pars, &o_c, &b, &O, &o_1 ) ;
	alpha = pars->alpha ;

	double temp = alpha*O*k*exp(-k/o_c)/(k*k-O*O) ;
	double val = temp/tanh(b*k/2) ;

	return val ;
}

/*
 * ===  FUNCTION  ======================================================================
 *         Name:  re_gs0
 *  Description:  Re g_s0
 * =====================================================================================
 */
int re_gs0 ( void* params, double* val, double* error )
{
	struct f_params* pars = (struct f_params*) params ;
	double O ;
	O = pars->Omega ;

	double r, err ;
	double r1, r2, r3, err1, err2, err3 ;

	gsl_function f, F, G ;
	f.function = &fu_coth ; F.function = &fu_cau ; G.function = &fu_inv ;
	f.params = pars ; F.params = pars ; G.params = pars ;

	gsl_integration_workspace *fu_fin_ws =
		gsl_integration_workspace_alloc (WS_SZ) ;

	gsl_integration_workspace *fu_inv_ws =
		gsl_integration_workspace_alloc (WS_SZ) ;

	gsl_integration_workspace *fu_inf_ws =
		gsl_integration_workspace_alloc (WS_SZ) ;

	int status1 ;
	status1 = gsl_integration_qagiu ( &G, 2/O, 1e-9, .001, WS_SZ,
		       	fu_inv_ws, &r1,	&err1 ) ;
	int status2 ;
	status2 = gsl_integration_qawc ( &F, O/2, 3*O/2, O, 1e-9, .001, WS_SZ,
			fu_fin_ws, &r2, &err2 ) ;

	int status3 ;
	status3 = gsl_integration_qagiu ( &f, 3*O/2, 1e-9, .001, WS_SZ,
		       	fu_inf_ws, &r3,	&err3 ) ;

	r = r1 + r2 + r3 ; err = err1 + err2 + err3 ;
	*val = r ; *error = err ;

	gsl_integration_workspace_free(fu_fin_ws) ;
	gsl_integration_workspace_free(fu_inv_ws) ;
	gsl_integration_workspace_free(fu_inf_ws) ;

	int status = status1 + status2 + status3 ;
	return status ;
}


/*
 *
 * ML fitting of size (sigma, s) and number of photons (n)
 * of already localized dots using a 2D Gaussian model provided by gaussianInt2.
 *
 * Only one dot at a time is considered, hence the 1 in mlfit1sn
 *
 * Uses 3D images however only a xy-plane at at time is used for the
 * fitting.
 *
 * See also:
 *   mlfit1.c, fitting of xy
 *   df_mlfit1sn.c, a MATLAB interface
 *
 * 2017.04.12. Valgrind ok.
 *
 * Future work:
 * - Return the fitting status
 * - Study the failed cases
 * - More restrictions on the search domain, i.e., cap sigma depending
 *   on the window size

 ==9245== Invalid write of size 8
==9245==    at 0x1094C9: mlfit1sn_dot (mlfit1sn.c:227)
==9245==    by 0x109A9B: mlfit1sn (mlfit1sn.c:278)
==9245==    by 0x109A9B: unit_tests (mlfit1sn.c:346)
==9245==    by 0x57F73F0: (below main) (libc-start.c:291)
==9245==  Address 0x5babf98 is 24 bytes after a block of size 32,000 in arena "client"
==9245==

*/

#include <assert.h>
#include <inttypes.h>
#include <stdint.h>
#include <stdio.h>

#include <gsl/gsl_multimin.h>
#include <gsl/gsl_statistics.h>

#include "gaussianInt2.h"
#include "mlfit.h"
#include "blit3.h"

//
// Headers
//

// Number of photons in signal
double estimateNphot(double *, size_t);
// Locate multiple dots in a volume
int    mlfit1sn(double * , size_t , size_t , size_t ,
                            double * , size_t , double * );
// Localization for single dot
int    mlfit1sn_dot(double *, size_t, double *,  double *);
// Cost function
double my_f (const gsl_vector *, void *);
// Random number in a range
double rand_range(double, double);
int    unit_tests(void);

// 0: no information, 1: per dot, 2: even more
#ifndef verbose
#define verbose 0
#endif

// When low on bugs,
// replace gsl_vector_get(v,i) by v->data[i*v->stride]
// #define GSL_RANGE_CHECK_OFF

// Globals
#if verbose>1
uint32_t maxInterations = 5;
#else
uint32_t maxInterations = 5000;
#endif

double convCriteria = 1e-6;
// In theory also the window size, denoted Ws in most places

// Optimization constants
typedef struct {
  double * R; // local region
  double * G; // A already allocated temporary space for the Gaussian kernel
  size_t Rw; // R and G has size Rw*Rw
  double sigma; // For constant sigma fitting
  double bg;
  double x;
  double y;
  double midvalue; // value of central pixel in model - bg
} optParams;

double my_f (const gsl_vector *v, void *params)
  // The function to optimize
{
  optParams *p = (optParams *) params;

  size_t Rw = p->Rw;
  double * R = p->R;
  double * GI = p->G;
  double bg = p->bg;
  double x = p->x;
  double y = p->y;

  // Get the other parameters ...
  double sigma = gsl_vector_get(v, 0);
  double Nphot = gsl_vector_get(v, 1);

#if verbose > 1
  printf("Rw: %lu\n", Rw);
  printf("Nphot: %f\n", Nphot);
  printf("sigma: %f\n", sigma);
  printf("bg   : %f\n", bg);
  printf("x: %f, y: %f\n", x, y);
  printf("GI:\n");
  showRegion(GI, Rw);
#endif

  if(sigma<0)
    return(INFINITY);

  if(Nphot<0)
    return(INFINITY);

  // Create Gaussian ...
  double mu[] = {x,y};
  gaussianInt2(GI, mu, &sigma, Rw);
  p->midvalue = Nphot*GI[ (Rw*Rw-1)/2 ];
  for(size_t kk = 0; kk<Rw*Rw; kk++)
    GI[kk] = bg+Nphot*GI[kk];

  /*
   * from LL2PG.m
   *   model = x(3)+x(4)*gaussianInt2([x(1), x(2)], x(5), (size(patch, 1)-1)/2);
   *   mask = disk2d((size(patch,1)-1)/2);
   *   %L = -sum(sum(-(patch-model).^2./model - .5*log(model)));
   *   L = -sum(sum( mask.*( -(patch-model).^2./model - .5*log(model) ) ));
   */

  double E = 0;
  for (size_t kk=0; kk<Rw*Rw; kk++)
  {
    E+= (GI[kk]-R[kk])*(GI[kk]-R[kk])/GI[kk] - .5*log(GI[kk]); // ML
    //E+= (GI[kk]-R[kk])*(GI[kk]-R[kk]); // Quadratic
  }
  //  E = -E;

  #if verbose > 1
  printf("E: %f\n", E);
#endif
  return E;
}

int mlfit1sn_dot(double * V, size_t Vm,
    double * D,  double * F)
  // Localization for a dot roughly centered in V of size Vm x Vm
  // D[0], D[1], D[3] are the global coordinates of the dot
  // F are the fitted coordinates
{
  // Non-optimized parameters
  optParams par;
  par.R = V;
  par.Rw = Vm;
  par.x = D[0]-nearbyint(D[0]);
  par.y = D[1]-nearbyint(D[1]);
  // par.z required for accurate nphot counting, sigma is invariant to
  // this shift
  par.bg = estimateBG(V, Vm);
  par.G = malloc(Vm*Vm*sizeof(double));

#if verbose>0
  printf("localizeDot\n");
  printf("x: %f y: %f\n", par.x, par.y);
  printf("bg: %f\n", par.bg);
#endif

  const gsl_multimin_fminimizer_type *T =
    gsl_multimin_fminimizer_nmsimplex2;
  gsl_multimin_fminimizer *s = NULL;
  gsl_vector *ss, *x;
  gsl_multimin_function minex_func;

  size_t iter = 0;
  int status;
  double size;

  /* Starting point */
  x = gsl_vector_alloc (2);
  gsl_vector_set(x, 0, 1.2); // sigma
  double nphot0 =estimateNphot(V, Vm);
  gsl_vector_set(x, 1, nphot0); // Nphot

  /* Set initial step sizes */
  ss = gsl_vector_alloc(2);
  gsl_vector_set(ss, 0, 0.1); // sigma
  gsl_vector_set(ss, 1, nphot0/100); // Number of photons

  /* Initialize method and iterate */
  minex_func.n = 2;
  minex_func.f = my_f;
  minex_func.params = ∥

  s = gsl_multimin_fminimizer_alloc (T, 2);
  gsl_multimin_fminimizer_set (s, &minex_func, x, ss);

  do
  {
    iter++;
    status = gsl_multimin_fminimizer_iterate(s);

    if (status)
      break;

    size = gsl_multimin_fminimizer_size(s);
    status = gsl_multimin_test_size(size, convCriteria);

    if (status == GSL_SUCCESS)
    {
#if verbose > 0
      printf ("converged to minimum at\n");
      printf ("%5lu sigma:%10.3e NP:%10.3e f() = %7.3f size = %10.3e\n",
          iter,
          gsl_vector_get (s->x, 0),
          gsl_vector_get (s->x, 1),
          s->fval, size);
#endif
    }
  }
  while (status == GSL_CONTINUE && iter < maxInterations);

  F[0] = gsl_vector_get(s->x, 0);
  F[1] = par.midvalue;
  F[2] = s->fval;
  F[3] = status;
  F[4] = par.bg;

  gsl_vector_free(x);
  gsl_vector_free(ss);
  gsl_multimin_fminimizer_free(s);

  free(par.G);

  return status;
}

int mlfit1sn(double * V, size_t Vm, size_t Vn, size_t Vp,
    double * D, size_t Dm, double * F)
  // run the localization routine for a list of dots, D [3xDm]
{

  // Return values have to be checked
  gsl_set_error_handler_off();
  // as a custom error handler can also be used.

  int Ws_pref = 15; // Window size is Ws x Ws
  int Ws = Ws_pref;
  double * W = malloc(Ws_pref*Ws_pref*sizeof(double));

  for(size_t kk =0; kk<Dm; kk++)
  {
    // Copy the neighbourhood around each D into W
    int hasRegion = 0;

    for(Ws = Ws_pref; Ws>7; Ws = Ws-2) {
      if(getRegion(W, Ws, V, Vm, Vn, Vp, D+kk*3) == 0) {
        hasRegion = 1;
        break;
      } else {
#if verbose >0
        printf("Ws: %d not possible\n", Ws);
#endif
      }
    }
#if verbose>0
    printf("Ws: %d\n", Ws);
#endif

    if(hasRegion == 1) {
#if verbose > 1
      printf("W:\n");
      showRegion(W, Ws);
#endif
      // Local fitting in W
#if verbose > 0
      int status = mlfit1sn_dot(W,  Ws, D+kk*3, F+kk*5);
      printf("Status: %d\n", status);
#else
     mlfit1sn_dot(W,  Ws, D+kk*3, F+kk*5);
#endif
    }
    else
    {
      F[kk*5] = -1;
      F[kk*5+1] = -1;
      F[kk*5+2] = -1;
      F[kk*5+3] =  -3;
      F[kk*5+4] = 0;
    }
  }
  free(W);
  return 0;
}

#ifdef standalone
int unit_tests()
{
  double * V; // image
  int Vm = 1024; int Vn = 1024; int Vp = 60;
  double * D; // list of dots
  size_t Dm = 1000; // number of dots
  double * F; // fitted dots

  printf("Image size: %dx%dx%d\n", Vm, Vn, Vp);
  printf("Localizing %lu dots\n", Dm);

  V = malloc(Vm*Vn*Vp*sizeof(double));
  D = malloc(3*Dm*sizeof(double));
  F = malloc(4*Dm*sizeof(double));

  // Initialize the data
  for(int kk=0; kk<Vm*Vn*Vp; kk++)
    V[kk] = rand_range(0,0);

  for(uint32_t kk=0; kk<Dm; kk++)
  {
    size_t pos = kk*3;
    D[pos] =   rand_range(-1, Vm+1);
    D[pos+1] = rand_range(6, Vn-7);
    D[pos+2] = rand_range(6, Vp-7);
    if(D[pos] < 6)
      D[pos] = 6;
    if(D[pos+1] < 6)
      D[pos+1] = 6;
    if(D[pos+2] < 6)
      D[pos+2] = 6;
#if verbose > 0
    printf("D %03d %f %f %f\n", kk, D[pos], D[pos+1], D[pos+2]);
#endif
  }

  for(uint32_t kk=0; kk<Dm; kk++)
  {
    size_t pos = D[kk*3] + D[kk*3+1]*Vm + D[kk*3+2]*Vm*Vn;
    V[pos] = 5;
  }

  D[0] = 100; D[1] = 100; D[2] = 30;
  V[(int) D[0]+(int) D[1]*Vm+(int) D[2]*Vm*Vn] = 7;
  V[(int) D[0]+1+(int) D[1]*Vm+(int) D[2]*Vm*Vn] = 6;

  // Run the optimization
  mlfit1sn(V, Vm, Vn, Vp, D, Dm, F);

  // In next version, also supply clustering information

#if verbose >0
  for(size_t kk = 0; kk<Dm; kk++)
  {
    size_t pos = kk*3;
    size_t dpos = kk*4;
    printf("%6lu (%f, %f, %f) : s: %f, n: %f e: %f status: %d\n", kk,
        D[pos], D[pos+1], D[pos+2],
        F[dpos], F[dpos+1], F[dpos+2], (int) F[dpos+3]);
  }
#endif
  free(F);
  free(D);
  free(V);
  return 0;
}

int main(int argc, char ** argv)
  // For testing, not used any more, see the MATLAB interface in
  // df_mlfit.c
{

  printf("%s\n", argv[0]);

  if(argc == 1) {
    return unit_tests();
  }

  return 1;
}
#endif


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

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

/*
 * This module contains an implementation of the Steihaug-Toint
 * conjugate gradient algorithm for nonlinear optimization problems.
 * This implementation closely follows the following works:
 *
 * [1] T. Steihaug, The conjugate gradient method and trust regions
 *     in large scale optimization, SIAM J. Num. Anal., 20(3) 1983.
 *
 * In the below algorithm, the Jacobian and gradient are scaled
 * according to:
 *
 * J~ = J D^{-1}
 * g~ = D^{-1}
 *
 * prior to any calculations which results in better numerical
 * stability when solving for the Gauss-Newton step. The resulting
 * step vector is then backtransformed as:
 *
 * dx = D^{-1} dx~
 */

typedef struct
{
  size_t n;                  /* number of observations */
  size_t p;                  /* number of parameters */
  gsl_vector *z;             /* Gauss-Newton step, size p */
  gsl_vector *r;             /* steepest descent step, size p */
  gsl_vector *d;             /* steepest descent step, size p */
  gsl_vector *workp;         /* workspace, length p */
  gsl_vector *workn;         /* workspace, length n */
  double norm_g;             /* || g~ || */

  double cgtol;              /* tolerance for CG solution */
  size_t cgmaxit;            /* maximum CG iterations */
} cgst_state_t;

#include "common.c"

static void * cgst_alloc (const void * params, const size_t n, const size_t p);
static void cgst_free(void *vstate);
static int cgst_init(const void *vtrust_state, void *vstate);
static int cgst_preloop(const void * vtrust_state, void * vstate);
static int cgst_step(const void * vtrust_state, const double delta,
                     gsl_vector * dx, void * vstate);
static int cgst_preduction(const void * vtrust_state, const gsl_vector * dx,
                           double * pred, void * vstate);
static double cgst_calc_tau(const gsl_vector * p, const gsl_vector * d,
                            const double delta);

static void *
cgst_alloc (const void * params, const size_t n, const size_t p)
{
  const gsl_multilarge_nlinear_parameters *par = (const gsl_multilarge_nlinear_parameters *) params;
  cgst_state_t *state;

  state = calloc(1, sizeof(cgst_state_t));
  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate st state", GSL_ENOMEM);
    }

  state->z = gsl_vector_alloc(p);
  if (state->z == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for z", GSL_ENOMEM);
    }

  state->r = gsl_vector_alloc(p);
  if (state->r == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for r", GSL_ENOMEM);
    }

  state->d = gsl_vector_alloc(p);
  if (state->d == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for d", GSL_ENOMEM);
    }

  state->workp = gsl_vector_alloc(p);
  if (state->workp == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workp", GSL_ENOMEM);
    }

  state->workn = gsl_vector_alloc(n);
  if (state->workn == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workn", GSL_ENOMEM);
    }

  state->n = n;
  state->p = p;

  state->cgmaxit = par->max_iter;
  if (state->cgmaxit == 0)
    state->cgmaxit = n;

  state->cgtol = par->tol;

  return state;
}

static void
cgst_free(void *vstate)
{
  cgst_state_t *state = (cgst_state_t *) vstate;

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

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

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

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

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

  free(state);
}

/*
cgst_init()
  Initialize cgst solver

Inputs: vtrust_state - trust state
        vstate       - workspace

Return: success/error
*/

static int
cgst_init(const void *vtrust_state, void *vstate)
{
  /* nothing to do */

  (void)vtrust_state;
  (void)vstate;

  return GSL_SUCCESS;
}

static int
cgst_preloop(const void * vtrust_state, void * vstate)
{
  /* nothing to do */

  (void)vtrust_state;
  (void)vstate;

  return GSL_SUCCESS;
}

/*
cgst_step()
  Calculate a new step vector

Return:
GSL_SUCCESS if CG solution found
GSL_EMAXITER if no solution found
*/

static int
cgst_step(const void * vtrust_state, const double delta,
          gsl_vector * dx, void * vstate)
{
  int status;
  const gsl_multilarge_nlinear_trust_state *trust_state =
    (const gsl_multilarge_nlinear_trust_state *) vtrust_state;
  cgst_state_t *state = (cgst_state_t *) vstate;
  const gsl_vector * x = trust_state->x;
  const gsl_vector * f = trust_state->f;
  const gsl_vector * swts = trust_state->sqrt_wts;
  const gsl_vector * diag = trust_state->diag;
  const gsl_multilarge_nlinear_parameters * params = trust_state->params;
  gsl_multilarge_nlinear_fdf * fdf = trust_state->fdf;
  double alpha, beta, u;
  double norm_Jd;   /* || J D^{-1} d_i || */
  double norm_r;    /* || r_i || */
  double norm_rp1;  /* || r_{i+1} || */
  size_t i;

  /* Step 1 of [1], section 2; scale gradient as
   *
   * g~ = D^{-1} g
   *
   * for better numerical stability
   */

  for (i = 0; i < state->p; ++i)
    {
      double gi = gsl_vector_get(trust_state->g, i);
      double di = gsl_vector_get(trust_state->diag, i);

      gsl_vector_set(state->z, i, 0.0);
      gsl_vector_set(state->r, i, -gi / di);
      gsl_vector_set(state->d, i, -gi / di);
      gsl_vector_set(state->workp, i, gi / di);
    }

  /* compute || g~ || */
  state->norm_g = gsl_blas_dnrm2(state->workp);

  for (i = 0; i < state->cgmaxit; ++i)
    {
      /* workp := D^{-1} d_i */
      gsl_vector_memcpy(state->workp, state->d);
      gsl_vector_div(state->workp, trust_state->diag);

      /* workn := J D^{-1} d_i */
      status = gsl_multilarge_nlinear_eval_df(CblasNoTrans, x, f, state->workp,
                                              swts, params->h_df, params->fdtype,
                                              fdf, state->workn, NULL, NULL);
      if (status)
        return status;

      /* compute || J D^{-1} d_i || */
      norm_Jd = gsl_blas_dnrm2(state->workn);

      /* Step 2 of [1], section 2 */
      if (norm_Jd == 0.0)
        {
          double tau = cgst_calc_tau(state->z, state->d, delta);

          /* dx = z_i + tau*d_i */
          scaled_addition(1.0, state->z, tau, state->d, dx);
          gsl_vector_div(dx, diag);

          return GSL_SUCCESS;
        }

      /* Step 3 of [1], section 2 */

      norm_r = gsl_blas_dnrm2(state->r);
      u = norm_r / norm_Jd;
      alpha = u * u;

      /* workp <= z_{i+1} = z_i + alpha_i*d_i */
      scaled_addition(1.0, state->z, alpha, state->d, state->workp);

      u = gsl_blas_dnrm2(state->workp);
      if (u >= delta)
        {
          double tau = cgst_calc_tau(state->z, state->d, delta);

          /* dx = z_i + tau*d_i */
          scaled_addition(1.0, state->z, tau, state->d, dx);
          gsl_vector_div(dx, diag);

          return GSL_SUCCESS;
        }

      /* store z_{i+1} */
      gsl_vector_memcpy(state->z, state->workp);

      /* Step 4 of [1], section 2 */

      /* compute: workp := alpha B d_i = alpha D^{-1} J^T J D^{-1} d_i,
       * where J D^{-1} d_i is already stored in workn */
      status = gsl_multilarge_nlinear_eval_df(CblasTrans, x, f, state->workn,
                                              swts, params->h_df, params->fdtype,
                                              fdf, state->workp, NULL, NULL);
      if (status)
        return status;

      gsl_vector_div(state->workp, trust_state->diag);
      gsl_vector_scale(state->workp, alpha);

      /* r_{i+1} = r_i - alpha*B*d_i */
      gsl_vector_sub(state->r, state->workp);
      norm_rp1 = gsl_blas_dnrm2(state->r);

      u = norm_rp1 / state->norm_g;
      if (u < state->cgtol)
        {
          gsl_vector_memcpy(dx, state->z);
          gsl_vector_div(dx, diag);
          return GSL_SUCCESS;
        }

      /* Step 5 of [1], section 2 */

      /* compute u = ||r_{i+1}|| / || r_i|| */
      u = norm_rp1 / norm_r;
      beta = u * u;

      /* compute: d_{i+1} = rt_{i+1} + beta*d_i */
      scaled_addition(1.0, state->r, beta, state->d, state->d);
    }

  /* failed to converge, return current estimate */
  gsl_vector_memcpy(dx, state->z);
  gsl_vector_div(dx, diag);

  return GSL_EMAXITER;
}

static int
cgst_preduction(const void * vtrust_state, const gsl_vector * dx,
                double * pred, void * vstate)
{
  const gsl_multilarge_nlinear_trust_state *trust_state =
    (const gsl_multilarge_nlinear_trust_state *) vtrust_state;
  cgst_state_t *state = (cgst_state_t *) vstate;

  *pred = quadratic_preduction(trust_state, dx, state->workn);

  return GSL_SUCCESS;
}

/*
cgst_calc_tau()
  Compute tau > 0 such that:

|| p + tau*d || = delta
*/

static double
cgst_calc_tau(const gsl_vector * p, const gsl_vector * d,
              const double delta)
{
  double norm_p, norm_d, u;
  double t1, t2, tau;

  norm_p = gsl_blas_dnrm2(p);
  norm_d = gsl_blas_dnrm2(d);

  /* compute (p, d) */
  gsl_blas_ddot(p, d, &u);

  t1 = u / (norm_d * norm_d);
  t2 = t1*u + (delta + norm_p) * (delta - norm_p);
  tau = -t1 + sqrt(t2) / norm_d;

  return tau;
}

static const gsl_multilarge_nlinear_trs cgst_type =
{
  "steihaug-toint",
  cgst_alloc,
  cgst_init,
  cgst_preloop,
  cgst_step,
  cgst_preduction,
  cgst_free
};

const gsl_multilarge_nlinear_trs *gsl_multilarge_nlinear_trs_cgst = &cgst_type;


/* gsl_math.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2004 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_MATH_H__
#define __GSL_MATH_H__
#include <math.h>
#include <gsl/gsl_sys.h>
#include <gsl/gsl_machine.h>
#include <gsl/gsl_precision.h>
#include <gsl/gsl_nan.h>
#include <gsl/gsl_pow_int.h>

#ifndef M_E
#define M_E        2.71828182845904523536028747135      /* e */
#endif

#ifndef M_LOG2E
#define M_LOG2E    1.44269504088896340735992468100      /* log_2 (e) */
#endif

#ifndef M_LOG10E
#define M_LOG10E   0.43429448190325182765112891892      /* log_10 (e) */
#endif

#ifndef M_SQRT2
#define M_SQRT2    1.41421356237309504880168872421      /* sqrt(2) */
#endif

#ifndef M_SQRT1_2
#define M_SQRT1_2  0.70710678118654752440084436210      /* sqrt(1/2) */
#endif

#ifndef M_SQRT3
#define M_SQRT3    1.73205080756887729352744634151      /* sqrt(3) */
#endif

#ifndef M_PI
#define M_PI       3.14159265358979323846264338328      /* pi */
#endif

#ifndef M_PI_2
#define M_PI_2     1.57079632679489661923132169164      /* pi/2 */
#endif

#ifndef M_PI_4
#define M_PI_4     0.78539816339744830961566084582     /* pi/4 */
#endif

#ifndef M_SQRTPI
#define M_SQRTPI   1.77245385090551602729816748334      /* sqrt(pi) */
#endif

#ifndef M_2_SQRTPI
#define M_2_SQRTPI 1.12837916709551257389615890312      /* 2/sqrt(pi) */
#endif

#ifndef M_1_PI
#define M_1_PI     0.31830988618379067153776752675      /* 1/pi */
#endif

#ifndef M_2_PI
#define M_2_PI     0.63661977236758134307553505349      /* 2/pi */
#endif

#ifndef M_LN10
#define M_LN10     2.30258509299404568401799145468      /* ln(10) */
#endif

#ifndef M_LN2
#define M_LN2      0.69314718055994530941723212146      /* ln(2) */
#endif

#ifndef M_LNPI
#define M_LNPI     1.14472988584940017414342735135      /* ln(pi) */
#endif

#ifndef M_EULER
#define M_EULER    0.57721566490153286060651209008      /* Euler constant */
#endif

#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

/* other needlessly compulsive abstractions */

#define GSL_IS_ODD(n)  ((n) & 1)
#define GSL_IS_EVEN(n) (!(GSL_IS_ODD(n)))
#define GSL_SIGN(x)    ((x) >= 0.0 ? 1 : -1)

/* Return nonzero if x is a real number, i.e. non NaN or infinite. */
#define GSL_IS_REAL(x) (gsl_finite(x))

/* Define MAX and MIN macros/functions if they don't exist. */

/* plain old macros for general use */
#define GSL_MAX(a,b) ((a) > (b) ? (a) : (b))
#define GSL_MIN(a,b) ((a) < (b) ? (a) : (b))

/* function versions of the above, in case they are needed */
double gsl_max (double a, double b);
double gsl_min (double a, double b);

/* inline-friendly strongly typed versions */
#ifdef HAVE_INLINE

extern inline int GSL_MAX_INT (int a, int b);
extern inline int GSL_MIN_INT (int a, int b);
extern inline double GSL_MAX_DBL (double a, double b);
extern inline double GSL_MIN_DBL (double a, double b);
extern inline long double GSL_MAX_LDBL (long double a, long double b);
extern inline long double GSL_MIN_LDBL (long double a, long double b);

extern inline int
GSL_MAX_INT (int a, int b)
{
  return GSL_MAX (a, b);
}

extern inline int
GSL_MIN_INT (int a, int b)
{
  return GSL_MIN (a, b);
}

extern inline double
GSL_MAX_DBL (double a, double b)
{
  return GSL_MAX (a, b);
}

extern inline double
GSL_MIN_DBL (double a, double b)
{
  return GSL_MIN (a, b);
}

extern inline long double
GSL_MAX_LDBL (long double a, long double b)
{
  return GSL_MAX (a, b);
}

extern inline long double
GSL_MIN_LDBL (long double a, long double b)
{
  return GSL_MIN (a, b);
}
#else
#define GSL_MAX_INT(a,b)   GSL_MAX(a,b)
#define GSL_MIN_INT(a,b)   GSL_MIN(a,b)
#define GSL_MAX_DBL(a,b)   GSL_MAX(a,b)
#define GSL_MIN_DBL(a,b)   GSL_MIN(a,b)
#define GSL_MAX_LDBL(a,b)  GSL_MAX(a,b)
#define GSL_MIN_LDBL(a,b)  GSL_MIN(a,b)
#endif /* HAVE_INLINE */

/* Definition of an arbitrary function with parameters */

struct gsl_function_struct
{
  double (* function) (double x, void * params);
  void * params;
};

typedef struct gsl_function_struct gsl_function ;

#define GSL_FN_EVAL(F,x) (*((F)->function))(x,(F)->params)

/* Definition of an arbitrary function returning two values, r1, r2 */

struct gsl_function_fdf_struct
{
  double (* f) (double x, void * params);
  double (* df) (double x, void * params);
  void (* fdf) (double x, void * params, double * f, double * df);
  void * params;
};

typedef struct gsl_function_fdf_struct gsl_function_fdf ;

#define GSL_FN_FDF_EVAL_F(FDF,x) (*((FDF)->f))(x,(FDF)->params)
#define GSL_FN_FDF_EVAL_DF(FDF,x) (*((FDF)->df))(x,(FDF)->params)
#define GSL_FN_FDF_EVAL_F_DF(FDF,x,y,dy) (*((FDF)->fdf))(x,(FDF)->params,(y),(dy))

/* Definition of an arbitrary vector-valued function with parameters */

struct gsl_function_vec_struct
{
  int (* function) (double x, double y[], void * params);
  void * params;
};

typedef struct gsl_function_vec_struct gsl_function_vec ;

#define GSL_FN_VEC_EVAL(F,x,y) (*((F)->function))(x,y,(F)->params)

__END_DECLS

#endif /* __GSL_MATH_H__ */


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

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>

#include "cquad_const.c"

/* Allocates a workspace for the given maximum number of intervals.
    Note that if the workspace gets filled, the intervals with the
    lowest error estimates are dropped. The maximum number of
    intervals is therefore not the maximum number of intervals
    that will be computed, but merely the size of the buffer.
    */

gsl_integration_cquad_workspace *
gsl_integration_cquad_workspace_alloc (const size_t n)
{

  gsl_integration_cquad_workspace *w;

  /* Check inputs */
  if (n < 3)
    GSL_ERROR_VAL ("workspace size n must be at least 3", GSL_EDOM, 0);

  /* Allocate first the workspace struct */
  if ((w =
       (gsl_integration_cquad_workspace *)
       malloc (sizeof (gsl_integration_cquad_workspace))) == NULL)
    GSL_ERROR_VAL ("failed to allocate space for workspace struct",
		   GSL_ENOMEM, 0);

  /* Allocate the intervals */
  if ((w->ivals =
       (gsl_integration_cquad_ival *)
       malloc (sizeof (gsl_integration_cquad_ival) * n)) == NULL)
    {
      free (w);
      GSL_ERROR_VAL ("failed to allocate space for the intervals", GSL_ENOMEM,
		     0);
    }

  /* Allocate the max-heap indices */
  if ((w->heap = (size_t *) malloc (sizeof (size_t) * n)) == NULL)
    {
      free (w->ivals);
      free (w);
      GSL_ERROR_VAL ("failed to allocate space for the heap", GSL_ENOMEM, 0);
    }

  /* Remember the size of the workspace */
  w->size = n;

  /* Return the result */
  return w;

}

/* Liberates the workspace memory.
    */

void
gsl_integration_cquad_workspace_free (gsl_integration_cquad_workspace * w)
{

  /* Nothing to be done? */
  if (w == NULL)
    return;

  /* Free the intervals first */
  if (w->ivals != NULL)
    free (w->ivals);

  /* Free the heap */
  if (w->heap != NULL)
    free (w->heap);

  /* Free the structure */
  free (w);

}

/* Compute the product of the fx with one of the inverse
    Vandermonde-like matrices. */

static void
Vinvfx (const double *fx, double *c, const int d)
{

  int i, j;

  switch (d)
    {
    case 0:
      for (i = 0; i <= 4; i++)
	{
	  c[i] = 0.0;
	  for (j = 0; j <= 4; j++)
	    c[i] += V1inv[i * 5 + j] * fx[j * 8];
	}
      break;
    case 1:
      for (i = 0; i <= 8; i++)
	{
	  c[i] = 0.0;
	  for (j = 0; j <= 8; j++)
	    c[i] += V2inv[i * 9 + j] * fx[j * 4];
	}
      break;
    case 2:
      for (i = 0; i <= 16; i++)
	{
	  c[i] = 0.0;
	  for (j = 0; j <= 16; j++)
	    c[i] += V3inv[i * 17 + j] * fx[j * 2];
	}
      break;
    case 3:
      for (i = 0; i <= 32; i++)
	{
	  c[i] = 0.0;
	  for (j = 0; j <= 32; j++)
	    c[i] += V4inv[i * 33 + j] * fx[j];
	}
      break;
    }

}

/* Downdate the interpolation given by the n coefficients c
    by removing the nodes with indices in nans. */

static void
downdate (double *c, int n, int d, int *nans, int nnans)
{

  static const int bidx[4] = { 0, 6, 16, 34 };
  double b_new[34], alpha;
  int i, j;

  for (i = 0; i <= n + 1; i++)
    b_new[i] = bee[bidx[d] + i];
  for (i = 0; i < nnans; i++)
    {
      b_new[n + 1] = b_new[n + 1] / Lalpha[n];
      b_new[n] = (b_new[n] + xi[nans[i]] * b_new[n + 1]) / Lalpha[n - 1];
      for (j = n - 1; j > 0; j--)
	b_new[j] =
	  (b_new[j] + xi[nans[i]] * b_new[j + 1] -
	   Lgamma[j + 1] * b_new[j + 2]) / Lalpha[j - 1];
      for (j = 0; j <= n; j++)
	b_new[j] = b_new[j + 1];
      alpha = c[n] / b_new[n];
      for (j = 0; j < n; j++)
	c[j] -= alpha * b_new[j];
      c[n] = 0;
      n--;
    }

}

/* The actual integration routine.
    */

int
gsl_integration_cquad (const gsl_function * f, double a, double b,
		       double epsabs, double epsrel,
		       gsl_integration_cquad_workspace * ws,
		       double *result, double *abserr, size_t * nevals)
{

  /* Some constants that we will need. */
  static const int n[4] = { 4, 8, 16, 32 };
  static const int skip[4] = { 8, 4, 2, 1 };
  static const int idx[4] = { 0, 5, 14, 31 };
  static const double w = M_SQRT2 / 2;
  static const int ndiv_max = 20;

  /* Actual variables (as opposed to constants above). */
  double m, h, temp;
  double igral, err, igral_final, err_final, err_excess;
  int nivals, neval = 0;
  int i, j, d, split, t;
  int nnans, nans[32];
  gsl_integration_cquad_ival *iv, *ivl, *ivr;
  double nc, ncdiff;

  /* Check the input arguments. */
  if (f == NULL)
    GSL_ERROR ("function pointer shouldn't be NULL", GSL_EINVAL);
  if (result == NULL)
    GSL_ERROR ("result pointer shouldn't be NULL", GSL_EINVAL);
  if (ws == NULL)
    GSL_ERROR ("workspace pointer shouldn't be NULL", GSL_EINVAL);

  /* Check for unreasonable accuracy demands */
  if (epsabs < 0.0 || epsrel < 0.0)
    GSL_ERROR ("tolerances may not be negative", GSL_EBADTOL);
  if (epsabs <= 0 && epsrel < GSL_DBL_EPSILON)
    GSL_ERROR ("unreasonable accuracy requirement", GSL_EBADTOL);

  /* Create the first interval. */
  iv = &(ws->ivals[0]);
  m = (a + b) / 2;
  h = (b - a) / 2;
  nnans = 0;
  for (i = 0; i <= n[3]; i++)
    {
      iv->fx[i] = GSL_FN_EVAL (f, m + xi[i] * h);
      neval++;
      if (!finite (iv->fx[i]))
	{
	  nans[nnans++] = i;
	  iv->fx[i] = 0.0;
	}
    }
  Vinvfx (iv->fx, &(iv->c[idx[0]]), 0);
  Vinvfx (iv->fx, &(iv->c[idx[3]]), 3);
  Vinvfx (iv->fx, &(iv->c[idx[2]]), 2);
  for (i = 0; i < nnans; i++)
    iv->fx[nans[i]] = GSL_NAN;
  iv->a = a;
  iv->b = b;
  iv->depth = 3;
  iv->rdepth = 1;
  iv->ndiv = 0;
  iv->igral = 2 * h * iv->c[idx[3]] * w;
  nc = 0.0;
  for (i = n[2] + 1; i <= n[3]; i++)
    {
      temp = iv->c[idx[3] + i];
      nc += temp * temp;
    }
  ncdiff = nc;
  for (i = 0; i <= n[2]; i++)
    {
      temp = iv->c[idx[2] + i] - iv->c[idx[3] + i];
      ncdiff += temp * temp;
      nc += iv->c[idx[3] + i] * iv->c[idx[3] + i];
    }
  ncdiff = sqrt (ncdiff);
  nc = sqrt (nc);
  iv->err = ncdiff * 2 * h;
  if (ncdiff / nc > 0.1 && iv->err < 2 * h * nc)
    iv->err = 2 * h * nc;

  /* Initialize the heaps. */
  for (i = 0; i < ws->size; i++)
    ws->heap[i] = i;

  /* Initialize some global values. */
  igral = iv->igral;
  err = iv->err;
  nivals = 1;
  igral_final = 0.0;
  err_final = 0.0;
  err_excess = 0.0;

  /* Main loop. */
  while (nivals > 0 && err > 0.0 &&
	 !(err <= fabs (igral) * epsrel || err <= epsabs)
	 && !(err_final > fabs (igral) * epsrel
	      && err - err_final < fabs (igral) * epsrel)
	 && !(err_final > epsabs && err - err_final < epsabs))
    {

      /* Put our finger on the interval with the largest error. */
      iv = &(ws->ivals[ws->heap[0]]);
      m = (iv->a + iv->b) / 2;
      h = (iv->b - iv->a) / 2;

/*      printf
        ("cquad: processing ival %i (of %i) with [%e,%e] int=%e, err=%e, depth=%i\n",
         ws->heap[0], nivals, iv->a, iv->b, iv->igral, iv->err, iv->depth);
*/
      /* Should we try to increase the degree? */
      if (iv->depth < 3)
	{

	  /* Keep tabs on some variables. */
	  d = ++iv->depth;

	  /* Get the new (missing) function values */
	  for (i = skip[d]; i <= 32; i += 2 * skip[d])
	    {
	      iv->fx[i] = GSL_FN_EVAL (f, m + xi[i] * h);
	      neval++;
	    }
	  nnans = 0;
	  for (i = 0; i <= 32; i += skip[d])
	    {
	      if (!finite (iv->fx[i]))
		{
		  nans[nnans++] = i;
		  iv->fx[i] = 0.0;
		}
	    }

	  /* Compute the new coefficients. */
	  Vinvfx (iv->fx, &(iv->c[idx[d]]), d);

	  /* Downdate any NaNs. */
	  if (nnans > 0)
	    {
	      downdate (&(iv->c[idx[d]]), n[d], d, nans, nnans);
	      for (i = 0; i < nnans; i++)
		iv->fx[nans[i]] = GSL_NAN;
	    }

	  /* Compute the error estimate. */
	  nc = 0.0;
	  for (i = n[d - 1] + 1; i <= n[d]; i++)
	    {
	      temp = iv->c[idx[d] + i];
	      nc += temp * temp;
	    }
	  ncdiff = nc;
	  for (i = 0; i <= n[d - 1]; i++)
	    {
	      temp = iv->c[idx[d - 1] + i] - iv->c[idx[d] + i];
	      ncdiff += temp * temp;
	      nc += iv->c[idx[d] + i] * iv->c[idx[d] + i];
	    }
	  ncdiff = sqrt (ncdiff);
	  nc = sqrt (nc);
	  iv->err = ncdiff * 2 * h;

	  /* Compute the local integral. */
	  iv->igral = 2 * h * w * iv->c[idx[d]];

	  /* Split the interval prematurely? */
	  split = (nc > 0 && ncdiff / nc > 0.1);

	}

      /* Maximum degree reached, just split. */
      else
	{
	  split = 1;
	}

      /* Should we drop this interval? */
      if ((m + h * xi[0]) >= (m + h * xi[1])
	  || (m + h * xi[31]) >= (m + h * xi[32])
	  || iv->err < fabs (iv->igral) * GSL_DBL_EPSILON * 10)
	{

/*          printf
            ("cquad: dumping ival %i (of %i) with [%e,%e] int=%e, err=%e, depth=%i\n",
             ws->heap[0], nivals, iv->a, iv->b, iv->igral, iv->err,
             iv->depth);
*/
	  /* Keep this interval's contribution */
	  err_final += iv->err;
	  igral_final += iv->igral;

	  /* Swap with the last element on the heap */
	  t = ws->heap[nivals - 1];
	  ws->heap[nivals - 1] = ws->heap[0];
	  ws->heap[0] = t;
	  nivals--;

	  /* Fix up the heap */
	  i = 0;
	  while (2 * i + 1 < nivals)
	    {

	      /* Get the kids */
	      j = 2 * i + 1;

	      /* If the j+1st entry exists and is larger than the jth,
	         use it instead. */
	      if (j + 1 < nivals
		  && ws->ivals[ws->heap[j + 1]].err >=
		  ws->ivals[ws->heap[j]].err)
		j++;

	      /* Do we need to move the ith entry up? */
	      if (ws->ivals[ws->heap[j]].err <= ws->ivals[ws->heap[i]].err)
		break;
	      else
		{
		  t = ws->heap[j];
		  ws->heap[j] = ws->heap[i];
		  ws->heap[i] = t;
		  i = j;
		}
	    }

	}

      /* Do we need to split this interval? */
      else if (split)
	{

	  /* Some values we will need often... */
	  d = iv->depth;

	  /* Generate the interval on the left */
	  ivl = &(ws->ivals[ws->heap[nivals++]]);
	  ivl->a = iv->a;
	  ivl->b = m;
	  ivl->depth = 0;
	  ivl->rdepth = iv->rdepth + 1;
	  ivl->fx[0] = iv->fx[0];
	  ivl->fx[32] = iv->fx[16];
	  for (i = skip[0]; i < 32; i += skip[0])
	    {
	      ivl->fx[i] =
		GSL_FN_EVAL (f, (ivl->a + ivl->b) / 2 + xi[i] * h / 2);
	      neval++;
	    }
	  nnans = 0;
	  for (i = 0; i <= 32; i += skip[0])
	    {
	      if (!finite (ivl->fx[i]))
		{
		  nans[nnans++] = i;
		  ivl->fx[i] = 0.0;
		}
	    }
	  Vinvfx (ivl->fx, ivl->c, 0);
	  if (nnans > 0)
	    {
	      downdate (ivl->c, n[0], 0, nans, nnans);
	      for (i = 0; i < nnans; i++)
		ivl->fx[nans[i]] = GSL_NAN;
	    }
	  for (i = 0; i <= n[d]; i++)
	    {
	      ivl->c[idx[d] + i] = 0.0;
	      for (j = i; j <= n[d]; j++)
		ivl->c[idx[d] + i] += Tleft[i * 33 + j] * iv->c[idx[d] + j];
	    }
	  ncdiff = 0.0;
	  for (i = 0; i <= n[0]; i++)
	    {
	      temp = ivl->c[i] - ivl->c[idx[d] + i];
	      ncdiff += temp * temp;
	    }
	  for (i = n[0] + 1; i <= n[d]; i++)
	    {
	      temp = ivl->c[idx[d] + i];
	      ncdiff += temp * temp;
	    }
	  ncdiff = sqrt (ncdiff);
	  ivl->err = ncdiff * h;

	  /* Check for divergence. */
	  ivl->ndiv = iv->ndiv + (fabs (iv->c[0]) > 0
				  && ivl->c[0] / iv->c[0] > 2);
	  if (ivl->ndiv > ndiv_max && 2 * ivl->ndiv > ivl->rdepth)
	    {
              /* need copysign(INFINITY, igral) */
	      *result = (igral >= 0) ? GSL_POSINF : GSL_NEGINF;
	      if (nevals != NULL)
		*nevals = neval;
	      return GSL_EDIVERGE;
	    }

	  /* Compute the local integral. */
	  ivl->igral = h * w * ivl->c[0];

	  /* Generate the interval on the right */
	  ivr = &(ws->ivals[ws->heap[nivals++]]);
	  ivr->a = m;
	  ivr->b = iv->b;
	  ivr->depth = 0;
	  ivr->rdepth = iv->rdepth + 1;
	  ivr->fx[0] = iv->fx[16];
	  ivr->fx[32] = iv->fx[32];
	  for (i = skip[0]; i < 32; i += skip[0])
	    {
	      ivr->fx[i] =
		GSL_FN_EVAL (f, (ivr->a + ivr->b) / 2 + xi[i] * h / 2);
	      neval++;
	    }
	  nnans = 0;
	  for (i = 0; i <= 32; i += skip[0])
	    {
	      if (!finite (ivr->fx[i]))
		{
		  nans[nnans++] = i;
		  ivr->fx[i] = 0.0;
		}
	    }
	  Vinvfx (ivr->fx, ivr->c, 0);
	  if (nnans > 0)
	    {
	      downdate (ivr->c, n[0], 0, nans, nnans);
	      for (i = 0; i < nnans; i++)
		ivr->fx[nans[i]] = GSL_NAN;
	    }
	  for (i = 0; i <= n[d]; i++)
	    {
	      ivr->c[idx[d] + i] = 0.0;
	      for (j = i; j <= n[d]; j++)
		ivr->c[idx[d] + i] += Tright[i * 33 + j] * iv->c[idx[d] + j];
	    }
	  ncdiff = 0.0;
	  for (i = 0; i <= n[0]; i++)
	    {
	      temp = ivr->c[i] - ivr->c[idx[d] + i];
	      ncdiff += temp * temp;
	    }
	  for (i = n[0] + 1; i <= n[d]; i++)
	    {
	      temp = ivr->c[idx[d] + i];
	      ncdiff += temp * temp;
	    }
	  ncdiff = sqrt (ncdiff);
	  ivr->err = ncdiff * h;

	  /* Check for divergence. */
	  ivr->ndiv = iv->ndiv + (fabs (iv->c[0]) > 0
				  && ivr->c[0] / iv->c[0] > 2);
	  if (ivr->ndiv > ndiv_max && 2 * ivr->ndiv > ivr->rdepth)
	    {
              /* need copysign(INFINITY, igral) */
	      *result = (igral >= 0) ? GSL_POSINF : GSL_NEGINF;
	      if (nevals != NULL)
		*nevals = neval;
	      return GSL_EDIVERGE;
	    }

	  /* Compute the local integral. */
	  ivr->igral = h * w * ivr->c[0];

	  /* Fix-up the heap: we now have one interval on top
	     that we don't need any more and two new, unsorted
	     ones at the bottom. */

	  /* Flip the last interval to the top of the heap and
	     sift down. */
	  t = ws->heap[nivals - 1];
	  ws->heap[nivals - 1] = ws->heap[0];
	  ws->heap[0] = t;
	  nivals--;

	  /* Sift this interval back down the heap. */
	  i = 0;
	  while (2 * i + 1 < nivals - 1)
	    {
	      j = 2 * i + 1;
	      if (j + 1 < nivals - 1
		  && ws->ivals[ws->heap[j + 1]].err >=
		  ws->ivals[ws->heap[j]].err)
		j++;
	      if (ws->ivals[ws->heap[j]].err <= ws->ivals[ws->heap[i]].err)
		break;
	      else
		{
		  t = ws->heap[j];
		  ws->heap[j] = ws->heap[i];
		  ws->heap[i] = t;
		  i = j;
		}
	    }

	  /* Now grab the last interval and sift it up the heap. */
	  i = nivals - 1;
	  while (i > 0)
	    {
	      j = (i - 1) / 2;
	      if (ws->ivals[ws->heap[j]].err < ws->ivals[ws->heap[i]].err)
		{
		  t = ws->heap[j];
		  ws->heap[j] = ws->heap[i];
		  ws->heap[i] = t;
		  i = j;
		}
	      else
		break;
	    }

	}

      /* Otherwise, just fix-up the heap. */
      else
	{
	  i = 0;
	  while (2 * i + 1 < nivals)
	    {
	      j = 2 * i + 1;
	      if (j + 1 < nivals
		  && ws->ivals[ws->heap[j + 1]].err >=
		  ws->ivals[ws->heap[j]].err)
		j++;
	      if (ws->ivals[ws->heap[j]].err <= ws->ivals[ws->heap[i]].err)
		break;
	      else
		{
		  t = ws->heap[j];
		  ws->heap[j] = ws->heap[i];
		  ws->heap[i] = t;
		  i = j;
		}
	    }

	}

      /* If the heap is about to overflow, remove the last two
         intervals. */
      while (nivals > ws->size - 2)
	{
	  iv = &(ws->ivals[ws->heap[nivals - 1]]);

/*          printf
            ("cquad: dumping ival %i (of %i) with [%e,%e] int=%e, err=%e, depth=%i\n",
             ws->heap[0], nivals, iv->a, iv->b, iv->igral, iv->err,
             iv->depth);
*/
	  err_final += iv->err;
	  igral_final += iv->igral;
	  nivals--;
	}

      /* Collect the value of the integral and error. */
      igral = igral_final;
      err = err_final;
      for (i = 0; i < nivals; i++)
	{
	  igral += ws->ivals[ws->heap[i]].igral;
	  err += ws->ivals[ws->heap[i]].err;
	}

    }

  /* Dump the contents of the heap. */
/*  for (i = 0; i < nivals; i++)
    {
      iv = &(ws->ivals[ws->heap[i]]);
      printf
        ("cquad: ival %i (%i) with [%e,%e], int=%e, err=%e, depth=%i, rdepth=%i\n",
         i, ws->heap[i], iv->a, iv->b, iv->igral, iv->err, iv->depth,
         iv->rdepth);
    }
*/
  /* Clean up and present the results. */
  *result = igral;
  if (abserr != NULL)
    *abserr = err;
  if (nevals != NULL)
    *nevals = neval;

  /* All is well that ends well. */
  return GSL_SUCCESS;

}


/* Pre-requisites for the package regression */
#include "config.h"

#ifdef HAVE_GSL_HEADER

#include <float.h>
#include <limits.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <unistd.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_statistics_double.h>
#include <gsl/gsl_vector.h>

#include "R.h"

#if _CUDA_
#include <cuda.h>
#include <cuda_runtime_api.h>
#include <cublas.h>
#include <cula_blas.h>
#include <cula.h>
#endif

#if _CUDA_
	#define PREC float
	#define PREC_EPS FLT_EPSILON
	#define GSL_TYPE(type) gsl_ ## type ## _float
	#define	GSL_FUNCTION(type,name) gsl_ ## type ## _float_ ## name
	#define GSL_STATS_FUNCTION(name) gsl_stats ## _float_ ## name
	#define MATHS_FUNCTION(name) name ## f
	#define SVD_FUNCTION svdAnyMatCuda
	#define BLAS_FUNCTION(name) my_cula_s ## name
	#define MY_FUNCTION(name) my_cula_ ## name
	#define UGAUSSIAN_FUNCTION my_ugaussian_function
	#define GSL_BLAS_FUNCTION(name) gsl_blas_s ## name
	#define PREPARE_FUNCTION(name) prepare ## name ## ForCoordinateDescentCuda
	#define PREC_DIFF -4
#else
	#define PREC double
	#define PREC_EPS DBL_EPSILON
	#define GSL_TYPE(type) gsl_ ## type
	#define	GSL_FUNCTION(type,name) gsl_ ## type ## _ ## name
	#define GSL_STATS_FUNCTION(name) gsl_stats ## _ ## name
	#define MATHS_FUNCTION(name) name
	#define SVD_FUNCTION svdAnyMat
	#define BLAS_FUNCTION(name) gsl_blas_d ## name
	#define MY_FUNCTION(name) my_gsl_ ## name
	#define UGAUSSIAN_FUNCTION gsl_cdf_ugaussian_P
	#define GSL_BLAS_FUNCTION(name) gsl_blas_d ## name
	#define PREPARE_FUNCTION(name) prepare ## name ## ForCoordinateDescent
	#define PREC_DIFF -6
#endif

#endif


#include <iostream>
#include <vector>
#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <time.h>
#include <math.h>
//#include <C:/Program Files (x86)/Intel/Composer XE/compiler/include/cilk/cilk.h>
#ifndef _HEART_H
#define _HEART_H

class heart
{
private:
	int L;  //sytem size length for now square

	std::vector<std::vector<int>> state;  //value indicates whether in excited state refactory or resting
	std::vector<std::vector<int>> new_state;  //using new_state and state to distinguish newly excited cells from previously excited
	std::vector<std::vector<int>> functional; //1 if functional 0 if disfunctional
	std::vector<std::vector<int>> bond; //1 if bond to site above current cell, 0 if no bond above site
	std::vector<std::vector<int>> last_excited;
	std::vector<std::vector<int>> last_excited_counter; // have a dummy counter so as to not interfere with APD dynamics
	std::vector<std::vector<int>> refractory;
	std::vector<std::vector<int>> total_rest; //have counter for total time cells spend in rest or refractory

	int heart_time; //internal clock
	//model parameters
	int tau; //refactory period
	int T; // pacemaker rate
	double nu, epsilom, delta; //probability vertical connection exists, probability of misfire given faulty and probability cell faulty

	//random number generator
	const gsl_rng_type*Type;
	gsl_rng*r;

	int total_bonds;
	int total_functional;
	int total_excited;
	double refractory_period;

public:

	heart()
	{

	}
	heart(int _L, int _T, int _tau, double _nu, double _epsilom, double _delta)
	{

		refractory_period =0;
		L = _L;
		nu = _nu;
		tau = _tau;
		T = _T;
		epsilom = _epsilom;
		delta = _delta;

		heart_time = T;

		state.resize(L);
		new_state.resize(L);
		functional.resize(L);
		bond.resize(L);
		last_excited.resize(L);
		last_excited_counter.resize(L);
		refractory.resize(L);
		total_rest.resize(L);
		//pacemaker action
		for (unsigned i = 0; i<L; i++)
		{
			state[i].resize(L, 0);  // all cells eligible for excitation
			new_state[i].resize(L, 0);
			functional[i].resize(L, 1);// none disfunctional at the moment
			bond[i].resize(L, 1); // all transverse bonds present at the moment
			last_excited[i].resize(L, T);
			last_excited_counter[i].resize(L, 0);
			refractory[i].resize(L, tau);
			total_rest[i].resize(L,0);
		}
		//	total_functional = L*L;
		//	total_bonds = L*L;
		total_excited = 0;//could be useful avoiding calling size of some vector

		//HAVE NOT USED gsl_rng in class before need to make sure is working correctly

		//  environment variable GSL_RNG_TYPE
		gsl_rng_env_setup();

		Type = gsl_rng_mt19937;//random number generator using the Mersenne Twister algorithm
		r = gsl_rng_alloc(Type);

		//gsl_rng_set(r, time(NULL));//random number generator seeded with time
		gsl_rng_set(r, 10);//random number generator seeded with time

		//assigning cells as having a bond above them or not and functional or not functional
		int healthy = 1;

		for (int j = 0; j<L; j++)
		{
			for (int i = 0; i<L; i++)
			{

				bond[i][j] = (gsl_rng_uniform(r)<nu);
				functional[i][j] = (gsl_rng_uniform(r)>delta);// inequality reversed
				//double x =  gsl_rng_uniform(r);
				//	std::cout <<x<< '\t' << (x<nu) << std::endl;

			}
		}

		gsl_rng_set(r, 20);//random number generator seeded with time
	}
	void operate()//one time step
	{
		total_excited = 0;
		refractory_period = 0;
		//Relax excited cells
		for (int i = 0; i < L; i++)
		{
			for (int j = 0; j<L; j++)
			{
				if (state[i][j] == 1)//searching 4 neighbours of excited cell trying to excite if resting and if don't misfire and if there is a bond
				{
					new_state[((i + 1)<L)*(i + 1)][j] += (new_state[((i + 1)<L)*(i + 1)][j]<1)*(state[((i + 1)<L)*(i + 1)][j]<1)*bond[i][j] * (1 - (1 - functional[((i + 1)<L)*(i + 1)][j])*(gsl_rng_uniform(r)<epsilom));

					new_state[(i - 1)*(i>0) + (L - 1)*(i<1)][j] += (new_state[(i - 1)*(i>0) + (L - 1)*(i<1)][j]<1)*(state[(i - 1)*(i>0) + (L - 1)*(i<1)][j]<1)*bond[(i - 1)*(i>0) + (L - 1)*(i<1)][j] * (1 - (1 - functional[(i - 1)*(i>0) + (L - 1)*(i<1)][j])*(gsl_rng_uniform(r)<epsilom));

					new_state[i][(j + 1)*(j<L - 1)] += (new_state[i][(j + 1)*(j<L - 1)]<1)*(j<L - 1)*(state[i][(j + 1)*(j<L - 1)]<1)*(1 - (1 - functional[i][(j + 1)*(j<L - 1)])*(gsl_rng_uniform(r)<epsilom));

					new_state[i][(j - 1)*(j>0)] += (new_state[i][(j - 1)*(j>0)]<1)*(j>0)*(state[i][(j - 1)*(j>0)]<1)*(1 - (1 - functional[i][(j - 1)*(j>0)])*(gsl_rng_uniform(r)<epsilom));

					refractory[i][j] = restitution(i, j);

					total_rest[i][j] += last_excited_counter[i][j];
				}
			}
		}
		//end relaxing
		//std::vector<std::pair<int,int>> next_excited_cells;

		//evolving cells that were not excited previously or now (some subtlty to those possibly excited now)
		for (int i = 0; i<L; i++)
		{
			for (int j = 0; j<L; j++)
			{
				new_state[i][j] += (state[i][j]>0);
				//if state was excited or refactory before +1 states that were resting may have been newly excited or not but this ignores them
				new_state[i][j] *= (state[i][j]<refractory[i][j]);//promote to resting after end of refactory which is now variable
				total_excited += (new_state[i][j]<2)*new_state[i][j];

				last_excited[i][j] += (state[i][j] < 1) + (state[i][j]>1);//counts if not excited
				last_excited[i][j] *= (state[i][j] < 1) + (state[i][j]>1); //cycles back to 0 if excited;  old states so don't put lastexcited to 0 before calculating new refractory
				last_excited_counter[i][j] += (state[i][j] < 1) + (state[i][j]>1);//dummy counter
				last_excited_counter[i][j] *= (state[i][j] < 1) + (state[i][j]>1); //dummy counter
				refractory_period += refractory[i][j];

			}
		}

		//SAN
		for (int i = 0; i<L; i++)
		{
			//new_state[i][0] += (state[i][0]<1)*(heart_time>T - 1)*(1 - (1 - functional[i][0])*(gsl_rng_uniform(r)<epsilom));
			new_state[i][0] += (state[i][0]<1)*(heart_time>T - 1);

		}

		heart_time++;
		heart_time *= (heart_time<T + 1);
		//end SAN

		state = new_state;

	}
	int get_state(int i, int j)//perhaps have alternative for 2d array
	{

		return state[i][j];

	}
	void set_state(int i, int j, int val)
	{

		state[i][j] = val;
	}
	int get_function(int i, int j)
	{
		return functional[i][j];
	}
	void set_function(int i, int j, int val)
	{
		functional[i][j] = val;
	}
	int get_bond(int i, int j)
	{
		return bond[i][j];
	}
	void set_bond(int i, int j, int val)
	{
		bond[i][j] = val;
	}
	void update_total_rest(int i, int j)
	{
		total_rest[i][j] += last_excited_counter[i][j];
	}
	double get_total_rest(int i,int j)
	{
		return total_rest[i][j];
	}
	void reset_total_rest(int i, int j)
	{
		total_rest[i][j] = 0;
		last_excited_counter[i][j] = 0;
	}

	int get_tau()
	{
		return tau;
	}
	int get_T()
	{
		return T;
	}

	int get_excited()
	{
		return total_excited;
	}
	int get_refractory(int i, int j)
	{
		return refractory[i][j];
	}
	int get_last_excited(int i, int j)
	{
		return last_excited[i][j];
	}
	double colour(int i, int j)
	{
		return (1 - state[i][j] / (1.0*tau))*(state[i][j]>1);
		//return state[i][j]/(1.0*T);

	}
	int restitution(int i, int j)
	{
		//some function of tau T and last_excited[i][j]
		if (last_excited[i][j] < T)
		{
			//return floor((1 - exp(-4.0*last_excited[i][j] / (1.0*T)))*tau);
			return tau;
		}
		else
		{
			return tau;
		}
		//if T since excited should be approx tau
		//if less shoud be less than tau

	}
	double get_avg_refractory()
	{

		return refractory_period/(L*L);
	}
};
#endif

standard model + apd  100% working/heart_james.h


#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
#include <Python.h>
#include <numpy/arrayobject.h>

#include <math.h>

#ifdef __INTEL_COMPILER
#include <mkl_cblas.h>
#else
#include <cblas.h>
#define kmp_set_blocktime(k)
#endif

static inline float minf (float a, float b) { return a < b ? a : b; }
static inline float maxf (float a, float b) { return a < b ? b : a; }

PyObject * compute_overlap (float bbx1, float bby1, float bbx2, float bby2,
                            int fdimy, int fdimx, int dimy, int dimx,
                            float scale, int pady, int padx, int h, int w) {
    int im_area = h*w;
    int bbox_area = (bbx2-bbx1)*(bby2-bby1);
    int im_clip = ((double)bbox_area / (double)im_area) < 0.7;
    npy_intp dims[2] = { dimy, dimx };
    int x, y;

    PyArrayObject * pyoverlap = (PyArrayObject*)PyArray_SimpleNew ((npy_intp)2, dims, NPY_FLOAT);

    for (x = 0; x < dimx; ++x) {
        for (y = 0; y < dimy; ++y) {
            float x1 = (x - padx) * scale;
            float y1 = (y - pady) * scale;
            float x2 = x1 + fdimx*scale - 1;
            float y2 = y1 + fdimy*scale - 1;

            if (im_clip) {
                x1 = minf(maxf(x1, 0.0f), w-1);
                y1 = minf(maxf(y1, 0.0f), h-1);
                x2 = minf(maxf(x2, 0.0f), w-1);
                y2 = minf(maxf(y2, 0.0f), h-1);
            }

            float xx1 = maxf(x1, bbx1);
            float yy1 = maxf(y1, bby1);
            float xx2 = minf(x2, bbx2);
            float yy2 = minf(y2, bby2);

            float intw = xx2 - xx1 + 1;
            float inth = yy2 - yy1 + 1;

            if (intw > 0 && inth > 0) {
                float filterw = x2 - x1 + 1;
                float filterh = y2 - y1 + 1;
                float filter_area = filterw*filterh;
                float int_area = intw*inth;
                float union_area = filter_area + bbox_area - int_area;

                *(float*)PyArray_GETPTR2(pyoverlap, y, x) = int_area / union_area;
            } else {
                *(float*)PyArray_GETPTR2(pyoverlap, y, x) = 0;
            }
        }
    }

    return Py_BuildValue("N", pyoverlap);
}

#if 0
PyObject * objective_function (PyObject * pyexamples) {
    int i;
    int j;
    int k;

    for (i = 0; i < PyList_Size(pyexamples); ++i) {
        PyObject * pyexample = PyList_GetItem (pyexamples, i);
        if (!PyList_Check(pyexample)) {
            PyErr_SetString(PyExc_TypeError, "example list elements must be list.");
            return NULL;
        }

        for (j = 0; j < PyList_Size(pyexample); ++j) {

            PyObject * pyentry = PyList_GetItem(pyexample, j);
            if (!PyTuple_Check(pyentry)) {
                PyErr_SetString(PyExc_TypeError, "entries must be tuples.");
                return NULL;
            }

            if (PyTuple_Size(pyentry) != 2) {
                PyErr_SetString(PyExc_TypeError, "entry tuples must be of size 2");
                return NULL;
            }

            PyObject * pyfeatures = PyTuple_GetItem(pyentry, 0);
            if (!PyDict_Check(pyfeatures)) {
                PyErr_SetString(PyExc_TypeError, "features must be a dict");
                return NULL;
            }

            PyObject * pyfeatureslist = PyDict_Items(pyfeatures);

            float score = 0;
            for (k = 0; k < PyDict_Size(pyfeatureslist); ++k) {
                PyObject * pyfeature = PyList_GetItem(pyfeatureslist, k);
                PyObject * pyblock = PyTuple_GetItem(pyfeature, 0);

                PyObject * pyw = PyTuple_GetItem(pyfeature, 1);
                if (!PyArray_Check(pyw)) {
                    PyErr_SetString(PyExc_TypeError, "w must be a numpy array");
                    return NULL;
                }

                PyArrayObject * pywarray = (PyArrayObject*)pyw;

            }

            PyObject * pydetection = PyTuple_GetItem(pyentry, 1);
        }
    }
    return Py_BuildValue("i", 0);
}

PyObject * gradient (PyObject * examples, PyObject * gradients) {
    Py_RETURN_NONE;
}
#endif

static PyObject * ComputeOverlap(PyObject * self, PyObject * args)
{
    float x1, x2, y1, y2;
    int fdimy, fdimx, dimy, dimx;
    float scale;
    int pady, padx;
    int w, h;
    if (!PyArg_ParseTuple(args, "ffffiiiifiiii", &x1, &y1, &x2, &y2, &fdimy, &fdimx, &dimy, &dimx, &scale, &pady, &padx, &h, &w))
        return NULL;
    return compute_overlap(x1, y1, x2, y2, fdimy, fdimx, dimy, dimx, scale, pady, padx, h, w);
}

#if 0
static PyObject * ObjectiveFunction (PyObject * self, PyObject * args) {
    PyObject * pyexamples;
    if (!PyArg_ParseTuple (args, "O!", &PyList_Type, &pyexamples)) {
        return NULL;
    }

    return objective_function (pyexamples);
}

static PyObject * Gradient (PyObject * self, PyObject * args) {
    PyObject * pyexamples;
    PyObject * pygradients;
    if (!PyArg_ParseTuple (args, "O!O!", &PyList_Type, &pyexamples, &PyDict_Type, &pygradients)) {
        return NULL;
    }

    return gradient (pyexamples, pygradients);
}
#endif

#if PY_MAJOR_VERSION >= 3
static struct PyModuleDef moduledef = {
    PyModuleDef_HEAD_INIT,
    "_train",
    "Routines for training the DPM.",
    -1,
    NULL,
    NULL,
    NULL,
    NULL,
    NULL
};
#endif

#if PY_MAJOR_VERSION < 3
static PyMethodDef _train_methods[] = {
    {"compute_overlap", ComputeOverlap, METH_VARARGS, "Compute detection overlaps with bbox."},
#if 0
    {"ObjectiveFunction", ObjectiveFunction, METH_VARARGS, "WL-SSVM objective function."},
    {"Gradient", Gradient, METH_VARARGS, "WL-SSVM gradient."},
#endif
    {NULL}
};
#endif

#if PY_MAJOR_VERSION >= 3
PyMODINIT_FUNC
PyInit__train(void)
#else
PyMODINIT_FUNC
init_train(void)
#endif
{
    import_array();

#if PY_MAJOR_VERSION >= 3
    PyObject *m = PyModule_Create(&moduledef);
#else
    Py_InitModule3("_train", _train_methods, "Compute detection overlaps with bbox.");
#endif

#if PY_MAJOR_VERSION >= 3
    return m;
#endif
}


#ifndef __INC_MULTIVARSUPPORT__
#define __INC_MULTIVARSUPPORT__

#include "multi_modelstruct.h"
#include "emulator_struct.h"
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

struct multi_modelstruct;
struct emulator_struct;

typedef struct multi_emulator {
	int nt;
	int nr;
	int nparams;
	int nmodel_points;
	int nregression_fns;
	int nthetas;
	multi_modelstruct *model;
	emulator_struct **emu_struct_array;

} multi_emulator;

multi_emulator *alloc_multi_emulator(multi_modelstruct *model);

void free_multi_emulator(multi_emulator *e);

void estimate_multi(multi_modelstruct *m, FILE* outfp);

void emulate_point_multi(multi_emulator *emu, gsl_vector *the_point,
												 gsl_vector *the_mean,
												 gsl_vector *the_variance);

void emulate_point_multi_covar(multi_emulator *emu, gsl_vector *the_point,
															 gsl_vector *the_mean, gsl_vector *the_variance,
															 gsl_matrix *the_covar);

void emulate_point_multi_pca(multi_emulator *emu, gsl_vector *the_point,
												 gsl_vector *the_mean,
												 gsl_vector *the_variance);

#endif