/* Version 6 of lawcjrk.cpp for mcmc.py */

/*      GENERAL DESCRIPTION
	This program uses Markov chain Monte Carlo to obtain an optimal
	solution to three basic equations for a rating scale experiment 
	with N stimuli and M responses. It allows constraints 
	on various subsets of parameters. The models are: the signal detection 
        model (SDT) with signals as Gaussian densities and criteria as Dirac pulses;
        the complementary SDT model (CSDT) with signals as Dirac pulses and criteria as
        Gaussian densities; and the full signal detection model (FSDT)
        where signals and criteria all form Gaussian densities.

	There are four types of parameters: Signal means, signal standard deviations,
	criterion means, and criterion standard deviations. Either all members of
	a type are unconstrained or else all are constrained.

	The program models probabilistic choices between three decision rules.
	Rule 1 from Rosner & Kochanski (2010) uses the largest negative difference
        between a stimulus sample and an array of criterion samples. Rule 2 from  
        Klauer & Kellen (2011) uses the smallest positive difference between a stimulus 
	sample and an array of criterion samples. Rule 3 from Klauer & Kellen (2011) 
	uses absolute differences. The three probabilities PR1, PR2, and PR3 determine how
	a probabilistic choice is made between rules. They give the probability of selecting rules
	1, 2, and 3, respectively. The program can optimize two or all three parameters, 
	or all three can be constrained. See table below under STARTING PARAMETERS.

	*********** INITIALIZATION *************************************

        Input to the program is via a file in the following format.  Numbers are in
	free format. All tokens are separated from each other by any combination of
	spaces, tabs, or newlines.
	  INITIALIZATION ITEMS
          header: a single text token (i.e. anything that's not whitespace);
	          this is just copied into the output file.
	  nums: The number of stimuli 
	  numr: The number of responses
	  sdtev: 1 if SDT equal-variance model with signal sds equal and constrained
	         and all criterion sds constrained to 0
	  sdtuv: 1 if SDT unequal-variance model with signal sds unconstrained
	         and all criterion sds constrained to 0
	  csdtev:  1 if CSDT model with signal sds constrained to 0
	         and all criterion sds equal
	  csdtuv:  1 if CSDT model with signal sds constrained to 0
	         and all criterion sds unconstrained
 	  fsdt: 1 if FSDT model with no  constraints
	  decr_r: Decision rule (11=Rule 1 only; 12=Rule 2 only; 13=Rule 3 only; 14=Rules 1 and 2;
	                         15=Rules 1 and 3; 16=Rules 2 and 3; 17=all three rules)
	  stdz: 1 if standard scle for Gaussian parameters. First signal mwan made 0; first
                  free sugnal or criterion sd set to 1.
	  qracc: qromox accuracy
	  liseed: seed for randomization
 	  simpx_mult: Multiplying factor to jitter simplex; must be < 1
	  append_output: 0 if new output file, 1 if output appended to previous
                          output file of same name.
	  gf_samps: number of MCMC samples for calculating significance

         STARTING PARAMETERS 
          header
	  A vector of length 2N+2M+3. Both unconstrained and constrained variables appear here.
	  The order is model parameters: signal means,  signal standard deviations, criterion means,
	  criterion standard deviations; and probabilities: PR1, PR2, and P3. Unconstrained model
	  parameters must be nonzero to avoid a degenerate simplex. Signal means and criterion
	  means must each be in increasing order. If all signal standard deviations are constrained 
	  to zero, the complementary model is invoked. If all criterion standard deviations 
	  are constrained to zero, the SDT model is invoked. Finally, PR1, PR2, and PR3 must 
	  add to unity. Each can be constrained to 0 or to 1 or else one or two of them are free (f).
	  These constraints determine which decision rule is being modelled:

	  DECISION RULE   PR1   PR2    PR3
	        11        1     0      0     Rule 1 only
		12        0     1      0     Rule 2 only
		13        0     0      1     Rule 3 only
		14        f    -1      0     Rule 1 and Rule 2
	        15        f     0     -1     Rule 1 and Rule 3
		16        0     f     -1     Rule 3 and Rule 3
		17        f     f     -1     Rule 1, Rule 2, and Rule 3
		(f=free; -1 means probability determined by free PR)  

          CONSTRAINTS
          header
	  A vector of length 2N+2M+3 containing values of constrained variables 
	  or -100 for each unconstrained variable. The order is the same as for the
	  starting values.

          DATA
          header
          data matrix: Results for one stimulus in each row, free format, nums X numr size
	  There is no carriage return after the final entry.

        END OF INITIALIZATION ITEMS         

	PARAMETERS AND OVERDETERMINATION
	There are N stimuli, M criteria, and M+1 responses. The data matrix has NM degrees
	of freedom. For  overdetermination of parameters in any model, NM must
	exceed the number of free parameters.

        The SDT model always has 2N+M free parameters. All criterion sd's must be 
	constrained to zero. PR1 must be constrained to unity, and PR2 and PR3 
	must be constrained to 0.

        For the CSDT_EV, CSDT_UV, and FSDT models, the number of free parameters depends on the
	decision rule. In the former model, all signal sd's are constrained to zero.
	This table shows the number of free parameters in the CSDT and FSDT models.

	         DECISION RULE dec_r    SDTEV    SDTUV        CSDTEV   CSDTUV        FSDT
		   11, 12, 13           N+1+M    2*N+M        N+M+1    N+2M          2N+2M
		   14, 15, 16                                 N+M+2    N+2M+1        2N+2M+1
		   17                                         N+M+3    N+2M+2        2N+2M+2

        END OF GENERAL DESCRIPTION     */

// Includes
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_sort_double.h>
#include <gpklib.h>
#include <gpklib/romberg.h>
#include <gpklib/ieee754.h>
#include "gpklib/NaN.h"
#include <assert.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include "lawcjcrk.h"
#include "wsd.h"

#define D	if(0)

C double KMAX = 5;
#define INTALG trapzd
C BOOL RATIONAL_INNER = TRUE;
C BOOL RATIONAL_OUTER = FALSE;


struct info_s {
    double mus;
    double sigmas;
    int n;	// How many criteria
    int j;	// Which criterion are we calculating
    double *mu; // Criterion means
    double *sigma;	// Criterion sigmas
    C struct initialization_s *isp;
    double qracc() C { return isp->qracc; }
    enum RKKKRule rule;
};

struct infoinner_s : info_s {
	double s;

	infoinner_s( info_s *outer, double ss);
	double qracc()  { return 3.0*isp->qracc; }
	};

infoinner_s::infoinner_s( info_s *outer, double ss)
		: s(ss)
{
 assert(outer);
 assert(outer->sigmas >= 0.0);
 assert(outer->isp);
 mus = outer->mus; sigmas = outer->sigmas;
 n = outer->n; j = outer->j;
 mu = outer->mu; sigma = outer->sigma;
 rule = outer->rule;
 assert(outer->isp);
 isp = outer->isp;
}

int initialization_s::num_decision_probs() const {
  if(dec_r == RULES1_2_3)
  	return 2;
  else if(dec_r == RULES1_2 || dec_r == RULES1_3 || dec_r == RULES2_3)
  	return 1;
  else return 0;
}

static bool NotConstrained(const struct initialization_s *lcjc_init, int which) {
    assert( which >= 0 && which < lcjc_init->nallparams );
    // Make sure -100.0 isn't lost due to rounding errors.
    return lcjc_init->constraints [ which ] <= -99.9999;
}

static double ReadFloat(FILE* inPtr) {
    char ahold[1024];
    bool ok = fscanf ( inPtr, "%s", ahold ) == 1;
    assert(ok);
    double rv = safe_atof(ahold);
    assert( finite(rv) );
    return rv;
}

static int ReadInt(FILE* inPtr) {
    char ahold[1024];
    bool ok = fscanf ( inPtr, "%s", ahold ) == 1;
    assert(ok);
    int rv = safe_atoi(ahold);
    assert( rv != INT_NAN && rv != INT_Max && rv != INT_Min);
    return rv;
}

DecRule CheckValidDecisionRule(int x) {
    assert(x >= RULE1 && x <= RULES1_2_3);
    return static_cast<DecRule>(x);
}
 
void print_model_type ( C struct initialization_s *init_ptr) {
    if ( init_ptr->sdtev )
    	fprintf ( init_ptr->outPtr, "\n   Classical equal-variance signal detection model" );
    if ( init_ptr->sdtuv )
    	fprintf ( init_ptr->outPtr, "\n   Classical unequal-variance signal detection model" );
    if ( init_ptr->csdtev )
	 fprintf ( init_ptr->outPtr, "\n   Complementary signal detection model with equal criterion variances" );
    if ( init_ptr->csdtuv )
	 fprintf ( init_ptr->outPtr, "\n   Complementary signal detection model with unequal criterion variances" );
    if ( init_ptr->fsdt ) {
         fprintf ( init_ptr->outPtr, "\n   Full signal detection model" );

	 switch ( init_ptr->dec_r ) {
	 case RULE1:
	      fprintf ( init_ptr->outPtr, "\n  Rule 1 only\n\n " );
	      break;

	 case RULE2:
	      fprintf ( init_ptr->outPtr, "\n  Rule 2 only\n\n " );
	      break;

	 case RULE3:
	      fprintf ( init_ptr->outPtr, "\n  Rule 3 only\n\n " );
	      break;

	 case RULES1_2: 
	      fprintf ( init_ptr->outPtr, "\n   Probability of Rule 1 vs Rule 2\n\n " );
	      break;

	 case RULES1_3:
	      fprintf ( init_ptr->outPtr, "\n    Probability of Rule 1 vs Rule 3\n\n " ); 
	      break;

	 case RULES2_3:
	      fprintf ( init_ptr->outPtr, "\n    Probability of Rule 2 vs Rule 3\n\n " ); 
	      break;

	 case RULES1_2_3:
	      fprintf ( init_ptr->outPtr, "\n     Probability of Rule 1 vs Rule 2 vs Rule 3\n\n " ); 
	      break;
	 }
    }
}


/* Do all initialization and read in the data.
   This fills in all the fields in structure lcjc_init. 
   No other global variables are allowed!
   Pointers to the structure are pased to other functions.
   struct initialization_s is defined in lawcjcrk.h.          */

