/*
//
// Ultra-fast version of the lazy learning algorithm.
//
//
// Local models                   constant
// Kernel functions               rectangular
// Identification                 recursive mean and variance
// Metric                         L1
// Model selection                Minimum of leave-one-out error
//
// -----------------------------------------------------------------------
//
//            Mauro Birattari       &        Gianluca Bontempi
//
//                               IRIDIA 
//                    Universite' Libre de Bruxelles
//                    email: {mbiro,gbonte}@ulb.ac.be
//
// -----------------------------------------------------------------------
//   
//   
//   How to compile:
//   If a C compiler is correctly installed, and if matlab knows its path,
//   you can compile me from the matlab prompt, as follows:
//   
//   	     >> mex conM.c
//   
//   How to use:
//   From matlab you can call the resulting mex file using the following syntax.
//   
//   	     >> [h,s,t,k,H,S,T,I] = conM(X,Y,Q,id_par);
//   
//   where 
//      INPUT:
//   	     X[m,d]              Examples: Input
//   	     Y[m,1]              Examples: Ouput
//   	     Q[q,d]              Query points
//   	     id_par[2,1]         Identification parameters = [idm,idM];
//
//      OUTPUT:
//           h[q,1]              Prediction with the selected number of 
//                               neighbors
//           s[q,1]              Estimated variance of the prediction.
// --------- t[0,0] ------------ DUMMY VARIABLE: for compatibility pourposes
//	     k[q,1]              Selected number of neighbors
//	     H[idM,q]            All the predictions obtained using a 
//                               number of neighbors in the range 1:idM
//	     S[idM,q]            Estimated variance of ALL the predictions
// --------- T[0,0,0] ---------- DUMMY VARIABLE: for compatibility pourposes
//           I[idM,q]            Index of idM-nearest-neighbors of each
//                               query point
//
//   
//   The identification parameters are [idm,idM]: the minimum and maximum
//   number of examples used in the identification.
//   
//   UNDOCUMENTED FEATURE: the function accept also a 5th input, a vector
//   W[d,1] of weights that can be used to rescale the dimensions.
//
//______________________________________________Mao: Feb 5, 1999___ 
*/

