/*
   
 noisy non-dominated ranking
 Matlab Mex-file.  Compiled on Sun Ultra 10, Matlab 5.3

 [prob(select), rank, sharecnt, q] = NoisyNondom(obj, sigmaobj, 
                                     sigmashare, objlimits, constraints, 
				     priority, selective_pressure)
   
 input:

 obj         - p x k : objective values to minimise
 sigmaobj    - 1 x k : standard dev. of objective noise
 sigmashare  - 1 x k : share distance for objectives
 objlimits   - 1 x k : objective constraint vector
 constraints - p x 1 : constraint vector - noise processing done externally
 priority    - 1 x k : [1 1 .. 1] gives all same pri.
 sel_press   - 1 x k : [1 1 .. 1] for maximum selective pressure 

 p is population size, k is number of objectives.

 output:

 prob(select) will sum to unity, best has largest probability.
        
 rank is 'rank fitness', smallest the best, minimising all objectives
 for maximisation, use negative std dev in sigmaobj.

 sharecnt is the share count for each individual.
 
 q is the final constraint value, after objective constraints,
    parameter constraints and sharing.

 time is O(n^2)   for tanh operations

 E.J.Hughes 28 July 2000

 Method detailed in report

 @TechReport{rep:mopsea,
  author =       {Evan J. Hughes},
  title =        {Multi-Objective Probabilistic Selection
   Evolutionary Algorithm},
  institution =  {Dept. Aerospace, Power, \& Sensors, Cranfield University},
  year =         2000,
  month =        sep,
  number =       {DAPS/EJH/56/2000},
  address =      {RMCS, Shrivenham, UK, SN6 8LA}
  }


*/

#include "mex.h"
#include <math.h>
#include <stdlib.h>

#ifndef PI
#define PI 3.141592653589793
#endif

#define SQ2 0.7071067811865475

#define obj(i,j) obji[(i)+(popsize*(j))]
#define t(i,j)    tmp[(i)+(popsize*(j))]