void lawcjc_init ( const char *filenamei, const char *filenameo, struct initialization_s *lcjc_init )
{
    FILE *inPtr;
    int ok;
    char ahold[1024];

    // Set up input file  
    inPtr = fopen ( filenamei, "r" );
    assert ( inPtr != NULL );

    // Set up output file  
    lcjc_init->outPtr = fopen ( filenameo, "w+t" );
    assert ( lcjc_init->outPtr != NULL );

    // Read control variables from file 
    ok = fscanf ( inPtr, "%s", lcjc_init->header ) == 1;
    assert (ok);
    int modtp = 0;
    lcjc_init->nums = ReadInt( inPtr );
    lcjc_init->numr = ReadInt( inPtr );
    lcjc_init->sdtev = ReadInt( inPtr );
    modtp += lcjc_init->sdtev;
    lcjc_init->sdtuv = ReadInt( inPtr );
    modtp += lcjc_init->sdtuv;
    lcjc_init->csdtev = ReadInt( inPtr );
    modtp += lcjc_init->csdtev;
    lcjc_init->csdtuv = ReadInt( inPtr );
    modtp += lcjc_init->csdtuv;
    lcjc_init->fsdt = ReadInt( inPtr );
    modtp += lcjc_init->fsdt;
    assert( modtp );
    lcjc_init->dec_r = CheckValidDecisionRule(ReadInt( inPtr ));
    lcjc_init->stdz = ReadInt( inPtr );
    lcjc_init->qracc = ReadFloat( inPtr );
    assert(lcjc_init->qracc > 0.0);

    // Read seed and set random number generator
    int liseed = ReadInt( inPtr );
    lcjc_init->random = gsl_rng_alloc( gsl_rng_taus );
    gsl_rng_set( lcjc_init->random, liseed );

    // Read remaining control variiables
    lcjc_init->simpx_mult = ReadFloat( inPtr );
    assert ( lcjc_init->simpx_mult < 1.0 && lcjc_init->simpx_mult > 0.0 );
    lcjc_init->append_output = ReadInt( inPtr );
    assert(lcjc_init->append_output==0 || lcjc_init->append_output==1);
    lcjc_init->gf_samps = ReadInt( inPtr );
    assert( lcjc_init->gf_samps >= 0 );
    // printf( "\n Initial values read " );

    // Calculate other needed values from control variables 
    lcjc_init->nallparams = 2 * (lcjc_init->nums + lcjc_init->numc()) + 3;
    lcjc_init->s_mstart = 0;
    lcjc_init->s_mend = lcjc_init->nums - 1;
    lcjc_init->s_sdstart = lcjc_init->s_mend + 1;
    lcjc_init->s_sdend = lcjc_init->s_sdstart + lcjc_init->nums - 1;
    lcjc_init->c_mstart = lcjc_init->s_sdend + 1;
    lcjc_init->c_mend = lcjc_init->c_mstart + lcjc_init->numc() - 1;
    lcjc_init->c_sdstart = lcjc_init->c_mend + 1;
    lcjc_init->c_sdend = lcjc_init->c_sdstart + lcjc_init->numc() - 1;
    lcjc_init->pstart = lcjc_init->c_sdend + 1;
    assert(lcjc_init->pstart == lcjc_init->nallparams - 3);

    // Allocate storage for parameters, constraints, data, and probability matrices      
    lcjc_init->allparameters = double1alloc(lcjc_init->nallparams );
    printf( "\n number parameters = %d\n", lcjc_init->nallparams );
    lcjc_init->constraints = double1alloc(lcjc_init->nallparams );
    lcjc_init->solution = double1alloc(lcjc_init->nallparams );
    lcjc_init->data = int2alloc(lcjc_init->nums, lcjc_init->numr );
    lcjc_init->psd_mat = int2alloc(lcjc_init->nums, lcjc_init->numr );
    lcjc_init->prop_mat = double2alloc(lcjc_init->nums, lcjc_init->numr );
    lcjc_init->dprob_1_mat = double2alloc(lcjc_init->nums, lcjc_init->numr );
    lcjc_init->dprob_2_mat = double2alloc(lcjc_init->nums, lcjc_init->numr );
    lcjc_init->dprob_3_mat = double2alloc(lcjc_init->nums, lcjc_init->numr );
    lcjc_init->dprob_mat = double2alloc(lcjc_init->nums, lcjc_init->numr );
    lcjc_init->dcump_mat = double2alloc(lcjc_init->nums, lcjc_init->numr );
    lcjc_init->re_mat = double2alloc(lcjc_init->nums, lcjc_init->numr );
    lcjc_init->e_mat = double2alloc(lcjc_init->nums, lcjc_init->numr );

    // Read parameters; last three are PR1, PR2, PR3; dump initial text   
    ok = fscanf ( inPtr, "%s", ahold ) == 1;
    assert (ok);
    printf( "Parameter Title: %s\n", ahold);
    for ( int i = 0; i < lcjc_init->nallparams; i++ ) {
        lcjc_init->allparameters [ i ] = ReadFloat( inPtr );
    }

    // Check that PR1, PR2, and PR3 add to unity.
    double psum = 0;
    psum = ( lcjc_init->allparameters [ lcjc_init->pstart ] +
	     lcjc_init->allparameters [ lcjc_init->pstart + 1 ] +
	     lcjc_init->allparameters [ lcjc_init->pstart + 2 ]  );

    // Read constraints; dump initial text   
    ok = fscanf ( inPtr, "%s", ahold ) == 1;
    assert (ok);
    printf( "Constraints Title: %s\n", ahold);
    for ( int i = 0; i < lcjc_init->nallparams; i++ ) {
        lcjc_init->constraints [ i ] = ReadFloat( inPtr );
    }
    
    // Get numbers of unconstrained and constrained parameters 
    lcjc_init->nunconstrained = 0;
    for ( int i = 0; i < lcjc_init->nallparams; i++ ) 
        if ( NotConstrained(lcjc_init, i) ) lcjc_init->nunconstrained++;
    printf( "\n nunconstrained = %d", lcjc_init->nunconstrained );
    lcjc_init->free_sm = 0;
    for ( int i = lcjc_init->s_mstart; i <= lcjc_init->s_mend; i++ ) 
        if ( NotConstrained(lcjc_init, i) ) lcjc_init->free_sm++;
    assert ( ( lcjc_init->free_sm == lcjc_init->nums ) || ( lcjc_init->free_sm == 0 ) ); 
    printf ( "\n free_sm = %d", lcjc_init->free_sm ); 
    lcjc_init->free_ssd = 0;
    for ( int i = lcjc_init->s_sdstart; i <= lcjc_init->s_sdend; i++ )
        if ( NotConstrained(lcjc_init, i) ) lcjc_init->free_ssd++;
    assert ( ( lcjc_init->free_ssd == lcjc_init->nums ) || ( lcjc_init->free_ssd == 0 ) ); 
    if ( lcjc_init->free_ssd == 0 ) assert ( ( lcjc_init->sdtev ) || ( lcjc_init->csdtev ) ||
					     ( lcjc_init->csdtuv ) ); 
    printf ( "\n free_ssd = %d",lcjc_init->free_ssd ); 
    lcjc_init->free_cm = 0;
    for ( int i = lcjc_init->c_mstart; i <= lcjc_init->c_mend; i++ ) 
        if ( NotConstrained(lcjc_init, i) ) lcjc_init->free_cm++;
    assert ( ( lcjc_init->free_cm == lcjc_init->numc() ) || ( lcjc_init->free_cm == 0 ) ); 
    printf ( "\n free_cm = %d", lcjc_init->free_cm ); 
    lcjc_init->free_csd = 0;
    for ( int i = lcjc_init->c_sdstart; i <= lcjc_init->c_sdend; i++ ) 
        if ( NotConstrained(lcjc_init, i) )
	    lcjc_init->free_csd++;
    assert ( ( lcjc_init->free_csd == lcjc_init->numc() ) || ( lcjc_init->free_csd == 0 ) ); 
    if ( lcjc_init->free_csd == 0 ) assert ( ( lcjc_init->sdtev == 1 ) || ( lcjc_init->sdtuv == 1 ) || ( lcjc_init->csdtev == 1 ) ); 
    printf ( "\n free_csd = %d\n", lcjc_init->free_csd );
    fprintf ( lcjc_init->outPtr, "\n\n\n  Optimization results");
    fprintf ( lcjc_init->outPtr, "\n  Law of categorical judgment: " );
    lcjc_init->nfree_modpars = lcjc_init->free_sm + lcjc_init->free_ssd + lcjc_init->free_cm + lcjc_init->free_csd;

    // assert( lcjc_init->nunconstrained ==  lcjc_init->nfree_modpars +
    //	                                      lcjc_init->num_decision_probs() );


    // Read data, dump initial text; get proportions.   
    ok = fscanf ( inPtr, "%s", ahold ) == 1;
    printf( "Data Title: %s\n", ahold );
    assert (ok);
    for ( int i = 0; i < lcjc_init->nums; i++ ) {
	int rsum = 0;
        for ( int j = 0; j < lcjc_init->numr; j++ ) {
            lcjc_init->data [ i ] [ j ] = ReadInt( inPtr );
	    rsum += lcjc_init->data [ i ] [ j ];
        }
        for ( int j = 0; j < lcjc_init->numr; j++ )
	    lcjc_init->prop_mat [ i ] [ j ] = lcjc_init->data [ i ] [ j ] / (double)rsum;
    }

    // Finally, this checks that we have reached the end of the file.
    ok = fscanf ( inPtr, "%s", ahold ) == 1;  // Read should fail.
    assert(!ok);
    // printf( "\n Initialization done " ); 
    fclose ( inPtr );
}


// Fill the prmvecs array with initial starting vectors, creating 'nguesses' guesses.
// prmvecs is a simplex with dimensions isp->nunconstrained by nguesses. 
void lawcjc_start ( int nguesses, double **prmvecs, C struct initialization_s *isp )
{

    // Start wp simplex with basic array of unconstrained parameters 
    int i = 0;
    for ( int j = 0; j < isp->nallparams; j++ ) {
	if ( NotConstrained(isp, j) ) {
            assert(i < isp->nunconstrained );
	    prmvecs [ 0 ] [ i ] = isp->allparameters [ j ];
	    i++;
	}
    }
    assert ( i == isp->nunconstrained );


    // Get the other ( nguesses - 1 ) arrays into prmvecs.
    for ( int i = 1; i < nguesses; i++ ) {
        const double dmult = isp->simpx_mult;
        const double umult = 2.0 - isp->simpx_mult;
	for ( int j = 0; j < isp->nfree_modpars; j++ ) {
            assert(j < isp->nunconstrained );
	    assert( fabs( prmvecs [0] [j]) > 1e-10);
	    // If any of the parameters are zero, then the following code leads to a
	    // degenerate simplex.
	    double ranno = gsl_rng_uniform_pos ( isp->random );
	    if ( ranno <= 0.5 )
	        prmvecs [ i ] [ j ] = dmult *  prmvecs [ 0 ] [ j ];
	    else
	        prmvecs [ i ] [ j ] = umult * prmvecs [ 0 ] [ j ];
	}

	int jj = isp->nfree_modpars;
	assert( jj <= isp->nunconstrained );
	// jj may now point to free probability
	// Check dec_r. If 14, 15, or 16, get one PR. If 17, get two.
	if ( isp->num_decision_probs() == 1 )
	    prmvecs [ i ] [ jj ] = gsl_rng_uniform_pos ( isp->random );
	else if ( isp->num_decision_probs() == 2 ) {
	    // We pick three random numbers whose sum is 1.0.
	    const double r1 = gsl_rng_uniform_pos( isp->random );
	    const double r2 = gsl_rng_uniform_pos( isp->random );
	    const double r3 = gsl_rng_uniform_pos( isp->random );
	    prmvecs [ i ] [ jj ] = r1 / (r1 + r2 + r3);
            assert( jj + 1 < isp->nunconstrained );
	    prmvecs [ i ] [ jj + 1 ] = r2 / (r1 + r2 + r3);

        }
	// end of scope for jj.


        // Sort signal means and criterion means in ascending order. 
	if ( isp->free_sm > 0 ) {
            double * sm_arr = double1alloc( isp->nums );
	    for ( int ic = 0; ic < isp->free_sm; ic++ ) {
                assert( ic < isp->nunconstrained  );
                sm_arr [ ic ] = prmvecs [ i ] [ ic ];
            }
	    gsl_sort( sm_arr, 1, isp->free_sm );
	    // Return values
	    for ( int ic = 0; ic < isp->free_sm; ic++ ) {
                assert(ic < isp->nunconstrained );
                prmvecs [ i ] [ ic ] = sm_arr [ ic ];
            }
            double1free( sm_arr );
	}

	if ( isp->free_cm > 0 ) {
            double * cm_arr = double1alloc( isp->numc() );
	    int j = 0;
	    // Check for sdt equal-variance or csdt which lack free signal sds
	    int pcm_st = isp->c_mstart;
	    int pcm_en = isp->c_mend; 
	    if ( ( isp->sdtev == 1 ) || ( isp->csdtev == 1 ) || ( isp->csdtuv == 1 ) ) {
	       pcm_st -= isp->nums;
	       pcm_en -= isp->nums;
	    }
	    // Now sort free criterion means
	    for ( int ic = pcm_st; ic <= pcm_en; ic++, j++ ) { 
                assert( ic < isp->nunconstrained  );
	        cm_arr [ j ] = prmvecs [ i ] [ ic ];
	    }
	    assert ( j==isp->free_cm );
	    gsl_sort( cm_arr, 1, isp->free_cm );
	    j = 0;
	    for ( int ic = pcm_st; ic <= pcm_en; ic++, j++ ) { 
                assert(ic < isp->nunconstrained );
	        if ( j > 0 ) assert ( cm_arr [ j ] >=  cm_arr [ j - 1 ] );
		prmvecs [ i ] [ ic ] = cm_arr [ j ];
            }
            assert ( j == isp->free_cm );
            double1free( cm_arr );
	}
    }
    printf( "\n  Simplex done\n" );
}

// Next two functions are used in function wproject below
// Projects x into some sensible value in [0,1].  This function is idempotent.
double ProjectInto01(double x) {
  bool again;
    do {
         again = true;
	 if ( ( x == 0.0 ) || ( x == 1.0 ) ) again = false;
	 else {
	   if (x < 0.0) x = -x;  // Fold.

	   if (x > 1.0) x = 2.0 - x;  // Fold.
	   if ( ( x < 1.0 ) && ( x > 0.0  ) ) again = false;
	 }
      } while( again );
      return x;
}


