/* Get log likelihood */

/*      GENERAL DESCRIPTION
	This program reads in a set of parameters and a frequency matrix for a rating scale
	experiment. It calculates a probability matrix from the parameters and then finds the 
	log likelihood. From the parameters, the program chooses one of the three basic equations
	for a rating scale experiment with N stimuli and M responses. 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.

	The program can model a 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 a pair or all three parameters, 
	or all three can be constrained.

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

        INITIALIZATION

        Input to the program is via a file with the following
        control parameters in free form.
          header: a general header of up to 50 characters
	  nums: The number of stimuli 
	  numr: The number of responses
	  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)
	  qracc: qromox accuracy
          (STARTING PARAMETERS)
	  A vector of length 2*(nums + numr - 1) + 3, including PR1, PR2, and R3. 
	  The order is signal means and then signal standard deviations. 
	  If all signal standard deviations are zero, the complementary model is invoked.
	  Next come criterion means and standard deviations.
	  If all criterion standard deviations are zero,
	  the SDT model is invoked. Finally, PR1, PR2, and PR3 appear. They must add to unity.
	  (S-R MATRIX)
	  This has nums rows and numr columns                                          */


// Includes
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <gsl/gsl_cdf.h>
#include <gpklib.h>
#include <gpklib/romberg.h>
#include <gpklib/ieee754.h>
#include "gpklib/NaN.h"
#include <assert.h>
#include "get_lli.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_gll *isp;
    double qracc() C { return isp->qracc; }
    int rule;   // Which RKKK rule, 1, 2, or 3
};

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

static float 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;
}

struct initialization_gll lcjgll_init;
struct initialization_gll *lcjgll = &lcjgll_init; 


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

    if ( argc < 2 ) exit( 1 );
    open_file( argv [ 1 ], lcjgll  );
    lcjcgll_init(  lcjgll );
    wsdo_logp ( lcjgll );
    free_dm( lcjgll );
    return 0;
}

void open_file( char gen_name [ 50 ], struct initialization_gll *isp )
{
    char sf_name[ 50 ];
    short open_indic = 0;

    while ( open_indic == 0 ) {
	strcpy( sf_name, gen_name );
	strcat( sf_name, ".rsx" );
	isp->inPtr = fopen( sf_name, "a+" );
	if ( isp->inPtr == NULL ) {
	    printf( "\n Error opening input file %s\n", sf_name );
	    printf( " get another name.\n" );
	    printf( "\n Program quitting." );
	    exit( 0 );
        }
	   else  open_indic = 1;
    }
}

/* Do all initialization and read in the data:
   This results in a structure lcjgll_init with all the fields filled in   */

