/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include <stdio.h>
#include <string.h>

#include "kernreg.h"

bool TMap::init(int sy,int su,int sv)
{
  uchar *ntmap;
  int nsize;

  nsize = sy * su * sv;
  ntmap = new uchar[nsize];
  if(!ntmap) return(false);

  delete[](tmap);
  size = nsize;
  tmap = ntmap;

  size_y = sy;
  size_u = su;
  size_v = sv;

  div_y = LEVELS / sy;
  div_u = LEVELS / su;
  div_v = LEVELS / sv;

  clear();
  return(true);
}

//====================================================================//

void KernelRegLearner::addList(example *e)
{
  example *ne;

  while(e){
    ne = e->next;
    tree.add(e);
    e = ne;
  }
}

void KernelRegLearner::build()
{
  tree.build();
}

void KernelRegLearner::learn(TMap &tmap,color_info *colors,int num)
{
  example e,*ne;
  int iy,iu,iv;
  double d;
  int i,k,m;
#ifdef USE_AMBIGUOUS
  int m2;
#endif

  double weight[num],w,wsum;
  // double md = 1E9;

  tmap.clear(0);

  for(iy=0; iy<tmap.size_y; iy++){
    for(iu=0; iu<tmap.size_u; iu++){
      for(iv=0; iv<tmap.size_v; iv++){
        // query location is the center of the box
        e.y = iy*tmap.div_y + tmap.div_y/2;
        e.u = iu*tmap.div_u + tmap.div_u/2;
        e.v = iv*tmap.div_v + tmap.div_v/2;
        tree.startQuery(e);

        // calculate class weights based on scaled neighbors
        for(i=0; i<num; i++) weight[i] = 0.0;
        k = 0;
        while((ne = tree.getNextNearest(d)) && (k<32 || d<16.0)){
          weight[ne->label] += 1.0 / (1.0 + sq(1.0*d));
          // weight[ne->label] += exp(-sq(0.25*d)) + (1E-9 / (1 + d));
          k++;
        }

        // if(k>8) printf("<%3d,%2d,%3d> k=%d\n",e.y, e.u, e.v, k);
        /*
        if(d < md){
          printf("<%3d,%2d,%3d> d=%f\n",
                 e.y, e.u, e.v, d);
          md = d;
        }
        */

        // scale weights by priors and get total weight
        wsum = 0.0;
        for(i=0; i<num; i++){
          w = weight[i] * colors[i].weight;
          weight[i] = w;
          wsum += w;
        }

        // pick label with highest weight that meets confidence threshold
#ifdef USE_AMBIGUOUS
        m  = -1;
        m2 = -1;
        for(i=0; i<num; i++){
          w = weight[i];
          if((m == -1 || w > weight[m]) && (w > wsum*colors[i].confidenceHigh)){
            if(m != -1)
              m2 = m;
            m = i;
          }else if((m2 == -1 || w > weight[m2]) && (w > wsum*colors[i].confidence)){
            m2 = i;
          }
        }
        if(m  == -1) m  = 0;
        if(m2 == -1) m2 = m;
        tmap.tmap[tmap.box_loc(iy,iu,iv)] = (((uchar)m2)<<4) | ((uchar)m);
#else
        m = 0;
        for(i=1; i<num; i++){
          w = weight[i];
          if((w > weight[m]) && (w > wsum*colors[i].confidence)) m = i;
        }
        tmap.tmap[tmap.box_loc(iy,iu,iv)] = m;
#endif
        tree.endQuery();
      }

      int p = 100*(iy*tmap.size_u + iu + 1) / (tmap.size_y*tmap.size_u);
      printf("\r  %d%%",p); fflush(stdout);
    }
  }
  printf("\n");
}