// Projects x and y into the triangle where x>=0, y>=0 and x+y <= 1.
void ProjectIntoTriangle(double *x, double *y) {
    bool again;
    do {
	again = true;
	if ( ( *x == 0.0 ) && ( *y == 0.0 ) ) again = false;
	else { 
          if (*x < 0.0) *x = -*x;
          if (*y < 0.0) *y = -*y;

          if (*x + *y > 1.0) {
            // Change coordinate system:
            double x_plus_y = *x + *y;
            double x_minus_y = *x - *y;
            // Fold
            x_plus_y = 2.0 - x_plus_y;
            // Convert back to original coordinate system:
            *x = 0.5 * (x_plus_y + x_minus_y);
            *y = 0.5 * (x_plus_y - x_minus_y);
          }
          if ( ( *x >= 0.0 ) && ( *y >= 0.0 ) && (*x + *y <= 1.0) ) again = false;
        }
    } while( again );
}


// Project into the allowable volume.
// p is a vector of parameters of length *isp->nunconstrained. Order: signal means; unconstrained signal
// sds; criterion means; unconstrained criterion sds.
int wprojct ( double *p, C struct initialization_s *isp )
{
    /*  Check that:
	    free signal means and free signal sds and free criterion sd's are positive;
	    free signal means are ordered and free criterion means are ordered;
	    each PR value is less than or equal to unity.
	Return 0 if any check fails.
	Normalize all signal and criterion parameters with average of any unconstrained sd's.
        Write values back into p          */


    double sdsum = 0;

    // Move unconstrained means into s_copy or c_copy and process
    // iq tracks progress through p
    int im = 0, iq = 0;
    printf( "\n  wproject started\n" );

    // All signal means are free, or else all are constrained so isp->free_sm = 0
    // If free, get means into s_copy
    if ( isp->free_sm > 0 ) {
        double *s_copy = double1alloc( isp->nums );
	for ( int ip = isp->s_mstart; ip <= isp->s_mend; ip++, im++ )
	           s_copy [ im ] = p [ ip ];
	assert ( im == isp->nums );
	iq += im;
        // Order signal means 
        gsl_sort( s_copy, 1, isp->free_sm );
	// Return signal means
	im = 0;
	for ( int ip = isp->s_mstart; ip <= isp->s_mend; ip++, im++ )  p [ ip ] = s_copy [ im ];
	assert ( im == isp->free_sm );
        printf( "\n s_copy means filled and sorted\n" );
        double1free( s_copy );
        // printf( "\n s_copy free\n " );
    }

    // Make sure that free signal sds are positive and affect count (iq) and sdsum.
    if ( isp->free_ssd > 0 ) {
       im = 0;
       for ( int ip = isp->s_sdstart; ip <= isp->s_sdend; ip++, im++ ) {
            p [ ip ] = fabs( p [ ip ] );
            sdsum += p [ ip ];
       }
       assert ( im == isp->free_ssd );
       iq += im;
    }

    // for ( int ip = isp->s_mstart; ip <= isp->s_mend; ip++ ) printf( " %6.3f", s_copy [ ip ]  );
    // printf ("\n");

    // If criterion means are free, getthem into c_copy and sort them
    // iq points to next free parameter in p
    if ( isp->free_cm > 0 ) {
        double *c_copy = double1alloc( isp->numc() );
        assert ( iq == isp->free_sm + isp->free_ssd );
	im = 0;
	for ( int ip = iq; ip < iq + isp->free_cm; ip++, im++ ) c_copy [ im ] = p [ ip ];
	assert( im == isp->numc());
        // Order criterion means 
        gsl_sort( c_copy, 1, isp->free_cm );
	// Return criterion means
	im = 0;
	for ( int ip = iq; ip < iq + isp->free_cm; ip++, im++ )  p [ ip ] = c_copy [ im ];
	assert ( im == isp->free_cm );
	assert( im == isp->numc());
	iq += im;
        printf( "\n c_copy means filled and sorted\n" );
        double1free( c_copy );
        // printf( "\n c_copy free " );
    }

    // Criterion standard deviations may be free. If so, iq points to the 
    //    possible first free criterion standard deviation. 
    // Check and add all free criterion standard deviations to sdsum.
    if ( isp->free_csd > 0 ) {
        for ( int ip = iq; ip < iq + isp->free_csd; ip++ ) {
           p [ ip ] = fabs( p [ ip ] );
           sdsum += p [ ip ];
        }
        assert( im == isp->numc());
	iq += im;
	assert ( iq == isp->nfree_modpars );
    }

    // for ( int ip = 0; ip < isp->numc(); ip++ ) printf( " %6.3f", c_copy [ ip ] [ 0 ] );
    // printf ("\n");
    // for ( int ip = 0; ip < isp->numc(); ip++ ) printf( " %6.3f", c_copy [ ip ] [ 1 ] );
    // printf( "\n sdsum = %f", sdsum );

    // Normalize unconstrained means and standard deviations 
    /* Get the normalizing value as the inverse of the sum of unconstrained
       standard deviations. If all are constrained, set normalizing value
       to unity.                                          */
    if ( sdsum > 0 )
    	sdsum /= ( isp->free_ssd + isp->free_csd ); 
    else sdsum = 1.0;
    // printf( "\n sdsum = %12.8f", sdsum );

    // printf ("\n p[] before chk_sort and  normalization\n");
    // for ( int ip = 0; ip < isp->nunconstrained; ip++ ) printf( " %6.3f", p[ ip ] );
    // printf ("\n");

    int ip;
    // Normalize all values of means and sds in p
    for ( ip = 0; ip < isp->nfree_modpars; ip++ ) p [ ip ] /= sdsum;
    assert( ip == isp->nfree_modpars);

    // printf ("\n p[] after chk_sort and normalization\n");
    // for ( int ip = 0; ip < isp->nunconstrained; ip++ ) printf( " %6.3f", p[ ip ] );
    // printf ("\n");

    // Check dec_r. If 14, 15, or 16, check one PR. If 17, check two.
    if ( isp->num_decision_probs() == 1 ) {
      p [ ip ] = ProjectInto01( p [ ip ] );
      ip++;
      // printf ( "\n PR = %f", p [ isp->pstart ] );
    } 
    if ( isp->num_decision_probs() == 2 ) {
        ProjectIntoTriangle( &p[ ip ], &p[ ip + 1 ] );
        ip += 2;
        //  printf ( "\n PR1 = %f, PR2 = %f", p [  isp->pstart - 1 ], p [ isp->pstart ] );
    }
    assert ( iq = isp->nunconstrained );
    printf( "\n nwproject calculations done " );
    fprintf( stderr, " \nwproject done\n\n " );
    return( 1 );
}


inline double dmax ( double a, double b )
{
    return a>b ? a : b;
}

inline double dmin ( double a, double b )
{
    return a<b ? a : b;
}


