///  @file rainbow-gen-usersk.c
///  @brief A command-line tool for generating the user secret keys.

#include <rainbow_keypair.h>
#include <stdlib.h>
#include <stdio.h>
#include <api.h>
#include <utils.h>
#include <string.h>
#include <blas.h>

int main(int argc, char **argv) {

    printf("%s\n", CRYPTO_ALGNAME);

    printf("msk size: %lu\n", CRYPTO_MASTER_SECRET_KEY_BYTES);
    printf("mpk size: %lu\n", CRYPTO_MASTER_PUBLIC_KEY_BYTES);
    printf("hash size: %d\n", _HASH_LEN);
    printf("signature size: %d\n\n", CRYPTO_BYTES);

    if (!(4 == argc || 5 == argc)) {
        printf("Usage:\n\n\trainbow-gen-usersk msk_file_name identity usk_file_name [upk_file_name]\n\n");
        return -1;
    }

    uint8_t *_msk = malloc(CRYPTO_MASTER_SECRET_KEY_BYTES);
    FILE *fp;
    unsigned r;

    fp = fopen(argv[1], "r");
    if (NULL == fp) {
        printf("fail to open master key file.\n");
        return -1;
    }
    r = byte_fget(fp, _msk, CRYPTO_MASTER_SECRET_KEY_BYTES);
    fclose(fp);
    if (CRYPTO_MASTER_SECRET_KEY_BYTES != r) {
        printf("fail to load key file.\n");
        return -1;
    }

    unsigned char *_identity = malloc((_ID + 1) / 2);
    r = 0;

    generate_identity_hash(_identity, (unsigned char *) argv[2], strlen(argv[2]));
    if (NULL == _identity) {
        printf("fail to create identity hash.\n");
        return -1;
    }
    printf("identity hash: ");
    for (unsigned i = 0; i < _ID; i++) {
        printf("%hhu ", gf16v_get_ele(_identity, i));
    }
    printf("\n\n");

    uint8_t *_usk = malloc(sizeof(usk_t));

    if (argc == 4) {
        //calculate usk with mpk and ID
        int re = calculate_usk((usk_t *) _usk, (msk_t *) _msk, _identity);
        if (0 != re) {
            printf("%s generate user-secret-key fails.\n", CRYPTO_ALGNAME);
            return -1;
        }
    } else {
        //calculate upk from usk (for debugging), if filename is given
        uint8_t *_upk = malloc(sizeof(upk_t));
        int re = calculate_usk_and_upk((usk_t *) _usk, (upk_t *) _upk, (msk_t *) _msk, _identity);
        if (0 != re) {
            printf("%s generate upk and usk fails.\n", CRYPTO_ALGNAME);
            return -1;
        }

        //write upk to disk
        fp = fopen(argv[4], "w+");
        if (NULL == fp) {
            printf("fail to write user public key file.\n");
            return -1;
        }
        byte_fdump(fp, CRYPTO_ALGNAME " user-public-key", _upk,
                   CRYPTO_USER_PUBLIC_KEY_BYTES); //upk speichern und beschriften
        fclose(fp);

        free(_upk);
    }

    //write usk to disk
    fp = fopen(argv[3], "w+");
    if (NULL == fp) {
        printf("fail to write user secret key file.\n");
        return -1;
    }
    byte_fdump(fp, CRYPTO_ALGNAME " user secret key", _usk, sizeof(usk_t)); //usk speichern und beschriften
    fclose(fp);

    //free usk and rest

    free(_msk);
    free(_identity);
    free(_usk);

    return 0;
}


/* 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:  current_tdel.c
 *
 *    Description:  Stationary current as a function of T/D
 *
 *        Version:  1.0
 *        Created:  10/05/2014 19:52:35
 *       Revision:  none
 *        License:  BSD
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:  Università degli Studi di Trieste
 *
 *
 */

#include <gsl/gsl_ieee_utils.h>
#include <stdlib.h>
#include <stdio.h>
#include "funcs.h"
#include "initial.h"

/*
 *      FUNCTION
 *         Name:  current_red_T
 *  Description:  Given the physical parameters and the ratio x = T/Delta,
 *  		  determine the generators matrix in Redfield case
 *  		  and the stationary current.
 *
 */
double current_red_T ( double x, void* params )
{
	struct f_params* pars = (struct f_params*) params ;

	/* Calculate beta and set into pars */
	double Temp = x*D ;
	pars->beta = 1.0/Temp ;

	/* Calculate the stationary current */
	gsl_vector* stat_state = gsl_vector_calloc (4) ;
	gsl_matrix* red_m = gsl_matrix_calloc ( 4, 4 ) ;
	red_mat ( red_m, pars ) ;

	stationary ( red_m, stat_state ) ;

	double curr = -VECTOR(stat_state,3)*OMEGA/pars->omega_1 ;

	gsl_matrix_free (red_m) ;

	return curr ;
}		/* -----  end of function current_red_T  ----- */

/*
 *      FUNCTION
 *         Name:  current_cp_T
 *  Description:
 *
 */
double current_cp_T ( double x, void* params )
{
	struct f_params* pars = (struct f_params*) params ;

	/* Calculate beta and set into pars */
	double Temp = x*D ;
	pars->beta = 1.0/Temp ;

	/* Calculate the stationary current */
	gsl_vector* stat_state = gsl_vector_calloc (4) ;
	gsl_matrix* cp_m = gsl_matrix_calloc ( 4, 4 ) ;
	cp_mat ( cp_m, pars ) ;

	stationary ( cp_m, stat_state ) ;

	double curr = -VECTOR(stat_state,3)*OMEGA/pars->omega_1 ;

	gsl_matrix_free (cp_m) ;

	return curr ;
}		/* -----  end of function current_cp_T  ----- */

/*
 *      FUNCTION
 *         Name:  write_red_curr_T
 *  Description:
 *
 */
int write_red_curr_T ( void* params )
{
	FILE* f = fopen ( "RED-STAT-CURR-T.dat" , "w" ) ;

	double x = 0.00 ;

	int i ;
	for ( i = 0 ; i < 1000 ; i++ )
	{
		x += 0.001 ;
		fprintf ( f, "%.3f %.9f\n", x, current_red_T (x, params) ) ;
	}

	for ( i = 0 ; i < 900 ; i++ )
	{
		x += 0.01 ;
		fprintf ( f, "%.2f %.9f\n", x, current_red_T (x, params) ) ;
	}

	for ( i = 0 ; i < 900 ; i++ )
	{
		x += 0.1 ;
		fprintf ( f, "%.1f %.9f\n", x, current_red_T (x, params) ) ;
	}

	fclose (f) ;

	return 0;
}		/* -----  end of function write_red_curr_T  ----- */

/*
 *      FUNCTION
 *         Name:  write_cp_curr_T
 *  Description:
 *
 */
int write_cp_curr_T ( void* params )
{
	FILE* f = fopen ( "CP-STAT-CURR-T.dat" , "w" ) ;

	double x = 0.00 ;

	int i ;
	for ( i = 0 ; i < 1000 ; i++ )
	{
		x += 0.001 ;
		fprintf ( f, "%.3f %.9f\n", x, current_cp_T (x, params) ) ;
	}

	for ( i = 0 ; i < 900 ; i++ )
	{
		x += 0.01 ;
		fprintf ( f, "%.2f %.9f\n", x, current_cp_T (x, params) ) ;
	}

	for ( i = 0 ; i < 900 ; i++ )
	{
		x += 0.1 ;
		fprintf ( f, "%.1f %.9f\n", x, current_cp_T (x, params) ) ;
	}

	fclose (f) ;

	return 0;
}		/* -----  end of function write_cp_curr_T  ----- */

/*
 *      FUNCTION
 *         Name:  main
 *  Description:
 *
 */
int main ( int argc, char *argv[] )
{
	double omega_1 = gsl_hypot(OMEGA,D) ;   /* omega' */

	struct f_params params;
	params.omega_c = omega_c ;
	params.beta = 1 ;                       /* it will be changed after... */
	params.Omega = OMEGA ;
	params.omega_1 = omega_1 ;
	params.alpha = alpha ;

	int status1 = write_red_curr_T ( &params ) ;
	int status2 = write_cp_curr_T ( &params ) ;

	return status1+status2 ;
}				/* ----------  end of function main  ---------- */


/**
* Various binning and weighting routines for aperture pixel tables.
*/

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

#include "aXe_grism.h"
#include "aper_conf.h"
#include "spc_driz.h"
#include "spce_output.h"
#include "spc_optimum.h"
#include "spce_binning.h"

#define SQR(x) ((x)*(x))
#define MIN(x,y) (((x)<(y))?(x):(y))
#define MAX(x,y) (((x)>(y))?(x):(y))

#define DEBUG_ME 0x200

#define PIXWEIGHT(x1,y1,x2,y2,pix) ((pix)->weight_function((x1), (y1), (x2),\
  (y2),(pix)))
#define NAIVE_VAL_TO_BININD(x) ((int)floor((x)+1e-6))

/**
 * Function: add_to_spec_table
 * adds some count to an entry in the spectrum table
 *
 * Parameters:
 * @param spec the spectrum table to work on
 * @param bin index of spectrum table entry to add cur_p to
 * @param cur_p the ap_pixel to add
 * @param weight weight of the count
 */
static void
add_to_spec_table (spectrum * const spec, const int bin,
		   const ap_pixel * const cur_p, const int quant_cont,
		   const double weight)
{

  spc_entry *const sp_e = spec->spec + bin;

  // some conditions which should not be violated
  if ((bin < 0) || (bin > spec->spec_len))
    {
      aXe_message (aXe_M_WARN4, __FILE__, __LINE__,
		   "Assignment out of spectrum: %d", bin);
      return;
    }
  if (weight < 0)
    {
      aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
		   "Weight cannot be negative " "but is %f", weight);
    }

  // check whether the spectral element is new
  // and without values up to now
  if (isnan (sp_e->lambda_mean))
    {

      // initialize the spectral element
      sp_e->lambda_mean = cur_p->lambda;
      sp_e->dlambda = cur_p->dlambda;
      sp_e->lambda_max = cur_p->lambda;
      sp_e->lambda_min = cur_p->lambda;
      sp_e->weight = weight;
      sp_e->count = cur_p->count * weight;
      sp_e->error = fabs(cur_p->error) * weight;
      sp_e->dq = cur_p->dq;

      // initialize the contamination
      // this is different for quantitative and
      // geometrical contamination
      if (quant_cont)
	{
	  if ((int)(sp_e->contam==-1)&&((int)cur_p->contam!=-1))
	    {
	      sp_e->contam = cur_p->contam * weight;
	    }
	}
      else
	{
	  if ((int)(sp_e->contam==-1)&&((int)cur_p->contam!=-1))
	    {
	      sp_e->contam = cur_p->contam;
	    }
	}
    }
  else
    {

      // update an existing spectral bin

      // find new maxima and minima
      sp_e->lambda_max = MAX (cur_p->lambda, sp_e->lambda_max);
      sp_e->lambda_min = MIN (cur_p->lambda, sp_e->lambda_min);

      // find new mean lambda via weighted summation
      sp_e->lambda_mean =
	(sp_e->lambda_mean * sp_e->weight +
	 cur_p->lambda * weight) / (weight + sp_e->weight);

      // NEWNEWNEW::
      // find dlambda via weighted summation
      sp_e->dlambda =
	(sp_e->dlambda * sp_e->weight +
	 cur_p->dlambda * weight) / (weight + sp_e->weight);

      // add the weight
      sp_e->weight += weight;

      // add the counts
      sp_e->count += cur_p->count * weight;

      // process the error
      sp_e->error = sqrt (SQR (sp_e->error) + SQR (fabs(cur_p->error) * weight));

      // logically XOR the dq
      sp_e->dq = (sp_e->dq | cur_p->dq);

      // update the contamination,
      // take into account the quantitative
      // contamination
      if (quant_cont)
	{
	  if (((int)sp_e->contam==-1)&&((int)cur_p->contam!=-1))
	    {
	      sp_e->contam = cur_p->contam * weight;
	    }
	  if (((int)sp_e->contam!=-1)&&((int)cur_p->contam!=-1))
	    {
	      sp_e->contam += cur_p->contam * weight;
	    }
	}
      else
	{
	  if (((int)sp_e->contam==-1)&&((int)cur_p->contam!=-1))
	    {
	      sp_e->contam = cur_p->contam;
	    }
	  if (((int)sp_e->contam!=-1)&&((int)cur_p->contam!=-1))
	    {
	      sp_e->contam += cur_p->contam;
	    }
	}
    }
}

/**
 * Function: pixweight_x
 * computes a weight for the bin between coordinates (x1,y1) and (x2,y2)
 * if the pi/4<=angle<=3 pi/4.
 *
 * Parameters:
 * @param x1 - x coordinate for the start point of the bin on the trace
 * @param y1 - y coordinate for the start point of the bin on the trace
 * @param x2 - x coordinate for the end point of the bin on the trace
 * @param y2 - y coordinate for the end point of the bin on the trace
 * @param pix the pixel to compute the weight for in the form of a
 *            w_pixel structure filled out by fill_w_pixel
 *
 * Returns:
 * @return sum - the pixel weight
 */