void lcjcgll_init ( struct initialization_gll *isp )
{
    int i, j, ok;
    char ahold[1024];


    /* Read control variables from file */
    ok = fscanf ( isp->inPtr, "%s", isp->header ) == 1;
    assert (ok);

    isp->nums = ReadInt( isp->inPtr );
    isp->numr = ReadInt( isp->inPtr );
    isp->dec_r = ReadInt( isp->inPtr );
    isp->qracc = ReadFloat( isp->inPtr );
    assert(isp->qracc > 0.0);

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

    /* Allocate storage for parameters, constraints, data, and probability matrices  */    
    printf( "\n number parameters = %d", isp->nallparams );
    isp->allparameters = double1alloc(isp->nallparams );
    isp->dprob_1_mat = double2alloc(isp->nums, isp->numr );
    isp->dprob_2_mat = double2alloc(isp->nums, isp->numr );
    isp->dprob_3_mat = double2alloc(isp->nums, isp->numr );
    isp->dprob_mat = double2alloc(isp->nums, isp->numr );
    isp->freq_mat = int2alloc(isp->nums, isp->numr );

    /* Read parameters; last three are PR1, PR2, PR3; dump initial text */  
    for ( int i = 0; i < isp->nallparams; i++ ) {
        isp->allparameters [ i ] = ReadFloat( isp->inPtr );
    }
    printf( "\nParameters read" );
    // Check that PR1, PR2, and PR3 add to unity.
    double psum = 0;
    psum = ( isp->allparameters [ isp->pstart ] +
	     isp->allparameters [ isp->pstart + 1 ] +
	     isp->allparameters [ isp->pstart + 2 ]  );
    assert ( ( psum > 0.9995 ) && ( psum < 1.0005 )  );
    printf( "\nParameters read" );

    /* Get ssdsum, csdsum */
    isp->ssdsum = 0.0;
    for ( i = isp->s_sdstart; i <= isp->s_sdend; i++ )
	isp->ssdsum += isp->allparameters [ i ];
    isp->csdsum = 0.0;
    for ( i = isp->c_sdstart; i <= isp->c_sdend; i++ )
	isp->csdsum += isp->allparameters [ i ];
    assert ( ( isp->ssdsum > 0 ) || ( isp->csdsum > 0 ) );

    // Read frequency matrix
    for ( i = 0; i < isp->nums; i++ )
      for ( j = 0; j < isp->numr; j++ ) isp->freq_mat [ i ] [ j ] = ReadInt( isp->inPtr );

    // This is the end of the file.
    ok = fscanf ( isp->inPtr, "%s", ahold ) == 1;  // Read should fail.
    assert(!ok);
}



