/*
# proc: treepnnhypscons - PNN classifier with tree indexed prototypes. Returns
# proc: 	          hypotheses and confidences.
# proc: pnn_hypscons - returns the value the maximum normalized PNN activation
# proc:                and its position.
# proc: pnn_normedacts - returns the normalized PNN activations. ie. the values
# proc:                  divided by their sum so that they add to 1.0;
# proc: pnn_fastacts - returns the unweighted unnormalized PNN activations. ie.
# proc:                the raw exp(-d squared / 2 sigma squared) values
*/

#include <stdio.h>
#include <malloc.h>
#include <math.h>
#include <values.h>
#include <defs.h>
#include <pnn.h>



/* translated from the fortran web. That code was originally the same tree search 	*/
/* algorithm but applied to KNN code. It was written by Jim Blue. The classification 	*/
/* part of the web code was converted to PNN style classification by Patrick Grother 	*/
/* who then ported Fortran Web to C, with this result 					*/
/* Some residual Fortranesque aesthetics remain; elsewhere the Fortran origin is clear 	*/
/* the major problem being the [0,N-1] versus [1,N] nature of array indices in the two 	*/
/* languages; the tree's effective index indirection making this a tad more tricky 	*/

#define ICHVAL -1 	/* this should likely be zero if the tree was built with C 	*/


/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/
treepnnhypsconsC(vK, idpatK, npatsK, vU, npatsU, ninp, nout,
         vctr, maxctrs, nctrs, Child, Idiscr, Npts, Ipts,
         Vmin, Vmax, maxnode, nchild, maxchild,
         hyps, cons, sigma, cutoff, ncutoffs)