double
pixweight_x (const double x1, const double y1, const double x2,
	     const double y2, const struct w_pixel_s *const pix)
{
  double a, b;
  double sum = 0;

  a = x1 - pix->cota * (y1 - pix->y0);
  b = x2 - pix->cota * (y2 - pix->y0);
  if ((b >= pix->p0) && (a <= pix->p1))
    {
      sum += pix->slope / 2 * (MIN (b, pix->p1) - MAX (a, pix->p0))
	* (MAX (a, pix->p0) - 2 * pix->p0 + MIN (b, pix->p1))
	* sin (pix->angle);
    }
  if ((b >= pix->p1) && (a <= pix->p2))
    {
      sum += pix->fmax * (MIN (b, pix->p2) - MAX (a, pix->p1))
	* sin (pix->angle);
       }
  if ((b >= pix->p2) && (a <= pix->p3))
    {
      sum += pix->slope / 2 * (MAX (a, pix->p2) - MIN (b, pix->p3))
	* (MAX (a, pix->p2) - 2 * pix->p3 + MIN (b, pix->p3))
	* sin (pix->angle);
    }
  return sum;
}

/**
 * Funtion: pixweight_y
 * Computes a weight for the bin between coordinates (x1,y1) and (x2,y2)
 * if the angle<=pi/4 or 3 pi/4<=angle.
 *
 * @param x1 - x coordinate for the start point of the bin on the trace
 * @param y1 - y coordinate for the start point of the bin on the trace
 * @param x2 - x coordinate for the end point of the bin on the trace
 * @param y2 - y coordinate for the end point of the bin on the trace
 * @param pix - the pixel to compute the weight for in the form of a
 *              w_pixel structure filled out by fill_w_pixel
 *
 * Returns:
 * @return sum - the pixel weight
 */
double
pixweight_y (const double x1, const double y1, const double x2,
	     const double y2, const struct w_pixel_s *const pix)
{
  double a, b;
  double sum = 0;

  a = y1 + pix->tana * (pix->x0 - x1);
  b = y2 + pix->tana * (pix->x0 - x2);
  if (a > b)
    {
      double tmp;
      tmp = a;
      a = b;
      b = tmp;
    }

  if ((b >= pix->p0) && (a <= pix->p1))
    {
      sum += pix->slope / 2 * (MIN (b, pix->p1) - MAX (a, pix->p0))
	* (MAX (a, pix->p0) - 2 * pix->p0 + MIN (b, pix->p1))
	* cos (pix->angle);
    }
  if ((b >= pix->p1) && (a <= pix->p2))
    {
      sum += pix->fmax * (MIN (b, pix->p2) - MAX (a, pix->p1))
	* cos (pix->angle);
    }
  if ((b >= pix->p2) && (a <= pix->p3))
    {
      sum += pix->slope / 2 * (MAX (a, pix->p2) - MIN (b, pix->p3))
	* (MAX (a, pix->p2) - 2 * pix->p3 + MIN (b, pix->p3))
	* cos (pix->angle);
    }
  return sum;
}

/**
 * Function: fill_w_pixel
 * precomputes some properties of a given pixel for purposes
 * of computing the weights it contributes to a given xi bin.
 *
 * Parameters:
 * @param pix a pointer to the w_pix structure to fill
 * @param x0 the x coordinate of the pixel's lower left corner
 * @param y0 the y coordinate of the pixel's lower left corner
 * @param size the size of the pixel
 * @param angle the orientation of the object that has generated the
 *        spectrum
 */
/* since we're only interested in weights here, the size of the square
 *  doesn't matter.  Where I left explicit ones, you could write size
 * and get the true area instead of the fraction of the total area.
 * Dunno why you'd want to do this, though.
 */
static void
fill_w_pixel (w_pixel * const pix, const double x0, const double y0,
	      const double angle)
{
  pix->tana = tan (angle);
  pix->cota = 1 / pix->tana;
  pix->angle = angle;
  pix->x0 = x0;
  pix->y0 = y0;

  if ((angle >= M_PI / 4) && (angle <= 3 * M_PI / 4))
    {
      pix->p0 = MIN (x0, x0 - 1 * pix->cota);
      pix->p1 = MAX (x0, x0 - 1 * pix->cota);
      pix->p2 = MIN (x0 + 1, x0 + 1 - 1 * pix->cota);
      pix->p3 = MAX (x0 + 1, x0 + 1 - 1 * pix->cota);
      pix->fmax = 1 / sin (angle);

      if (fabs (pix->p1 - pix->p0) < 1e-7)
	pix->slope = 0;
      else
	pix->slope = pix->fmax / (pix->p1 - pix->p0);

      pix->weight_function = &pixweight_x;
    }
  else
    {			/* angle between 0 and pi/4 or 3*pi/4 and pi */
      pix->p0 = MIN (y0, y0 - 1 * pix->tana);
      pix->p1 = MAX (y0, y0 - 1 * pix->tana);
      pix->p2 = MIN (y0 + 1, y0 + 1 - 1 * pix->tana);
      pix->p3 = MAX (y0 + 1, y0 + 1 - 1 * pix->tana);
      pix->fmax = 1 / cos (angle);

      if (fabs (pix->p1 - pix->p0) < 1e-7)
	pix->slope = 0;
      else
	pix->slope = pix->fmax / (pix->p1 - pix->p0);

      pix->weight_function = &pixweight_y;
    }
}

/**
 * Function: bin_naive
 * computes a spectrum from a table of aperture pixels generated from
 * spc_extract.  This is adds up the pixel values, distributing them
 * over the trace, taking into account the fracton of the pixel that
 * projects onto the given [xi,xi+1] interval.
 * Return NULL if ap_p is NULL
 *
 * Parameters:
 * @param ap_p the table of aperture pixels
 * @param px_width width of a pixel (=1 if not subsampled)
 * @param ob_width the width of the object that has generated the spectrum
 * @param ob_orientation the orientation of the object that has
 *             generated the spectrum
 * @param flags problems that were accumulated in generating ap_p;
 *   possible flags are defined for the warning member of the spectrum struct
 *
 * Returns:
 * @return spec - the 1D spectrum
 */
spectrum *
bin_naive (const ap_pixel * const ap_p, const double ob_width,
	   const double ob_orient, const int quant_cont)
{
  const ap_pixel *cur_p;
  int upper, lower;

  int bin;
  spectrum *spec, *tspec;
  double phi_trace;
  spc_entry *spec_table;
  double d, frac;

  // immediately return empty PET's
  if (ap_p==NULL)
    return NULL;

  // go through the PET,
  // find the minimum and
  // maximum in trace distance
  cur_p = ap_p;
  upper = NAIVE_VAL_TO_BININD (cur_p->xi);
  lower = NAIVE_VAL_TO_BININD (cur_p->xi);
  while (cur_p->p_x != -1)
    {
      bin = NAIVE_VAL_TO_BININD (cur_p->xi);
      upper = MAX (bin, upper);
      lower = MIN (bin, lower);
      cur_p++;
    }

  // check whether the spectrum
  // will ahve a finite length,
  // exit if not
  if (upper == lower)
    {
      aXe_message (aXe_M_WARN4, __FILE__, __LINE__,
		   "Pixel table empty.\n");
      return NULL;
    }

  // Thresh in some headway so we don't need to worry too much about
  //     accessing invalid elements now and then
  lower -= 10;
  upper += 10;
  spec = allocate_spectrum (upper - lower);

  spec_table = spec->spec;
  cur_p = ap_p;

  while (cur_p->p_x != -1)
    {
      // Compute any fractional pixel that might fall within the
      //desired extraction width
      d = ob_width;
      frac = 1.;

      // continue if the pixel is outside
      // of the extraction region
      if (fabs (cur_p->dist) > d + .5)
	{
	  cur_p++;
	  continue;
	}
      if ((fabs (cur_p->dist) >= d - .5) && (fabs (cur_p->dist) <= d + .5))
	{
	  frac = fabs (d - (fabs (cur_p->dist) - 0.5));
	}

      // store the local trace angle
      phi_trace = cur_p->dxs;

      if (1)
	{
	  double xi;
	  w_pixel pix;
	  double sinp = sin (phi_trace);
	  double cosp = cos (phi_trace);
	  double xc, yc;
	  double w;
	  double sum = 0;

	  fill_w_pixel (&pix, cur_p->x,	cur_p->y, ob_orient);

	  xc = cur_p->xs;
	  yc = cur_p->ys;

	  // at cur_p->xi, there has to be some contribution. We go back
	  // collecting, until w is zero
	  for (xi = cur_p->xi;; xi -= 1)
	    {
	      bin = NAIVE_VAL_TO_BININD (xi);
	      w = PIXWEIGHT (xc + (bin - cur_p->xi) * cosp,
			     yc + (bin - cur_p->xi) * sinp,
			     xc + (bin + 1 - cur_p->xi) * cosp,
			     yc + (bin + 1 - cur_p->xi) * sinp,
			     &pix);
	      if (w < 1e-10)
		break;

	      add_to_spec_table (spec, bin - lower, cur_p, quant_cont,
				 w * frac * cur_p->weight);
	      //	      if (cur_p->weight > 10.0){
	      //		fprintf(stdout,"weight: %f, distance: %f, ewidth: %f\n",
	      //			cur_p->weight, cur_p->dist, d+0.5);
	      //	      }
	      sum += w;
	    }

	  /* Now collect contributions upward of cur_p->xi */
	  for (xi = cur_p->xi + 1;; xi += 1)
	    {
	      bin = NAIVE_VAL_TO_BININD (xi);

	      w = PIXWEIGHT (xc + (bin - cur_p->xi) * cosp,
			     yc + (bin - cur_p->xi) * sinp,
			     xc + (bin + 1 - cur_p->xi) * cosp,
			     yc + (bin + 1 - cur_p->xi) * sinp,
			     &pix);
	      if (w < 1e-10)
		break;

	      add_to_spec_table (spec, bin - lower, cur_p, quant_cont,
				 w * frac * cur_p->weight);
	      sum += w;
	    }

	  if (fabs (sum - 1) > 1e-6)
	    {
	      fprintf(stdout,
		      "Weights added up to only %f for pixel from %4d,%4d\n",
		      sum, cur_p->p_x, cur_p->p_y);
	    }
	}
      cur_p++;
    }

  /* Trimming the INDEF beginning and ending values in spectrum */
  tspec = trim_spectrum (spec);
  free_spectrum (spec);
  spec = NULL;

  // return the spectrum
  return tspec;
}

/**
 * Function: bin_optimal
 * computes a spectrum from a table of aperture pixels generated from
 * spc_extract.  This is adds up the pixel values, distributing them
 * over the trace, taking into account the fracton of the pixel that
 * projects onto the given [xi,xi+1] interval.
 * Return NULL if ap_p is NULL
 *
 * Parameters:
 * @param ap_p the table of aperture pixels
 * @param px_width width of a pixel (=1 if not subsampled)
 * @param ob_width the width of the object that has generated the spectrum
 * @param ob_orientation the orientation of the object that has
 *             generated the spectrum
 * @param flags problems that were accumulated in generating ap_p;
 *   possible flags are defined for the warning member of the spectrum struct
 *
 * Returns:
 * @return spec - the 1D spectrum
 */