/* Compute log likelihood: */
void wsdo_logp ( struct initialization_gll *isp )
{

    int iwf, jwf, wfdum;
    double partprob, PR1=0.0, PR2=0.0, PR3=0.0, wl_f = 0;

    printf( "\n  wsdo_logp started\n" );

    // Get rule weights PR1, PR2, PR3 
    PR1 = isp->allparameters [ isp->pstart ];
    PR2 = isp->allparameters [ isp->pstart + 1 ];
    PR3 = isp->allparameters [ isp->pstart + 2 ];
    switch ( isp->dec_r ) {
        case 14: 
		 assert ( PR3 == 0 );
		 break;
        case 15: 
		 assert( PR2 == 0 );
		 break;
        case 16: 
		 assert ( PR1 == 0 );
		 break;

        default: break;
    }
    printf( "\nProbabilities set" );


    printf( "\n Main loop" );
    /* Main loop 1. Get probability matrices required by isp->dec_r. Combine them via PR1, PR2, PR3.
       Get cumulative probability matrix               */
    assert(0<=isp->numc && isp->numc < isp->numr);
    for ( iwf = 0; iwf < isp->nums; iwf++ ) {
      // Check for fsdt  or csdt
	if ( isp->csdsum != 0 ) {
	   for ( jwf = 0; jwf < isp->numc; jwf++ ) {
	      // Check for csdt
	      if ( isp->ssdsum == 0 ) {
	        // Rule 1
	        if ( PR1 != 0 ) {
	              isp->dprob_1_mat [ iwf ] [ jwf ] =  eq3a_wrap ( 1, iwf, jwf, isp->allparameters, isp );
	              if ( isp->dprob_1_mat [ iwf ] [ jwf ] < DBL0 ) isp->dprob_1_mat [ iwf ] [ jwf ] = DBL0;
	        }
	        else ( isp->dprob_1_mat [ iwf ] [ jwf ] = 0 );

	        // Rule 2
	        if ( PR2 != 0 ) {
	            isp->dprob_2_mat [ iwf ] [ jwf ] = eq3a_wrap ( 2, iwf, jwf, isp->allparameters, isp );
	            if ( isp->dprob_2_mat [ iwf ] [ jwf ] < DBL0 ) isp->dprob_2_mat [ iwf ] [ jwf ] = DBL0;
	        }
	        else ( isp->dprob_2_mat [ iwf ] [ jwf ] = 0 );

	        // Rule 3
	        if ( PR3 == 0 ) ( isp->dprob_3_mat [ iwf ] [ jwf ] = 0 );
	        else {
	          if ( jwf == 0 ) {
	             isp->dprob_3_mat [ iwf ] [ jwf ] = eq3a_wrap ( 3, iwf, jwf, isp->allparameters, isp );
	             if ( isp->dprob_3_mat [ iwf ] [ jwf ] < DBL0 ) isp->dprob_3_mat [ iwf ] [ jwf ] = DBL0;
	          }

	          else {
	             isp->dprob_3_mat [ iwf ] [ jwf ] = eq3a_wrap ( 3, iwf, jwf, isp->allparameters, isp );
	             partprob  = eq3a_wrap ( 4, iwf, jwf - 1, isp->allparameters, isp );
	             isp->dprob_3_mat [ iwf ] [ jwf ] += partprob;
	             if ( isp->dprob_3_mat [ iwf ] [ jwf ] < DBL0 ) isp->dprob_3_mat [ iwf ] [ jwf ] = DBL0;
                  }

	        }
	      }
	      else {
                 // FSDT
	         // Rule 1
	         if ( PR1 != 0 ) {
	            isp->dprob_1_mat [ iwf ] [ jwf ] = eq3_wrap ( 1, iwf, jwf, isp->allparameters, isp );
	            if ( isp->dprob_1_mat [ iwf ] [ jwf ] < DBL0 ) isp->dprob_1_mat [ iwf ] [ jwf ] = DBL0;
	         }
	         else ( isp->dprob_1_mat [ iwf ] [ jwf ] = 0 );
	         // Rule 2
	         if ( PR2 != 0 ) {
	             isp->dprob_2_mat [ iwf ] [ jwf + 1 ] = eq3_wrap ( 2, iwf, jwf, isp->allparameters, isp );
	             if ( isp->dprob_2_mat [ iwf ] [ jwf + 1 ] < DBL0 ) isp->dprob_2_mat [ iwf ] [ jwf +1 ] = DBL0;
	         }
	         else ( isp->dprob_2_mat [ iwf ] [ jwf ] = 0 );
	         // Rule 3
	         if ( PR3 == 0 ) ( isp->dprob_3_mat [ iwf ] [ jwf ] = 0 );
	         else {
	           if ( jwf == 0 ) {
	              isp->dprob_3_mat [ iwf ] [ jwf ] = eq3_wrap ( 3, iwf, jwf, isp->allparameters, isp );
	              if ( isp->dprob_3_mat [ iwf ] [ jwf ] < DBL0 ) isp->dprob_3_mat [ iwf ] [ jwf ] = DBL0;
	           }
	           else {
	              isp->dprob_3_mat [ iwf ] [ jwf ] = eq3_wrap ( 3, iwf, jwf, isp->allparameters, isp );
	              partprob  = eq3_wrap ( 4, iwf, jwf - 1, isp->allparameters, isp );
	              isp->dprob_3_mat [ iwf ] [ jwf ] += partprob;
	              if ( isp->dprob_3_mat [ iwf ] [ jwf ] < DBL0 ) isp->dprob_3_mat [ iwf ] [ jwf ] = DBL0;
                   }
	         }
	      }
           }
           // 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 ( 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 to get dprob_mat
           for ( 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 ];
               assert ( ( isp->dprob_mat [ iwf ] [ jwf ] <= 1 ) && ( isp->dprob_mat [ iwf ] [ jwf ] >= 0 ) );
           }
        }
        else {
    	    // SDT
            // Rule1 and Rule2 sdt probabilities are identical
	    // Function sdt computes p(R=j) over jwf=1, ... , numr
	    wfdum = sdt ( iwf, isp );
	}
    }

    // Print dprob_mat
    fprintf( isp->inPtr, "\n Probability matrix\n ");
    for ( iwf = 0; iwf < isp->nums; iwf++ ) {
	fprintf( isp->inPtr, "\n" );
        for ( jwf = 0; jwf < isp->numr; jwf++ ) fprintf( isp->inPtr, "  %5.4f", isp->dprob_mat [ iwf ] [ jwf ] );
    }
    fprintf( isp->inPtr, "\n" );
    fprintf( isp->inPtr, "\n" );
	   
    // Get log_p
    wsdo_l_f ( isp );
    fprintf( stderr, " \nwsdo_logp done " );
}