void NoisyNondom(double *err, double *nerr, double *qerr, double *errx, 
		 double *obji, double *st, double *shr, double *q,
                 double *c, double *pr, double *sp, int popsize, int nobj)
{
  double *tmp, *tmpn, *tmpq, *pq, *stmp, *prtmp, *prtmpb, *sptmp, *sptmpb;
  double tt,st1,st2,tn,sumq,numdiv,tx, shfact, prtot;
  int i,j,k;


  tmp = (double *)mxCalloc(popsize*nobj,sizeof(double));
  tmpn = (double *)mxCalloc(popsize,sizeof(double));
  pq = (double *)mxCalloc(popsize,sizeof(double));
  tmpq = (double *)mxCalloc(nobj,sizeof(double));
  stmp = (double *)mxCalloc(nobj,sizeof(double));
  prtmp = (double *)mxCalloc(nobj,sizeof(double));
  prtmpb = (double *)mxCalloc(nobj,sizeof(double));
  sptmp = (double *)mxCalloc(nobj,sizeof(double));
  sptmpb = (double *)mxCalloc(nobj,sizeof(double));

  /* precalc sharing factors */

  shfact=0;
  for(k=0;k<nobj;++k)
    { 
      st1=shr[k]/(st[k]+1.0e-10)/sqrt(2.0);/*see 11/9/00 s=sigs/sign/sqrt(2) */
      shfact+=log(st1)-log(st1*st1+1)/2.0;/* log[s/sqrt(s^2+1)] */
      stmp[k]=1.0/4.0/(st1*st1+1);  /* Gaussian sharing */
    }

  /* precalc effects of sigma 
       - beware sigma == 0, not trapped properly*/

  /* process fitness values */ 
  for (i=0;i<popsize;++i)
    for (k=0;k<nobj;++k)
      t(i,k)=obj(i,k)/(st[k]+1.0e-10);

  if(q)
  /* process restriction point values */ 
    for (k=0;k<nobj;++k)
      tmpq[k]=q[k]/(st[k]+1.0e-10)/1.6;

  /* zero elements */

  for (i=0;i<popsize;++i)
    tmpn[i]=pq[i]=err[i]=0.0;   
  
  if(q)
  /* calc probability of dominating the restriction point Q */
    for (i=0;i<popsize;++i)
      {
	st1=1.0;
	for (k=0;k<nobj;++k)
	  st1*=(1.0+tanh(tmpq[k]-t(i,k)/1.6))/2.0;  /* probability */
	pq[i]=st1;
      }
  else
    for (i=0;i<popsize;++i)
      pq[i]=1.0;   

  if(c)
    for (i=0;i<popsize;++i)   /* apply constraints */
      pq[i]*=c[i];   

  if(pr)
    for (i=0;i<nobj;++i)   /* set priority */
      prtmp[i]=pr[i];   
  else
    for (i=0;i<nobj;++i)   /* set default priority */
      prtmp[i]=1.0;

  prtot=1.0;
  for (i=0;i<nobj;++i)   /* pre-calc for priority */
    {
      prtmpb[i]=1.0-prtmp[i];
      prtot*=prtmpb[i];
    }

  prtot = 1.0-prtot;

  if(sp)
    for (i=0;i<nobj;++i)   /* set selective pressure */
      sptmp[i]=sp[i];   
  else
    for (i=0;i<nobj;++i)   /* set default SP */
      sptmp[i]=1.0;

  for (i=0;i<nobj;++i)   /* pre-calc for SP */
      sptmpb[i]=(1.0-sptmp[i])/2.0;
  

  /* Calculate share count */
  for (i=0;i<(popsize-1);++i)
    for (j=i+1;j<popsize;++j)
      {
	tn=shfact;
	for (k=0;k<nobj;++k)
	  {
	    tx=t(i,k)-t(j,k);   /* calculated k */
	    tn-=tx*tx*stmp[k];  /* sharing */
	  }
	tn=exp(tn/(double)nobj);
	tmpn[i]+=tn;  /* share info */
	tmpn[j]+=tn;
      }

    for (i=0;i<popsize;++i)
     pq[i]/=(tmpn[i]+1);    /* add 1 to account for self, and apply sharing */


  /* calc probability table */
  /* sum probability so if on Pareto, will calc how many we dominate */ 
  /* do not count self comparison */

  for (i=0;i<(popsize-1);++i)
    for (j=i+1;j<popsize;++j)
      {
	st1=prtot; /* for priority */
	st2=prtot; /* for priority */
	for (k=0;k<nobj;++k)
	  {
	    tt=(1.0+tanh((t(i,k)-t(j,k))/1.6))/2.0;  /* probability A_i>B_i*/
	    /* 	    st1*=tt;
		    st2*=1.0-tt; */
	    /* do priority & SP */
	    st1*=(tt*sptmp[k]+sptmpb[k])*prtmp[k]+prtmpb[k];   
	    st2*=((1.0-tt)*sptmp[k]+sptmpb[k])*prtmp[k]+prtmpb[k]; 
	  }
	st1=st1*pq[i]*pq[j]+pq[j]*(1-pq[i]);  /* apply constraints */
	st2=st2*pq[i]*pq[j]+(1-pq[j])*pq[i];
	tx=(1-st1-st2)/2.0;      /* non-domination */
	err[i]+=st1+tx;
	err[j]+=st2+tx;
      }

    for (i=0;i<popsize;++i)
     ++tmpn[i];           /* add 1 to account for self */


  if(errx)                   /* save raw info */
    for (i=0;i<popsize;++i)
      errx[i]=err[i];       

  if(nerr)                   /* save raw info */
    for (i=0;i<popsize;++i)
      nerr[i]=tmpn[i];       

  if(qerr)                   /* save raw info */
    for (i=0;i<popsize;++i)
      qerr[i]=pq[i];       

/*     correct to make probabilities that sum to unity  */

  tt=(double)(popsize-1);
  sumq=2.0/(double)(popsize*(popsize-1)); 
  for (i=0;i<popsize;++i) 
    err[i]=(tt-err[i])*sumq;  /*normalise into prob of selection*/ 


  mxFree(sptmpb);
  mxFree(sptmp);
  mxFree(prtmpb);
  mxFree(prtmp);
  mxFree(stmp);
  mxFree(tmpq);
  mxFree(pq);
  mxFree(tmpn);
  mxFree(tmp);

}