spectrum *
bin_optimal (const ap_pixel * const ap_p, const beam curbeam,
	     const int quant_cont, const gsl_matrix *weights,
	     const drzstamp_dim dimension,gsl_matrix *coverage)
{

  const ap_pixel *cur_p;
  ap_pixel *tmp_p;
  spectrum *spec;
  spectrum *tspec;
  spc_entry *spec_table;

  quadrangle quad;
  //  drzstamp_dim dimension;

  double jacob, arr;
  double frac, totweight;

  int jcen, icen;
  int jupp, iupp;
  int jlow, ilow;

  int ii, jj;
  int stpi, stpj;

  // return NULL if
  // empty PET
  if (ap_p==NULL)
    return NULL;

  // allocate memory
  tmp_p = (ap_pixel *) malloc(sizeof(ap_pixel));

  // allocate memory for the spectrum
  spec = allocate_spectrum (weights->size1);
  spec_table = spec->spec;

  // go over each PET pixel
  cur_p = ap_p;
  for (cur_p = ap_p; cur_p->p_x != -1; cur_p++)
    {

      // continue if the pixel is outside
      // of the extraction region
      if (fabs (cur_p->dist) > curbeam.width + .5)
	  continue;

      // determine which fraction
      // of the pixel is inside of the extraction area
      if ((fabs (cur_p->dist) >= curbeam.width - .5) && (fabs (cur_p->dist) <= curbeam.width + .5))
	frac = fabs (curbeam.width - (fabs (cur_p->dist) - 0.5));
      else
	frac = 1.;

      // transfer values to the temporary pixel
      tmp_p->lambda  = cur_p->xi;
      tmp_p->dist    = cur_p->dist;
      tmp_p->dxs     = cur_p->dxs;
      tmp_p->dlambda = 1.0;

      // create the quadrangle for the current pixel
      quad = get_quad_from_pixel(tmp_p, curbeam.orient, dimension);

      // get the jacobian (well, easy here)
      // the term "cos(cur_p->dxs)" must be there
      // to correct the enlargement necessary
      // to cover the whole lambda-crossdispersion area!
      // NOT COMPLETELY understood
      jacob = cos(cur_p->dxs);

      // get the central pixel (icen, jcen) of the current PET-pixel
      icen = (int) floor(cur_p->xi   - dimension.xstart+.5);
      jcen = (int) floor(cur_p->dist - dimension.ystart+.5);

      // get the uper and lower extend of the quadrangle in x
      iupp = (int)floor(quad.xmax - (double)icen + 0.5)+1;
      ilow = (int)floor(quad.xmin - (double)icen + 0.5);

      // get the uper and lower extend of the quadrangle in x
      jupp = (int)floor(quad.ymax - (double)jcen + 0.5)+1;
      jlow = (int)floor(quad.ymin - (double)jcen + 0.5);

      // go over the extend in x
      for (ii=ilow;ii<iupp;ii++)
	{
	  // go over the extend in x
	  for (jj=jlow;jj<jupp;jj++)
	    {

	      // get the coordinates of the current output pixel
	      stpi = icen+ii;
	      stpj = jcen+jj;

	      // check whether the current output pixel is within
	      // the stamp image; continue if not
	      if ( (stpi>=dimension.xsize)||(stpi<0)||(stpj>=dimension.ysize)||(stpj<0) )
		continue;

	      // get the area which falls onto the current output pixel
	      arr = boxer(stpi,stpj,quad.x,quad.y);

	      // compute the pixel weight from
	      // the various inputs
	      totweight =  arr*frac*jacob*gsl_matrix_get(weights, stpi, stpj);
	      //totweight =  arr*frac*jacob;//*gsl_matrix_get(weights, stpi, stpj);

	      //gsl_matrix_set(coverage, stpi, stpj, gsl_matrix_get(coverage, stpi, stpj) + arr*frac*jacob);

	      // add the pixel to the spectrum
	      if (totweight > 0.0)
		add_to_spec_table (spec, stpi, cur_p, quant_cont,totweight);
	      //	      gsl_matrix_set(coverage, stpi, stpj, sqrt (SQR (gsl_matrix_get(coverage, stpi, stpj)) + SQR (fabs(cur_p->error) * totweight)));
	    }
	}
    }

  /* Trimming the INDEF beginning and ending values in spectrum */
  tspec = trim_spectrum (spec);
  free_spectrum (spec);
  spec = NULL;

  free(tmp_p);

  // return the spectrum
  return tspec;
}

/**
  does a straight forward summation/binning of an aperture pixel table
  with appropriate weights (cf. Hornes 1986)

  @param ap_p the table of aperture pixels
  @param ob_orient the orientation of the object that has
    generated the spectrum
  @param flags problems that were accumulated in generating ap_p;
    possible flags are defined for the warning member of the spectrum struct
  @param n_sub subsampling factor
  @return an allocated spectrum
  @see spectrum
*/

spectrum *
bin_weighted (const ap_pixel * const ap_p, const double ob_orient,
	      const trace_func * const trace, const int n_sub,
	      const int flags)
{
     gsl_vector *binned_table;
     gsl_vector *wei_table;
     gsl_vector *wei2_table;

     double xi, d, w, wei, wei2;
     int xii, num_bin, bin;
     const ap_pixel *cur_p = ap_p;
     double upper = cur_p->xi, lower = cur_p->xi;
     spectrum *spec;

     while (cur_p->p_x != -1)
       {
	    upper = MAX (cur_p->xi, upper);
	    lower = MIN (cur_p->xi, lower);
	    cur_p++;
       }

     lower -= 10;
     upper += 10;

     lower = floor (lower);
     upper = floor (upper + 1);

     num_bin = floor ((upper - lower) / n_sub);
     spec = allocate_spectrum (num_bin);

     binned_table = gsl_vector_alloc (num_bin);
     wei_table = gsl_vector_alloc (num_bin);
     wei2_table = gsl_vector_alloc (num_bin);

     gsl_vector_set_all (binned_table, 0);
     gsl_vector_set_all (wei_table, 0);
     gsl_vector_set_all (wei2_table, 0);

     cur_p = ap_p;
     while (cur_p->p_x != -1)
       {
	    xi = (cur_p->xi - lower) / n_sub;
	    xii = floor (xi);

	    d = fabs(cur_p->dist);
	    w = exp (-d * d / (2 * 6.66));
	    w = 1.;
	    add_to_spec_table (spec, xii - 1,cur_p, 0, w * (1 - (xi - xii)));
	    add_to_spec_table (spec, xii   , cur_p, 0, w * (xi - xii));

	    gsl_vector_set (wei_table, xii - 1,
			    gsl_vector_get (wei_table, xii - 1) + w);
	    gsl_vector_set (wei_table, xii,
			    gsl_vector_get (wei_table, xii) + w);

	    gsl_vector_set (wei2_table, xii - 1,
			    gsl_vector_get (wei2_table, xii - 1) + w * w);
	    gsl_vector_set (wei2_table, xii,
			    gsl_vector_get (wei2_table, xii) + w * w);

	    //fprintf(stderr,"%f,%d,%d\n",xi,xii,xii+1);
	    cur_p++;
       }
     cur_p = ap_p;
     for (bin = 0; bin < num_bin; bin++)
       {
	    wei = gsl_vector_get (wei_table, bin);
	    wei2 = gsl_vector_get (wei2_table, bin);
	    if (wei2 != 0)
	      {
		   spec->spec[bin].count = spec->spec[bin].count * wei / wei2;
	      }
       }

     spec->warning = 0;
     return spec;
}


/* bcsolver.c
   $Revision: 283 $ $Date: 2006-12-17 20:27:56 -0800 (Sun, 17 Dec 2006) $

   ----------------------------------------------------------------------
   This file is part of BCLS (Bound-Constrained Least Squares).

   Copyright (C) 2006 Michael P. Friedlander, Department of Computer
   Science, University of British Columbia, Canada. All rights
   reserved. E-mail: <mpf@cs.ubc.ca>.

   BCLS is free software; you can redistribute it and/or modify it
   under the terms of the GNU Lesser General Public License as
   published by the Free Software Foundation; either version 2.1 of the
   License, or (at your option) any later version.

   BCLS is distributed in the hope that it will be useful, but WITHOUT
   ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
   or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General
   Public License for more details.

   You should have received a copy of the GNU Lesser General Public
   License along with BCLS; if not, write to the Free Software
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
   USA
   ----------------------------------------------------------------------
*/
/*!
   \file
   The main algorithm for the BCLS solver.
*/

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

#include "bcls.h"
#include "bclib.h"
#include "bccgls.h"
#include "bclsqr.h"
#include "bcsolver.h"

// =====================================================================
// Private (static) function declarations.
// =====================================================================

// Compute a Newton step on the free variables, dx(free).
static int
bcls_newton_step( BCLS *ls, int m, int nFree, int ix[], double damp,
		  int itnLim, double tol, double dxFree[], double x[],
		  double c[], double r[], int *itns, double *rTol );

// Find the minimizer along the projected search direction.
static int
bcls_proj_search( BCLS *ls, int m, int n, double x[], double dx[],
		  double f, double g[], double aBreak[], int iBreak[],
		  int ix[], double bl[], double bu[],
		  double ex[], double Ad[], double Ae[], int *totHits );

// Simple backtracking to satisfy an Armijo condition.
static int
bcls_proj_backtrack( BCLS *ls, int m, int n, double x[], double dx[],
		     double f, double g[], double bl[], double bu[],
		     double s[], int ix[], double As[], int *nSteps );

/*!

  \brief Implementation of the BCLS projected Newton/gradient search.

  \param[in,out]  ls     The BCLS problem context.
  \param[in]      m      Number of rows in A.
  \param[in]      n      Number of columns in A.
  \param[in,out]  bNorm  Norm of the RHS.
  \param[in,out]  x      The current iterate.
  \param[in]      b      The RHS vector.
  \param[in]      c      The linear-term vector.
  \param[in]      bl     Lower bounds on x.
  \param[in]      bu     Upper bounds on x.
  \param[in,out]  r      The residual vector r = b - Ax.
  \param[in,out]  g      The objective gradient.
  \param[in,out]  dx     The step direction in the full space.
  \param[in,out]  dxFree The step direction on the free variables.
  \param[in,out]  ix     Indices of the free variables.
  \param[in,out]  aBreak A list of breakpoints for a search direction.
  \param[in,out]  iBreak The indices of the breakpoints.
  \param[in,out]  anorm  Column norms of A.

*/
void
bcls_solver( BCLS *ls, int m, int n, double *bNorm,
	     double x[], double b[], double c[], double bl[], double bu[],
	     double r[], double g[], double dx[], double dxFree[],
	     int ix[], double aBreak[], int iBreak[], double anorm[] )
{
    int
        err = 0,       // Error indicator.
        j,             // Misc. counter.
        jInf,          // Variable with largest dual infeasibility.
        itnLimk,       // No. of allowed itns for kth subproblem.
        itnsk = 0,     // No. of iterations performed on kth subproblem.
        nFree,         // No. of variables floating between their bounds.
        nSteps = 0;    // No. of projected search steps.

    double
        optk = 0.0,    // kth     subproblem achieved optimality.
        optkm1 = 0.0,  // (k-1)th ...
        subTol,        // Required subproblem optimality.
        rTol = 0.1,    // Inexact Newton optimality reduction.
        dInf,          // Dual infeasibility.
        rNorm,         // Norm of the residual.
        xNorm,         // Norm of the current solution estimate.
        f;             // Objective value = 1/2 * rNorm^2.

    const int scaled_steepest = anorm != NULL;
    const int linear     = c != NULL;
    const int damped     = ls->damp > 0.0;
    const double damp    = ls->damp;
    const double damp2   = damp*damp;
    const double rTolMin = ls->optTol;

    // Initialize various variables.
    ls->itnMaj = 0;    // Reset the major iteration counter.
    ls->itnMin = 0;    // Reset the minor iteration counter.

    // Push x into the box.
    bcls_mid( n, x, bl, bu );

    // Convergence will be normalized by ||A'b|| + ||c||.
    for (j = 0; j < n; j++) ix[j] = j;
    bcls_aprod( ls, BCLS_PROD_At, n, ix, g, b );
    const double AbNorm = cblas_dnrm2( n, g, 1 );             // ||A'b||
    const double cnorm  = (c) ? cblas_dnrm2( n, c, 1 ) : 0.0; // ||  c||
    *bNorm = cblas_dnrm2( m, b, 1 );                          // ||  b||

    // -----------------------------------------------------------------
    // Start BCLS major iterations.
    // -----------------------------------------------------------------
    while (1) {

        // Need to include contribution for *all* cols of A.
        for (j = 0; j < n; j++) ix[j] = j;

        // Evaluate the (unregularized) residual:
        // r(1:m) = b - Ax
        bcls_aprod( ls, BCLS_PROD_A, n, ix, x, r );  // r(1:m) =  Ax
        cblas_daxpy( m, -1.0, b, 1, r, 1 );          // r(1:m) = -b + Ax
        cblas_dscal( m, -1.0, r, 1 );                // r(1:m) =  b - Ax

        // Evaluate the gradient:
        // g = A'(Ax - b)  +  damp^2 x = -A'r + damp^2 x.
        bcls_aprod( ls, BCLS_PROD_At, n, ix, g, r ); // g =  A'r
        cblas_dscal( n, -1.0, g, 1 );                // g = -A'r
        if (damped)
            cblas_daxpy( n, damp2, x, 1, g, 1 );     // g = g + damp^2 x
        if (linear)
            cblas_daxpy( n, 1.0, c, 1, g, 1 );       // g = g + c

        // Norm of the current solution and residual.
        xNorm = cblas_dnrm2( n, x, 1 );              //  || x ||
        rNorm = cblas_dnrm2( m, r, 1 );              //  || r ||

        // Objective value:  f = 1/2 ||r||^2 + 1/2 damp^2 ||x||^2.
        f = 0.5 * rNorm * rNorm;
        if (damped)
            f = f + 0.5 * damp2 * xNorm * xNorm;
        if (linear)
            f = f + cblas_ddot( n, c, 1, x, 1 );

        // -------------------------------------------------------------
        // Evaluate optimality, active set, and exit conditions.
        // -------------------------------------------------------------

        // Store free variable indices in ix[:nFree].
        nFree = bcls_free_vars( ls, n, ix, x, g, bl, bu );

        // Evaluate optimality of the current iterate x.
        bcls_dual_inf( n, x, g, bl, bu, &dInf, &jInf );

        // Test for exit conditions.  Acted on below.
        if ( dInf <= ls->optTol * (1.0 + AbNorm + cnorm) ) {
            ls->exit      = BCLS_EXIT_CNVGD;
            ls->soln_stat = BCLS_SOLN_OPTIM;    // Optimal soln found.
        }
        else if ( ls->itnMaj >= ls->itnMajLim ) // Too many majors.
            ls->exit      = BCLS_EXIT_MAJOR;
        else if ( ls->itnMin >= ls->itnMinLim ) // Too many minors.
            ls->exit      = BCLS_EXIT_MINOR;
        else if ( bcls_callback( ls ) )         // Exit requested by user.
            ls->exit      = BCLS_EXIT_CALLBK;

        // -------------------------------------------------------------
        // Print out the "kth" iteration log.
        // -------------------------------------------------------------
        /*PRINT1(" %5d  %5d  %11.4e  %9.2e  %11.4e  %9.2e %7d %7d",
               ls->itnMaj, itnsk, rNorm, xNorm, dInf, optk, nSteps, nFree );

        // Check if rTol was "tight enough" for the last major.
        if ( ls->itnMaj <  1 ) {
            PRINT1(" %7.0e", rTol);
        }
        else if ( optk  <= .2 * optkm1 )
            ; // Relax. Optimality has made a reasonable reduction.
        else if ( rTol  <= rTolMin ) {
              // Inexact Newton factor is alrady small.
            PRINT1(" %7.0e", rTol);
        }
        else if ( itnsk <= 1 ) {
            rTol /= 10.0;
            PRINT1(" %7.0e", rTol);
        }

        // Finish the log (eg, print a new-line).
        PRINT1("\n");
	*/
        // Execute the exit condition (possibly set above).
        if ( ls->exit != BCLS_EXIT_UNDEF ) break;

        // =============================================================
        // ----  New major iteration starts here ----
        // =============================================================
        ls->itnMaj++;

        // Zero out the step direction: dx = 0.
        bcls_dload( n, 0.0, dx, 1 );

        // -------------------------------------------------------------
        // Compute a Newton step on the free variables: dxFree.
        // -------------------------------------------------------------
        if (nFree) {

            // Find out the number of remaining allowed minors.
            itnLimk = ls->itnMinLim - ls->itnMin;

            // Save the previous subproblem optimality value and
            // compute the required optimality for this next
            // subproblem.
            optkm1 = optk;
            if (ls->unconstrained)
                subTol = rTol * ls->optTol;
            else
                subTol = rTol;// * bcls_vec_dnorm2( nFree, ix, g );

            // Note that ls->dx is used as workspace.
            err = bcls_newton_step( ls, m, nFree, ix, damp, itnLimk,
                                    subTol, dxFree, x, c, r,
                                    &itnsk, &optk );

            // Accumulate the minor iteration count.
            ls->itnMin += itnsk;
        }

        // -------------------------------------------------------------
        // Load steepest descent into the fixed variables: dx(fixed).
        // -------------------------------------------------------------
        // Scale steepest descent if user provided column scales.
        // Note that this needs to be done *after* bcls_newton_step.
        cblas_dcopy( n, g, 1, dx, 1 );      //  dx <-  g
        cblas_dscal( n, -1.0, dx, 1 );      //  dx <- -g

        if ( scaled_steepest )
            for (j = 0; j < n; j++)
                dx[j] /= anorm[j] * anorm[j];

        // Load Newton step on free variables into dx.
        bcls_scatter( nFree, ix, dxFree, dx );

        // Projected search along dx.
        // Note that ix will be reset.
        if (ls->proj_search == BCLS_PROJ_SEARCH_EXACT)
            err = bcls_proj_search( ls, m, n, x, dx, f, g, aBreak, iBreak,
                                    ix, bl, bu,
                                    ls->wrk_u, ls->wrk_v, ls->wrk_w,
                                    &nSteps );
        else
            err = bcls_proj_backtrack( ls, m, n, x, dx, f, g, bl, bu,
                                       ls->wrk_v, ix, ls->wrk_u,
                                       &nSteps );

        // -------------------------------------------------------------
        // Check for linesearch failures.
        // -------------------------------------------------------------

        // Exit if linesearch found a direction of unbounded descent.
        if (err == BCLS_EXIT_UNBND) {
            ls->exit = err;
            break;
        }

        // Generic linesearch failure.  Further tests needed.
        else if (err == BCLS_EXIT_LFAIL) {

            // If using approx linesearch, revert to exact linesearch.
            if (ls->proj_search == BCLS_PROJ_SEARCH_APPROX) {
                ls->proj_search =  BCLS_PROJ_SEARCH_EXACT;
                PRINT1(" W: Too many approx backtrack iterations."
                       " Switching to exact linesearch.\n");
            }
            // Already using exact linesearch.  Exit.
            else {
                ls->exit = err;
                break;
            }
        }

    } // end of while (1) -- the main iteration loop.

    // Collect various solution statistics.
    ls->soln_obj   = f;
    ls->soln_rNorm = rNorm;
    ls->soln_jInf  = jInf;
    ls->soln_dInf  = dInf;

    // Exit.
    return;
}