// Compute log likelihood: 
// *prmvec is a vector of parameters of length *isp->nunconstrained
double wsdo_logp ( C double *prmvec, C struct initialization_s *isp, bool dont_add_fudge_factor )
{

    printf( "\n Decision probs = %d\n ",  isp->num_decision_probs() );
    // printf( "\n  wsdo_logp started\n" );
    // Get rule weights PR1, PR2, PR3 into isp->allparameters [ ]
    switch ( isp->dec_r ) {
        case RULE1:
        case RULE2:
        case RULE3:
                break;
        case RULES1_2: 
	         isp->allparameters [ isp->pstart ] = prmvec [isp->nunconstrained - 1 ];
                 isp->allparameters [ isp->pstart + 1 ] = 1.0 - isp->allparameters [ isp->pstart ];
	         assert ( isp->allparameters[isp->pstart+2] == 0 );
	       break;
       case RULES1_3: 
	         isp->allparameters [ isp->pstart ] = prmvec [isp->nunconstrained - 1 ];
	         isp->allparameters [ isp->pstart + 2 ] = 1.0 - isp->allparameters [ isp->pstart ];
	         assert( isp->allparameters[ isp->pstart + 1] == 0 );
	         break;
        case RULES2_3: 
	         isp->allparameters [ isp->pstart + 1 ] = prmvec [isp->nunconstrained - 1 ];
	         isp->allparameters [ isp->pstart + 2 ] = 1.0 - isp->allparameters [ isp->pstart + 1 ];
	         assert ( isp->allparameters[isp->pstart] == 0 );
		 break;
        case RULES1_2_3: 
	         isp->allparameters [ isp->pstart ] = prmvec [isp->nunconstrained - 2 ];
	         isp->allparameters [ isp->pstart + 1 ] = prmvec [isp->nunconstrained - 1  ];
	         isp->allparameters [ isp->pstart + 2 ] = 1.0 - isp->allparameters [ isp->pstart ]
	                                                      - isp->allparameters [ isp->pstart + 1 ];
	        break;
    }
    double PR1 = isp->allparameters [ isp->pstart ];
    double PR2 = isp->allparameters [ isp->pstart + 1 ];
    double PR3 = isp->allparameters [ isp->pstart + 2 ];
    assert(PR1>=0.0 && PR1 <= 1.0);
    assert(PR2>=0.0 && PR2 <= 1.0);
    assert(PR3>=0.0 && PR3 <= 1.0);
    {
      // CHECK THAT PROBABILITIES ADD TO UNITY
    double psum = PR1 + PR2 + PR3;
    assert ( ( psum > 1.0 - EPS ) && ( psum < 1.0 + EPS )  );
    // printf( "\nProbabilities set; PR1 = %f, PR2 = %f, PR3 = %f", PR1, PR2, PR3 );

    }

    // Allocate p_hold and read in processed values
    double *p_hold = double1alloc( isp->nallparams );
    {
    int iwf = 0;
    // READ UNCONSTRAINED PARAMETERS FROM prmvec, CONSTRAINED ONES FROM isp->allparameters
    for ( int jwf = 0; jwf < isp->nallparams; jwf++ ) {
	if ( NotConstrained(isp, jwf) ) {
	    p_hold [ jwf ] = prmvec[ iwf ];
	    iwf++;
	}
        else p_hold [ jwf ] = isp->allparameters[ jwf ];
    }
    assert ( iwf = isp->nunconstrained );
    }
    printf( "\n" );
    fflush( stdout );
    // printf( "\n Parameters before main loop calculations\n ");
    // for ( int jwf = 0; jwf < isp->nallparams; jwf++ ) printf( "%12.6f", p_hold [ jwf ] );

    printf( "\n\n Main loop" );
    /* Main loop 1. Get probability matrices required by isp->dec_r. Combine them via PR1, PR2, PR3.
       Get cumulative probability matrix               */

    // CHECK THAT numc() IS WITHIN BOUNDS
    assert(0<=isp->numc() && isp->numc() < isp->numr);
    // THIS for loop GOES OVER MATRIX ROWS
    for ( int iwf = 0; iwf < isp->nums; iwf++ ) {
        // Check for fsdt  or csdt
        if ( ( isp->csdtev == 1 ) || ( isp->csdtuv == 1 ) || ( isp->csdtev == 1 ) || ( isp->fsdt == 1 ) ) {
	    // THIS for loop GOES ACROSS ALL BUT THE LAST ENTRY IN THE CHOSEN MATRIX ROW
	    // FOR dprob_1_mat and dprob_3_mat 
	    // AND ACROSS ALL BUT THE FIRST ENTRY IN THE CHOSEN MATRIX ROW FOR dprob_2_mat
	    // These missing entries are obtained by simpler and faster calculations by
	    // function missing_entry( ...  ) below.
	    for ( int jwf = 0; jwf < isp->numc(); jwf++ ) {
	        // CHECK FOR CSDT
	      if (( isp->csdtev == 1 ) || ( isp->csdtuv == 1 )) {
                    // CSDT MODEL. CHECK WHICH DECISION RULE IT USES
	            // Rules RULE1, RULES1_2, RULES1_3, or RULES1_2_3 (RK1)
	            if ( PR1 != 0 ) isp->dprob_1_mat [ iwf ] [ jwf ] = eq3a_wrap ( RKKK1, iwf, jwf, p_hold, isp );
	            else isp->dprob_1_mat [ iwf ] [ jwf ] = 0;

	            // Rules RULE2, RULES1_2, RULES2_3, or RULES1_2_3 (KK 2) DON'T GET INITIAL ENTRY, HENCE jwf + 1;
		    // FUNCTION missing_entry GETS FIRST ENTRY
	            if ( PR2 != 0 ) isp->dprob_2_mat [ iwf ] [ jwf + 1 ] = eq3a_wrap ( RKKK2, iwf, jwf, p_hold, isp );
	            else isp->dprob_2_mat [ iwf ] [ jwf + 1 ] = 0;

	            // Rules RULE3, RULES1_3, RULES2_3, or RULES1_2_3 (KK3)
	            if ( PR3 != 0 )	isp->dprob_3_mat [ iwf ] [ jwf ] = eq3a_wrap ( RKKK3B1, iwf, jwf, p_hold, isp );
	            else isp->dprob_3_mat [ iwf ] [ jwf ] = 0;
	        }
	        else {
                     // FSDT MODEL INVOKES eq3. CHECK WHICH DECISION RULE IT USES

 	            // Rules RULE1, RULES1_2, RULES1_3, or RULES1_2_3 (RK1)
	            if ( PR1 != 0 ) isp->dprob_1_mat [ iwf ] [ jwf ] = eq3_wrap ( RKKK1, iwf, jwf, p_hold, isp );
	            else ( isp->dprob_1_mat [ iwf ] [ jwf ] = 0 );

	            // Rules RULE2, RULES1_2, RULES2_3, or RULES1_2_3 (KK 2)  DON'T GET INITIAL ENTRY, HENCE jwf + 1;
		    // FUNCTION missing_entry GETS FIRST ENTRY
	            if ( PR2 != 0 ) isp->dprob_2_mat [ iwf ] [ jwf + 1 ] = eq3_wrap ( RKKK2, iwf, jwf, p_hold, isp );
	            else ( isp->dprob_2_mat [ iwf ] [ jwf + 1 ] = 0 );

	            // Rules RULE3, RULES1_3, RULES2_3, or RULES1_2_3 (KK3)
	            if ( PR3 != 0 )	isp->dprob_3_mat [ iwf ] [ jwf ] = eq3_wrap ( RKKK3B1, iwf, jwf, p_hold, isp );
	            else isp->dprob_3_mat [ iwf ] [ jwf ] = 0;

	        }
            }
            // Get last entry in each row of dprob_1_mat and dprob_3_mat 
            // Get first entry in each row of dprob_2_mat 
            // missing_entry CHECKS WHETHER MISSING ENTRY SHOULD BE ZERO 
            missing_entry ( isp->dprob_1_mat, isp, 0, isp->numc() - 1, PR1 );
            missing_entry ( isp->dprob_2_mat, isp, 1, isp->numc(), PR2 );
            missing_entry ( isp->dprob_3_mat, isp, 0, isp->numc() - 1, PR3 );
            // Combine Rule 1, Rule 2, and Rule 3 results
            for ( int jwf = 0; jwf < isp->numr; jwf++ ) {  
                isp->dprob_mat [ iwf ] [ jwf ] = PR1 *  isp->dprob_1_mat [ iwf ] [ jwf ]
	                                         + PR2 *  isp->dprob_2_mat [ iwf ] [ jwf     ]
				                 + PR3 *  isp->dprob_3_mat [ iwf ] [ jwf ];
                if ( ( isp->dprob_mat [ iwf ] [ jwf ] > 1.0 ) || ( isp->dprob_mat [ iwf ] [ jwf ] < 0 ) ) {
	            printf( "\n dprobs1,2,3 %6.5f, %6.5f, %6.5f ", 
                                isp->dprob_1_mat [ iwf ] [ jwf ],  isp->dprob_2_mat [ iwf ] [ jwf ],
		                isp->dprob_3_mat [ iwf ] [ jwf ] );
	            printf( "\n dprob, PR1, PR2, PR3 %6.5f, %6.5f, %6.5f, %6.5f ",
		               isp->dprob_mat [ iwf ] [ jwf ], PR1, PR2, PR3 );
	            exit( 5 );
	        }
                // Get cumulative probability matrix
                if ( jwf == 0 )
	            isp->dcump_mat [ iwf ] [ jwf ] = isp->dprob_mat [ iwf ] [ jwf ];
                else
	            isp->dcump_mat [ iwf ] [ jwf ] = isp->dcump_mat [ iwf ] [ jwf - 1 ] + 
     	                                             isp->dprob_mat [ iwf ] [ jwf ];
            }
        }     
        else {
    	    // SDT
            // Rule1 and Rule2 sdt probabilities are identical
	    // Function sdt computes p(R=j) over jwf=1, ... , numr
	    sdt ( iwf, p_hold, isp );
	}
    }
    // printf( "\n Parameters after main loop calculations\n ");
    // for ( int jwf = 0; jwf < isp->nallparams; jwf++ ) printf( "%12.6f", p_hold [ jwf ] );


    // Store possible solution
    for ( int jwf = 0; jwf < isp->nallparams; jwf++ )
        isp->solution [ jwf ] = p_hold [ jwf ];

    // GET LOGP
    double wl_f = wsdo_l_f ( isp );
    if ( dont_add_fudge_factor ) {
	printf( "\n logP no adjustment = %f", wl_f );
        double1free( p_hold );
        return wl_f ;
    }

    // ELSE GET FUDGE FACTOR
    double wl_fx = 0.0;
    // printf( "\n Signal means " );
    // for ( jwf = 0; jwf < isp->s_sdstart; jwf++ ) printf( "%3.7f ", p_hold [ jwf ] );
    // printf( "\n Signal sd's " );
    if ( isp->free_ssd != 0 ) {
      for ( int jwf = isp->s_sdstart; jwf <= isp->s_sdend; jwf++ ) {
	   wl_fx += .1/ p_hold [ jwf ];
	   // printf( "%3.7f ", p_hold [ jwf ] );
      }
    }
    // printf( "\n Criterion means " );
    // for ( jwf = isp->c_mstart; jwf < isp->c_sdstart; jwf++ ) printf( "%3.7f ", p_hold [ jwf ] );
    // printf( "\n Criterion sd's " );
    if ( isp->free_csd != 0 ) {
        for ( int jwf = isp->c_sdstart; jwf <= isp->c_sdend; jwf++ ) {
	    if ( NotConstrained(isp, jwf) )	wl_fx += .1/ p_hold [ jwf ];
	    // printf( "%3.7f ", p_hold [ jwf ] );
        }
    }
    // CLEAN UP AND ADJUST LOGP
    // printf( "\n logP = %f", wl_f );
    wl_f -= wl_fx;
    // if ( wl_f < -10000 ) {
    //  for ( int jwf = 0; jwf < isp->nallparams; jwf++ ) {
    //	printf( "\n %f", isp->solution [ jwf ] );
    //	printf( "\n wl_fx = %f", wl_fx );
    //  }
      // }
      // printf( "\n  Adjusted logP = %f", wl_f );
    // printf( "\n" );
    double1free( p_hold );
    return wl_f;
}

// IF rstart = 0, THIS FUNCTION GETS THE LAST ENTRY IN EACH ROW OF dpmat
// IF rstart = 1, THIS FUNCTION GETS THE FIRST ENTRY IN EACH ROW OF dpmat
// rstart AND rend SHOW HOW MANY ENTRIES ALREADY HAVE BEEN CALCULATED BY INTEGRATION
// IF PRx IS 0, MISSING ENTRY IS ZERO; OTHERWISE, MISSING ENTRY EQUALS 1 - SUM(ENTRIES IN ROW)
void missing_entry( double **dpmat, C struct initialization_s *isp, int r_start, int r_end, double PRx )
{

    if ( PRx == 0.0 ) {
      // Zero matrix, so Zero is the missing entry in each row
      for ( int iwf = 0; iwf < isp->nums; iwf++ ){
	// Rules 1 & 3
	if ( r_start == 0 ) dpmat [ iwf ] [ isp->numc() ] = 0;
	// Rule 2
	else  dpmat [ iwf ] [ 0 ] = 0;
      }
      return;
    }

    // PRx>0 This loop goes over each row of dpmat
    for ( int iwf = 0; iwf < isp->nums; iwf++ ) {
        double r_sum = 0.0;
        for ( int jwf = r_start; jwf <= r_end; jwf++ ) {
	   // Take care of rounding errors that give -0.0000 
	   dpmat [ iwf ] [ jwf ] = fabs( dpmat [ iwf ] [ jwf ]);
           r_sum += dpmat [ iwf ] [ jwf ];
	 }
      	 if ( r_sum < 1.0 ) {
	    if ( r_start == 0 ) dpmat [ iwf ] [ isp->numc() ] = 1.0 - r_sum;
	    else dpmat [ iwf ] [ 0 ] = 1.0 - r_sum;
	 }
	 else {
	     // Normalize current entries in row and zero missing entry 
             for ( int jwf = r_start; jwf <= r_end; jwf++ ) dpmat [ iwf ] [ jwf ] /= r_sum;
	     if ( r_start == 0 ) dpmat [ iwf ] [ isp->numc() ] = 0;
	     else dpmat [ iwf ] [ 0 ] = 0;
	 }

         // Check probabilities are in bounds
	 for ( int jwf = 0; jwf < isp->numr; jwf++ ) {
	   // if ( ( dpmat [ iwf ] [ jwf ] > 1.0 ) || ( dpmat [ iwf ] [ jwf ] < 0.0 ) ){
	   // printf ( "\n Illegal probability!" );
	   // printf ( "\n iwf = %d, jwf = %d, entry = %12.10f ", iwf, jwf, dpmat [ iwf ] [ jwf ] ); 
                 assert ( ( dpmat [ iwf ] [ jwf ] <= 1 ) && ( dpmat [ iwf ] [ jwf ] >= 0 ) );
	   // }
         }
    }
}

// Gaussian probability density.
double normal ( double x, double mu, double sigma )
{
    assert( sigma > 0 );
    double z = ( x - mu ) / sigma;
    return exp ( -z * z / 2.0 ) / (sqrt ( 2 * M_PI) * sigma );
}


// THIS FUNCTION APPLIES TO THE FSDT MODEL. IT STARTS COMPUTATION OF THE CONTENTS OF CELL [ ie3, je3 ] IN A
//  PROBABILITY MATRIX.
// rk1kk23 TELLS WHETHER COMPUTATION USES RULE 1, RULE 2, OR RULE 3. IT MUST BE 1, 2, OR 3. IF IT IS 3 AND
//   je !=0, SUBSEQUENT COMPUTATIONS GET BOTH PARTS A AND B OF KK EQUATION 3. 
// pp HOLDS ONE FULL SET OF PARAMETERS, WHETHER UNCONSTRAINED OR NOT. 
double eq3_wrap ( enum RKKKRule rk1kk23, int ie3, int je3, double *pp, C struct initialization_s *isp )
{

    int j3w = je3;

    // Get probability for rule rk1kk23 for cell ie3, je3
    // First, get signal mean and sigma, criterion means and sigmas
    C int nc = isp->numc();
    assert ( j3w >= 0 && j3w <= nc );
    C double mus = pp [ isp->s_mstart + ie3 ];
    C double sigmas = pp [ isp->s_sdstart + ie3 ];
    double *mu = double1alloc(nc + 1);
    double *sigma = double1alloc(nc + 1);
    double mumax = mus + 6 * sigmas; 
    double dprob;
    for( int i3w = 0; i3w < nc; i3w++ )	{
	mu [ i3w ] = pp [ isp->c_mstart + i3w ];
	sigma [ i3w ] = pp [ isp->c_sdstart + i3w ];
	if( mumax < mu [ i3w ] + 6 * sigma[ i3w ] )
		mumax = mu[ i3w ] + 6 * sigma[ i3w ];
    }
    mu [ nc ] = mumax + 6.0 * sigmas;
    sigma [ nc ] = sigmas;
    D printf ( "mu[%d]=%g sigma[%d]=%g\n", nc, mu [ nc ], nc, sigma [ nc ] );

    // Get probability from parameters. nc is number of criteria in mu, which
    // holds means and sigmas of criteria. mus and sigmas are signal mean and sd
    dprob  = eq3( rk1kk23, j3w, nc, mu, sigma, mus, sigmas, isp );

    double1free ( mu );
    double1free ( sigma );
    return dprob;
}