void missing_entry( double **dpmat, C struct initialization_gll *isp, int r_start, int r_end, double PRx )
{

    int iwf, jwf;
    double r_sum;

    if ( ( r_start == 0 ) && ( PRx == 0.0 ) ) {
      for ( iwf = 0; iwf < isp->nums; iwf++ ) dpmat [ iwf ] [ isp->numc ] = 0;
      return;
    }
    if ( ( r_start == 1 ) && ( PRx == 0.0 ) ) {
      for ( iwf = 0; iwf < isp->nums; iwf++ ) dpmat [ iwf ] [ 0 ] = 0;
      return;
    }
    else {
      for ( iwf = 0; iwf < isp->nums; iwf++ ) {
           r_sum = 0.0;
           for ( jwf = r_start; jwf <= r_end; jwf++ ) r_sum += dpmat [ iwf ] [ jwf ];
	   if ( r_sum > 1.03 ) {
	       printf ( "\n r_sum = %7.5f\n", r_sum );
	       fflush ( stdout );
	   }
      	   if ( r_sum <= 1.0 ) {
	      if ( r_start == 0 ) dpmat [ iwf ] [ isp->numc ] = 1.0 - r_sum;
	      else if ( r_start == 1 ) dpmat [ iwf ] [ 0 ] = 1.0 - r_sum;
	   }
	   /* Normalize row if needed  */
	   if ( r_sum > 1.0 ) for ( jwf = 0; jwf < isp->numr; jwf++ ) dpmat [ iwf ] [ jwf ] /= r_sum;
           for ( jwf = 0; jwf < isp->numr; jwf++ )
               assert ( ( dpmat [ iwf ] [ jwf ] <= 1 ) && ( dpmat [ iwf ] [ jwf ] >= 0 ) );
      }
    }
}

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


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

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