#include <mex.h>
#include<math.h>
#include <string.h>


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

  // size of input matrices
  int mx, nx, my, ny;
  int mq, nq, mw, nw;

  // identification parameters
  double* id;
  int mid, nid;
  int idm, idM;

  // inititial distance 
  // to  be set to infinite
  double initDist;

  // input matrices
  double* Xvec;
  double* Qvec;
  double* Wvec;
  double*  Y;
  double** X;
  double** Q;

  // output matrices
  double* y_hat;
  double* s_hat;
  double* k_hat;
  double* Y_hat;
  double* S_hat;
  double* I_hat;
  int dimSize[3];
  int* BestIndx;
  double* BestDist;

  double dist;
  double H, MSE;
  double  hB, eB;
  int kB;

  int i, j, k, q, p;


  // initialization
  initDist = mxGetInf();
  Wvec = NULL;

  // distance weights
  switch (nrhs){
    case 5:
      mw = mxGetM(prhs[4]);  
      nw = mxGetN(prhs[4]);
      Wvec = mxGetPr(prhs[4]);
    case 4:
      break;
    default:
      mexErrMsgTxt("Function needs 4 input arguments.");
  }

  // Examples: input
  mx = mxGetM(prhs[0]);                //number of examples
  nx = mxGetN(prhs[0]);             

  // Examples: output
  my = mxGetM(prhs[1]);                //number of examples
  ny = mxGetN(prhs[1]);

  // Queries
  mq = mxGetM(prhs[2]);                //number of queries
  nq = mxGetN(prhs[2]);

  // Range identification examples
  mid = mxGetM(prhs[3]);
  nid = mxGetN(prhs[3]);

  
  // consistency check
  if (  (ny != 1)   ||
	(mid*nid !=2)  ||
	(mx != my)  ||
	(nq != nx)    )
    mexErrMsgTxt("Matrix dimensions must agree.");

  if ( (nrhs==5) && (mw*nw!=nx) )
    mexErrMsgTxt("Vector of weights no good.");

  Xvec = mxGetPr(prhs[0]);
  Y = mxGetPr(prhs[1]);
  Qvec = mxGetPr(prhs[2]);

  id = mxGetPr(prhs[3]);
  idm = (int)id[0];
  idM = (int)id[1];
  idm = (idm<2)? 2 : idm;
  idM = (idM>mx)? mx : idM;
  idM = (idM<idm)? idm : idM;

  // allocate output
  plhs[0] = mxCreateDoubleMatrix(mq,1,mxREAL); 
  y_hat = mxGetPr(plhs[0]); 

  plhs[1] = mxCreateDoubleMatrix(mq,1,mxREAL); 
  s_hat = mxGetPr(plhs[1]); 

  plhs[2] = mxCreateDoubleMatrix(0,0,mxREAL); 

  plhs[3] = mxCreateDoubleMatrix(mq,1,mxREAL); 
  k_hat = mxGetPr(plhs[3]); 
  
  if (nlhs > 4){
    plhs[4] = mxCreateDoubleMatrix(idM,mq,mxREAL); 
    Y_hat = mxGetPr(plhs[4]); 
  }else
    Y_hat = 0;
  

  if (nlhs > 5){
    plhs[5] = mxCreateDoubleMatrix(idM,mq,mxREAL); 
    S_hat = mxGetPr(plhs[5]); 
  }else
    S_hat = 0;
  

  if (nlhs > 6){
    dimSize[0] = 0;
    dimSize[1] = 0;
    dimSize[2] = 0;
    plhs[6] = mxCreateNumericArray(3,dimSize,mxDOUBLE_CLASS,mxREAL);

  }

  if (nlhs > 7){
    plhs[7] = mxCreateDoubleMatrix(idM,mq,mxREAL); 
    I_hat = mxGetPr(plhs[7]); 
  }else
    I_hat = 0;
  



  // Create Fortran-style matrices from Matlab vectors
  X = mxCalloc(nx,sizeof(double*));
  Q = mxCalloc(nq,sizeof(double*));

  for (i=0; i<nx; i++,Xvec+=mx,Qvec+=mq){
    X[i] = Xvec;
    Q[i] = Qvec;
  }


  // allocate search vectors
  BestIndx = mxCalloc(idM+1,sizeof(int));
  BestDist = mxCalloc(idM+2,sizeof(double));
  *BestDist = 0;


  for( q=0; q<mq; q++){

    for (p=1; p<=idM; p++)
      BestDist[p] = initDist;

    if (Wvec){
      for (i=0; i<mx; i++){
	dist = 0.0;
	for (j=0; j<nx && dist < BestDist[idM] ; j++)
	  dist += Wvec[j]*fabs(X[j][i]-Q[j][q]);
	for(p=idM; dist < BestDist[p] ; p--){
	  BestDist[p+1] = BestDist[p];
	  BestIndx[p] = BestIndx[p-1];
	}
	BestDist[p+1] = dist;
	BestIndx[p] = i;
      }
    }else {
      for (i=0; i<mx; i++){
	dist = 0.0;
	for (j=0; j<nx && dist < BestDist[idM] ; j++)
	  dist += fabs(X[j][i]-Q[j][q]);
	for(p=idM; dist < BestDist[p] ; p--){
	  BestDist[p+1] = BestDist[p];
	  BestIndx[p] = BestIndx[p-1];
	}
	BestDist[p+1] = dist;
	BestIndx[p] = i;
      }

    }

    
    if (I_hat)
      for(i=0;i<idM;i++)
	*(I_hat++) = (double)BestIndx[i];


    H = Y[BestIndx[0]];
    if (Y_hat)
      *(Y_hat++) = H;

    eB = mxGetInf();
    if (S_hat)
      *(S_hat++) = eB;

    // MSE doesn't need to be inatialized
    // in the first iteration of the for
    // loop the first addendum is zero:
    // pow(k-1,2) = 0 since k=1

    for (k=1; k<idM; k++){
      MSE = MSE*(k+1)*pow(k-1,2)/pow(k,3) + pow(Y[BestIndx[k]]-H,2)/k;
      H = (k*H + Y[BestIndx[k]])/(k+1);

      if (Y_hat)
	*(Y_hat++) = H;

      if (S_hat)
	*(S_hat++) = MSE;

      if ( (k>=idm-1) && (MSE<eB) ) {
	eB = MSE;
	hB = H;
	kB = k+1;
      }
    }
    *(y_hat++) = hB;
    *(k_hat++) = (double)kB;
    *(s_hat++) = eB;
  }
  /*
  mxFree(BestIndx);
  mxFree(BestDist);
  mxFree(X);
  mxFree(Q);
  */
}
