#include "mex.h"
#include "math.h"
#include "myheader.h"


/* NOTE! Here we explicitly use the fact that matlab allocates all of these with the initial value 0 */

#define pow(a,b) (exp((b)*log(a)))

int ih,jh,mh,nh;
TENSOR2 P;
TENSOR2i X,X1,ul,uin,zl,zjn,ii,jj,oi,oi1,jjn,oijjn,oijjn1,dims,dimz,dimjn;

void vhconcat(TENSOR2i *A,TENSOR2i B,TENSOR2i C)
{
  /* matlab's A = [B C] */
  int k;
  setdim2((*A),1,d2(B)+d2(C));
  for(k=0; k<d2(B); k++)
    _((*A),0,k) = _(B,0,k);
  for(k=0; k<d2(C); k++)
    _((*A),0,k+d2(B)) = _(C,0,k);
}


void vextract(TENSOR2i A,TENSOR2i I,TENSOR2i B)
{
  /* matlab's A(I) = B */
  int i;
  for(i=0; i<d2(I); i++)
    _(A,0,_(I,0,i)) = _(B,0,i);
}

void complement(TENSOR2i *J,TENSOR2i I,int n)
{
  /* matlab's J = setdiff(1:n,I) */
  /* assuming I is given in incr order */
  int i=0;
  int j=0;
  int k;
  setdim2((*J),1,n-d2(I));
  for(k=0; k<n; k++) {
    if(_(I,0,i)==k)
      i++;
    else {
      _((*J),0,j) = k;
      j++;
    }
  }
}

double getPX(TENSOR2i Z,TENSOR2i ii)
{
    int mn1,n1,k;
    int zi,ui;
    double p;

    n1 = nh - d2(ii);
    mn1 = (int) (.5+pow(1.0*mh,1.0*n1));
    complement(&jj,ii,nh);
    setdim2(dimz,1,n1);
    setdim2(zl,1,n1);
    setdim2(ul,1,nh);

    printf("mn1= %d, dimz=; dims =\n",mn1);
    for(k=0;k<n1;k++)
    _(dimz,0,k) = mh;
    printTENSOR2i(dimz);
    printTENSOR2i(dims);

    printf("getPX; Z= \n");/**/
    printTENSOR2i(Z);
    printf("getPX; ii= \n");/**/
    printTENSOR2i(ii);
    printf("getPX; jj= \n");/**/
    printTENSOR2i(jj);

    p = 0.0;
    for(zi=0;zi<mn1;zi++) {
      /*printf("zi = %d \n",zi);/**/
      ind2coord(zl,zi,dimz);
      /*printTENSOR2i(z);/**/
      vextract(ul,ii,Z);  /* u(ii) = Z */
      vextract(ul,jj,zl);  /* u(jj) = z */
      /*printTENSOR2i(ul);/**/
      ui = coord2ind(ul,dims);
      /*printf("ui = %d \n",ui);/**/
      p += _(P,ui,0);
      /*printf("zi = %d ui = %d p = %1.6f\n",zi+1,ui+1,_(P,ui,0));/**/
    }
    printf("p = %1.6f \n",p);
    return(p);
}