double eq3_wrap ( int rk1kk23, int ie3, int je3, double *pp, C struct initialization_gll *isp )
{
    // ssdsum != 0
    int j3w = je3;

    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;
    int i3w;
    for( 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 [ i3w ] = mumax + 6.0 * sigmas;
    sigma [ i3w ] = sigmas;
    D printf ( "mu[%d]=%g sigma[%d]=%g\n", i3w, mu [ i3w ], i3w, sigma [ i3w ] );
    dprob  = eq3( rk1kk23, j3w, nc, mu, sigma, mus, sigmas, isp );
    if ( isnan( dprob ) ) dprob = 1E-50;
    double1free ( mu );
    double1free ( sigma );
    return dprob;
}

double eq3 ( int rk1kk23, int j, int n, double *mu, double *sigma, double mus, double sigmas,
		C struct initialization_gll *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;

    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;
    C int j = info->j;
    assert( info->isp );
    assert( info->sigma[ j ] >= 0.0 );
    infoinner_s  iinner(info, s);
    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 );

    C double qbreak = info->mu[j] + info->sigma[ j ]*2.0;
    C double mbreak = dmin( info->mu[j] - info->sigma[ j ]*2.0, s - info->sigma[ j ] );
    C double pminf = dmin ( s, info->mu[j] ) - info->sigma[ j ]*6.0;
    assert ( info->sigma[j] >= 0.0);
    assert ( pminf <= info->mu[j]);
    assert ( pminf <= s );
    double sC=0.0, sN=0.0, sP=0.0;


    if ( ( info->rule == 1 ) || ( ( info->rule == 3 ) && ( j == 0 ) ) ) {
        // EQs (1) or (3A)
	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);
	sC = qromox( innerkernel, dmax(s, nbreak), pbreak, INTALG, &iinner, nc, info->qracc(), sguess, KMAX, RATIONAL_INNER );
	if(!finite(sC))	return quiet_nan(0);
	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);
	sP = qromox( innerkernel, pbreak, pinf, INTALG, &iinner, nc, info->qracc(), 0.1*sguess+sC+sN, KMAX, RATIONAL_INNER );
    }

    if ( info->rule == 2 ) {
        // EQ (2)
	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 > mbreak);
	assert(mbreak < qbreak);
        sC = qromox( innerkernel, mbreak, dmin(s, qbreak), INTALG, &iinner, nc, info->qracc(), sguess, KMAX, RATIONAL_INNER );
	if(!finite(sC))	return quiet_nan(0);
        sN = 0.0;
	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);
    	}
	assert(mbreak > pminf);
        sP = qromox( innerkernel, pminf, mbreak, INTALG, &iinner, nc, info->qracc(), 0.1*sguess+sC+sN, KMAX, RATIONAL_INNER );
    }

    if ( ( info->rule == 3 ) && ( j != 0 ) ) {
	// EQ (3B)
	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);
	sC = qromox( innerkernel, dmax(s, nbreak), pbreak, INTALG, &iinner, nc, info->qracc(), sguess, KMAX, RATIONAL_INNER );
	if(!finite(sC))	return quiet_nan(0);
	sN = 0.0;
	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);
	sP = qromox( innerkernel, pbreak, pinf, INTALG, &iinner, nc, info->qracc(), 0.1*sguess+sC+sN, KMAX, RATIONAL_INNER );
    }

    if ( info->rule == 4 ) {
	// EQ (3C)
	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 > mbreak);
	assert(mbreak < qbreak);
        sC = qromox( innerkernel, mbreak, dmin(s, qbreak), INTALG, &iinner, nc, info->qracc(), sguess, KMAX, RATIONAL_INNER );
	if(!finite(sC))	return quiet_nan(0);
        sN = 0.0;
	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);
    	}
	assert(mbreak > pminf);
        sP = qromox( innerkernel, pminf, mbreak, INTALG, &iinner, nc, info->qracc(), 0.1*sguess+sC+sN, KMAX, RATIONAL_INNER );
    }
    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 );
    // fprintf(stderr, "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 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 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 3:
	   assert( cj >= ( 2 * info->s - cj ) );
	    if ( j == 0 ) {
	      // Rule 3A
	      if ( k == 0 ) {
		term = 1.0;
	        break;
	      }
	      z2 = ( cj - info->mu[ k ] ) / ( r2 * info->sigma[ k ] );
	      z1 = ( ( 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;
            }
	    else {
	      // EQ (3B)
	      if ( k == j )  {
		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 4:   
	      // EQ (3C)
	      assert ( ( 2 * info->s - cj ) >= cj );
	      if ( k == j ) {
		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;
}

double eq3a_wrap ( int rk1kk23, int ie3a, int je3a, double *pp, C struct initialization_gll *isp)
{
    // CSDT: ssdsum == 0
    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 ( int rk1kk23, int j, int n, double *mu, double *sigma, double mus, double sigmas, C struct initialization_gll *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);
    C double pinf = iinner.mu[ j ] + iinner.sigma[ j ] * 6.0;
    C double minf = iinner.mu[ j ] - iinner.sigma[ j ] * 6.0;
    D printf ( "\ts=%g pinf=%g\n", s, pinf );
    if ( ( rk1kk23 == 1 ) && ( pinf <= s ) ) return 0.0;
    if ( ( rk1kk23 == 2 ) && ( 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;
    if ( ( rk1kk23 == 1 ) || ( ( rk1kk23 == 3 ) && ( j == 0 ) ) ) {
       if ( s < iinner.mu [ j ] ) {
 	    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);
	    qr += qromox ( innerkernel, iinner.mu[j], pinf, INTALG, &iinner, nc, iinner.qracc(), 0.5*(sguess+fabs(qr)), KMAX, RATIONAL_INNER );
        }
        else qr = qromox ( innerkernel, s>minf?s:minf, pinf, INTALG, &iinner, nc, iinner.qracc(), sguess, KMAX, RATIONAL_INNER );
    }
    if ( rk1kk23 == 2 ) {
        if ( s > iinner.mu [ j ] ) {
            qr = qromox ( innerkernel, iinner.mu [ j ], s, INTALG, &iinner, nc, iinner.qracc(), sguess, KMAX, RATIONAL_INNER );
	    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);
        }
        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 == 3 ) && ( j != 0 ) ) {
	// First part of rule 3
       if ( s < iinner.mu [ j ] ) {
 	    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);
	    qr += qromox ( innerkernel, iinner.mu[j], pinf, INTALG, &iinner, nc, iinner.qracc(), 0.5*(sguess+fabs(qr)), KMAX, RATIONAL_INNER );
        }
        else qr = qromox ( innerkernel, s>minf?s:minf, pinf, INTALG, &iinner, nc, iinner.qracc(), sguess, KMAX, RATIONAL_INNER );
       // Second part
        if ( s > iinner.mu [ j ] ) {
            qr += qromox ( innerkernel, iinner.mu [ j ], s, INTALG, &iinner, nc, iinner.qracc(), sguess, KMAX, RATIONAL_INNER );
	    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);
        }
        else qr += qromox ( innerkernel,  minf, s<iinner.mu[j]?s:iinner.mu[j], INTALG, &iinner, nc, iinner.qracc(), sguess, KMAX, RATIONAL_INNER );
    }
    return qr;
}


int sdt ( int isdtx, struct initialization_gll *isp )
{
    /* For signal detection theory, this function calculates 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 = isp->allparameters [ isp->s_sdstart + isdtx ];
    if ( ddenom <  DBL0 ) ddenom = DBL0;
    // Get all but last p=jsdt
    for ( int jsdt = 0; jsdt < isp->numr - 1; jsdt++ ) {
        /* Get z value  */
        double z_sdt = ( isp->allparameters [ isp->c_mstart + jsdt ] - isp->allparameters [ isdtx ] ) / ddenom;
        newcp = gsl_cdf_ugaussian_P ( z_sdt );
	assert ( newcp >= lastcp );
        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;
    return ( 0 );
}


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

void wsdo_l_f ( struct initialization_gll *isp )
{
    /* This function takes the (pseudo)data matrix to get l_f, log likelihood. */
    assert(isp);

    /* Get l_f */
    double l_f = 0.0;
    for ( int ilf = 0; ilf < isp->nums; ilf++ )  {
	for ( int jlf = 0; jlf < isp->numr; jlf++ ) {
	    assert ( isp->freq_mat [ ilf ] [ jlf ] >= 0 );
	    double prby = Max(isp->dprob_mat [ ilf ] [ jlf ], 1e-20);
	    l_f += log( prby ) * isp->freq_mat [ ilf ] [ jlf ];
        }
    }
    isp->log_P = l_f;
    // Write log_P after frequency matrix
    fprintf ( isp->inPtr, "\n\n\n decision rule %d \n log_P = %9.5f\n", isp->dec_r, isp->log_P );
}

void free_dm ( C struct initialization_gll *isp )
{
    // Clean up
    fflush( stdout );
    fflush(stderr);

    /* Free allocated memory */
    int2free ( isp->freq_mat );
    double2free ( isp->dprob_1_mat );
    double2free ( isp->dprob_2_mat );
    double2free ( isp->dprob_3_mat );
    double2free ( isp->dprob_mat );
    double1free ( isp->allparameters );
    fclose ( isp->inPtr );
}


