/*
 
Function: Computing the NPMLE of the distribution function from
          interval censored data, and imputing.

*/

#include <sys/time.h>
#include <sys/resource.h>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>


#define MAX_N_Obs  400

static int comp(double *f1, double *f2)
{
 if (*f1 < *f2) return(-1);
    else if (*f1 > *f2) return(1);
            else return(0);
}

/* L_infinite distance */
double Linf_dist(double *a, double *b, int n)
{
 int i;
 double s1, s=0;

 for(i=1; i<=n; i++) {
    s1 = (s1=a[i]-b[i])>0? s1 : -s1;
    if (s < s1) s=s1;
   }
/*
    s += (a[i]-b[i])*(a[i]-b[i]);
 s = (float)sqrt((double)s);
*/
 return(s);
}

/* 
   Generate a Greatest Convex Minorant 
   Input: q--weights, nh--function value g()
   Output: nh--GCM of g() with weights q.
*/

gcm(q,nh,n)
double *q, *nh;
int n;
{
 int i,j,k,last;
 double  min_slope, slope;

 nh[0]=q[0]=0;
 for(i=1; i<=n; i++){
   nh[i] = nh[i]*q[i] + nh[i-1];
   q[i] += q[i-1];
   }
 i=0;

 while(i<n-1){
    min_slope=(nh[i+1]-nh[i])/(q[i+1]-q[i]);
    last=i+1;
    for(j=i+2; j<=n; j++){
       slope=(nh[j]-nh[i])/(q[j]-q[i]);
       if (slope<min_slope){
          /*
          printf("i=%d j=%d min_slope=%f    slope=%f\n",i,j,min_slope,slope);
          */
          min_slope=slope;
          last=j;
          }
       }

    for(k=i+1; k<last; k++)
       nh[k]=nh[i]+ (double)min_slope*(q[k]-q[i]);

    i=last;
    }

 for(i=n; i>=1; i--){
   q[i] -= q[i-1];
   nh[i] = (nh[i]-nh[i-1])/q[i];
   }
}

#define TB_EPSILON     1.0e-100
#define LM_EPSILON      1.0e-10
#define RIGHTCENSORING     9999
#define LARGENUM       1.0e+10
#define TB_MAX_ITER    500 

#define NdiscreteT 11