/*!

  \brief Compute a Newton step on the free variables.

  Compute a Newton step on the variables indexed by ix.  The integer
  array ix (of length nFree) stores the indices of free variables.
  Thus, the kth free variable, with 0 <= k < nFree, is actually
  variable ix[k].

  Conceptually, suppose that x and A are partitioned as

      x = [ x_B  x_N ]   and   A = [ B  N ]

  with x_N denoting the free variables.  The objective function is
  given by

      f(x) = 1/2 || Ax - b ||^2 + c'x + 1/2 damp^2 || x ||^2.

  With respect to the free variables, the gradient and Hessian are

       g_N(x) = N'(Ax - b) + cF + damp^2 xF = -N'r + cF + damp^2 xF
  and  H_N(x) = N'N + damp^2 I,

  where
  -      r = b - Ax      is the usual residual vector.
  -     cF = c(free)     is the vector of linear terms indexed by ix.
  -     xF = x(free)     is the vector of variables    indexed by ix.
  -      I = eye(nFree).

  The (regularized) Newton step dx_N is then the solution of

                 H_N(x) dx_N = - g_N(x)                       (*)

  or equivalently,

       (N'N + damp^2 I) dx_N = N'r - cF - damp^2 xF,          (**)

  which is really the normal equations for the least-squares problem

         || (      N )       (           r           ) ||
     min || (        ) dx -  (                       ) ||.   (***)
         || ( damp I )       ( - 1/damp cF - damp xF ) ||

  Recall that  I  has dimension (nFree,nFree).

  Inexact Newton
  --------------
  An inexact Newton step is based on approximately satisfying (*).
  The optimality criteria for this step is given by

        || H_N(x) dx_N + g_N(x) || <= rTol || g_N(x) ||,     (****)

  where rTol < 1.  This routine (bcls_newton_step) computes the RHS
  of (****) and passes that to the subroutines as a required
  optimality.

  SUBROUTINES
  -----------
  This routine uses either one of the following
     -  CGLS (via bcls_newton_step_cgls) to solve (**)
     -  LSQR (via bcls_newton_step_lsqr) to solve (***).

  \param[in,out]  ls     The BCLS problem context.
  \param[in]      m      Number of rows in A.
  \param[in]      nFree  Number of columns in N.
  \param[in]      ix     Index of free variables (cols of N).
  \param[in]      damp   Sqrt of regularization parameter on x.
  \param[in]      itnLim Maximum number of minor iterations allowed.
  \param[in]      tol    Required subproblem optimality.
  \param[out]     dxFree The Newton step (solution of the LS subprob).
  \param[in]      x      The current iterate.
  \param[in]      c      The linear term.
  \param[in,out]  r      The current residual.
  \param[out]     itns   No. of iterations taken by the subprob solver.
  \param[out]     opt    The optimality of the subproblem solution.

  \return
  - 0 - Required accuracy was achieved.
  - 1 - The iteration limit was reached.
  - 2 - The matrix is excessively ill-conditioned.
  - 3 - Instability seems likely.

*/
static int
bcls_newton_step( BCLS *ls, int m, int nFree, int ix[], double damp,
		  int itnLim, double tol, double dxFree[], double x[],
		  double c[], double r[], int *itns, double *opt )
{
    int err;
    const int preconditioning = ls->Usolve != NULL;
    double *dx = ls->dx;  // Used as workspace.

    // -----------------------------------------------------------------
    // Initialize the Aprod routine, and the Usolve routine if given.
    // -----------------------------------------------------------------
    bcls_aprod ( ls, BCLS_PROD_INIT, nFree, ix, NULL, NULL );
    if (preconditioning)
        bcls_usolve( ls, BCLS_PRECON_INIT, nFree, ix, NULL, NULL );

    // -----------------------------------------------------------------
    // Print dividing line in the subproblem log.
    // -----------------------------------------------------------------
    if (ls->minor_file) {
        fprintf(ls->minor_file,
                "\n"
                "------------------------- "
                "BCLS Major Iteration: %6d "
                "------------------------- "
                "\n\n", ls->itnMaj);
        fflush(ls->minor_file);
    }

    // -----------------------------------------------------------------
    // Call one of the subproblem solvers.
    // -----------------------------------------------------------------
    if (ls->newton_step == BCLS_NEWTON_STEP_LSQR)

        err = bcls_newton_step_lsqr( ls, m, nFree, ix, damp,
                                     itnLim, tol, dxFree, x,
                                     c, r, itns, opt );

    else

        err = bcls_newton_step_cgls( ls, m, nFree, ix, damp,
                                     itnLim, tol, dxFree, x,
                                     c, r, itns, opt );

    // -----------------------------------------------------------------
    // If the user is preconditioning, then dxFree currently lives in
    // the preconditioned space.  We need one more call to Usolve to
    // recover dxFree in the original space.  Take this opportunity to
    // call Usolve in its termination mode.
    // -----------------------------------------------------------------
    if (preconditioning) {

        bcls_usolve( ls, BCLS_PRECON_U, nFree, ix, dx, dxFree );
        cblas_dcopy( nFree, dx, 1, dxFree, 1 );  //  dxFree <- dx

        bcls_usolve( ls, BCLS_PRECON_TERM, nFree, ix, NULL, NULL );
    }

    // Call Aprod in its termination mode.
    bcls_aprod ( ls, BCLS_PROD_TERM, nFree, ix, NULL, NULL );

    // Exit.
    return err;
}