float *vK; 		/* known class prototypes: really dimensioned as (ninp,npatsK); 	*/
float *vU; 		/* unknown vectors: really dimensioned as (ninp,npatsU); 		*/
float *cons; 		/* confidences: one for each unknown 					*/
int   *hyps; 		/* hypptheses: one for each unknown 					*/
int   *idpatK; 		/* known classes of the prototypes (npatsK); 				*/
float *cutoff; 		/* breadth of search params 						*/
float *vctr; 		/* cluster centers really dimensioned as (ninp,maxctrs); 		*/
int   *Child; 		/* (maxnode,maxchild); node indexes of children of node 		*/
int   *Idiscr;   /* which dim this node is split on 				*/
int   *Npts;     /* how many data points in this subtree dim = maxnode 		*/
int   *Ipts;     /* index of first data point in this subtree dim = maxnode 	*/
float *Vmin;     /* minimum values for split data at this node dim = maxnode 	*/
float *Vmax;     /* maximum values for split data at this node dim = maxnode 	*/
float sigma; 	 /* width of radial gaussian kernel 				*/
int   npatsK, npatsU, ninp, nout, maxnode, maxchild, nchild, ncutoffs;
{
float *vx;
int   stackend; 	/* index of last node in the stac 			*/

int   node[500]; 	/* node stack for search 				*/
int     *idKU;		/* pattern id of nearest neighbor 			*/
float *distKU; 		/* distances from query to nearest neighbor 		*/
float *contrib;
float deltav; 		/* half-range of allowed dist from query point. 	*/

float dthis, dbest, twoss, fexpy;
float *xptr, *uptr, *vptr, *kptr;
int   lastk, c, n, i, k, oorange, *cptr;
int   id, ichild, jp, nc, ncur, np, kdiscr, kc, kbest;
float vd, sum, slopy, d, dnp, vU1, vU2;
float closev, maxfar, farv;
int   *useit;

   if ((vx      = (float *)calloc(ninp, sizeof(float))) == NULL)
      syserr("treepnnhypscons", "calloc", "space for ninp vx");
   if ((contrib = (float *)calloc(nout, sizeof(float))) == NULL)
      syserr("treepnnhypscons", "calloc", "space for nout contrib");
   if ((distKU  = (float *)calloc(npatsK, sizeof(float))) == NULL)
      syserr("treepnnhypscons", "calloc", "space for npatsK distKU");
   if ((idKU    = (int   *)calloc(npatsK, sizeof(int  ))) == NULL)
      syserr("treepnnhypscons", "calloc", "space for npatsK idKU");
   if ((useit   = (int   *)calloc(npatsK, sizeof(int  ))) == NULL)
      syserr("treepnnhypscons", "calloc", "space for npatsK useit");

   twoss = 2.0 * sigma * sigma;
   slopy = LAMBDA * LN10 * twoss;
   fexpy = -1.0 / twoss;


   lastk = npatsK;
   for ( jp = 0, uptr = vU ; jp < npatsU ; jp++ )
   {
      /* make a local copy of this unknown feature vector 		*/
      for ( n = 0, xptr = vx ; n < ninp ; n++ )
         *xptr++ = *uptr++;

      /* compare it to all the class cluster vectors 			*/
      for ( c = 0, dbest = BIG, vptr = vctr ; c < nctrs ; c++ )
      {
         for ( n = 0, dthis = 0, xptr = vx ; n < ninp ; n++ )
            d     = fabs(*vptr++ - *xptr++),
            dthis = max(d, dthis);

         if (dthis < dbest)
            dbest = dthis;
      }

      /* then do progressively widening search, if necessary 		*/
      for ( nc = 0, id = -1 ; id < 0  &&  nc < ncutoffs ; nc++ )
      {
         /* ------ begin @<Find near neighbors@>; ---------------------	*/
         /* ------ begin @<Initialize search@>; -----------------------	*/

         /* set the distances to BIG. First time set all of them so 	*/
         /* thereafter set only the number used for the last unknown 	*/
         /* this saves some time for large prototype sets 		*/
         for ( k = 0 ; k < lastk ; k++ )
            useit[k] = 0;

         for ( k = 0 ; k < nout  ; k++ )
            contrib[k] = 0.0;

         deltav = cutoff[nc] * dbest;
         closev = BIG;
         maxfar = BIG;
         farv   = 0.0;
         lastk    = 0;
         node[1]  = 0;   /* in the web this was  1; */

         /* ------ end @<Initialize search@>; -------------------------	*/

         for (stackend = 1 ; stackend ; stackend-- )
         {
            /* ------ begin @<Process last node on list@>; ------------	*/

            ncur = node[stackend];
            if ((ichild = Child[ncur]) == ICHVAL)      /* terminal node */
            {
               /* At a terminal node. Check all points in this node. If	*/
               /* any is nearer than the worst one stored in the       	*/
               /* tentative list of nearest neighs, insert that point  	*/
               /* into the list. (The list is ordered, with smallest   	*/
               /* distance in position 1, the largest in position lastk	*/
               /* where lastk is a random variable.) 			*/

               /* ----- begin @<Handle terminal node@>; ------------------ */
               for ( np = Ipts[ncur], kptr = vK + np * ninp ;
                     np < Ipts[ncur] + Npts[ncur] ; np++, kptr += ninp )
               {
                  /* Distance measurement of unknown point from nth known point. 		*/
                  /* macro calculates dnp from vx to vK(ncur); 				*/
                  /* L2  : calculate dist**2; later sqrt if nec. dnp += diff**2; 		*/
                  /* L1  : calculate sum of absolute values dnp += abs(diff); 		*/
                  /* Linf: calculate max of absolute values dnp = max(dnp, abs(diff)); 	*/
                  for ( dnp = 0, xptr = vx, vptr = kptr, n = 0 ; n < ninp ; n++ )
                  {
                     d    = *xptr++ - *vptr++;
                     dnp += d * d;

                     /* Don't care how big distance is if it's further 			*/
                     /* than the perimeter defined by the smallest distance + slopy		*/
                     if (dnp > maxfar)
                        break;
                  }


                  if (dnp < maxfar)
		  { 				/* use this prototype: keep index and distance 	*/
 						/* and a flag indicating it is close enough     */
                     distKU[lastk] = dnp;
                       idKU[lastk] =  np;
                      useit[lastk] =   1;

                     if (dnp < closev)
                     {
                        closev = dnp;
                        maxfar = closev + slopy;
                        for ( n = 0 ; n < lastk ; n++ )
                           if (useit[n])
                              if (distKU[n] > maxfar)
                                 useit[n] = 0;
                     }
                     lastk++;

                     /* the latest distance is elligible. If it is greater than the 	*/
                     /* the largest eligible distance so far then keep this value 	*/
                     /* useit is true for this prototype, because even if maxfar was	*/
		     /* just redefined it was this prototype that caused it, i.e. it	*/
		     /* is the closest so far. 						*/

                     /* redefine the widths according to the distance of the 		*/
                     /* furthest eligible distance, farv. It works - is it wise? 	*/
                     if (dnp > farv)
                        if ((farv = dnp) < deltav)
                           deltav = farv;
                  }
               }
               /* ----- end   @<Handle terminal node@>; ------------------ */
            }
            else
            {
               ichild = Child[ncur];
               kdiscr = Idiscr[ichild];
               vU1    = vx[kdiscr] - deltav;
               vU2    = vx[kdiscr] + deltav;
               for ( kc = 0, cptr = Child + ncur, kbest = -1 ; kc < nchild ; kc++, cptr += maxnode )
               {
                  if ((ichild = *cptr) != ICHVAL)
                  {
                     if (vx[kdiscr] >= Vmin[ichild] && vx[kdiscr] <= Vmax[ichild] )
                     {
                        kbest = kc;
                        break;
                     }
                  }
                  else
                     break;
               }
               for ( kc = 0, cptr = Child + ncur ; kc < nchild ; kc++, cptr += maxnode )
               {
                  if (kc != kbest)
                  {
                     if ((ichild = *cptr) != ICHVAL)  /* range overlaps with child */
                     {
                        if (vU1 < Vmax[ichild] && vU2 > Vmin[ichild])
                           node[stackend++] = ichild;
                     }
                     else
                        break;
                  }
               }
               if (kbest != -1)
                  node[stackend++] = Child[kbest*maxnode + ncur];
            }
            /* ------ end   @<Process last node on list@>; --------------- */
         }
         /* ------ end   @<Find near neighbors@>; ---------------------- */


         /* ------ begin @<Classify unknown pattern@>; ------------------------	*/
         /* From the |lastk| nearest neighbors that have been found, guess the class 	*/
         /* of the unknown pattern: Use the standard PNN rad symmetric gaussian kernel 	*/

         for ( k = lastk ; k ; k-- )
            if (useit[k])  /* if any protos are good enough then classify */
               break;

         if (k)
         {   
            for ( k = 0 ; k < lastk ; k++ )
               if (useit[k])
                  d = (float)exp((double)(fexpy * distKU[k])),
                  contrib[idpatK[idKU[k]]] += d;
             
            for ( k = 0, sum = 0.0 ; k < nout ; k++ )
               sum += contrib[k];
             
            for ( k = 0 ; k < nout ; k++ )
               contrib[k] /= sum;
             
            maxpv(contrib, nout, &vd, &id);
         }
         /* ------ end   @<Classify unknown pattern@>; ------------------------	*/
      }

      /* if after using all the cutoffs there is still no hypothesis */
      /* then use the all prototype original PNN for classification  */ 
      if (id == -1)
      {
         int   *safe_h; float *safe_c;
         pnn_hypscons(vK, npatsK, vx, 1, idpatK, ninp, nout, sigma, &safe_h, &safe_c);
         id = safe_h[0]; free(safe_h);
         vd = safe_c[0]; free(safe_c);
      }

      hyps[jp] = id;
      cons[jp] = vd;
   }
   free(vx); free(contrib);
   free(idKU); free(distKU); free(useit);
}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/
pnn_hypscons(known, nknown, found, nfound, class_known,
                  nInps, nClass, sigma,
             hyps, cons)