void IcmNPMLE(double * LC, double *RC, long *Npt)
{
char OUTfname[100],OUTfname2[100];
int i,j,k,N,n,n0, iter, iter2;
int INDlc[MAX_N_Obs], INDrc[MAX_N_Obs];
    /* indices of the corrsponding LC,RC in ordered y[] */
long perm[MAX_N_Obs];
double Tolerence=0.00001, M ;
double pTolerence=0.01 ;
double *tp1,
      y[MAX_N_Obs],
      wt[MAX_N_Obs],
      dW[MAX_N_Obs], dG[MAX_N_Obs], dpis[MAX_N_Obs];
double lr[2*MAX_N_Obs], d1[2*MAX_N_Obs], d2[2*MAX_N_Obs], 
       pis[2*MAX_N_Obs],npis[3*MAX_N_Obs];
double L0, Li, li, nu, dn, stepsize;
FILE *fp;
struct rusage ru; /* resource utilization, such as time */

N= *Npt;
j=1;
for(i=1; i<=N; i++){
   lr[i]=LC[i];
   lr[i+N]=RC[i];
  }

tp1=&(lr[1]);
qsort(tp1, N,sizeof(double), comp);
tp1=&(lr[1+N]);
qsort(tp1, N,sizeof(double), comp);

n=0; i=1; j=N+1;
y[0]=0;
while (i<=N && j<=2*N ){
  if (lr[i]>=lr[j]) j++;
     else if (i<N && lr[i+1]<lr[j]) i++; /* ==? */
	     else {
		   n=n+1;
                   y[n]=lr[j];
                   i++; j++;
                   /* added on Sun Nov 10 18:49:49 CST 1996 */
                   while (lr[j]== y[n] && j<=2*N) j++;
                  }
  }

for(i=1; i<=N; i++){
 INDlc[i]=INDrc[i]=-1;
 for(j=1; j<=n; j++) {
   /*
   following ``=" in LC[i]<=y[j] added on April 22, 97: [LC, RC)
   */
   if (INDlc[i]==-1 && LC[i]<=y[j] ) INDlc[i]=j-1;
   if (INDrc[i]==-1 && RC[i]<y[j] ) {
       INDrc[i]=j-1;
       break;
       }
   }
   if (INDrc[i]==-1) INDrc[i]=n;
if(INDlc[i]==-1 || INDrc[i]==-1)
   perror("Indices wrong!!!!"); 
 } 
/* initial cumulative hazards */
npis[0]=pis[0]=0;
for(j=0; j<=n; j++) {
  pis[j]=j*3.0/(n);
  wt[j]=j;
  }

L0=0;
for(i=1; i<=N; i++){
  dn = exp(-pis[INDlc[i]]);
  if (RC[i]<RIGHTCENSORING) 
          dn -= exp(-pis[INDrc[i]]);
  li=dn;
  if (li<TB_EPSILON) {
/*
     printf("li from obs %d = %lf\n", i, li);
*/
     li=TB_EPSILON;
     }
  L0 += log(li);
  }

iter=0;
while(iter<TB_MAX_ITER) {
iter=iter+1;

for(j=1; j<=n; j++){
 dW[j]=dG[j]=0;
 for(i=1; i<=N; i++){
   if (RC[i]>=RIGHTCENSORING){
      if (INDlc[i]==j) dW[j] -= 1;
      }
   else {
         if (INDlc[i]==j ) {
             dW[j] -= exp(-pis[j])/(exp(-pis[j])-exp(-pis[INDrc[i]]));
             dG[j] += exp(-pis[j]-pis[INDrc[i]])/((exp(-pis[j])-exp(-pis[INDrc[i]]))*(exp(-pis[j])-exp(-pis[INDrc[i]])));
             }
         if (INDrc[i]==j ) {
             dW[j] += exp(-pis[j])/(exp(-pis[INDlc[i]])-exp(-pis[j]));
             dG[j] += exp(-pis[j]-pis[INDlc[i]])/((exp(-pis[j])-exp(-pis[INDlc[i]]))*(exp(-pis[j])-exp(-pis[INDlc[i]])));
             }
        } 
   }
 }

for(j=1; j<=n; j++) {
  if (dG[j]>LM_EPSILON) dpis[j] = dW[j]/dG[j];
  else {
        dpis[j] =  dW[j]/LM_EPSILON ;
        dG[j]=LM_EPSILON;
        }
  }
	
stepsize=1;
while (stepsize>1e-10){
for(j=1; j<=n; j++) {
  npis[j] = pis[j]+ stepsize*dpis[j];
  }

gcm(dG, npis, n);
for(j=1; j<=n; j++) {
  if (npis[j]<0) npis[j] = 0;
  if (isinf(npis[j]) || isnan(npis[j])) npis[j] = 999;
/*
  if (isinf(npis[j])) npis[j] = 999;
*/
  }

npis[0]=0;

Li=0;
for(i=1; i<=N; i++){
  dn = exp(-npis[INDlc[i]]);
  if (RC[i]<RIGHTCENSORING) 
          dn -= exp(-npis[INDrc[i]]);
  li=dn;
  if (li<TB_EPSILON) li=TB_EPSILON;
  Li += log(li);
  }
/*
printf("Iteration=%d : DeltalogL=%lf Li=%lf  L0=%lf  \n", iter, Li-L0, Li,L0);
*/
if (Li>L0) break;
stepsize /= 2;
}
/*
for(j=1; j<=n; j++) printf(" %f ", npis[j]);
printf(" npis[n]= %lf \n", npis[n]);
*/

if (Li-L0<Tolerence && Linf_dist(pis,npis,n)<pTolerence) break;
   else 
       for(j=1; j<=n; j++)
        pis[j]=npis[j];
L0=Li;
}
if (Li>L0){
    for(j=1; j<=n; j++)
        pis[j]=npis[j];
    L0=Li;
    }

if (iter>=TB_MAX_ITER){
   printf("\n\nWARNING: No convergence after %d iterations!!\n\n",iter);
   for(j=1; j<=n; j++) printf("\n %lf ", pis[j]);
   printf("\n");
   }

/* returning values */
pis[0]=LC[0]=0;
RC[0]=1;
j=1;
k=0;
while (j<=n){
  while(j<n && fabs(pis[j+1]-pis[j])<1.0e-8) j++;
  k++;
  LC[k]=y[j];
  RC[k] = exp(-pis[j]);
  if (RC[k]<1.e-10) RC[k]=0;
  j++;
  }
*Npt = k;
}