/*!

  \brief Find a minimizer along the projected search direction.

  Find the exact minimizer of the quadratic function

    q(t) = 1/2 || A X(t) - b ||^2 + 1/2 damp^2 || X(t) ||^2,

  where X(t) is the projection of (x0 + t dx) onto the box defined by
  bl and bu.

  The arguments  y  and  e  are workspace vectors.  They must be at
  least length n and their contents will be overwritten.

  \return
  -                0 - No errors encountered.
  - #BCLS_EXIT_UNBND - Encountered direction of infinite descent.

*/
static int
bcls_proj_search( BCLS *ls, int m, int n, double x[], double dx[],
		  double f, double g[], double aBreak[], int iBreak[],
		  int ix[], double bl[], double bu[],
		  double ex[], double Ad[], double Ae[], int *totHits )
{
    int j, k;
    int err     = 0;  // Error flag.
    int nBrkPts = 0;  // No. of breakpoints encountered.
    int nFree   = 0;  // No. of variables that are free to move.
    int xlower;
    int xupper;
    int xfree;
    int stepstat = 0; // Flag if step is
    const int STEP_UND = -1; //   undefined
    const int STEP_LHS =  0; //   on LHS
    const int STEP_MID =  1; //   to interior of interval
    const int STEP_RHS =  2; //   to RHS
    const int STEP_INF =  3; //   unbounded descent.
    int gdpos;        // Flag if gd is "positive".
    int gdzero;       // Flag if gd is "zero".
    int dHdNonNeg;    // Flag if dHd is "nonnegative".
    int dHdNonPos;    // Flag if dHd is "nonpositive".
    int jHits;        // No. of vars that hit ther bound at break point j.

    double lhs;       // The start (LHS) of the current interval.
    double rhs;       // The end (RHS) of the current interval.
    double step;      // Fraction of the current search interval.
    double maxStep;   // The width of the current search interval.
    double q;         // The piecewise-quadratic objective function.
    double gd;        // Projected gradient of q along current arc.
    double ge;        // Projected gradient of q along last arc.
    double dHd;       // Projected Hessian  of q along current arc.
    double dHe;       // Inner product of Hessian along d and update e.

    const int damped = ls->damp > 0.0;
    const double damp2 = ls->damp * ls->damp;
    const double eps = ls->eps;
    const double epsx = ls->epsx;
    const double epsfixed = ls->epsfixed;
    const double BigNum = ls->BigNum;

    PRINT6("\n%3s %10s %10s %10s %10s %10s\n",
	   "Var","Lower","x","Upper","dx","Break");

    // -----------------------------------------------------------------
    // Find out which variables are free to move.
    // -----------------------------------------------------------------
    for (j = 0; j < n; j++) {

	if ( bu[j] - bl[j] < epsfixed ) {
	    // This variable is fixed.  Zero out its search direction.
	    dx[j] = 0.0;
	    continue;
	}

	xupper = bu[j] - x[j] <= epsx; // Variable on upper bound.
	xlower = x[j] - bl[j] <= epsx; // Variable on lower bound.
	xfree  = !xlower && !xupper;   // Variable is floating btwn bnds.

	assert( xupper || xlower || xfree );

	if ( xfree ) {
	    if ( fabs(dx[j]) > eps ) {
		iBreak[nFree] = j;
		nFree++;
	    }
	    else
		dx[j] = 0.0;
	}

	else if ( xupper ) {
	    if ( dx[j] < - eps ) {
		iBreak[nFree] = j;
		nFree++;
	    }
	    else
		dx[j] = 0.0;
	}

	else { // if ( xlower )
	    if ( dx[j] > eps ) {
		iBreak[nFree] = j;
		nFree++;
	    }
	    else
		dx[j] = 0.0;
	}
    }

    // -----------------------------------------------------------------
    // Compute the breakpoints of the free variables.
    //
    // Each breakpoint measures the maximum allowable step that will
    // take the corresponding free variable to its boundary.
    // -----------------------------------------------------------------
    for (j = 0; j < nFree; j++) {
	k = iBreak[j];
	if ( dx[k] > 0.0 )
	    aBreak[j] = ( bu[k] - x[k] ) / dx[k];
	else
	    aBreak[j] = ( bl[k] - x[k] ) / dx[k];
    }

    // -----------------------------------------------------------------
    // Diagnostic printing.
    // -----------------------------------------------------------------
    if (ls->print_level >= 6) {
	k = 0;
	for (j = 0; j < n; j++) {
	    PRINT6("%3d %10.3e %10.3e %10.3e %10.3e ",
		   j, bl[j], x[j], bu[j], dx[j]);
	    if ( j == iBreak[k] ) {
		PRINT6("%10.3e\n",aBreak[k]);
		k++;
	    }
	    else
		PRINT6("%10s\n","--");
	}
	PRINT6("\n");
    }

    // Exit if no variables are free to move.
    if ( !nFree ) return 0;

    // No. of breakpoints remaining is initially the number of free vars.
    nBrkPts = nFree;

    // Build the breakpoints into a heap.
    bcls_heap_build( nBrkPts, aBreak, iBreak );

    // -----------------------------------------------------------------
    // Projected gradient of q along dx.
    // -----------------------------------------------------------------
    gd = 0.0;
    for (j = 0; j < nFree; j++) {
	k = iBreak[j];
	gd += g[k] * dx[k];
    }

    // Quick exit if the directional derivative is positive.
    if ( gd > epsx ) return 0;

    // -----------------------------------------------------------------
    // Projected Hessian of q along dx:
    // dHd = dx' A' A dx = (A dx)' (A dx) = Ad' Ad.
    // -----------------------------------------------------------------
    bcls_aprod( ls, BCLS_PROD_A, nFree, iBreak, dx, Ad );
    dHd = cblas_ddot( m, Ad, 1, Ad, 1 );
    if (damped)
	dHd += damp2 * cblas_ddot( n, dx, 1, dx, 1 );

    // Initialization.
    q        = 0.0; // Value of piecewise quadratic.
    rhs      = 0.0; // This initializes the very first breakpoint.
    *totHits = 0;   // Initially no hits.

    // -----------------------------------------------------------------
    // Explore each piecewise quadratic intervals until non remain.
    // -----------------------------------------------------------------
    while (nBrkPts > 0) {

	// Set the status of the current step to "undefined".
	stepstat = STEP_UND;

	// Grab the boundaries of the current search interval.
	lhs     = rhs;
	rhs     = aBreak[0];  // The next breakpoint (top of the heap).
	maxStep = rhs - lhs;  // The maximum step.
	k       = iBreak[0];  // Var corresponding to the current break.

	// The smallest breakpoint has been sampled.  Move it to the
	// top of the heap and restore the heap property.
	nBrkPts = bcls_heap_del_min( nBrkPts, aBreak, iBreak );

	// -------------------------------------------------------------
	// Test the various exit or continuation conditions.
	// -------------------------------------------------------------
	gdpos     = gd       >    epsx;
	gdzero    = fabs(gd) <=   epsx;
	dHdNonNeg = dHd      >= - epsx;
	dHdNonPos = dHd      <    epsx;

	if ( gdpos || ( gdzero && dHdNonNeg) ) {
	    // ---------------------------------------------------------
	    // Minimimum is on LHS. (Captures the  dx = 0  case.)  Done.
	    // ---------------------------------------------------------
	    step = 0.0;
	    stepstat = STEP_LHS;
	    break;
	}

	// If we got this far, gd is zero or negative.

	else if ( dHdNonPos ) {
	    // ---------------------------------------------------------
	    // Found dir of neg curvature.  Minimizer is either on RHS,
	    // or at -infinity.  Check.
	    // ---------------------------------------------------------
	    if ( maxStep >= BigNum ) {
		stepstat = STEP_INF;
		break;
	    }
	    else {
		step = maxStep;
		stepstat = STEP_RHS;
	    }
	}
	else {
	    // ---------------------------------------------------------
	    // Minimum is on RHS or inside the interval.  Check.
	    // ---------------------------------------------------------
	    step = - gd / dHd;
	    if ( step >= BigNum  &&  step < maxStep ) {
		// Step is unbounded.  Exit with unbounded flag.
		stepstat = STEP_INF;
		break;
	    }
	    else if ( step <  maxStep ) {
		// Minimum found in the interval's interior.  Done.
		stepstat = STEP_MID;
		break;
	    }
	    else if ( step >= maxStep ) {
		// Minimum is on RHS.  Truncate the step and continue.
		step = maxStep;
		stepstat = STEP_RHS;
	    }
	}
	// -------------------------------------------------------------
	// Take the step.
	// -------------------------------------------------------------
	cblas_daxpy( n, step, dx, 1, x, 1 );

	// -------------------------------------------------------------
	// Other variables may share (essentially) the same breakpoint.
	//
	// Loop over these (and the current) variables, pushing them
	// onto their bounds, zeroing out the corresponding search
	// direction.
	// -------------------------------------------------------------
	jHits = 0; // No. of vars that hit their bound at this breakpoint.
	while(1) {

	    // There may have been roundoff error.  Push the var that
	    // hit its breakpoint exactly onto its bound.
	    if (dx[k] < 0.0)
		x[k] = bl[k];
	    else
		x[k] = bu[k];

	    // Record the index and step of the var that just hit its bound.
	    ex[k] = dx[k];
	    ix[jHits] = k;
	    jHits++;

	    // That var no longer contributes.  Zero out its step.
	    dx[k] = 0.0;

	    // Print something out.
	    PRINT6("%3d %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e\n",
		   k,bl[k],x[k],bu[k],dx[k],gd,dHd);

	    // No more breakpoints left to explore.
	    if (nBrkPts == 0)
		break;

	    // Next breakpoint is significantly different.
	    else if (aBreak[0] > rhs + 10*eps)
		break;

	    // Next breakpoint is essentially the same.
	    else {
		k = iBreak[0];
		nBrkPts = bcls_heap_del_min( nBrkPts, aBreak, iBreak );
	    }
	}

	// Update the aggregate no. of breakpoints encountered so far.
	*totHits += jHits;

	// -------------------------------------------------------------
	// Update the function value.  (Used for printing only.)
	// -------------------------------------------------------------
	q  = q + step * (gd + 0.5 * step * dHd);

	// -------------------------------------------------------------
	// Update the directional derivative and Hessian for the next
	// search direction.
	// -------------------------------------------------------------

	// Inner products: g'e.
	ge = 0.0;
	for (j = 0; j < jHits; j++) {
	    k = ix[j];
	    ge += g[k]*ex[k];
	}

	// Inner product: d' H e.
	bcls_aprod( ls, BCLS_PROD_A, jHits, ix, ex, Ae ); //  Ae = A * ex
	dHe = cblas_ddot( m, Ad, 1, Ae, 1 );              // dHe = d' A' A e
	if (damped) {
	    for (j = 0; j < jHits; j++) {
		k = ix[j];
		dHe += damp2 * dx[k] * ex[k];
	    }
	}

	// Update projected gradient.
	gd  = gd - ge + step * ( dHd - dHe );

	// Update projected Hessian.
	cblas_daxpy( m, -1.0, Ae, 1, Ad, 1 );     //  Ad <- Ad - Ae
	dHd = cblas_ddot( m, Ad, 1, Ad, 1 );      // dHd <- d' A' A d
	if (damped)
	    dHd = dHd + damp2 * cblas_ddot( n, dx, 1, dx, 1 );

    } // end of while (nBrkPs > 0)

    if ( stepstat == STEP_MID )
	// Step into the middle of an interval and exit.
	cblas_daxpy( n, step, dx, 1, x, 1 );

    else if ( stepstat == STEP_INF )
	err = BCLS_EXIT_UNBND;

    // Return the status of the last step.
    return err;
}

/*!

  \brief Simple backtracking to satisfy an Armijo condition.

  Do a simple backtracking search on the quadratic function

    q(x + s) = f(x) + g(x)'s + 0.5 s' H s,

  where H = A'A + damp^2 I and s = P[dx] is the projection of dx onto
  the box defined by bl and bu.  The function value f = f(x) and the
  gradient g = g(x) are defined at entry and left unchanged.  The step
  dx is reduced by a contant amount at each iteration until its
  projection s satisfies an Armijo condition, ie, sufficient descent is
  achieved when

    g's + 0.5 s' H s <= mu g's,

  and mu is a fixed constant.

  \return
  -                0 - No errors encountered.
  - #BCLS_EXIT_UNBND - Encountered direction of infinite descent.
  - #BCLS_EXIT_LFAIL - Linesearch failure.

*/
static int
bcls_proj_backtrack( BCLS *ls, int m, int n, double x[], double dx[],
		     double f, double g[], double bl[], double bu[],
		     double s[], int ix[], double As[], int *nSteps )
{
    int j;
    int armijo = 0;   // Flag: Armijo condition satisfied.
    int unbounded;    // Flag: Step may be unbounded.
    int descent;      // Flag: Descent direction.
    int negCurv;      // Flag: Nonpositive curvature.
    int xupper;       // Flag: Variable on upper bound.
    int xlower;       // Flag: Variable on lower bound.
    int xdecr;        // Flag: Variable is decreasing.
    int xincr;        // Flag: Variable is increasing.
    int nBreakPts;    // No. of break points.
    int jBreakMin;    // Index of variable with the nearest breakpoint.
    double Break;     // Step length to the break point for variable j.
    double BreakMin;  // Step length to the nearest breakpoint.
    double q;         // The quadratic objective function.
    double gs;        // Projected gradient along current search direction.
    double sHs;       // Projected Hessian  along current search direction.
    double step = 1;  // Current step length.

    const int    backlimit = ls->backtrack_limit;
    const int    damped    = ls->damp > 0.0;
    const double damp2     = ls->damp * ls->damp;
    const double mu        = ls->mu;
    const double backtrack = ls->backtrack;
    const double BigNum    = ls->BigNum;
    const double eps       = ls->eps;
    const double epsx      = ls->epsx;

    *nSteps = 0;      // No. of backtracking steps.

    // -----------------------------------------------------------------
    // Need to include contribution for *all* columns of A.
    // -----------------------------------------------------------------
    for (j = 0; j < n; j++) ix[j] = j;

    // -----------------------------------------------------------------
    // Find first breakpoint, and store as BreakMin.
    // -----------------------------------------------------------------
    nBreakPts = 0;
    BreakMin  = BigNum;
    for (j = 0; j < n; j++) {
        xupper = bu[j] - x[j] <= epsx; // Variable on upper bound.
        xlower = x[j] - bl[j] <= epsx; // Variable on lower bound.
        xdecr  = dx[j] < -eps;         // Variable is decreasing.
        xincr  = dx[j] >  eps;         // Variable is increasing.

        if ( !xlower  &&  xdecr ) {
            nBreakPts++;
            Break = ( bl[j] - x[j] ) / dx[j];
            if ( Break < BreakMin ) {
                BreakMin  = Break;
                jBreakMin = j;
            }
        }
        else if ( !xupper  &&  xincr ) {
            nBreakPts++;
            Break = ( bu[j] - x[j] ) / dx[j];
            if ( Break < BreakMin ) {
                BreakMin  = Break;
                jBreakMin = j;
            }
        }
    }

    if ( nBreakPts == 0 )
        BreakMin = 0.0;

    PRINT3("I: Proj Backtrack: Steplength to the nearest bound: %9.2e\n",
           BreakMin);

    // -----------------------------------------------------------------
    // Backtracking loop.
    // -----------------------------------------------------------------
    while ( !armijo  &&  step > BreakMin ) {

	if (*nSteps >= backlimit)
	    return BCLS_EXIT_LFAIL;

        // -------------------------------------------------------------
        // Store the projected step into s:  s = P[x + step*dx] - x.
        // -------------------------------------------------------------
        bcls_project_step( n, s, step, x, dx, bl, bu );

	// -------------------------------------------------------------
	// Compute q(s) = g's + 0.5 s' H s.
	// -------------------------------------------------------------
	bcls_aprod( ls, BCLS_PROD_A, n, ix, s, As ); //  As = A * s
	sHs = cblas_ddot( m, As, 1, As, 1 );         // sHs = (A s)'(A s)
	if (damped)
	    sHs = sHs + damp2 * cblas_ddot( n, s, 1, s, 1 );
	gs  = cblas_ddot( n, g, 1, s, 1 );    //  gs = g's
	q   = gs + 0.5 * sHs;                 //   q = g's + 0.5 s' H s

	// -------------------------------------------------------------
	// If sHs <= 0 and gs < 0, there may be a direction of
	// unbounded descent.  Check.
	// -------------------------------------------------------------
	descent  =  gs  <= -epsx;
	negCurv  =  sHs <=  epsx;

	if ( descent  &&  negCurv ) {

	    unbounded = 1;
	    for (j = 0; j < n; j++) {
		if (s[j] > eps)
		    unbounded = bu[j] >=   BigNum;

		else if (s[j] < eps)
		    unbounded = bl[j] <= - BigNum;

		if (!unbounded)
                    break;
	    }

	    if (unbounded)
                return BCLS_EXIT_UNBND;
	}

	// -------------------------------------------------------------
	// Check if the sufficient decrease criteria was satisfied.
	// Otherwise, reduce the step and continue.
	// -------------------------------------------------------------
        PRINT3("I: Proj Backtrack: step = %9.2e  q = %10.2e  gs = %10.2e\n",
               step, q, gs);

	armijo = ( q <= mu * gs );

	if (!armijo) {
	    step = backtrack * step;
	    *nSteps += 1;
	}
    }

    // -----------------------------------------------------------------
    // Force a variable onto its nearest bound: If the armijo
    // condition is satisfied by a truncated step that falls short of
    // the nearest breakpoint, then we need to be a bit more
    // aggressive and continue the step until the nearest boundary.
    //
    // 17 Dec 06: Removed this step.  Why was I doing it?!
    // -----------------------------------------------------------------
#ifdef UNDEF
    if ( step < 1.0  &&  step < BreakMin ) {
        step = BreakMin;
        PRINT3("I: Proj Backtrack: Force index %4i onto bound\n",
               jBreakMin );
    }
#endif

    // Compute the final projected step.
    bcls_project_step( n, s, step, x, dx, bl, bu );

    // Take the step: x = x + s.
    cblas_daxpy( n, 1.0, s, 1, x, 1 );

    // Push x onto bounds that are very close.
    for (j = 0; j < n; j++) {
        if      (x[j] < bl[j] + 10*eps)
                 x[j] = bl[j];
        else if (x[j] > bu[j] - 10*eps)
                 x[j] = bu[j];
    }

    return 0;
}