/* gateway function */


void mexFunction(int nlhs,mxArray *plhs[], int nrhs, const mxArray *prhs[])

{   
  double *errx, *nerr, *qerr, *c, *q, *pr, *sp;
  double freq,ap;
  int m,n,popsize,nobj;

  if (nrhs > 7 || nrhs <3) {
    mexErrMsgTxt("NoisyNondom requires three to seven input arguments.");
  } 
  if (nlhs >4) {
    mexErrMsgTxt("NoisyNondom requires one to four output arguments.");
  } 

  m = mxGetM(prhs[0]);
  n = mxGetN(prhs[0]);
  
  if ((m<2)||(n <1))
    mexErrMsgTxt("NoisyNondom requires that obj be a pop x nobj matrix");
  
  popsize = m;
  nobj=n;
        
  m = mxGetM(prhs[1]);
  n = mxGetN(prhs[1]);
  
  if ((m!=1)||(n !=nobj))
    mexErrMsgTxt("NoisyNondom requires that sigma be 1 x nobj");

  m = mxGetM(prhs[2]);
  n = mxGetN(prhs[2]);
  
  if ((m!=1)||(n !=nobj))
    mexErrMsgTxt("NoisyNondom requires that sigma share be 1 x nobj");

  if(nrhs>=4)
    {
      m = mxGetM(prhs[3]);
      n = mxGetN(prhs[3]);
  
      if ((m!=1)||(n !=nobj))
	mexErrMsgTxt("NoisyNondom requires that Q be 1 x nobj");

      q=mxGetPr(prhs[3]);
    }
  else
    q=NULL;

  if(nrhs>=5)
    {
      m = mxGetM(prhs[4]);
      n = mxGetN(prhs[4]);
  
      if ((m!=popsize)||(n !=1))
	mexErrMsgTxt("NoisyNondom requires that C be npop x 1");

      c=mxGetPr(prhs[4]);
    }
  else
    c=NULL;

  if(nrhs>=6)
    {
      m = mxGetM(prhs[5]);
      n = mxGetN(prhs[5]);
  
      if ((m!=1)||(n !=nobj))
	mexErrMsgTxt("NoisyNondom requires that priority be 1 x nobj");

      pr=mxGetPr(prhs[5]);
    }
  else
    pr=NULL;

  if(nrhs>=7)
    {
      m = mxGetM(prhs[6]);
      n = mxGetN(prhs[6]);
  
      if ((m!=1)||(n !=nobj))
	mexErrMsgTxt("NoisyNondom requires that SP be 1 x nobj");

      sp=mxGetPr(prhs[6]);
    }
  else
    sp=NULL;

  plhs[0] = mxCreateDoubleMatrix(popsize,1,mxREAL);

  errx=NULL;
  nerr=NULL;
  qerr=NULL;
  if(nlhs>=2)
    {
      plhs[1] = mxCreateDoubleMatrix(popsize,1,mxREAL);
      errx=mxGetPr(plhs[1]);
    }

  if(nlhs>=3)
    {
      plhs[2] = mxCreateDoubleMatrix(popsize,1,mxREAL);
      nerr=mxGetPr(plhs[2]);
    }

  if(nlhs==4)
    {
      plhs[3] = mxCreateDoubleMatrix(popsize,1,mxREAL);
      qerr=mxGetPr(plhs[3]);
    }

  NoisyNondom(mxGetPr(plhs[0]), nerr, qerr, errx, mxGetPr(prhs[0]),
	      mxGetPr(prhs[1]), mxGetPr(prhs[2]), q, c, pr, sp, popsize, nobj);

} /* mexFunction */