float	*known, *found, sigma, **cons;
int 	nknown, nfound, *class_known, nInps, nClass, **hyps;
{
float *acts, *atr, *ctr, *pnn_normedacts();
int i, *htr;

   if ((*cons = (float *)malloc(nfound * sizeof(float))) == NULL)
      syserr("pnn_hypscons", "malloc", "space for cons");
   if ((*hyps = (int   *)malloc(nfound * sizeof(int  ))) == NULL)
      syserr("pnn_hypscons", "malloc", "space for hyps");

   acts = pnn_normedacts(known, nknown, found, nfound, class_known,
                   nInps, nClass, sigma);

   for ( i = 0, atr = acts, htr = *hyps, ctr = *cons ; i < nfound ; i++ )
      maxpv(atr, nClass, ctr++, htr++), atr += nClass;

   free(acts);
}


/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/
float *pnn_normedacts(known, nknown, found, nfound, class_known,
                      nInps, nClass, sigma)
float	*known, *found, sigma;
int 	nknown, nfound, *class_known, nInps, nClass;
{
float *acts, *atr, *btr, sum, *pnn_fastacts();
int i, j;

   acts = pnn_fastacts(known, nknown, found, nfound, class_known,
                   nInps, nClass, sigma);

   for ( i = 0, atr = acts ; i < nfound ; i++, atr += nClass )
   {
      for ( j = 0, btr = atr, sum = 0.0 ; j < nClass ; j++ )
         sum += *btr++;
      for ( j = 0, btr = atr            ; j < nClass ; j++ )
         *btr++ /= sum;
   }

   return acts ;
}