/*
 * Copyright (c) 1997-1999 Massachusetts Institute of Technology
 *
 * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* This file was automatically generated --- DO NOT EDIT */
/* Generated on Sun Nov  7 20:44:57 EST 1999 */

#include <fftw-int.h>
#include <fftw.h>

/* Generated by: ./genfft -magic-alignment-check -magic-twiddle-load-all -magic-variables 4 -magic-loopi -twiddleinv 2 */

/*
 * This function contains 6 FP additions, 4 FP multiplications,
 * (or, 4 additions, 2 multiplications, 2 fused multiply/add),
 * 10 stack variables, and 8 memory accesses
 */

/*
 * Generator Id's :
 * $Id: exprdag.ml,v 1.41 1999/05/26 15:44:14 fftw Exp $
 * $Id: fft.ml,v 1.43 1999/05/17 19:44:18 fftw Exp $
 * $Id: to_c.ml,v 1.25 1999/10/26 21:41:32 stevenj Exp $
 */

void fftwi_twiddle_2(fftw_complex *A, const fftw_complex *W, int iostride, int m, int dist)
{
     int i;
     fftw_complex *inout;
     inout = A;
     for (i = m; i > 0; i = i - 1, inout = inout + dist, W = W + 1) {
	  fftw_real tmp1;
	  fftw_real tmp8;
	  fftw_real tmp6;
	  fftw_real tmp7;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp1 = c_re(inout[0]);
	  tmp8 = c_im(inout[0]);
	  {
	       fftw_real tmp3;
	       fftw_real tmp5;
	       fftw_real tmp2;
	       fftw_real tmp4;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp3 = c_re(inout[iostride]);
	       tmp5 = c_im(inout[iostride]);
	       tmp2 = c_re(W[0]);
	       tmp4 = c_im(W[0]);
	       tmp6 = (tmp2 * tmp3) + (tmp4 * tmp5);
	       tmp7 = (tmp2 * tmp5) - (tmp4 * tmp3);
	  }
	  c_re(inout[iostride]) = tmp1 - tmp6;
	  c_re(inout[0]) = tmp1 + tmp6;
	  c_im(inout[0]) = tmp7 + tmp8;
	  c_im(inout[iostride]) = tmp8 - tmp7;
     }
}

static const int twiddle_order[] =
{1};
fftw_codelet_desc fftwi_twiddle_2_desc =
{
     "fftwi_twiddle_2",
     (void (*)()) fftwi_twiddle_2,
     2,
     FFTW_BACKWARD,
     FFTW_TWIDDLE,
     55,
     1,
     twiddle_order,
};


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"
#include "fresnel.h"

#define acc 1e-8

/* Used for previous version of branching between cases 1a, 1b and 2 in ComputeInt */
/* static const double betathreshold = 1./10; */
/* static const double invbetathreshold = 10.; */
/* static const double lambdathreshold = 0.01; */

/* Used for previous version of branching between cases 1a, 1b and 2 in ComputeInt */
/* static const double factorbranchingcondition = 0.2154434690031884; */
/* static const double case1athreshold = 0.1; */
/* Used for case 0 - hoping to cover previous instability */
/* static const double case0threshold = 0.05; */

/* Used for branching between cases 1a, 1b, 2, 3 and 4 in ComputeInt */
/* FIXME: another round of refinement would be welcome, case 3 has some cases with 1e-4 error */
static const double p2threshold = 0.02;
static const double p1threshold1 = 0.1;
static const double p1threshold2 = 100.;
static const double p2p1slopethreshold = 0.2;

static const double coeffdeltaC[13] = {-0.1,-0.0462962962963,-0.0230769230769,-0.0136554621849,-0.00899470899471,-0.00636363636364,-0.00473664266768,-0.00366161616162,-0.00291467938527,-0.00237483953787,-0.0019721019721,-0.00166370896185,-0.00142235123367};
static const double coeffdeltaS[13] = {-0.0714285714286,-0.0318181818182,-0.0174603174603,-0.0109649122807,-0.00750988142292,-0.00546058879392,-0.00414746543779,-0.00325630252101,-0.00262408157145,-0.00215946843854,-0.00180809015222,-0.00153594771242,-0.0013209013209};

static const double fcoeff[12] = {0.3989422275318915, 1.839999368141345e-7, -0.2992429124371606, 0.0029447692036381, 2.48395059327971, 3.897790465813753, -140.2709114924737, 952.371347498219, -3660.703989590287, 8649.34061360465, -11740.93693284928, 7032.8915074629};
static const double gcoeff[12] = {0, 0.1994718004197629, -0.000125952146250442, -0.7386823670326993, -0.353224701618227, 19.45013452321292, -97.8846433217499, 221.1470704754588, -32.29876222016862, -1102.56036216157, 2554.184335427029, -1962.422566478277};

static double invn[12] = {1., 0.5, 0.3333333333333333, 0.25, 0.2, 0.1666666666666667, 0.1428571428571428, 0.125, 0.1111111111111111, 0.1, 0.0909090909090909, 0.0833333333333333};

/* Use for Chebyshev, case 3 */
static double ChebyshevNodes[6] = {0.965925826289068,0.7071067811865475,0.2588190451025207,-0.2588190451025207,-0.7071067811865475,-0.965925826289068};
static double ChebyshevWeights[6][6] = {{0.3333333333333333,0.3333333333333333,0.3333333333333333,0.3333333333333333,0.3333333333333333,0.3333333333333333},{0.3219752754296894,0.2357022603955158,0.0862730150341736,-0.0862730150341736,-0.2357022603955158,-0.3219752754296894},{0.2886751345948129,0.,-0.2886751345948129,-0.2886751345948129,0.,0.2886751345948129},{0.2357022603955158,-0.2357022603955158,-0.2357022603955158,0.2357022603955158,0.2357022603955158,-0.2357022603955158},{0.1666666666666667,-0.3333333333333333,0.1666666666666667,0.1666666666666667,-0.3333333333333333,0.1666666666666667},{0.0862730150341736,-0.2357022603955158,0.3219752754296894,-0.3219752754296894,0.2357022603955158,-0.0862730150341736}};

/* Function computing cexp(I*x)-1 without loss of accuracy*/
static double complex cexpm1i(double x){
  double rr;//rr=cos(x)-1=2*sin(x/2)**2
  double ri=sin(x);
  double sinhalf=sin(0.5*x);
  rr=-2.0*sinhalf*sinhalf;
  return rr+I*ri;
};

