//
// Filename:       paxx.cc
// Created:        Mon Dec 14 13:56:02 2009
// By user:        rzon
// On machine:     rzon-PC
// Implements the definitions in the header file paxx.h
// More documentation may be found in the header.
//

#include "paxx.h"

// Implementation of things defined in paxx.h:

#include <cmath>

namespace pa
{

// some local helper functions
static double pa_sqr(double x)
{ 
  return x*x;
}

static double pa_min(double x, double y)
{ 
  return x<y?x:y; 
}

// computation of the q values from the Kolmogorov-Smirnov test 
static double pa_q(int n, const double *f, int *j)
{
  double a, b, d, e, c, g, h, q;
  int i;

  b = sqrt(n);
  e = g = h = q = 0.0;
  c = 2;
  *j = 0;
  for (i = 0; i < n; ++i) {
    d = f[i]-e;
    if (d > h) {
      *j = i;
      h = d;
    }
    e = (double)(i+1)/n;
    d = e-f[i];
    if (d > h) {
      *j = i;
      h = d;
    }
  }
  d = b*h+0.12*h+0.11*h/b;
  a = -2*d*d;
  for(i = 1;i<= 200;++i){
    d = c*exp(a*i*i);
    q += d;
    if (fabs(d) <= g)
      break;
    c = -c;
    g = fabs(d)/1000;
  }
  if (i > 200)
    printf("#pa_q: no convergence.\n");
  return q;
}

// constructor 
Fit::Fit(): km_(-1), mm_(-1)
{
  k=0;
  a=b=c=f=g=0;
  d=0;
}

void Fit::dealloc()
{
  if (km_>=0) {
    for (int i = 0; i < km_; ++i)
      delete[] d[i];
    delete[] a;
    delete[] b;
    delete[] c;
    delete[] d;
    delete[] f;
    delete[] g;
  }
  km_=-1;
  mm_=-1;
}

void Fit::realloc(int km, int mm)
{
  dealloc();
  km_=km;
  mm_=mm;
  k = 0;
  a = new double[km+1];
  b = new double[km];
  c = new double[km];
  d = new double*[km];
  f = new double[km];
  g = new double[km];
  for (int i = 0; i < km; ++i)
    d[i] = new double[mm+1];
}

// deconstructor 
Fit::~Fit()
{
  dealloc();
}

// computes the parameters of the piecewise analytic fit to a
// probability distribution from data. 
double Fit::fit(int n, const double *r, double qm,
                int km, int mm, bool nodiscont)
{
  if(km>km_ or mm>mm_)
    realloc(km,mm);
  int     i, j , m , mu, nu, imax, nToDo; 
  double  q, mp, xs, x1, x2, s1, s2, c1, c2, pr, pl, dp, qAim;
  double  mpx, smpx, cmpx, mpx2, dx, sdx, cdx;
  int* index = new int[km];
  int* ibegin = new int[km];
  int* iend = new int[km];
  int* np = new int[km];
  int* toDo = new int[km];
  double* qcut = new double[km];
  double* F = new double[n];
  double* z = new double[n];
  double* dummy = new double[mm+1];
  bool* done = new bool[km];
  for(i=0;i<km;++i)done[i]=false;
  k = 1; //start analysis with one interval
  ibegin[0] = 0;
  iend[0] = n-1;
  qcut[0] = qm;
  toDo[0] = 0; 
  nToDo = 1;
  while (nToDo) {  
    nu = toDo[--nToDo]; //get interval index
    np[nu] = iend[nu]-ibegin[nu]+1;
    for(i = ibegin[nu];i<= iend[nu];++i) 
      F[i] = z[i] = (r[i]-r[ibegin[nu]])/(r[iend[nu]]-r[ibegin[nu]]);
    q = pa_q(np[nu],F+ibegin[nu],&imax);  
    m = 0; //start with zero modes
    while (q<qcut[nu] && m<mm) {   
      ++m; //add a fourier mode
      mpx = 0.0;
      smpx = 0.0;
      cmpx = 1.0;
      mp = m*M_PI;
      x2 = 0.0;
      s2 = 0.0;
      c2 = 1.0;
      d[nu][m] = 0.0;
      for (i = 1; i < iend[nu]-ibegin[nu]; ++i) {
        x1 = x2;
        s1 = s2;
        c1 = c2;
        xs = i/(double)np[nu];
        x2 = z[i+ibegin[nu]];
        mpx2 = mp*x2;
        dx = mpx2-mpx;
        if (dx < 0.018) {
          //compute faster using a series (accurate up to 10^-6)
          sdx = dx;
          cdx = 1.0-0.5*dx*dx;
          s2 = smpx*cdx+cmpx*sdx;
          c2 = cmpx*cdx-smpx*sdx;
        } else {
          s2 = sin(mpx2); 
          c2 = cos(mpx2);
          mpx = mpx2;
          smpx = s2;
          cmpx = c2;            
        }
        d[nu][m]+= 2*((xs-x1)*c1-(xs-x2)*c2+(s1-s2)/mp)/mp;
      }
      for (i = ibegin[nu]+1; i < iend[nu]; ++i)
        F[i] += d[nu][m]*sin(mp*z[i]);
      q = pa_q(np[nu], F+ibegin[nu], &imax);
    }
    d[nu][0] = (double)m; //store number of fourier modes
    if (q < qcut[nu] && k < km) { 
      if (r[ibegin[nu]+imax] != r[ibegin[nu]] 
          && r[ibegin[nu]+imax] != r[iend[nu]]) { 
        //add an interval
        ibegin[k] = ibegin[nu]+imax;
        iend[k] = iend[nu];
        iend[nu] = ibegin[k];
        qcut[k] = (1.0-(double)imax/np[nu])*qcut[nu];
        qcut[nu]*= (double)imax/np[nu];
        toDo[nToDo++] = nu;
        toDo[nToDo++] = k;
        ++(k);
      } else {
        printf("#pafit: no convergence\n");
        printf("#pafit: convergence trouble at %f\n", r[ibegin[nu]+imax]);
      }
    }
  }
  //sort intervals
  for (mu = 0; mu < k; ++mu) 
    index[mu] = mu;
  i = 1;
  while (i<k && ibegin[index[i-1]]<ibegin[index[i]]) 
    ++i;    
  while (i<k) {
    mu = index[i];
    for (j = i; j>0 && ibegin[index[j-1]]>ibegin[mu]; j--)
      index[j] = index[j-1];
    index[j] = mu;
    while (i<k && ibegin[index[i-1]]<ibegin[index[i]]) 
      ++i;
  } 
  for (mu = 0; mu < k; ++mu) //apply permutation in "index"
    if (!done[mu] && index[mu] != mu) {
      for (i = 0; i <= mm; ++i) 
        dummy[i] = d[mu][i]; 
      for (nu = mu; index[nu] != mu; nu = index[nu]) {
        for(i = 0; i <=  mm; ++i) 
          d[nu][i] = d[index[nu]][i];
        done[nu] = true;
      } 
      for (i = 0; i <= mm; ++i) 
        d[nu][i] = dummy[i];
      done[nu] = true;
    }
  delete[]done;
  delete[]dummy;
  for (mu = 0; mu < k; ++mu) {
    a[mu] = r[ibegin[index[mu]]];
    b[mu] = 0; 
    c[mu] = 0; 
    f[mu] = (double)np[index[mu]]/n;
    g[mu] = (mu!= 0)?(g[mu-1]+f[mu-1]):0.0;
    F[ibegin[index[mu]]] = 0.0;
    for (i = ibegin[index[mu]]; i < iend[index[mu]]; ++i) 
      F[i] = g[mu]+f[mu]*F[i];   
  }
  a[k] = r[n-1];
  F[n-1] = 1.0;  
  q = pa_q(n,F,&imax); //final q test
  if (nodiscont && k>1) { //smooth out discontinuities
    qAim = pa_min(q,qm);
    for (mu = 1; mu < k; ++mu) {
      pl = 0.0;
      pr = 0.0; 
      m = (int)(d[mu-1][0]);
      for (j = 1; j <= m; ++j) 
        pl+= ((j&1)!= 0?-j:j)*d[mu-1][j];
      m = (int)(d[mu][0]);
      for (j = 1; j <= m; ++j)
        pr+= j*d[mu][j];
      dp = f[mu]*(1+M_PI*pr)/(a[mu+1]-a[mu]) 
        - f[mu-1]*(1+M_PI*pl)/(a[mu]-a[mu-1]);
      c[mu] = 0.5*pa_min(a[mu+1]-a[mu],a[mu]-a[mu-1]);
      b[mu] = 0.25*dp/c[mu];         
    }
    i = 0;
    for (nu = 1; nu < k; ++nu) {
      for(; r[i] < a[nu]-c[nu]; ++i)
        z[i] = F[i];
      for(; r[i] < a[nu]; ++i)
        z[i] = F[i]+b[nu]*pa_sqr(r[i]-(a[nu]-c[nu]));
      for(; r[i] < a[nu]+c[nu]; ++i) 
        z[i] = F[i]+b[nu]*pa_sqr(r[i]-(a[nu]+c[nu]));
    }
    for (; i < n; ++i)
      z[i] = F[i];
    q = pa_q(n, z, &imax);        
    while (q < qAim) {
      nu = 0;
      while (a[nu+1] < r[imax])
        ++nu;
      if (nu < (k)-1 && a[nu+1]-r[imax]<c[nu+1])
        ++nu;
      i = 0;
      while (r[i] < a[nu]-c[nu])
        ++i;
      for(; r[i] < a[nu]+c[nu]; ++i) 
        z[i] = F[i];
      c[nu]/= 2;
      b[nu]*= 2;
      if (c[nu] < (a[nu+1]-a[nu])/np[index[nu]])  
        b[nu] = c[nu] = 0;
      i = 0;
      while (r[i] < a[nu]-c[nu])
        ++i;
      for (; r[i] < a[nu]; ++i)
        z[i] = F[i]+b[nu]*pa_sqr(r[i]-(a[nu]-c[nu]));
      for (; r[i] < a[nu]+c[nu]; ++i) 
        z[i] = F[i]+b[nu]*pa_sqr(r[i]-(a[nu]+c[nu]));
      q = pa_q(n,z,&imax);
    }
  }
  delete[]F;
  delete[]z;
  delete[]index;
  delete[]ibegin;
  delete[]iend;
  delete[]np;
  delete[]toDo;
  delete[]qcut;
  return q;
}

// evaluate the piecewise analytic probability distribution at a point r
double Fit::pd(double r)
{
  int nu, m ,j;
  double p, patch, z;

  if (r < a[0] || r > a[k] || k == 0) {
    return 0;
  } else {
    nu = 0;
    while (a[nu+1] < r)
      ++nu;
    z = M_PI*(r-a[nu])/(a[nu+1]-a[nu]);
    m = (int)(d[nu][0]);
    p = 0;
    for (j = 1;j<= m;++j)
      p += d[nu][j]*j*M_PI*cos(j*z);
    patch = 0;
    if (nu>0 && r-a[nu]<c[nu])
        patch += 2*b[nu]*(r-(a[nu]+c[nu]));
    if (nu<k-1 && a[nu+1]-r<c[nu+1])
        patch += 2*b[nu+1]*(r-(a[nu+1]-c[nu+1]));
    return f[nu]/(a[nu+1]-a[nu])*(1+p)+patch;
  }
}

// evaluate the piecewise analytic cumulative distribution
double Fit::cd(double r)
{
  if (r < a[0])
    return 0.0;
  else if (r > a[k])
    return 1.0;
  else {
    int nu, m, j;
    double z, zp, dz, patch;
    nu = 0;
    while(a[nu+1]<r)
      ++nu;
    z = (r-a[nu])/(a[nu+1]-a[nu]);
    zp = z*M_PI;
    m = (int)(d[nu][0]);
    dz = 0.0;
    for(j = 1; j <= m; ++j)
      dz += d[nu][j]*sin(j*zp);
    patch = 0.0;
    if(nu>0 && r-a[nu]<c[nu])
        patch += b[nu]*pa_sqr(r-(a[nu]+c[nu]));
    if(nu<k-1 && a[nu+1]-r<c[nu+1])
        patch += b[nu+1]*pa_sqr(r-(a[nu+1]-c[nu+1]));
    return g[nu]+f[nu]*(z+dz)+patch;
  }
}

// write the fit parameters found by pafit to a file. 
void Fit::write(FILE *file)
{
  int mu, j;

  fprintf(file,"# k %d\n",k);
  for (mu = 0; mu < k; mu++) {
    fprintf(file,"# a%d %f",mu,a[mu]);
    if (mu && b[mu]!= 0.0)
      fprintf(file," b%d %f c%d %f",mu,b[mu],mu,c[mu]);
    fprintf(file," f%d %f d%d %d ",mu,f[mu],mu,(int)(d[mu][0]));
    for (j = 1; j <= (int)(d[mu][0]); j++)
      fprintf(file,"%f ",d[mu][j]);
    fprintf(file,"\n");
  }
  fprintf(file,"# a%d %f\n",k,a[k]);
  fprintf(file,"# EOP\n");
}

  
// perform a weighted resampling of data. 
double bias(int n,const double*r,
            double (*w)(double),
            double (*rnd)(void*),
            void*s,
            double*x)
{
  double nrm;
  if (w) {
    int i, k;
    double pmax, *p;
    p = new double[n];
    pmax = 0.0;
    nrm = 0.0;
    for (i = 0; i < n; ++i) {
      p[i] = w(r[i]);
      nrm += p[i];
      if (p[i] > pmax)
        pmax = p[i];
    }
    nrm /= n;
    for (i = 0; i < n; ++i)  
      p[i] /= pmax;
    for (i = 0; i < n; ++i) {
      do {
        k    = (int)(n*rnd(s));
        x[i] = r[k];
      } while (rnd(s) >= p[k]);
    }
    delete[]p;
    return nrm;
  } else {
    boot(n, r, rnd, s, x);  
    return 1.0;
  }
}

// perform a weighted resampling of data for use with a jackknife. 
double jbias(int j, int m, int n, const double *r,
             double (*w)(double), 
             double (*rnd)(void*),
             void *s,
             double*x)
{
  double nrm;
  if (w) {
    int     i, k, nb,jnb;
    double  pmax, *p;
    nb   = n/m;
    jnb  = j*nb;
    p    = new double[n];
    pmax = 0.0;
    nrm  = 0.0;
    for ( i = 0; i < n; ++i) {
      p[i] = w(r[i]);
      nrm += p[i];
      if (p[i] > pmax)
        pmax = p[i];
    }
    nrm /= n;
    for ( i = 0; i < n; ++i)
      p[i] /= pmax;
    for (i = 0; i < n-nb; ++i) {
      do {
        k = (int)((n-nb)*rnd(s));
        if (k > jnb)
          k += nb;
        x[i] = r[k];
      } while (rnd(s) >= p[k]);
    }
    delete[] p;
    return nrm;
  } else {
    jackboot(j,m,n,r,rnd,s,x);
    return 1.0;
  }
}

// perform a resampling of data for use with a bootstrap estimator. 
void boot(int n,const double*r,double (*rnd)(void*),void *s,double*x)
{
  for (int i = 0; i < n; ++i) 
    x[i] = r[(int)(n*rnd(s))];
}

// perform a simple block-jackknife. 
void jack(int j, int m, int n, const double *r, double *x)
{
  int nb = n/m;
  int jnb = j*nb;
  for (int i = 0; i < n-nb; ++i) {
    int k = i;
    if (k > jnb)
      k += nb;
    x[i] = r[k];
  }
}

// perform a resampling of data for use with a block-jackknife. 
void jackboot(int j,int m,int n,const double*r,
              double(*rnd)(void*),void*s,double*x)
{
  int nb = n/m;
  int jnb = j*nb; 
  for (int i = 0; i< n-nb; ++i) {
    int k = (int)((n-nb)*rnd(s));
    if (k > jnb)
      k += nb;
    x[i] = r[k];
  }
}

// read a binary data file of a specific format into an array.
double* newreadbinfile(const char *s, double&y1, double&y2, int&m, int&n)
{
  FILE    *f;
  double  *buf, dummy;
  size_t   d;
  f = fopen(s,"r");
  fseek(f, 0, SEEK_END);
  n  = ftell(f)/sizeof(double)-3;           
  buf = new double[n];
  fseek(f, 0, SEEK_SET); 
  d = fread(&y1, sizeof(double), 1, f);
  d = fread(&y2, sizeof(double), 1, f);
  d = fread(&dummy, sizeof(double), 1, f);
  d = fread(buf, sizeof(double), n, f);
  fclose(f);
  m = dummy;
  return buf;
}

} // end of namespace pa
// end of file