// CALLED FROM EQ3_WRAP
// Args:
//   rk1kk23: SELECTS RULE 1, RULE 2, OR RULE 3
//   j: WHICH CRITERION DENSITY ARE WE USING?
//   n: NUMBER OF CRITERIA
//   mu: vector of CRITERION means
//   sigma: vector of CRITERION sigmas
//   mus: SIGNAL MEAN ?
//   sigmas: SIGNAL STANDARD DEVIATION ?
double eq3 ( enum RKKKRule rk1kk23, int j, int n, double *mu, double *sigma, double mus, double sigmas,
		C struct initialization_s *isp )
{
    assert(isp);
    struct info_s info;
    info.j = j;
    info.sigma = sigma;
    info.n = n;
    info.mu = mu;
    info.mus = mus;
    info.sigmas = sigmas;
    info.isp = isp;
    info.rule = rk1kk23;
    C double minf = mus-sigmas * 6.0;
    C double mbreak = mus-sigmas * 2.0;
    C double pbreak = mus+sigmas * 2.0;
    C double pinf = mus+sigmas * 6.0;
    double sL;
    double sC;
    double sR;
    D printf ( "j=%d minf=%g, pinf=%g\n", j, minf, pinf );
    C double sguess = 0.2;
    C int kmax = 5;


    // This integration in three parts over the signal density applies to all values of rk1kk23 (1, 2, 3). 
    // n is number of criteria. info carries rule, signal mu and sd, and criteria means and sds
    sC = qromox ( outerkernel, mbreak, pbreak, trapzd, &info, n, info.qracc(), sguess, kmax, RATIONAL_OUTER );
    if(!finite(sC)) return quiet_nan(0);
    sL = qromox ( outerkernel, minf, mbreak, trapzd, &info, n, info.qracc(), 0.1*sguess+sC, kmax, 
		  RATIONAL_OUTER );
    if(!finite(sL)) return quiet_nan(0);
    sR = qromox ( outerkernel, pbreak, pinf, trapzd, &info, n, info.qracc(), 0.1*sguess+sC+sL, kmax, 
		  RATIONAL_OUTER );
    return sL + sC + sR;
}

double outerkernel ( double s,  void *infoptr, int nc )
{
    // fprintf(stderr, "Outerkernel %g\n", s);
    struct info_s *info = ( struct info_s * )infoptr;
    assert( info->isp );
    infoinner_s  iinner(info, s);
    double sC=0.0, sN=0.0, sP=0.0;

    int j = info->j;
    C double sguess = 0.2;
    C double pinf = dmax( info->mu[ j ], s ) + info->sigma[ j ] * 6.0;
    C double pbreak = dmax( info->mu[j] + info->sigma[ j ]*2.0, s + info->sigma[j]*1.0 );
    C double nbreak = info->mu[j] - info->sigma[ j ]*2.0;
    assert ( info->sigma[j] >= 0.0);
    assert ( pinf >= info->mu[j]);
    assert ( pinf >= s );
    double qbreak = info->mu[j] + info->sigma[ j ]*2.0;
    double mbreak = dmin( info->mu[j] - info->sigma[ j ]*2.0, s - info->sigma[ j ] );
    double pminf = dmin ( s, info->mu[j] ) - info->sigma[ j ]*6.0;
    assert ( pminf <= info->mu[j]);
    assert ( pminf <= s );

    if ( ( info->rule == RKKK1 ) || ( ( info->rule == RKKK3B1 ) && ( j == 0 ) ) ) {
        // FSDT model 
        // Rules (1) or (3A): integration from signal sample s to +infinity 
        // over jth criterion density. Next if() catches s=pinf.
        if ( pinf <= s ) return 0.0; 
        assert ( info->sigma[j] >= 0.0);
        assert ( pinf >= info->mu[j]);
        assert ( pinf >= s );
	// GPK: Shouldn't last assert be (pinf>s)?
	D printf ( "\ts=%g pinf=%g\n", s, pinf );
	D printf ( "\t*s=%g pinf=%g n=%g ik=%g\n", s, pinf, normal ( s, info->mus, info->sigmas ),
		   qromox ( innerkernel, s, pinf, INTALG, &iinner, nc, info->qracc(), sguess,
			    KMAX, RATIONAL_INNER ) );

	assert(s < pbreak);
	assert(nbreak < pbreak);
	// Get middle part of integral
	sC = qromox( innerkernel, dmax(s, nbreak), pbreak, INTALG, &iinner, nc, info->qracc(), sguess,
		     KMAX, RATIONAL_INNER );
	if(!finite(sC))	return quiet_nan(0);
	// Get bottom part
	if(s < nbreak) {
	  sN = qromox ( innerkernel, s, nbreak, INTALG, &iinner, nc, info->qracc(), 0.1*sguess+sC,
			KMAX, RATIONAL_INNER );
	  if(!finite(sN)) return quiet_nan(0);
    	}
	assert(pbreak < pinf);
	// Get top part
	sP = qromox( innerkernel, pbreak, pinf, INTALG, &iinner, nc, info->qracc(), 0.1*sguess+sC+sN,
		     KMAX, RATIONAL_INNER );
    }

    if ( info->rule == RKKK2 ) {
        // Rule (2)  This integrates over criterion density j from -infinity (pminf) to s, signal sample
	if ( pminf == s ) return 0.0;
	assert ( pminf <= s );
	// GPK Shouldn't that be: assert ( pminf < s )?
	D printf ( "\ts=%g pinf=%g\n", s, pminf );
	D printf ( "\t*s=%g pinf=%g n=%g ik=%g\n", pminf, s, normal ( s, info->mus, info->sigmas ),
		   qromox ( innerkernel, pminf, s, INTALG, &iinner, nc, info->qracc(), sguess, KMAX,
			    RATIONAL_INNER ) );
	assert(s > mbreak);
	assert(mbreak < qbreak);
	// Middle part of integral
        sC = qromox( innerkernel, mbreak, dmin(s, qbreak), INTALG, &iinner, nc, info->qracc(), sguess,
		     KMAX, RATIONAL_INNER );
	if(!finite(sC))	return quiet_nan(0);
	assert(mbreak > pminf);
	// Bottom of integral
        sN = qromox( innerkernel, pminf, mbreak, INTALG, &iinner, nc, info->qracc(), 0.1*sguess+sC+sN,
		     KMAX, RATIONAL_INNER );
	// Upper part of integral
	if(s > qbreak) {
	  sP = qromox ( innerkernel, qbreak, s, INTALG, &iinner, nc, info->qracc(), 0.1*sguess+sC,
			KMAX, RATIONAL_INNER );
	  if(!finite(sN)) return quiet_nan(0);
    	}
    }

    if ( ( info->rule == RKKK3B1 ) && ( j != 0 ) ) {
	// Rule (3B) Integral over criterion density j from s to infinity PLUS
        // integral over criterion density j-1 from  -infinity to s
        // fprintf( info->isp->outPtr, "\n outerkernel rule = %d\n", info->rule );
	// Start portion of integral over jth criterion density
	if ( pinf <= s ) return 0.0;
        assert ( info->sigma[j] >= 0.0);
        assert ( pinf >= info->mu[j]);
        assert ( pinf >= s );
	D printf ( "\ts=%g pinf=%g\n", s, pinf );
	D printf ( "\t*s=%g pinf=%g n=%g ik=%g\n", s, pinf, normal ( s, info->mus, info->sigmas ),
		   qromox ( innerkernel, s, pinf, INTALG, &iinner, nc, info->qracc(), sguess, KMAX,
		   RATIONAL_INNER ) );
	assert(s < pbreak);
	assert(nbreak < pbreak);

	// Middle part of first integral
	sC = qromox( innerkernel, dmax(s, nbreak), pbreak, INTALG, &iinner, nc, info->qracc(), sguess,
		     KMAX, RATIONAL_INNER );
	if(!finite(sC))	return quiet_nan(0);
        // fprintf( info->isp->outPtr, "\n sC done\n" );
	sN = 0.0;
	if(s < nbreak) {
	// Bottom part of first integral
	  sN = qromox ( innerkernel, s, nbreak, INTALG, &iinner, nc, info->qracc(), 0.1*sguess+sC,
			KMAX, RATIONAL_INNER );
	  if(!finite(sN)) return quiet_nan(0);
    	}
        // fprintf( info->isp->outPtr, "\n sN done\n" );
	// Upper part of first integral
	assert(pbreak < pinf);
	sP = qromox( innerkernel, pbreak, pinf, INTALG, &iinner, nc, info->qracc(), 0.1*sguess+sC+sN,
		     KMAX, RATIONAL_INNER );
	// End portion of integral over jth criterion density
        // fprintf( info->isp->outPtr, "\n sP done\n" );

	// Start portion of integral over criterion density j-1 from -infinity to s
        j--;
	iinner.j--;
	iinner.rule = RKKK3B2;
	double pminf = dmin( info->mu[ j ], s ) - info->sigma[ j ] * 6.0;
	double nbreak = dmin( info->mu[j] - info->sigma[ j ]*2.0, s - info->sigma[j]*1.0 );
	double qbreak = dmin( s, info->mu[j] + info->sigma[ j ]*2.0 );
	assert ( info->sigma[j] >= 0.0);
	assert ( pminf <= info->mu[j]);
	assert ( pminf <= s );
	assert ( s >=qbreak );
	if ( pminf == s ) return 0.0;
	assert ( pminf <= s );
	D printf ( "\ts=%g pinf=%g\n", s, pminf );
	D printf ( "\t*s=%g pinf=%g n=%g ik=%g\n", pminf, s, normal ( s, info->mus, info->sigmas ),
		   qromox ( innerkernel, pminf, s, INTALG, &iinner, nc, info->qracc(), sguess, KMAX,
			    RATIONAL_INNER ) );
	assert(s > nbreak);
	assert(nbreak < qbreak);
	// Middle part of integral over criterion density j-1
        sC += qromox( innerkernel, nbreak, dmin(s, qbreak), INTALG, &iinner, nc, info->qracc(), sguess,
		      KMAX, RATIONAL_INNER );
	if(!finite(sC))	return quiet_nan(0);
	// Upper part of integral over criterion density j-1 to s
	if(s > qbreak) {
	  sN += qromox ( innerkernel, qbreak, s, INTALG, &iinner, nc, info->qracc(), 0.1*sguess+sC,
			KMAX, RATIONAL_INNER );
	  if(!finite(sN)) return quiet_nan(0);
    	}
	// Bottom part of integral over criterion density j-1 from -infinity
	assert(nbreak > pminf);
        sP += qromox( innerkernel, pminf, nbreak, INTALG, &iinner, nc, info->qracc(), 0.1*sguess+sC+sN,
		     KMAX, RATIONAL_INNER );
	iinner.j++;
	j++;
	iinner.rule = RKKK3B1;
    }
    return normal ( s, info->mus, info->sigmas ) * (sN+sC+sP);
}