void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[] )
{
  int k,nz,zi;
  double pmargu,pmargu1,pmargx,pmargx1,h,*hptr;

/* ---------------- I/O bureaucracy -------------- */

  /* h = etaijX(P,m,n,i,j,X)   */

  makeT2ad(P,IN(1));
  mh  = (int) *mxGetPr(IN(2));
  nh  = (int) *mxGetPr(IN(3));
  ih  = (int) *mxGetPr(IN(4)) -1;
  jh  = (int) *mxGetPr(IN(5)) -1;

  allocTENSOR2i(&X,1,nh);
  allocTENSOR2i(&X1,1,nh);
  allocTENSOR2i(&oi,1,nh);
  allocTENSOR2i(&oi1,1,nh);
  allocTENSOR2i(&jjn,1,nh);
  allocTENSOR2i(&oijjn,1,nh);
  allocTENSOR2i(&oijjn1,1,nh);
  allocTENSOR2i(&ii,1,nh);
  allocTENSOR2i(&jj,1,nh);
  allocTENSOR2i(&dims,1,nh);
  allocTENSOR2i(&ul,1,nh);
  allocTENSOR2i(&uin,1,nh);
  allocTENSOR2i(&dimz,1,nh);
  allocTENSOR2i(&dimjn,1,nh);
  allocTENSOR2i(&zjn,1,nh);
  allocTENSOR2i(&zl,1,nh);

  for(k=0;k<nh;k++)
    _(dims,0,k) = mh;

  /* convert X to int */
  copyarr2tensi(IN(6),&X);
  /* adjust X */
  for(k=0;k<d2(X);k++)
    _(X,0,k)--;
  setdim2(X1,1,d2(X)-1);
  for(k=0;k<d2(X1);k++)
      _(X1,0,k) = _(X,0,k);

  setdim2(jjn,1,nh-jh);
  for(k=jh;k<nh;k++)
      _(jjn,0,k-jh)=k;
  setdim2(oi,1,ih+1);
  for(k=0;k<ih;k++)
      _(oi,0,k)=k;
  setdim2(oi1,1,ih);
  for(k=0;k<ih-1;k++)
      _(oi1,0,k)=k;
  vhconcat(&oijjn ,oi ,jjn);
  vhconcat(&oijjn1,oi1,jjn);

/*----------- actual computation ----------------*/

  nz = (int) (.5+pow(1.0*mh,1.0*(nh-jh)));
  printf("nz = %d\n",nz);  

  /*debug */
  /*setdim2(jj,1,2);_(jj,0,0) = 1;  _(jj,0,1) = 2; /**/
  /*setdim2(jj,1,3);_(jj,0,0) = 0;  _(jj,0,1) = 1; _(jj,0,2) = 2; /**/
  
  
  /*setdim2(u,1,3); _(u,0,0)  = 0;  _(u,0,1) = 0;_(u,0,2) = 0;  /**/
  /*setdim2(u,1,2);_(u,0,0) = 0;  _(u,0,1) = 0;  /**/
  /*setdim2(u,1,3); _(u,0,0)  = 0;  _(u,0,1)  = 1; _(u,0,2)  = 0;  /**/
  /*  setdim2(u,1,2);_(u,0,0) = 1;  _(u,0,1) = 0;  /**/
  /*printf("P(X(ii)=u) = %1.9f\n",getPX(u,oijjn1));  */

    pmargx = getPX(X,oi);
    printf("pmargx = %1.9f\n",pmargx);  
    pmargx1 = getPX(X1,oi1);
    printf("pmargx1 = %1.9f\n",pmargx1);  

  h = 0;  
  setdim2(dimjn,1,d2(jjn));
  for(k=0;k<d2(jjn);k++)
      _(dimjn,0,k) = mh;
  setdim2(zjn,1,d2(jjn));

  /*printTENSOR2i(jjn);
    printTENSOR2i(oi);
    printTENSOR2i(oi1);*/
    printTENSOR2i(oijjn);
  printTENSOR2i(oijjn1);

  for(zi=0;zi<nz;zi++) {
    /*printf("zi = %d \n",zi);/**/
    ind2coord(zjn,zi,dimjn);
    /*printTENSOR2i(zjn);
      printTENSOR2i(X1);*/
    vhconcat(&uin,X,zjn);
    /*printTENSOR2i(u);*/
    pmargu = getPX(uin,oijjn);
    /*printf("pmargu = %1.9f \n",pmargu);/**/
    vhconcat(&uin,X1,zjn);
    /*printTENSOR2i(u);/**/
    pmargu1 = getPX(uin,oijjn1);
    /*printf("pmargu1 = %1.9f \n",pmargu1);/**/
    /*printf("[p0 p1] =  %1.5f %1.5f \n",pmargu/pmargx,pmargu1/pmargx1);/**/
    /*printf("[u u1] =  %1.5f %1.5f \n",pmargu,pmargu1);/**/
    h += 0.5 * ABS(pmargu/pmargx - pmargu1/pmargx1);
  }


  OU(1) = mxCreateDoubleMatrix(1,1,mxREAL);
  hptr = mxGetPr(OU(1));
  *hptr = h;

  freeTENSOR(X);
  freeTENSOR(X1);
  freeTENSOR(oi);
  freeTENSOR(oi1);
  freeTENSOR(jjn);
  freeTENSOR(oijjn);
  freeTENSOR(oijjn1);
  freeTENSOR(ii);
  freeTENSOR(jj);
  freeTENSOR(dims);
  freeTENSOR(ul);
  freeTENSOR(uin);
  freeTENSOR(dimz);
  freeTENSOR(dimjn);
  freeTENSOR(zl);
  freeTENSOR(zjn);
}