/*  The below has been adapted from R package rmutil : A Library of Special
 *  Functions for Repeated Measurements
 *  Copyright (C) 1998, 1999, 2000, 2001 J.K. Lindsey
 *
 *  Only psimplex_c() and its dependencies (romberg2, evalfn, interp,
 *  dsimplex) have been extracted. No other changes have been made.
 *
 *  rmutil is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 *  GNU General Public License for more details.
 */

#include <math.h>
#include <stddef.h>
#include "R.h"
#include "Rmath.h"

/* romberg integration routines */
static void interp(double x[], double fx[], int pts, double tab1[],
		   double tab2[], double *f, double *df, int *err)
{
  int i,j,ni=0;
  double diff1,diff2,tmp1,tmp2,lim1,lim2;
 
  *err=0;
  tmp1=fabs(x[0]);
  for(i=0;i<pts;i++){
    tmp2=fabs(x[i]);
    if(tmp2<tmp1){
      ni=i;
      tmp1=tmp2;}
    tab1[i]=fx[i];
    tab2[i]=fx[i];}
  *f=fx[ni--];
  for(j=0;j<pts-1;j++){
    for(i=0;i<pts-j-1;i++){
      lim1=x[i];
      lim2=x[i+j+1];
      diff1=tab1[i+1]-tab2[i];
      diff2=lim1-lim2;
      if(diff2==0.0){
	*err=2;
	return;}
      diff2=diff1/diff2;
      tab2[i]=lim2*diff2;
      tab1[i]=lim1*diff2;}
    *df=2*ni<(pts-j-3)?tab1[ni+1]:tab2[ni--];
    *f+=*df;}}

static void evalfn(void (*fcn)(double*, double*, double*, double*, int, double*), double a[], double b[], int n,
		   int len, double sum[], double tmpsum[], double zz[],
		   double pnt1[], double pnt2[], double arg1[],
		   double arg2[], double arg3[], double x[])
{
  int i,j,k,nn;

  if(n==1){
    for(k=0;k<len;k++)zz[k]=0.5*(a[k]+b[k]);
    fcn(zz,arg1,arg2,arg3,len,x);
    for(k=0;k<len;k++)sum[k]=(b[k]-a[k])*x[k];
    return;}
  else {
    for(i=1,j=1;j<n-1;j++) i*=3;
    nn=i;
    for(k=0;k<len;k++){
      pnt1[k]=(b[k]-a[k])/(3.0*nn);
      pnt2[k]=2.0*pnt1[k];
      zz[k]=a[k]+0.5*pnt1[k];
      tmpsum[k]=0.0;}
    for(j=1;j<=i;j++){
      fcn(zz,arg1,arg2,arg3,len,x);
      for(k=0;k<len;k++){
	tmpsum[k]+=x[k];
	zz[k]+=pnt2[k];}
      fcn(zz,arg1,arg2,arg3,len,x);
      for(k=0;k<len;k++){
	tmpsum[k]+=x[k];
	zz[k]+=pnt1[k];}}
    for(k=0;k<len;k++)sum[k]=(sum[k]+(b[k]-a[k])*tmpsum[k]/nn)/3.0;
    return;}}

static void romberg2(void (*fcn)(double*, double*, double*, double*, int, double*), double *a, double *b, int len,
		    double *arg1, double *arg2, double *arg3, double eps,
		    int pts, int max, int *err, double sumlen[])
{
  int i,j,j1,finish;
  double errsum,*tab1,*tab2,*x,*fx,*sum,*tmpsum,*zz,*pnt1,*pnt2,*y;

  x=(double*)R_alloc((size_t)(max*len),sizeof(double));
  fx=(double*)R_alloc((size_t)(max*len),sizeof(double));
  sum=(double*)R_alloc((size_t)len,sizeof(double));
  tmpsum=(double*)R_alloc((size_t)len,sizeof(double));
  zz=(double*)R_alloc((size_t)len,sizeof(double));
  pnt1=(double*)R_alloc((size_t)len,sizeof(double));
  pnt2=(double*)R_alloc((size_t)len,sizeof(double));
  tab1=(double*)R_alloc((size_t)pts,sizeof(double));
  tab2=(double*)R_alloc((size_t)pts,sizeof(double));
  y=(double*)R_alloc((size_t)len,sizeof(double));
  if(!x||!fx||!sum||!tmpsum||!zz||!pnt1||!pnt2||!tab1||!tab2||!y){
    *err=1;
    return;}
  *err=0;
  for(i=0;i<len;i++)x[i*max]=1.0;
  for(j=0;j<max;j++){
    j1=j+1;
    evalfn(fcn,a,b,j1,len,sum,tmpsum,zz,pnt1,pnt2,arg1,arg2,arg3,y);
    finish=(j1>=pts?1:0);
    for(i=0;i<len;i++){
      fx[j+i*max]=sum[i];
      if(j1>=pts){
	interp(&x[j1-pts+i*max],&fx[j1-pts+i*max],pts,tab1,tab2,&sumlen[i],&errsum,err);
	if(*err)return;
	if(fabs(errsum)>eps*fabs(sumlen[i]))finish=0;}
      x[j1+i*max]=x[j+i*max]/9.0;
      fx[j1+i*max]=fx[j+i*max];}
    if(finish)return;}
  *err=3;
  return;}

/* simplex distribution */
static void dsimplex(double y[], double m[], double s[], double f[], int len,
	   double res[]){
  int i;
  for(i=0;i<len;i++)res[i]=exp(-pow((y[i]-m[i])/(m[i]*(1-m[i])),2)/(2*y[i]*(1-y[i])*s[i]))/sqrt(2*M_PI*s[i]*pow(y[i]*(1-y[i]),3));}

void psimplex_c(double y[], double m[], double s[], double f[], int *len,
	   double *eps, int *pts, int *max, int *err, double res[]){
  double *x;
  int i;
  x=(double*)R_alloc((size_t)(*len),sizeof(double));
  for(i=0;i<*len;i++)x[i]=0;
  romberg2(dsimplex, x, y, *len, m, s, f, *eps, *pts, *max, err, res);}