void impute(double *St, double *S, long *Npt, double *LC, double *RC,long *Nobs)
{
int i,j,k,n, N, left, right;
int d[MAX_N_Obs];
double Sleft, Sright;
double y[MAX_N_Obs], tmp1;

extern float genunf(float,float);

 n=*Npt;
 N=*Nobs;

 for(i=1; i<=N; i++){
   if (RC[i]>=RIGHTCENSORING){
       y[i]=LC[i];
       d[i]=0;
       }
   else{
          for(j=1; j<=n; j++)
            if (LC[i]<St[j]) break;
          j--;
          if (j<0) j=0;
          left = j;
          for(j=1; j<=n; j++)
            if (RC[i]<St[j]) break;
          if (j>n) j--;
          if (j<0) j=0;
          right = j;
          if (left==right) y[i]=St[left];
          else{
               if (LC[i]<=St[left]) Sleft=S[left];
               else Sleft= S[left]-(S[left]-S[left+1])*(LC[i]-St[left])/
                                                (St[left+1]-St[left]);
               if (RC[i]>=St[n]) Sright=S[n];
               else Sright=S[right-1]-(S[right-1]-S[right])*
                              (RC[i]-St[right-1])/(St[right]-St[right-1]);
                      
               /* should not happen, but for...*/
               if (Sright>=Sleft) Sleft=Sright=(Sright+Sleft)/2; 
               tmp1 = genunf((float)Sright,(float)Sleft);
/*
          if (left>=right)
             y[i] = St[left];
          else{
          tmp1 = (double) rand()/(pow((double)2,(double)15)-1);    
          tmp1 = S[right] + (1-tmp1)*(S[left]-S[right]);
*/
/*
printf("tmp1=%lf w/n (%lf %lf) in (%lf, %lf), obs%d=(%lf %lf)\n", 
tmp1,S[right],S[left],St[left],St[right],i,LC[i],RC[i]);
*/
          for(j=left+1; j<=right; j++)
            if (tmp1 >= S[j]) break;
          if (j>right) {
             j=right;
             tmp1=S[right];
             }
          if (fabs(S[j-1]-S[j])<1.0e-6) y[i] = (St[j]+St[j-1])/2;
          else
/*
              y[i] = St[j]-(tmp1-S[j])*(St[j]-St[j-1])/
                                (S[j-1]-S[j]);
*/
              y[i] = St[j-1]+(S[j-1]-tmp1)/(S[j-1]-S[j])*(St[j]-St[j-1]);
          if (y[i]<St[j-1]) y[i]=St[j-1];
          if (y[i]>St[j]) y[i]=St[j];
/*
              if (y[i]<0){
                 printf("y[%d]=%lf j=%d St[j-1]=%lf St[j]=%lf
                         S[j]=%lf S[j-1]=%lf dS=%lf tmp1=%lf\n",i,y[i],j,
                         St[j-1],St[j],S[j],S[j-1],fabs(S[j-1]-S[j]),tmp1);
                 exit(-1);
                 }
*/
              }
          d[i]=1;
       }
   }
  /* retruning: */
  for(i=1; i<=N; i++){
    LC[i]=y[i];
    RC[i]=d[i];
    }
}