double innerkernel ( double cj,  void *infoptr, int nc )
{
    int k;
    C struct infoinner_s *info = ( C struct infoinner_s * ) infoptr;
//	    printf ( "cj=%g\n", cj );
//	    printf ( "s=%g\n", info->s );
    // printf( "\n Innerkernel %g %g\n", cj, info->s);
    C int j = info->j;
    C int rkkkrule=info->rule;
    C double r2 = sqrt ( 2.0 );
    double product = 1.0;
    double term=0.0, z1=0.0, z2=0.0;
    for ( k = 0; k <= info->n; k++ )	{
        assert (info->sigma[k] > 0.0);
	D printf ( "cj=%g k=%d muk=%g sigma[k]=%g\n", cj, k, info->mu[k], info->sigma[k] );
	D printf ( "s=%g k=%d muk=%g sigma[k]=%g\n", info->s, k, info->mu[k], info->sigma[k] );

	switch (rkkkrule )
        {
	  case RKKK1:
	    // Rule (1)
	    assert ( info->s <= cj );
	    if ( k == j )  {
		term = 1.0;
	        break;
	    }
            z1 = ( cj - info->mu[ k ] ) / ( r2 * info->sigma[ k ] );
	    z2 = ( info->s - info->mu[ k ] ) / ( r2 * info->sigma[ k ] );
	    assert ( z2 <= z1 );
	    term = 1.0 - ( erf( z1  ) - erf( z2 ) ) / 2.0;
	    D printf ( "z1=%g z2=%g term=%g\n", z1, z2, term );
            break;
         case RKKK2:
	   // Rule (2)
	   assert ( info->s >= cj );
	   if ( k == j ) {
		term = 1.0;
	        break;
	    }
	    z1 = (cj - info->mu[ k ] ) / ( r2 * info->sigma[ k ] );
	    z2 = ( info->s - info->mu[ k ] ) / ( r2 * info->sigma[ k ] );
	    assert ( z2 >= z1 );
	    term = 1.0 - ( erf( z2 ) - erf( z1 ) ) / 2.0;
            break;	
	 case RKKK3B1:
	   // First part of rule 3B
	   // printf( "\n cj = %f  info->s = %f", cj, info->s );
	   // assert ( cj >= info->s );
	   // assert( cj >= ( 2 * info->s - cj ) );
	   //  EQ3A and first portion of EQ 3B)
	   if ( ( k == j ) || ( cj <= info->s ) ) {
	     term = 1.0;
	     break;
	   }
	   z1 = ( ( 2 * info->s - cj ) - info->mu[ k ] ) / ( r2 * info->sigma[ k ] );
	   z2 = ( cj - info->mu[ k ] ) / ( r2 * info->sigma[ k ] );
	   assert ( z2 >= z1 );
	   term = 1.0 - ( erf( z2  ) - erf( z1 ) ) / 2.0;
	   D printf ( "z1=%g z2=%g term=%g\n", z1, z2, term );
           break;
	 case RKKK3B2:   
	   // Second part of Rule 3B. j is now 1 less than in first part
	   // assert ( ( 2 * info->s - cj ) >= cj );
           if ( ( k == j ) || ( info->s  <= cj ) )  {
	     term = 1.0;
	     break;
	   }
	   z1 = ( cj - info->mu[ k ] ) / ( r2 * info->sigma[ k ] );
	   z2 = ( ( 2 * info->s - cj ) - info->mu[ k ] ) / ( r2 * info->sigma[ k ] );
	   assert ( z2 >= z1 );
	   term = 1.0 - ( erf( z2  ) - erf( z1 ) ) / 2.0;
	   D printf ( "z1=%g z2=%g term=%g\n", z1, z2, term );
           break;

        }
	assert ( term >= -EPS );
	assert ( term <= 1.0 + EPS );
	product *= term;
    }
    return normal ( cj, info->mu[ j ], info->sigma[ j ] ) * product;
}

// THIS FUNCTION APPLIES TO THE CSDT MODEL. IT STARTS COMPUTATION OF THE PROBABILITY IN CELL [ ie3, je3 ] IN A
//  PROBABILITY MATRIX. rk1kk23 TELLS WHETHER COMPUTATION USES RULE 1, RULE 2, OR RULE 3. 
// pp HOLDS ONE FULL SET OF PARAMETERS, WHETHER UNCONSTRAINED OR NOT. 
double eq3a_wrap (  enum RKKKRule rk1kk23, int ie3a, int je3a, double *pp, C struct initialization_s *isp)
{

    int iq, jq;
    double dprob, mumax, sigmasum = 0.0;

    jq = je3a;
    C int nc = isp->numc();
    assert ( jq >= 0 && jq <= nc );
    C double mus = pp [ isp->s_mstart + ie3a ];
    C double sigmas = pp [ isp->s_sdstart + ie3a ];
    double *mu = double1alloc(nc + 1);
    double *sigma = double1alloc(nc + 1);
    mumax = mus;
    for( iq = 0; iq <nc; iq++ )	{
	mu [ iq ] = pp [ isp->c_mstart + iq ];
	sigma [ iq ] = pp [ isp->c_sdstart + iq ];
	sigmasum += sigma[ iq ];
	if( mumax < mu [ iq ] + 6 * sigma[ iq ] )
		mumax = mu[ iq ] + 6 * sigma[ iq ];
    }
    mu[ iq ] = mumax;
    sigma[ iq ] = sigmasum / nc;
    D printf ( "mu[%d]=%g sigma[%d]=%g\n", iq, mu [ iq ], iq, sigma [ iq ] );
    dprob = eq3a ( rk1kk23, jq, nc, mu, sigma, mus, sigmas, isp );
    double1free ( mu );
    double1free ( sigma );
    return ( dprob );
}

double eq3a ( enum RKKKRule  rk1kk23, int j, int n, double *mu, double *sigma, double mus, double sigmas, 
              C struct initialization_s *isp )
{
    assert(isp);
    struct info_s info;
    C double s = mus;
    C int nc = isp->numc();
    info.j = j;
    info.sigma = sigma;
    info.n = n;
    info.mu = mu;
    info.mus = mus;
    info.sigmas = sigmas;
    info.isp = isp;
    info.rule = rk1kk23;
    infoinner_s iinner(&info, s);
    double pinf = iinner.mu[ j ] + iinner.sigma[ j ] * 6.0;
    double minf = iinner.mu[ j ] - iinner.sigma[ j ] * 6.0;
    D printf ( "\ts=%g pinf=%g\n", s, pinf );
    if ( ( rk1kk23 != RKKK2 ) && ( pinf <= s ) ) return 0.0;
    if ( ( rk1kk23 == RKKK2 ) && ( minf >= s ) ) return 0.0;
    C double sguess = 0.2;
    D printf ( "\t*s=%g pinf=%g n=%g ik=%g\n", s, pinf, normal ( s, iinner.mus, iinner.sigmas ),
	      qromox ( innerkernel, s, pinf, INTALG, &iinner, nc, iinner.qracc(), sguess, KMAX, 
		       RATIONAL_INNER ) );

    double qr = 0.0;
    // NO INTEGRATION OVER SIGNAL DURAC PULSE. SO s IS ALWAYS THE SIGNAL MEAN
    if ( ( rk1kk23 == RKKK1 ) || ( ( rk1kk23 == RKKK3B1 ) && ( j == 0 ) ) ) {
       // EQS (1) or (3A)
       if ( s < iinner.mu [ j ] ) {
	    // INTEGRAL RUNS FROM s TO INFINITY, ALLOWING s=-INFINITY; LOWER PART
 	    qr = qromox ( innerkernel, s>minf?s:minf, iinner.mu[j], INTALG, &iinner, nc, iinner.qracc(),
			  sguess, KMAX, RATIONAL_INNER );
	    if(!finite(qr)) return quiet_nan(0);
	    // UPPER PART OF INTEGRAL
	    qr += qromox ( innerkernel, iinner.mu[j], pinf, INTALG, &iinner, nc, iinner.qracc(),
			   0.5*(sguess+fabs(qr)), KMAX, RATIONAL_INNER );
        }
	// ELSE ENTIRE INTEGRAL
        else qr = qromox ( innerkernel, s>minf?s:minf, pinf, INTALG, &iinner, nc, iinner.qracc(), sguess,
			   KMAX, RATIONAL_INNER );
    }

    if ( rk1kk23 == RKKK2 ) {
        // EQ (2) INTGRAL FROM -INFINITY TO s, ALLOWING s= mu[ j ]
        if ( s > iinner.mu [ j ] ) {
	    // UPPER PART
            qr = qromox ( innerkernel, iinner.mu [ j ], s, INTALG, &iinner, nc, iinner.qracc(), sguess, KMAX,
			  RATIONAL_INNER );
	    // LOWER PART
	    qr += qromox ( innerkernel, minf,  iinner.mu[j], INTALG, &iinner, nc, iinner.qracc(),
			   0.5*(sguess+fabs(qr)), KMAX, RATIONAL_INNER );
	    if(!finite(qr)) return quiet_nan(0);
        } 
	// ENTIRE INTEGRAL
        else qr = qromox ( innerkernel,  minf, s<iinner.mu[j]?s:iinner.mu[j], INTALG, &iinner, nc,
			   iinner.qracc(), sguess, KMAX, RATIONAL_INNER );
    }

    if ( ( info.rule == RKKK3B1 ) && ( j != 0 ) ) {
       // EQ (3B) Integral over criterion density j from s to infinity PLUS
       // integral over criterion density j-1 from  -infinity to s

       // Start portion of integral over jth criterion density
       if ( s < iinner.mu [ j ] ) {
	    // BOTTOM PART, ALLOWING s =-INFINITY
 	    qr = qromox ( innerkernel, s>minf?s:minf, iinner.mu[j], INTALG, &iinner, nc, iinner.qracc(), 
			  sguess, KMAX, RATIONAL_INNER );
	    if(!finite(qr)) return quiet_nan(0);
	    // TOP PART
	    qr += qromox ( innerkernel, iinner.mu[j], pinf, INTALG, &iinner, nc, iinner.qracc(),
			   0.5*(sguess+fabs(qr)), KMAX, RATIONAL_INNER );
        }
        // ENTIRE INTGRAL
        else qr = qromox ( innerkernel, s>minf?s:minf, pinf, INTALG, &iinner, nc, iinner.qracc(), sguess,
			   KMAX, RATIONAL_INNER );
        // END INTGRAL OVER jth CRITERION DENSITY

	// START portion of integral over criterion density j-1 from -infinity to s
        j--;
	iinner.j--;
	iinner.rule = RKKK3B2;
	double minf = iinner.mu[ j ] - iinner.sigma[ j ] * 6.0;

        if ( s > iinner.mu [ j ] ) {
	    // TOP PART OF INTEGRAL
            qr += qromox ( innerkernel, iinner.mu [ j ], s, INTALG, &iinner, nc, iinner.qracc(), sguess,
			   KMAX, RATIONAL_INNER );
	    // BOTTOM PART OF INTEGRAL
	    qr += qromox ( innerkernel, minf,  iinner.mu[j], INTALG, &iinner, nc, iinner.qracc(),
			   0.5*(sguess+fabs(qr)), KMAX, RATIONAL_INNER );
	    if(!finite(qr)) return quiet_nan(0);
        }
	// ENTIRE INTEGRAL, ALLOWING s = mu[ j ]
        else qr += qromox ( innerkernel,  minf, s<iinner.mu[j]?s:iinner.mu[j], INTALG, &iinner, nc,
			    iinner.qracc(), sguess, KMAX, RATIONAL_INNER );
	info.j++;
	j++;
	info.rule = RKKK3B1;
    }
    return qr;
}


// FUNCTION CALCULATES ALL PROBABILITIES IN ROW isdtx OF A POBABILITY MATRIX, USING THE SDT MODEL
//  pp: PARAMETER ARRAY, SIZE 2*NUMS + NUMC
void sdt ( int isdtx, double *pp, C struct initialization_s *isp )
{
    /* For signal detection theory, this function calculates p( R<=j|Si ) for dcump_mat
       and  p( R=j|Si ) for dprob_mat. 
       Rules 1 and 2 give identical results. Parameters are in pp */ 

    assert(isp);
    double ddenom, lastcp = 0, newcp = 0;

    // Get probability matrix value from scale values.
    ddenom = pp [ isp->s_sdstart + isdtx ];
    if ( ddenom <  EPS ) ddenom = EPS;
    // Get all but last p=jsdt
    for ( int jsdt = 0; jsdt < isp->numc(); jsdt++ ) {
        // Get z value  
        double z_sdt = ( pp [ isp->c_mstart + jsdt ] - pp [ isdtx ] ) / ddenom;
        newcp = gsl_cdf_ugaussian_P ( z_sdt );
	assert ( newcp >= lastcp );
        isp->dcump_mat [ isdtx ] [ jsdt ] = newcp;
        isp->dprob_mat [ isdtx ] [ jsdt ] = newcp - lastcp;
	lastcp = newcp;
    }
    // Get last entry for R=numr
    assert ( lastcp <= 1 );
    isp->dprob_mat [ isdtx ] [ isp->numr - 1 ] = 1 - lastcp;
}

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

// FUNCTION GETS A DATA MATRIX AND A PROBABILITY MATRIX VIA isp.
// IT RETURNS LOG LIKELIHOOD, logP
double wsdo_l_f ( C struct initialization_s *isp )
{

    assert(isp);

    // Get logP 
    double logP = 0.0;
    for ( int ilf = 0; ilf < isp->nums; ilf++ )  {
	for ( int jlf = 0; jlf < isp->numr; jlf++ ) {
	    assert ( isp->data [ ilf ] [ jlf ] >= 0 );
	    double prby = Max(isp->dprob_mat [ ilf ] [ jlf ], 1e-20);
	    logP += log( prby ) * isp->data [ ilf ] [ jlf ];
        }
    }
    return logP;
}