/*--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------*/
float *pnn_fastacts(known, nknown, found, nfound, class_known,
              nInps, nClass, sigma)
float	*known, *found, sigma;
int 	nknown, nfound, *class_known, nInps, nClass;
{
float twoss, t, f, closev, *dists, *acts, *atr, *ftr;
int   i, j, k, end, *useit, *index;
float dis, dif, maxfar, slop, *fptr, *kptr;

  if (sigma == 0.0)
     fatalerr("pnn_acts", "zero sigma value", "divide by zero follows");
  twoss = 2.0 * sigma * sigma;
  f     = -1.0 / twoss;
  slop  = twoss * LAMBDA * LN10;


  if ((dists = (float *)calloc(nknown,  sizeof(float))) == NULL)
     syserr("pnn_fastacts", "calloc", "space for dists to unknown");
  if ((useit = (int   *)calloc(nknown,  sizeof(int  ))) == NULL)
     syserr("pnn_fastacts", "calloc", "space for use it flags");
  if ((index = (int   *)malloc(nknown * sizeof(int  ))) == NULL)
     syserr("pnn_fastacts", "malloc", "space for voting protos' indices");
  if ((acts  = (float *)malloc(nClass * nfound * sizeof(float))) == NULL)
     syserr("pnn_acts", "malloc", "space for class sums");
  for ( j = 0, atr = acts ; j < nfound * nClass ; j++ )
     *atr++ = 0.0;


  end = nknown;
  for ( i = 0, atr = acts, ftr = found ; i < nfound ;
        i++, ftr += nInps, atr += nClass )
  {
     for ( j = 0 ; j < end ; j++ )
        useit[j] = 0;

     closev = BIG; maxfar = BIG;
     for ( j = 0, kptr = known, end = 0 ; j < nknown ; j++ )
     {
        for ( k = 0, fptr = ftr, dis = 0.0 ; k < nInps ; k++ )
        {
           dif  = *fptr++ - *kptr++;
           dis += dif * dif;
           /* if prototype is already too far away then don't accumulate */
           /* further. This saves a lot of time                          */
           if (dis > maxfar)
           {
              kptr += nInps - k - 1;
              break;
           }
        }

        if (dis <= maxfar) /* use this prototype: put its index and distance */
        {                  /* into a increasing distance stack               */
			   /* the number of items in the stack is a random   */
                           /* variable determined by a LAMBDA                */

           dists[end] = dis;
           useit[end] = 1;
           index[end] = j;

           /* if a new prototype is closest to the unknown then prune the */
           /* tail end of the list so that the closeness criteria is met  */ 
           if (dis < closev)
           {
              closev = dis;
              maxfar = closev + slop;
              for ( k = 0 ; k < end ; k++ )
                 if (useit[k] && (dists[k] > maxfar))
                    useit[k] = 0;
           }
           end++;
        }
     }

     /* only "end" prototypes were close enough in the euclidean sense */
     /* in the PNN sense their exponentials are small                  */ 
     for ( j = 0 ; j < end ; j++ )
        if (useit[j])
           t = (float)exp((double)(dists[j] * f)),
           atr[class_known[index[j]]] += t;
  }

  free(index), free(dists), free(useit);
  return acts ;
}