/* Function computing the Fresnel integral for any real (positive) number, with 1e-8 accuracy */
/* Reference used: Computation of Fresnel Integrals. II by K. Mielenz, [J. Res. Natl. Inst. Stand. Technol. 105, 589 (2000)] */
static double complex FresnelE(const double x) {
  double complex res;

  /* Check argument */
  if(x<0) return -FresnelE(-x);

  /* If x<2, Taylor expansion */
  else if(x<2) {
    double C = 0;
    double S = 0;
    double x3 = x*x*x;
    double x4 = x3*x;
    int n = 0;
    double deltaC = x;
    double deltaS = x3/3.;
    while(true) {
      C += deltaC;
      S += deltaS;
      //deltaC = -(4*n+1.)/(4*n+5.)/(2*n+1.)/(2*n+2.) * x4 * deltaC;
      //deltaS = -(4*n+3.)/(4*n+7.)/(2*n+2.)/(2*n+3.) * x4 * deltaS;
      deltaC = coeffdeltaC[n] * x4 * deltaC;
      deltaS = coeffdeltaS[n] * x4 * deltaS;
      n++;
      if( (fabs(deltaC)<acc && fabs(deltaS)<acc) || n>12) break;
    }
    res = C + I*S;
  }

  /* If x>2, Mielenz-Boersma formula using tabulated coeffs */
  else {
    double xinv = 1./x;
    double xinv2 = xinv*xinv;
    double y[12] = {0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
    y[0] = xinv;
    for(int i=1; i<12; i++) y[i] = y[i-1]*xinv2;
    double f = 0;
    double g = 0;
    for(int i=0; i<12; i++) {
      f += fcoeff[i] * y[i];
      g += gcoeff[i] * y[i];
    }
    res = sqrt(PI/2) * ( (1+I)/2 - (g+I*f) * cexp(I*x*x));
  }

  return res;
}

/* In fact equivalent to Case 2 */
//static double complex ComputeIntCase0(
//   gsl_vector* coeffsAreal,         /* */
//   gsl_vector* coeffsAimag,         /* */
//   double p0,                       /* */
//   double p1,                       /* */
//   double p2,                       /* */
//   double eps)                      /* */
// {
//   /* Get poly coeffs and rescale by epsilon */
//   double complex coeffs[4] = {0.,0.,0.,0.};
//   double epspow[4] = {0.,0.,0.,0.};
//   epspow[0] = 1.; epspow[1] = eps; epspow[2] = eps*eps; epspow[3] = eps * epspow[2];
//   for(int i=0; i<=3; i++) {
//     coeffs[i] = epspow[i] * (gsl_vector_get(coeffsAreal, i+1) + I*gsl_vector_get(coeffsAimag, i+1));
//   }
//   double p0r = p0;
//   double p1r = p1*epspow[1];
//   double p2r = p2*epspow[2];
//
//   /* In case p2<0, conjugation */
//   int conjug = 0;
//   if(p2r<0) {
//     conjug=1;
//     p2r = -p2r;
//     p1r = -p1r;
//     p0r = -p0r;
//     for(int i=0; i<=3; i++) coeffs[i] = conj(coeffs[i]) ;
//   }
//
//   //
//   //printf("case0\n");
//
//   /* Prefactor and bounds after change of variable */
//   double sqrtp2r = sqrt(p2r);
//   double complex factor = 1./sqrtp2r*cexp(I*p0r);
//   double a = p1r/(2*sqrtp2r);
//   double b = p1r/(2*sqrtp2r) + sqrtp2r;
//   /* Coeffs in polynomial changed by shift */
//   double alpha = 1./sqrtp2r;
//   double beta = -p1r/(2*p2r);
//   double complex a0 = coeffs[0]; double complex a1 = coeffs[1]; double complex a2 = coeffs[2]; double complex a3 = coeffs[3];
//   double alpha2 = alpha*alpha; double beta2 = beta*beta;
//   double complex a0p = a0 + a1*beta + a2*beta2 + a3*beta2*beta;
//   double complex a1p = a1*alpha + 2*a2*alpha*beta + 3*a3*alpha*beta2;
//   double complex a2p = a2*alpha2 + 3*a3*alpha2*beta;
//   double complex a3p = a3*alpha2*alpha;
//
//   /* Result */
//   double complex res = 0.;
//   res = factor*((a0p + I/2*a2p) * (FresnelE(b) - FresnelE(a)) + (1./2*(a3p - I*a1p) - I/2*a2p*b - I/2*a3p*b*b) * cexp(I*b*b) - (1./2*(a3p - I*a1p) - I/2*a2p*a - I/2*a3p*a*a) * cexp(I*a*a));
//   //
//   //printf("%g, %g, %g \n", creal(factor), creal((a0p + I/2*a2p) * (FresnelE(b) - FresnelE(a))), creal(+ (1./2*(a3p - I*a1p) - I/2*a2p*b - I/2*a3p*b*b) * cexp(I*b*b) - (1./2*(a3p - I*a1p) - I/2*a2p*a - I/2*a3p*a*a) * cexp(I*a*a)));
//   //exit(0);
//
//   /* Remembering conjugation */
//   if(conjug) res = conj(res);
//   return res;
// }

double complex ComputeIntCase1a(
  const double complex* coeffsA,         /* */
  const double p1,                       /* */
  const double p2,                       /* */
  const double scale)                    /* */
{
  /* Setting up the coefficients and the required accuracy */
  double complex coeffs[12] = {0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
  for(int i=0; i<=3; i++) coeffs[i] = coeffsA[i];
  int maxj0 = 3;
  double acctol = 1.e-5/scale;

  /* Polynomial given by the expansion of cexp(I*p1*x) */
  double complex poly1[8] = {0.,0.,0.,0.,0.,0.,0.,0.};
  double p1abs = fabs(p1);
  double complex term1 = 1.; double term1absmax = 1.;
  int maxj1 = 0;
  for(int i=0; i<8; i++) {
    poly1[i] = term1;
    term1absmax = term1absmax * p1abs * invn[i]; /* Note: invn[i] = 1/(i+1) */
    if(term1absmax<acctol) break;
    term1 = term1 * I*p1 * invn[i];
    maxj1++;
  }

  /* Polynomial given by the expansion of cexp(I*p2*x2) */
  double complex poly2[11] = {0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
  double p2abs = fabs(p2);
  double complex term2 = 1.; double term2absmax = 1.;
  int maxj2 = 0;
  for(int i=0; i<4; i++) {
    poly2[2*i] = term2;
    term2absmax = term2absmax * p2abs * invn[i]; /* Note: invn[i] = 1/(i+1) */
    if(term2absmax<acctol) break;
    term2 = term2 * I*p2 * invn[i];
    maxj2 += 2;
  }

  /* Multiplying in place the polynomials - we cut according to the fact that we allow for only 12 coeffs up to x^11 (assuming the rest would be negligible anyway), and we know the constant in poly1 and poly2 is always 1. */
  for(int i=min(11, maxj0+maxj1); i>=0; i--) {
    for(int j=max(1, i-maxj0); j<=min(i, maxj1); j++) {
      coeffs[i] += coeffs[i-j] * poly1[j];
    }
  }
  maxj0 = min(11, maxj0 + maxj1);
  for(int i=min(11, maxj0+maxj2); i>=0; i--) {
    for(int j=max(1, i-maxj0); j<=min(i, maxj2); j++) {
      coeffs[i] += coeffs[i-j] * poly2[j];
    }
  }
  maxj0 = min(11, maxj0 + maxj2);

  /* Computing the integral itself */
  double complex res = 0.;
  for(int i=0; i<=maxj0; i++) {
    res += coeffs[i] * invn[i]; /* Note: invn[i] = 1/(i+1) */
  }
  return res;
}

double complex ComputeIntCase1b(
  const double complex* coeffsA,         /* */
  const double p1,                       /* */
  const double p2,                       /* */
  const double scale)                    /* */
{
  /* Setting up the coefficients and the required accuracy */
  double complex coeffs[12] = {0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
  for(int i=0; i<=3; i++) coeffs[i] = coeffsA[i];
  int maxj0 = 3;
  double acctol = 1.e-5/scale;

  /* Polynomial given by the expansion of cexp(I*p2*x2) */
  double complex poly2[11] = {0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
  double p2abs = fabs(p2);
  double complex term2 = 1.; double term2absmax = 1.;
  int maxj2 = 0;
  for(int i=0; i<4; i++) {
    poly2[2*i] = term2;
    term2absmax = term2absmax * p2abs * invn[i]; /* Note: invn[i] = 1/(i+1) */
    if(term2absmax<acctol) break;
    term2 = term2 * I*p2 * invn[i];
    maxj2 += 2;
  }

  /* Multiplying in place the polynomials - we cut according to the fact that we allow for only 12 coeffs up to x^11 (assuming the rest would be negligible anyway), and we know the constant in poly2 is always 1. */
  for(int i=min(11, maxj0+maxj2); i>=0; i--) {
    for(int j=max(1, i-maxj0); j<=min(i, maxj2); j++) {
      coeffs[i] += coeffs[i-j] * poly2[j];
    }
  }
  maxj0 = min(11, maxj0 + maxj2);

  /* Go down the recursion relation */
  double invp1 = 1./p1;
  double complex coeffE1 = 0.; double complex coeffE1minus1 = 0;
  for(int i=maxj0; i>=1; i--) {
    coeffE1 += -I*invp1 * coeffs[i];
    coeffs[i-1] += I*invp1 * i * coeffs[i];
  }
  coeffE1minus1 = -I*invp1 * coeffs[0];
  /* Result */
  double complex cexpm1ip1 = cexpm1i(p1);
  double complex res = (coeffE1*(cexpm1ip1+1.) + coeffE1minus1*cexpm1ip1);
  return res;
}

double complex ComputeIntCase2(
  const double complex* coeffsA,         /* */
  const double p1,                       /* */
  const double p2)                       /* */
{
  /* Setting up the coefficients */
  double complex coeffs[4] = {0.,0.,0.,0.};
  for(int i=0; i<=3; i++) coeffs[i] = coeffsA[i];
  double complex coeffE2 = 0; double complex coefffresnel = 0; double complex constant = 0;
  /* Conjugate if required */
  int conjug = 0; double p1b = p1; double p2b = p2;
  if(p2<0) { conjug = 1; p1b = -p1; p2b = -p2; for(int i=0; i<=3; i++) coeffs[i] = conj(coeffs[i]); }

  /* n=3 and n=2 */
  double inv2p2 = 1. / (2.*p2b);
  for(int i=3; i>=2; i--) {
    coeffE2 += -I*inv2p2 * coeffs[i];
    coeffs[i-2] += I*inv2p2 * (i-1) * coeffs[i];
    coeffs[i-1] += -p1b * inv2p2 * coeffs[i];
  }
  /* n=1 */
  coeffE2 += -I*inv2p2 * coeffs[1];
  constant = I*inv2p2 * coeffs[1];
  coefffresnel += -p1b * inv2p2 * coeffs[1];
  /* n=0 */
  coefffresnel += coeffs[0];

  /* Fresnel integral and complex exponential */
  double complex E2 = cexp(I * (p1b + p2b));
  double sqrtp2 = sqrt(p2b);
  double invsqrtp2 = 1./sqrtp2;
  double halfratio = 0.5 * p1b * invsqrtp2;
  double complex fresnel = cexp(-I*halfratio*halfratio) * invsqrtp2 * (FresnelE(halfratio + sqrtp2) - FresnelE(halfratio));

  /* Result */
  double complex res = (constant + coeffE2*E2 + coefffresnel*fresnel);
  if(conjug) res = conj(res); /* Remember possible conjugation */
  return res;
}

double complex ComputeIntCase3(
  const double complex* coeffsA,         /* */
  const double p1,                       /* */
  const double p2)                       /* */
{
  /* Prepare change of variables */
  double r = p2/p1;

  /* Compute integrand F on nodes */
  double Fvalues[6] = {0.,0.,0.,0.,0.,0.};
  double xk; double y; double sqrtterm; double factor; double x;
  for(int i=0; i<6; i++) {
    xk = ChebyshevNodes[i];
    y = (1.+xk)/2.;
    sqrtterm = sqrt(1. + 4.*r*(1.+r)*y);
    factor = (1.+r)/sqrtterm;
    x = (2.*(1.+r)*y)/(1. + sqrtterm);
    Fvalues[i] = coeffsA[3];
    for(int j=2; j>=0; j--) Fvalues[i] = coeffsA[j] + x*Fvalues[i];
    Fvalues[i] = factor * Fvalues[i];
  }

  /* Compute Chebyshev coefficients ci */
  double complex c[6] = {0.,0.,0.,0.,0.,0.};
  for(int i=0; i<6; i++) {
    for(int j=0; j<6; j++) c[i] += ChebyshevWeights[i][j] * Fvalues[j];
  }

  /* Coefficients of polynomial in 1/(I*p), p=p1*(1+r) */
  double p = p1*(1.+r);
  double complex coeffszcos[6] = {0.,0.,0.,0.,0.,0.};
  double complex coeffszsin[6] = {0.,0.,0.,0.,0.,0.};
  coeffszcos[0] = c[1] + c[3] + c[5];
  coeffszcos[1] = -8.*c[2] - 32.*c[4];
  coeffszcos[2] = 96.*c[3] + 800.*c[5];
  coeffszcos[3] = -1536.*c[4];
  coeffszcos[4] = 30720.*c[5];
  coeffszcos[5] = 0.;
  coeffszsin[0] = 0.5*c[0] + c[2] + c[4];
  coeffszsin[1] = -2.*c[1] - 18.*c[3] - 50.*c[5];
  coeffszsin[2] = 16.*c[2] + 320.*c[4];
  coeffszsin[3] = -192.*c[3] -6720.*c[5];
  coeffszsin[4] = 3072.*c[4];
  coeffszsin[5] = -61440.*c[5];

  /* Coefficients of cos(p/2) and sin(p/2) */
  double complex z = 1./(I*p);
  double complex coeffcos = coeffszcos[5];
  for(int i=4; i>=0; i--) coeffcos = coeffszcos[i] + z*coeffcos;
  double complex coeffsin = coeffszsin[5];
  for(int i=4; i>=0; i--) coeffsin = coeffszsin[i] + z*coeffsin;
  coeffcos = z * coeffcos;
  coeffsin = z * coeffsin;

  /* Result */
  double pov2 = p/2.;
  return cexp(I*pov2) * (2*cos(pov2)*coeffcos + 2*I*sin(pov2)*coeffsin);
}

double complex ComputeIntCase4(
  const double complex* coeffsA,         /* */
  const double p1,                       /* */
  const double p2)                       /* */
{
  double complex a0 = coeffsA[0];
  double complex a1 = coeffsA[1];
  double complex a2 = coeffsA[2];
  double complex a3 = coeffsA[3];
  double r = 2.*p2/p1;
  double inv1plusr = 1./(1.+r); double inv1plusr2 = inv1plusr*inv1plusr;
  double inv1plusr4 = inv1plusr2*inv1plusr2;
  double invp1 = 1./p1; double invp12 = invp1*invp1; double invp13 = invp12*invp1;

  double complex term1 = cexp(I*(p1 + p2))*(-I*invp1*(a0 + a1 + a2 + a3)*inv1plusr + invp12*(a1 + 2.*a2 + 3.*a3)*inv1plusr2 + I*invp13*(2.*a2 + 6.*a3 + r*(3.*a3 - a1))*inv1plusr4);
  double complex term2 = - (-I*invp1*a0 + invp12*a1 + I*invp13*(2.*a2 - a1*r));
  return term1 + term2;
}

double complex ComputeInt(
  gsl_matrix* splinecoeffsAreal,         /*  */
  gsl_matrix* splinecoeffsAimag,         /*  */
  gsl_matrix* quadsplinecoeffsphase)     /*  */
{
  double complex res = 0.;
  double complex resint = 0.;
  /* Number of points - i.e. nb of intervals + 1 */
  /* Assumes that the dimensions match and that the frequency vectors are the same between the different splinecoeffs */
  int nbpts = (int) quadsplinecoeffsphase->size1;

  //clock_t tbegtotal = clock();
  for(int j=0; j<nbpts-1; j++) {
    double eps = gsl_matrix_get(quadsplinecoeffsphase, j+1, 0) - gsl_matrix_get(quadsplinecoeffsphase, j, 0);
    double epspow[4];
    epspow[0] = 1.;
    epspow[1] = eps;
    epspow[2] = eps*eps;
    epspow[3] = eps*epspow[2];
    double p0 = gsl_matrix_get(quadsplinecoeffsphase, j, 1);

    /* Rescale the interval to [0,1] and scale out the constant amplitude term */
    double p1 = gsl_matrix_get(quadsplinecoeffsphase, j, 2) * eps;
    double p2 = gsl_matrix_get(quadsplinecoeffsphase, j, 3) * epspow[2];
    double complex A0 = gsl_matrix_get(splinecoeffsAreal, j, 1) + I*gsl_matrix_get(splinecoeffsAimag, j, 1);
    double complex A0inv = 1./A0;
    double A0abs = cabs(A0); /* Used for accuracy requirement of adaptive Taylor expansions Ia and Ib */
    double complex coeffsA[4] = {0.,0.,0.,0.};
    coeffsA[0] = 1.;
    for(int i=1; i<=3; i++) coeffsA[i] = epspow[i] * A0inv * (gsl_matrix_get(splinecoeffsAreal, j, i+1) + I*gsl_matrix_get(splinecoeffsAimag, j, i+1));

    /* Factor scaled out */
    double complex factor = eps * A0 * cexp(I*p0);

    double absp1 = fabs(p1); double absp2 = fabs(p2);
    if(absp1<p1threshold1 && absp2<p2threshold) {
      resint = factor * ComputeIntCase1a(coeffsA, p1, p2, A0abs);
    }
    else if(p1threshold1<=absp1 && absp1<p1threshold2 && absp2<p2threshold) {
      resint = factor * ComputeIntCase1b(coeffsA, p1, p2, A0abs);
    }
    else if(absp2>=p2p1slopethreshold*absp1 && absp2>=p2threshold) {
      resint = factor * ComputeIntCase2(coeffsA, p1, p2);
    }
    else if(absp2<p2p1slopethreshold*absp1 && absp2>=p2threshold && absp1<p1threshold2) {
      resint = factor * ComputeIntCase3(coeffsA, p1, p2);
    }
    else {
      resint = factor * ComputeIntCase4(coeffsA, p1, p2);
    }
    res += resint;
  }

  return res;
}


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

/* Runge-Kutta 8(9), Prince-Dormand
 *
 * High Order Embedded Runge-Kutta Formulae
 * P.J. Prince and J.R. Dormand
 * J. Comp. Appl. Math.,7, pp. 67-75, 1981
 */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>

#include "odeiv_util.h"

/* Prince-Dormand constants */

static const double Abar[] = {
  14005451.0 / 335480064.0,
  0.0,
  0.0,
  0.0,
  0.0,
  -59238493.0 / 1068277825.0,
  181606767.0 / 758867731.0,
  561292985.0 / 797845732.0,
  -1041891430.0 / 1371343529.0,
  760417239.0 / 1151165299.0,
  118820643.0 / 751138087.0,
  -528747749.0 / 2220607170.0,
  1.0 / 4.0
};

static const double A[] = {
  13451932.0 / 455176623.0,
  0.0,
  0.0,
  0.0,
  0.0,
  -808719846.0 / 976000145.0,
  1757004468.0 / 5645159321.0,
  656045339.0 / 265891186.0,
  -3867574721.0 / 1518517206.0,
  465885868.0 / 322736535.0,
  53011238.0 / 667516719.0,
  2.0 / 45.0
};

static const double ah[] = {
  1.0 / 18.0,
  1.0 / 12.0,
  1.0 / 8.0,
  5.0 / 16.0,
  3.0 / 8.0,
  59.0 / 400.0,
  93.0 / 200.0,
  5490023248.0 / 9719169821.0,
  13.0 / 20.0,
  1201146811.0 / 1299019798.0
};

static const double b21 = 1.0 / 18.0;
static const double b3[] = { 1.0 / 48.0, 1.0 / 16.0 };
static const double b4[] = { 1.0 / 32.0, 0.0, 3.0 / 32.0 };
static const double b5[] = { 5.0 / 16.0, 0.0, -75.0 / 64.0, 75.0 / 64.0 };
static const double b6[] = { 3.0 / 80.0, 0.0, 0.0, 3.0 / 16.0, 3.0 / 20.0 };
static const double b7[] = {
  29443841.0 / 614563906.0,
  0.0,
  0.0,
  77736538.0 / 692538347.0,
  -28693883.0 / 1125000000.0,
  23124283.0 / 1800000000.0
};
static const double b8[] = {
  16016141.0 / 946692911.0,
  0.0,
  0.0,
  61564180.0 / 158732637.0,
  22789713.0 / 633445777.0,
  545815736.0 / 2771057229.0,
  -180193667.0 / 1043307555.0
};
static const double b9[] = {
  39632708.0 / 573591083.0,
  0.0,
  0.0,
  -433636366.0 / 683701615.0,
  -421739975.0 / 2616292301.0,
  100302831.0 / 723423059.0,
  790204164.0 / 839813087.0,
  800635310.0 / 3783071287.0
};
static const double b10[] = {
  246121993.0 / 1340847787.0,
  0.0,
  0.0,
  -37695042795.0 / 15268766246.0,
  -309121744.0 / 1061227803.0,
  -12992083.0 / 490766935.0,
  6005943493.0 / 2108947869.0,
  393006217.0 / 1396673457.0,
  123872331.0 / 1001029789.0
};
static const double b11[] = {
  -1028468189.0 / 846180014.0,
  0.0,
  0.0,
  8478235783.0 / 508512852.0,
  1311729495.0 / 1432422823.0,
  -10304129995.0 / 1701304382.0,
  -48777925059.0 / 3047939560.0,
  15336726248.0 / 1032824649.0,
  -45442868181.0 / 3398467696.0,
  3065993473.0 / 597172653.0
};
static const double b12[] = {
  185892177.0 / 718116043.0,
  0.0,
  0.0,
  -3185094517.0 / 667107341.0,
  -477755414.0 / 1098053517.0,
  -703635378.0 / 230739211.0,
  5731566787.0 / 1027545527.0,
  5232866602.0 / 850066563.0,
  -4093664535.0 / 808688257.0,
  3962137247.0 / 1805957418.0,
  65686358.0 / 487910083.0
};
static const double b13[] = {
  403863854.0 / 491063109.0,
  0.0,
  0.0,
  -5068492393.0 / 434740067.0,
  -411421997.0 / 543043805.0,
  652783627.0 / 914296604.0,
  11173962825.0 / 925320556.0,
  -13158990841.0 / 6184727034.0,
  3936647629.0 / 1978049680.0,
  -160528059.0 / 685178525.0,
  248638103.0 / 1413531060.0,
  0.0
};

typedef struct
{
  double *k[13];
  double *ytmp;
  double *y0;
}
rk8pd_state_t;

static void *
rk8pd_alloc (size_t dim)
{
  rk8pd_state_t *state = (rk8pd_state_t *) malloc (sizeof (rk8pd_state_t));
  int i, j;

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

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

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

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

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

  for (i = 0; i < 13; i++)
    {
      state->k[i] = (double *) malloc (dim * sizeof (double));

      if (state->k[i] == 0)
        {
          for (j = 0; j < i; j++)
            {
              free (state->k[j]);
            }
          free (state->y0);
          free (state->ytmp);
          free (state);
          GSL_ERROR_NULL ("failed to allocate space for k's", GSL_ENOMEM);
        }
    }

  return state;
}

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

  size_t i;

  double *const ytmp = state->ytmp;
  double *const y0 = state->y0;
  /* Note that k1 is stored in state->k[0] due to zero-based indexing */
  double *const k1 = state->k[0];
  double *const k2 = state->k[1];
  double *const k3 = state->k[2];
  double *const k4 = state->k[3];
  double *const k5 = state->k[4];
  double *const k6 = state->k[5];
  double *const k7 = state->k[6];
  double *const k8 = state->k[7];
  double *const k9 = state->k[8];
  double *const k10 = state->k[9];
  double *const k11 = state->k[10];
  double *const k12 = state->k[11];
  double *const k13 = state->k[12];

  DBL_MEMCPY (y0, y, dim);

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

      if (s != GSL_SUCCESS)
	{
	  return s;
	}
    }

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

  /* k2 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[0] * h, ytmp, k2);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

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

  /* k3 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[1] * h, ytmp, k3);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

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

  /* k4 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[2] * h, ytmp, k4);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + h * (b5[0] * k1[i] + b5[2] * k3[i] + b5[3] * k4[i]);

  /* k5 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[3] * h, ytmp, k5);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + h * (b6[0] * k1[i] + b6[3] * k4[i] + b6[4] * k5[i]);

  /* k6 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[4] * h, ytmp, k6);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b7[0] * k1[i] + b7[3] * k4[i] + b7[4] * k5[i] +
                  b7[5] * k6[i]);

  /* k7 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[5] * h, ytmp, k7);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b8[0] * k1[i] + b8[3] * k4[i] + b8[4] * k5[i] +
                  b8[5] * k6[i] + b8[6] * k7[i]);

  /* k8 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[6] * h, ytmp, k8);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b9[0] * k1[i] + b9[3] * k4[i] + b9[4] * k5[i] +
                  b9[5] * k6[i] + b9[6] * k7[i] + b9[7] * k8[i]);

  /* k9 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[7] * h, ytmp, k9);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b10[0] * k1[i] + b10[3] * k4[i] + b10[4] * k5[i] +
                  b10[5] * k6[i] + b10[6] * k7[i] + b10[7] * k8[i] +
                  b10[8] * k9[i]);

  /* k10 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[8] * h, ytmp, k10);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b11[0] * k1[i] + b11[3] * k4[i] + b11[4] * k5[i] +
                  b11[5] * k6[i] + b11[6] * k7[i] + b11[7] * k8[i] +
                  b11[8] * k9[i] + b11[9] * k10[i]);

  /* k11 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[9] * h, ytmp, k11);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b12[0] * k1[i] + b12[3] * k4[i] + b12[4] * k5[i] +
                  b12[5] * k6[i] + b12[6] * k7[i] + b12[7] * k8[i] +
                  b12[8] * k9[i] + b12[9] * k10[i] + b12[10] * k11[i]);

  /* k12 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp, k12);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b13[0] * k1[i] + b13[3] * k4[i] + b13[4] * k5[i] +
                  b13[5] * k6[i] + b13[6] * k7[i] + b13[7] * k8[i] +
                  b13[8] * k9[i] + b13[9] * k10[i] + b13[10] * k11[i] +
                  b13[11] * k12[i]);

  /* k13 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp, k13);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  /* final sum  */
  for (i = 0; i < dim; i++)
    {
      const double ksum8 =
        Abar[0] * k1[i] + Abar[5] * k6[i] + Abar[6] * k7[i] +
        Abar[7] * k8[i] + Abar[8] * k9[i] + Abar[9] * k10[i] +
        Abar[10] * k11[i] + Abar[11] * k12[i] + Abar[12] * k13[i];
      y[i] += h * ksum8;
    }

  /* Evaluate dydt_out[]. */

  if (dydt_out != NULL)
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);

      if (s != GSL_SUCCESS)
	{
	  /* Restore initial values */
	  DBL_MEMCPY (y, y0, dim);
	  return s;
	}
    }

  /* error estimate */
  for (i = 0; i < dim; i++)
    {
      const double ksum8 =
        Abar[0] * k1[i] + Abar[5] * k6[i] + Abar[6] * k7[i] +
        Abar[7] * k8[i] + Abar[8] * k9[i] + Abar[9] * k10[i] +
        Abar[10] * k11[i] + Abar[11] * k12[i] + Abar[12] * k13[i];
      const double ksum7 =
        A[0] * k1[i] + A[5] * k6[i] + A[6] * k7[i] + A[7] * k8[i] +
        A[8] * k9[i] + A[9] * k10[i] + A[10] * k11[i] + A[11] * k12[i];
      yerr[i] = h * (ksum7 - ksum8);
    }

  return GSL_SUCCESS;
}

static int
rk8pd_reset (void *vstate, size_t dim)
{
  rk8pd_state_t *state = (rk8pd_state_t *) vstate;

  int i;

  for (i = 0; i < 13; i++)
    {
      DBL_ZERO_MEMSET (state->k[i], dim);
    }

  DBL_ZERO_MEMSET (state->y0, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk8pd_order (void *vstate)
{
  rk8pd_state_t *state = (rk8pd_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 8;
}

static void
rk8pd_free (void *vstate)
{
  rk8pd_state_t *state = (rk8pd_state_t *) vstate;
  int i;

  for (i = 0; i < 13; i++)
    {
      free (state->k[i]);
    }
  free (state->y0);
  free (state->ytmp);
  free (state);
}

static const gsl_odeiv_step_type rk8pd_type = { "rk8pd",        /* name */
  1,                            /* can use dydt_in */
  1,                            /* gives exact dydt_out */
  &rk8pd_alloc,
  &rk8pd_apply,
  &rk8pd_reset,
  &rk8pd_order,
  &rk8pd_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rk8pd = &rk8pd_type;


/* multifit/gsl_multifit.h
 *
 * Copyright (C) 2000 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_MULTIFIT_H__
#define __GSL_MULTIFIT_H__

#include <stdlib.h>
#include <gsl/gsl_math.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 n; /* number of observations */
  size_t p; /* number of parameters */
  gsl_matrix * A;
  gsl_matrix * Q;
  gsl_matrix * QSI;
  gsl_vector * S;
  gsl_vector * t;
  gsl_vector * xt;
  gsl_vector * D;
}
gsl_multifit_linear_workspace;

gsl_multifit_linear_workspace *
gsl_multifit_linear_alloc (size_t n, size_t p);

void
gsl_multifit_linear_free (gsl_multifit_linear_workspace * work);

int
gsl_multifit_linear (const gsl_matrix * X,
                     const gsl_vector * y,
                     gsl_vector * c,
                     gsl_matrix * cov,
                     double * chisq,
                     gsl_multifit_linear_workspace * work);

int
gsl_multifit_linear_svd (const gsl_matrix * X,
                         const gsl_vector * y,
                         double tol,
                         size_t * rank,
                         gsl_vector * c,
                         gsl_matrix * cov,
                         double *chisq,
                         gsl_multifit_linear_workspace * work);

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);

int
gsl_multifit_wlinear_svd (const gsl_matrix * X,
                          const gsl_vector * w,
                          const gsl_vector * y,
                          double tol,
                          size_t * rank,
                          gsl_vector * c,
                          gsl_matrix * cov,
                          double *chisq,
                          gsl_multifit_linear_workspace * work);

int
gsl_multifit_linear_est (const gsl_vector * x,
                         const gsl_vector * c,
                         const gsl_matrix * cov, double *y, double *y_err);

__END_DECLS

#endif /* __GSL_MULTIFIT_H__ */