void print_rule_prob1(C struct initialization_s *init_ptr) {
	switch ( init_ptr->dec_r ) {
          case RULE1:
          case RULE2:
          case RULE3:
             break;
	  case RULES1_2:
             fprintf ( init_ptr->outPtr, "\n  Probability of Rule 1 = %6.4f", init_ptr->solution [ init_ptr->pstart ] );
             fprintf ( init_ptr->outPtr, "\n  Probability of Rule 2  = %6.4f", 1.0 - init_ptr->solution [ init_ptr->pstart ] );
	     break;
	  case RULES1_3:
             fprintf ( init_ptr->outPtr, "\n  Probability of Rule 1 = %6.4f", init_ptr->solution [ init_ptr->pstart ] );
             fprintf ( init_ptr->outPtr, "\n  Probability of Rule 3  = %6.4f", 1.0 - init_ptr->solution [ init_ptr->pstart ] );
	     break;
	  case RULES2_3:
             fprintf ( init_ptr->outPtr, "\n  Probability of Rule 2 = %6.4f", init_ptr->solution [ init_ptr->pstart ] );
             fprintf ( init_ptr->outPtr, "\n  Probability of Rule 3  = %6.4f", 1.0 - init_ptr->solution [ init_ptr->pstart ] );
	     break;
	  case RULES1_2_3:
             fprintf ( init_ptr->outPtr, "\n  Probability of Rule 1 = %6.4f", init_ptr->solution [ init_ptr->pstart ] );
             fprintf ( init_ptr->outPtr, "\n  Probability of Rule 2 = %6.4f", init_ptr->solution [ init_ptr->pstart + 1 ] );
             fprintf ( init_ptr->outPtr, "\n  Probability of Rule 3 = %6.4f", 
                                           1.0 - init_ptr->solution [ init_ptr->pstart ]- init_ptr->solution [ init_ptr->pstart + 1 ] );
	     break;
	}
}

// Evaluate maximum likelihood results in prmvec and write the output. 
void wsdo_max_output ( C double *prmvec, C struct initialization_s *isp )
{
    assert(isp);
    int iwo, jwo;
    static int max_ct = 0;
    double **roc_zmat;
    static struct eval_stats stats;
    double new0, new1;
    if ( !isp->append_output ) rewind( isp->outPtr );
 
    stats.logp = wsdo_logp( prmvec, isp, 2 );
    // Proportion, probability, and cumulative probability matrices and solution now in lcjc_init 

    wsdo_eval ( isp, &stats );
    // Statistics now in structure stats 

    /* Write the optimization results to the outFile
    Write solution in isp->solution
    Write the matrix isp->dprob_mat     */
    fprintf ( isp->outPtr, "\n  %s", isp->header );
    print_model_type ( isp );

    max_ct++;
    if ( max_ct == 1 ) fprintf ( isp->outPtr, "\n\n First maximum " );
    else fprintf ( isp->outPtr, "\n\n Maximum number %d", max_ct );

    // Set up standard scale
    if ( isp->stdz == 1 ) {
       new0 = isp->solution [ isp-> s_mstart ];
       if ( ( isp->sdtev ) || ( isp->sdtuv ) || ( isp->fsdt ) ) new1 = isp->solution [ isp-> s_sdstart ];
       else new1 = isp->solution [ isp-> c_sdstart ];
    }

    fprintf ( isp->outPtr, "\n\n Final parameters: ");
    fprintf ( isp->outPtr, "\n  Signal means:\n" );

    if ( isp->stdz ) for ( int insc = isp->s_mstart; insc <= isp->s_mend; insc++ ) {
			      isp->solution [ insc ] -= new0;
			      isp->solution [ insc ] /= new1;
                          }
    w_block ( isp->outPtr, isp->s_mstart, isp->s_mend, isp->solution );

    if ( ( isp->csdtev  ) || ( isp->csdtuv  ) )
        fprintf ( isp->outPtr, "  All signal standard deviations are zero:\n");
    else {
        fprintf ( isp->outPtr, "  Signal standard deviations:\n");
        if ( isp->stdz  ) for ( int insc = isp->s_sdstart; insc <= isp->s_sdend; insc++ )
			      isp->solution [ insc ] /= new1;
        w_block ( isp->outPtr, isp->s_sdstart, isp->s_sdend, isp->solution );
    }


    fprintf ( isp->outPtr, "  Criterion means:\n");
    if ( isp->stdz  ) for ( int insc = isp->c_mstart; insc <= isp->c_mend; insc++ ) {
			      isp->solution [ insc ] -= new0;
			      isp->solution [ insc ] /= new1;
                      }
    w_block ( isp->outPtr, isp->c_mstart, isp->c_mend, isp->solution );

    if ( ( isp->sdtev  ) || ( isp->sdtuv  ) )
       fprintf ( isp->outPtr, "  All criterion standard deviations are zero:\n");
    else {
        fprintf ( isp->outPtr, "  Criterion standard deviations:\n");
        if ( isp->stdz ) for ( int insc = isp->c_sdstart; insc <= isp->c_sdend; insc++ )
 			          isp->solution [ insc ] /= new1;
        w_block ( isp->outPtr, isp->c_sdstart, isp->c_sdend, isp->solution );
    }
    print_rule_prob1( isp );
    
    fprintf ( isp->outPtr, "\n\n Theoretical probability matrix:\n");
    mat_out ( isp, isp->dprob_mat );

    fprintf ( isp->outPtr, "\n  Observed proportion matrix\n" );
    mat_out ( isp, isp->prop_mat );

    fprintf ( isp->outPtr, "\n Matrix of observed minus theoretical proportions\n" );
    mat_out ( isp, isp->e_mat );

    fprintf ( isp->outPtr, "\n  Theoretical cumulative probability matrix\n" );
    mat_out ( isp, isp->dcump_mat );

    // Get z-matrix 
    roc_zmat = double2alloc( isp->nums, isp->numr );
    for ( iwo = 0; iwo < isp->nums; iwo++ ) {
	for ( jwo = 0; jwo < isp->numr; jwo++ ) {
	    if ( isp->dcump_mat [ iwo ] [ jwo ] < 0.0000003) roc_zmat [ iwo ] [ jwo ] = -5.0;
	    else {
		if (  isp->dcump_mat [ iwo ] [ jwo ] >= 0.9999997) roc_zmat [ iwo ] [ jwo ] = 5.0;
		else roc_zmat [ iwo ] [ jwo ] = gsl_cdf_ugaussian_Pinv (  isp->dcump_mat [ iwo ] [ jwo ] );
	    }
	}
    }
    fprintf ( isp->outPtr, "\n\n Roc_z matrix:\n");
    mat_out ( isp, roc_zmat );
    fprintf ( isp->outPtr, " \n" );
    double2free( roc_zmat );


    fprintf ( isp->outPtr, "\n Regression error matrix\n" );
    mat_out ( isp, isp->re_mat );

    fprintf ( isp->outPtr, " \n  Optimized function value %10.4f ", stats.logp );
    fprintf ( isp->outPtr, " \n  Corrected Akaike information criterion  %10.4f ", stats.AICc );
    fprintf( isp->outPtr, "\n\n  r = %7.3f, r-sq = %7.3f, df for r = %d", stats.r, stats.rsq, stats.df );
    fprintf( isp->outPtr, "\n  RMSD = %8.3f", stats.rmsd );
    fprintf( isp->outPtr, "\n b0 = %6.3f, confidence limits = %6.3f, %6.3f", 
                    stats.b0, stats.b0 - stats.c95b0, stats.b0 + stats.c95b0 ); 
    fprintf( isp->outPtr, "\n b1 = %6.3f, confidence limits = %6.3f, %6.3f", 
                    stats.b1, stats.b1 - stats.c95b1, stats.b1 + stats.c95b1 );
    fprintf ( isp->outPtr, " \n" );
    fprintf( isp->outPtr, "\n  Kullback-Leibler divergence = %8.5f\n\n", stats.k_l_div );
}

void mat_out ( C struct initialization_s *isp, double **mn )
{
    int iwo, jwo;

    for ( iwo = 0; iwo < isp->nums; iwo++ ) {
        for ( jwo = 0; jwo < isp->numr; jwo++ )
            fprintf( isp->outPtr, "%7.3f", mn [ iwo ] [ jwo ] );
        fprintf( isp->outPtr, "\n" );
    }
    fflush ( isp->outPtr );
}

void wsdo_eval ( C struct initialization_s *isp, struct eval_stats *stp )
{
    /* As a final step before output, this function calculates various measures of
       goodness of fit. Results fed to structure stats*/

    // Get sums, sumsquares, sumxprod, sumdiffssq 
    assert(isp);
    int iev, jev;
   
    for ( iev = 0; iev < isp->nums; iev++ ) {
        for ( jev = 0; jev < isp->numr; jev++ ) {
	    isp->e_mat [ iev ] [ jev ] = isp->prop_mat [ iev ] [jev ] - isp->dprob_mat [ iev ] [jev ];
	}
    }
    double sumd = 0.0, sumsqd = 0.0, sumt = 0.0, sumsqt = 0.0, sumdiffsq = 0.0, sumdt = 0.0;
    for ( iev = 0; iev < isp->nums; iev++ ) {
        for ( jev = 0; jev < isp->numr; jev++ ) {
            sumd += isp->prop_mat [ iev ] [jev ];
            sumsqd += isp->prop_mat [ iev ] [jev ] * isp->prop_mat [ iev ] [jev ];
            sumt += isp->dprob_mat [ iev ] [jev ];
            sumsqt += isp->dprob_mat [ iev ] [jev ] * isp->dprob_mat [ iev ] [jev ];
            sumdt += isp->prop_mat [ iev ] [jev ] * isp->dprob_mat [ iev ] [jev ];
            sumdiffsq += isp->e_mat [ iev ] [jev ] * isp->e_mat [ iev ] [jev ];
        }
    }
    // Get Akaike criterion
    stp->AICc = 2 * isp->nfree_modpars - 2 * stp->logp;
    double Acorr = 2 * isp->nfree_modpars * ( 2 * isp->nfree_modpars + 1);
    double Adenom = 0;
    for ( iev =0; iev < isp->nums; iev++ )
      for ( jev =0; jev < isp->numr; jev++ ) Adenom += isp->data [ iev ] [jev ];
    Adenom -= isp->nfree_modpars + 1;
    Acorr /= Adenom; 
    stp->AICc += Acorr;

    int totn = isp->nums * isp->numr;
    stp->df = totn - isp->nums - 2;
    stp->rmsd = sqrt( sumdiffsq / totn );
    double md = sumd / totn;
    double mt = sumt / totn;
    double mtsq = mt * mt;
    double mtmd = mt * md;
    double sigd = sqrt( ( sumsqd  - md * md * totn ) / ( totn - 1 ) );
    double sigt = sqrt( ( sumsqt  - mtsq * totn ) / ( totn - 1 ) );
    stp->r = ( sumdt / totn - mtmd ) / ( sigd * sigt );
    stp->rsq = stp->r * stp->r; 
    stp->b1 = ( sumdt - totn * mtmd )/( sumsqt - totn * mtsq );
    stp->b0 = md - stp->b1 * mt;
    double mse = 0.0;
    for ( int iev = 0; iev < isp->nums; iev++ )
        for ( int jev = 0; jev < isp->numr; jev++ ) {
	    isp->re_mat [ iev ] [ jev ] = isp->prop_mat [ iev ] [jev ] - stp->b1 * isp->dprob_mat [ iev ] [jev ] - stp->b0;
	    mse += isp->re_mat [ iev ] [ jev ] * isp->re_mat [ iev ] [ jev ];
        }
    mse /= totn;
    double tfac = sumsqt - totn * mtsq;
    double studt = gsl_cdf_tdist_Pinv( 0.975, stp->df );
    stp->c95b0 = studt * sqrt ( ( mse * sumsqt )/( totn * tfac ) ); 
    stp->c95b1 = studt * sqrt ( mse / tfac ); 
    // Prevent true zeroes in dprob_mat when data >0 before Kullback-Leibler calculation
    stp->k_l_div = 0;
    for ( int iev = 0; iev < isp->nums; iev++ ) {
        for ( int jev = 0; jev < isp->numr; jev++ ) {
	    if ( isp->prop_mat [ iev ] [jev ] > 0 ) {
		double dprb = isp->dprob_mat [ iev ] [ jev ];
		if (dprb == 0) dprb = 1.0E-15;
		stp->k_l_div += isp->prop_mat [ iev ] [ jev ] * log ( ( isp->prop_mat [ iev ] [ jev ] ) / dprb );
	    }
	}
    }
}

int w_block ( FILE *o1P, int b_start, int b_end, double *b )
{
    for ( int irw = b_start; irw <= b_end; irw++ )
	fprintf ( o1P, "%7.3f ", b [ irw ] );
    fprintf ( o1P, "\n" );
    fflush ( o1P );
    return 0;
}

void print_rule_probs2(C struct initialization_s *isp, double **mcmc_vecs,
                       int snum, int l95bnd, int u95bnd) {
        switch ( isp->dec_r ) {
            case RULE1:
            case RULE2:
            case RULE3:
               break;
            case RULES1_2:
	       fprintf ( isp->outPtr, "\n    Rule 1 probability \n" );
	       fprintf ( isp->outPtr, "%7.3f\t", mcmc_vecs [ snum ] [ u95bnd ] );
	       fprintf ( isp->outPtr, "\n" );
	       fprintf ( isp->outPtr, "%7.3f\t", mcmc_vecs [ snum ] [ l95bnd ] );
	       fprintf ( isp->outPtr, "\n" );
	       snum++;
	       break; 

            case RULES1_3:
	       fprintf ( isp->outPtr, "\n    Rule 1 probability \n" );
	       fprintf ( isp->outPtr, "%7.3f\t", mcmc_vecs [ snum ] [ u95bnd ] );
	       fprintf ( isp->outPtr, "\n" );
	       fprintf ( isp->outPtr, "%7.3f\t", mcmc_vecs [ snum ] [ l95bnd ] );
	       fprintf ( isp->outPtr, "\n" );
	       snum++;
	       break; 

            case RULES2_3:
	       fprintf ( isp->outPtr, "\n    Rule 2 probability \n" );
	       fprintf ( isp->outPtr, "%7.3f\t", mcmc_vecs [ snum ] [ u95bnd ] );
	       fprintf ( isp->outPtr, "\n" );
	       fprintf ( isp->outPtr, "%7.3f\t", mcmc_vecs [ snum ] [ l95bnd ] );
	       fprintf ( isp->outPtr, "\n" );
	       snum++;
	       break; 
    
            case RULES1_2_3:
	       fprintf ( isp->outPtr, "\n    Rule 1 probability \n" );
	       fprintf ( isp->outPtr, "%7.3f\t", mcmc_vecs [ snum ] [ u95bnd ] );
	       fprintf ( isp->outPtr, "\n" );
	       fprintf ( isp->outPtr, "%7.3f\t", mcmc_vecs [ snum ] [ l95bnd ] );
	       fprintf ( isp->outPtr, "\n" );
	       snum++;
	       fprintf ( isp->outPtr, "\n    Rule 2 probability \n" );
	       fprintf ( isp->outPtr, "%7.3f\t", mcmc_vecs [ snum ] [ u95bnd ] );
	       fprintf ( isp->outPtr, "\n" );
	       fprintf ( isp->outPtr, "%7.3f\t", mcmc_vecs [ snum ] [ l95bnd ] );
	       fprintf ( isp->outPtr, "\n" );
	       snum++;
	       break; 
        }
}

// Get mcmc results and clean up. All parameter vectors handed over at once; mcmc_num is number of samples. 
void wsdo_mcmc_output ( int mcmc_num, double **mcmc_vecs, C struct initialization_s *isp )
{
    assert(isp);
    fprintf( isp->outPtr, "\n mcmc_num = %d ", mcmc_num );
    // Set up standard scale
    double new0 = 0, new1 = 1;
    if ( isp->stdz == 1 ) {
       new0 = isp->solution [ isp-> s_mstart ];
       if ( ( isp->sdtev ) || ( isp->sdtuv ) || ( isp->fsdt ) ) new1 = isp->solution [ isp-> s_sdstart ];
       else new1 = isp->solution [ isp-> c_sdstart ];
    }
    // Allocate arrays 
    const int lgth = isp->nums * isp->numr;
    double * vec_hold = double1alloc( mcmc_num );
    double * logP = double1alloc( mcmc_num );
    double * vece_hold = double1alloc( isp->nunconstrained );
    double ** prob95 = double2alloc( mcmc_num, lgth );

    // Get upper and lower 95% bounds 
    double wdum = 0.975 * mcmc_num;
    int u95bnd = trunc( wdum );
    int l95bnd = mcmc_num - u95bnd - 1;
    u95bnd--;
    fprintf( isp->outPtr, "\n Low and high bounds calculated, arrays allocated ");

    /* Get parameter values and probabilities from mcmc_vecs whose rows
       and columns represent parameters and samples, respectively.     */
    for ( int jmc = 0; jmc < mcmc_num; jmc++ ) {
	for ( int imc = 0; imc < isp->nunconstrained; imc++ )
	    vece_hold [ imc ] = mcmc_vecs [ imc ] [ jmc ];
	logP[ jmc ] = wsdo_logp( vece_hold, isp, 2 );
	int pmi = 0;
	int pmj = 0;
	for ( int kmc = 0; kmc < lgth; kmc++ ) {
	    prob95 [ jmc ] [ kmc ] = isp->dprob_mat [ pmi ] [ pmj ];
	    pmj++;
	    if ( pmj % isp->numr == 0 ) {
		pmi++;
		pmj = 0;
	    }
	}
    }

    // Sort each column vector in prob95 in ascending order 
    // printf( "\n Sorted samples\n" );
    fprintf ( isp->outPtr, "\n Low and high bounds at %d and %d", l95bnd, u95bnd );
    for ( int jmc = 0; jmc < lgth; jmc++ ) {
	for ( int imc = 0; imc < mcmc_num; imc++ )
	    vec_hold [ imc ] = prob95 [ imc ] [ jmc ];
	gsl_sort( vec_hold, 1, mcmc_num ); 
	for ( int imc = 0; imc < mcmc_num; imc++ )
	    prob95 [ imc ] [ jmc ] = vec_hold [ imc ];
    }

    // Sort logP in ascending order 
    gsl_sort( logP, 1, mcmc_num ); 
    // Sort each row vector in mcmc_vecs in ascending order 
    for ( int imc = 0; imc < isp->nunconstrained; imc++ ) {
	for ( int jmc = 0; jmc < mcmc_num; jmc++ ) 
	    vec_hold [ jmc ] = mcmc_vecs [ imc ] [ jmc ];
	gsl_sort( vec_hold, 1, mcmc_num ); 
	for ( int jmc = 0; jmc < mcmc_num; jmc++ ) 
	     mcmc_vecs [ imc ] [ jmc ] = vec_hold [ jmc ];
    }

    // Write the upper and lower bounds for parameters and probabilities to the output file 
    fprintf( isp->outPtr, "\n 95 per cent confidence limits for all  %d unconstrained parameters ", isp->nunconstrained ); 
    fprintf( isp->outPtr, "\n Samples = %d, upper and lower bounds at samples %d and %d\n", mcmc_num, u95bnd, l95bnd ); 
    int snum = 0;
    if ( isp->free_sm > 0 ) {
	fprintf ( isp->outPtr, "\n    Signal means\n" );
	for ( int imc = 0; imc < isp->free_sm; imc++ ) {
	  fprintf ( isp->outPtr, "%7.3f\t", ( mcmc_vecs [ snum ] [ u95bnd ] - new0 )/ new1 );
	    snum++;
	}
	fprintf ( isp->outPtr, "\n" );
	snum = 0;
	for ( int imc = 0; imc < isp->free_sm; imc++ ) {
	  fprintf ( isp->outPtr, "%7.3f\t", ( mcmc_vecs [ snum ] [ l95bnd ] - new0 )/ new1 );
	    snum++;
	}
	fprintf ( isp->outPtr, "\n" );
    }
    // assert ( snum == isp->free_sm );
    if ( isp->free_ssd > 0 ) {
	int snuma = snum;
	fprintf ( isp->outPtr, "\n    Signal standard deviations\n" );
	for ( int imc = 0; imc < isp->free_ssd; imc++ ) {
	  fprintf ( isp->outPtr, "%7.3f\t", ( mcmc_vecs [ snum ] [ u95bnd ] ) / new1 );
	    snum++;
	}
	fprintf ( isp->outPtr, "\n" );
	snum = snuma;
	for ( int imc = 0; imc < isp->free_ssd; imc++ ) {
	  fprintf ( isp->outPtr, "%7.3f\t", ( mcmc_vecs [ snum ] [ l95bnd ] ) / new1 );
	    snum++;
	}
	fprintf ( isp->outPtr, "\n" );
    }
    // assert ( snum == isp->free_sm + isp->free_ssd );
    if ( isp->free_cm > 0 ) {
	int snuma = snum;
	fprintf ( isp->outPtr, "\n    Criterion means\n" );
	for ( int imc = 0; imc < isp->free_cm; imc++ ) {
	  fprintf ( isp->outPtr, "%7.3f\t", ( mcmc_vecs [ snum ] [ u95bnd ] - new0 ) / new1 );
	    snum++;
	}
	fprintf ( isp->outPtr, "\n" );
	snum = snuma;
	for ( int imc = 0; imc < isp->free_cm; imc++ ) {
	  fprintf ( isp->outPtr, "%7.3f\t", ( mcmc_vecs [ snum ] [ l95bnd ] - new0 ) / new1 );
	    snum++;
	}
	fprintf ( isp->outPtr, "\n" );
    }
    // assert ( snum == isp->free_sm + isp->free_ssd + isp->free_cm );
    if ( isp->free_csd > 0 ) {
	int snuma = snum;
	fprintf ( isp->outPtr, "\n    Criterion standard deviations\n" );
	for ( int imc = 0; imc < isp->free_csd; imc++ ) {
	  fprintf ( isp->outPtr, "%7.3f\t", ( mcmc_vecs [ snum ] [ u95bnd ] ) / new1 );
	    snum++;
	}
	fprintf ( isp->outPtr, "\n" );
	snum = snuma;
	for ( int imc = 0; imc < isp->free_csd; imc++ ) {
	  fprintf ( isp->outPtr, "%7.3f\t", ( mcmc_vecs [ snum ] [ l95bnd ] ) / new1 );
	    snum++;
	}
	fprintf ( isp->outPtr, "\n" );
    }
    assert ( snum == isp->nfree_modpars );

    if ( ( isp->csdtev == 1 ) || ( isp->csdtuv == 1 ) || ( isp->fsdt == 1 ) ) print_rule_probs2(isp, mcmc_vecs, snum, l95bnd, u95bnd);

    int kmc = 0;
    fprintf( isp->outPtr, "\n\n  Theoretical probabilities\n" );
    for ( int imc = 0; imc < isp->nums; imc++ ) {
	fprintf( isp->outPtr, "\n  Stimulus %d\n", imc + 1 );
	for ( int jmc = 0; jmc < isp->numr; jmc++ ) {
	    fprintf ( isp->outPtr, "%7.3f\t",  prob95[ u95bnd  ] [kmc ] );
	    kmc++;
	}
	fprintf ( isp->outPtr, "\n" );
	for ( int jmc = 0; jmc < isp->numr; jmc++ ) {
	    fprintf ( isp->outPtr, "%7.3f\t",  prob95[ l95bnd  ] [kmc ] );
	    kmc++;
	}
	fprintf ( isp->outPtr, "\n\n" );
    }
    fprintf( isp->outPtr, "\n" );
    // Write matrix, mcmc_num rows, sorted logP then parameters as columns 
    /* for ( int imc = 0; imc < mcmc_num; imc++ ) {
	fprintf( isp->outPtr, "\n%8.3f\t", logP[ imc ]);
	for ( int jmc = 0; jmc <isp->nunconstrained; jmc++ ) 
            fprintf( isp->outPtr, "%6.3f\t", mcmc_vecs[ jmc ] [ imc ] );
	    }  */
    // Clean up
    fclose ( isp->outPtr );
    fflush( stdout );
    fflush(stderr);
    // Free allocated memory 
    double1free( vec_hold );
    double1free( logP );
    double1free( vece_hold );
    double2free( prob95 );
    double1free( isp->allparameters );
    double1free( isp->constraints );
    double1free( isp->solution );
    int2free( isp->data );
    int2free( isp->psd_mat );
    double2free( isp->prop_mat );
    double2free( isp->dprob_1_mat );
    double2free( isp->dprob_2_mat );
    double2free( isp->dprob_mat );
    double2free( isp->dcump_mat );
    double2free( isp->e_mat );
    double2free( isp->re_mat );
    // double2free( isp->gfs_mat );
    gsl_rng_free(isp->random);
}


