#include "includes.h"
 
 BitMap3D *FindPool_SampleImg3D;
 
 
 int GridQuant(float Dist, int Size)
  {
   float s=(float)(Size)/2.0;
   int i=(int)(Dist+s-0.5);
   if (i>=Size) i=Size-1;
   if (i<0)     i=0;
   
   return i;
  }
 
/**************************************|****************************************
Routine   :FindNodes3D
Input  par:BitMap3D *Image 
           PoolStructure *Pool,int B,int delta,
                          int FeatureDim,float fTShadeEdge, int domain
Output par:
Function  :
***************************************|***************************************/

 PoolNode ****FindNodes3D(BitMap3D *Image,PoolStructure *Pool,int B,int delta,
                          int FeatureDim,float fTShadeEdge, int domain)
  {
   PoolNode ****Array;
   PoolNode *Node;
   float *Features;
   int x,y,z,XMax,YMax,ZMax,Count=0;
   
   if (domain) XMax=((Image->XSize<<1)-B)/delta +1;
   else        XMax=(Image->XSize)/B;
   if (domain) YMax=((Image->YSize<<1)-B)/delta +1;
   else        YMax=(Image->YSize)/B;
   if (domain) ZMax=((Image->ZSize<<1)-B)/delta +1;
   else        ZMax=(Image->ZSize)/B;
   
   Array=GimmeAPoolNodeArray3D(XMax,YMax,ZMax);
   
   if (domain)
    {
     Pool->DomainX=XMax;
     Pool->DomainY=YMax;
     Pool->DomainZ=ZMax;
    }
   else
    {
     Pool->RangeX=XMax;
     Pool->RangeY=YMax;
     Pool->RangeZ=ZMax;
    }
   
   for(x=0;x<XMax;x++)
   for(y=0;y<YMax;y++)
   for(z=0;z<ZMax;z++)
    {
     Array[x][y][z]=Node=GimmeAPoolNode();
     
     if (domain) Features=Classify3D(Image,(x*delta)>>1,(y*delta)>>1,
                                     (z*delta)>>1,B>>1,FeatureDim,fTShadeEdge);
     else        Features=Classify3D(Image,x*delta,y*delta,
                                     z*delta,B,FeatureDim,fTShadeEdge);

     Node->Features=Features;
     Node->Distance=NULL;
     
     if (Features[VAR]>fTShadeEdge)  
      {
       Node->x=x*delta;
       Node->y=y*delta;
       Node->z=z*delta;
       Count++;
      }
     else /* decode grey range block to original domain image */
     if(!domain)
      {
       register unsigned int X,Y,Z;
       register int Par=Features[MEAN];
       
       for(X=x*delta;X<x*delta+B;X++)      
       for(Y=y*delta;Y<y*delta+B;Y++)        
       for(Z=z*delta;Z<z*delta+B;Z++) Image->Map[X][Y][Z]=Par;
      }
    }
    
   vprintf(stderr,"block size %3d, edges: %6d total: %6d  = %3.1f%c ",B,Count,
                   XMax*YMax*ZMax,Count*100.0/(XMax*YMax*ZMax),'%');
   
   return Array;
  }
 
 FeatureSpace *FindSpace3D(PoolStructure *Pool,int FeatureDim,int Size,float fTShadeEdge)  
  {
   FeatureSpace *S=GimmeAFeatureSpace(FeatureDim,Size);
   Grid         *Tmp;
   GridTerm     *T;
   PoolNode ****DPool=Pool->DomainNodes;
   PoolNode ****RPool=Pool->RangeNodes;
   PoolNode    *Temp;
   ListNode *TempNode;
   
   float *Count=GimmeAFloatArray(FeatureDim);
   float *Mean =GimmeAFloatArray(FeatureDim);
   float *Var  =GimmeAFloatArray(FeatureDim);
   float *InvDev=GimmeAFloatArray(FeatureDim);
   
   float temp;
   double t;
   register int x,y,z,i,itemp;
   
   for(i=0;i<FeatureDim;i++)   /* find mean for all (domain) features */
    {
     double mean=0.0,var=0.0;  /* find variance cov_ii for all features */
     Count[i]=0.0;
     for(x=0;x<Pool->DomainX;x++)
     for(y=0;y<Pool->DomainY;y++)
     for(z=0;z<Pool->DomainZ;z++)
      {
       Temp=DPool[x][y][z];
       if (Temp!=NULL && Temp->Features[VAR]>fTShadeEdge)
        {
         t=(double)Temp->Features[i+PREDEFFEATURES];
         mean += t;
         var += t*t;
         Count[i]++;
        }
      }
     Mean[i]=(float)(mean/(double)Count[i]);
     t=(var-Count[i]*Mean[i]*Mean[i])/(Count[i]-1.0);
     Var[i]=(float)t;
     InvDev[i]=1.0/(float)sqrt(t);
    }
   
   for(x=0;x<Pool->DomainX;x++) /* insert domain blocks in feature grid */
   for(y=0;y<Pool->DomainY;y++) /* using Mahalanobis distance    */
   for(z=0;z<Pool->DomainZ;z++) 
    {
     Temp=DPool[x][y][z];
     if (Temp!=NULL && Temp->Features[VAR]>fTShadeEdge)
      {
       Temp->Distance=GimmeALongArray(FeatureDim);
       Temp->Index   =GimmeAIntArray(FeatureDim);
       Tmp=S->GridArray;
       for(i=0;i<FeatureDim;i++)
        {
         temp=(Temp->Features[i+PREDEFFEATURES]-Mean[i])*InvDev[i];
         Temp->Distance[i]=(long)(temp*F2LONG);
         itemp=GridQuant(temp,Size);
         Temp->Index[i] = itemp;
         Tmp=Tmp->Next[itemp];
        }
       
       T=(GridTerm *)Tmp;
       if (T->Class==NULL)
        {
         T->Class=GimmeAListNode();
         TempNode=T->Class;
        }
       else
        {
         TempNode=T->Class;
         while (TempNode->Next!=NULL) TempNode=TempNode->Next;
         
         TempNode->Next=GimmeAListNode();
         TempNode=TempNode->Next;
        }
       
       TempNode->Domain=Temp;
       T->Count++;
      }
    }
   
   for(x=0;x<Pool->RangeX;x++) /* insert range blocks in feature grid */
   for(y=0;y<Pool->RangeY;y++) /* using Mahalanobis distance    */
   for(z=0;z<Pool->RangeZ;z++) 
    {
     Temp=RPool[x][y][z];
     if (Temp->Features[VAR]>fTShadeEdge)
      {
       Temp->Distance=GimmeALongArray(FeatureDim);
       Temp->Index   =GimmeAIntArray(FeatureDim);
       for(i=0;i<FeatureDim;i++)
        {
         temp=(Temp->Features[i+PREDEFFEATURES]-Mean[i])*InvDev[i];
         Temp->Distance[i]=(long)(temp*F2LONG);
         itemp=GridQuant(temp,Size);
         Temp->Index[i]=itemp;
        }
      }
    }
   
   FreeMeAFloatArray(Count);
   FreeMeAFloatArray(Mean);
   FreeMeAFloatArray(Var);
   FreeMeAFloatArray(InvDev);
   
   return S;
  }
 
 
/**************************************|****************************************
Routine   : *FindPool
Input  par: BitMap *Image, int D, int B, int delta, int keepdelta
Output par: none
Function  : Finds the pool structure, calls FindNodes recursively finding domain
            and range pool for every resolution 
***************************************|***************************************/
 
 PoolStructure *FindPool3D(BitMap3D *Image,int FeatureDimensions, int GridRes,
                           int TShadeEdge, int NSquare,int B,int delta)
  {
   if (NSquare>=0)
    {
     PoolStructure *Pool=GimmeAPoolStructure();
     register unsigned int x,y,z,i,j,k;
     register int Sum;
     static int first=TRUE;
     
     vprintf(stderr,"\n   Range  ");
     Pool->RangeNodes=FindNodes3D(Image,Pool,B,B,FeatureDimensions,(float)TShadeEdge,FALSE);
     
     Pool->Next=FindPool3D(Image,FeatureDimensions,GridRes,
                           TShadeEdge,NSquare-1,B>>1,delta);

     if(first) /* build a sampled version of the original image */
      {
       first=FALSE;
       FindPool_SampleImg3D=GimmeABitMap3D(Image->XSize>>1,Image->YSize>>1,
                                           Image->ZSize>>1,Image->ImgType);
       for(x=0;x<Image->XSize;x+=2)      
       for(y=0;y<Image->YSize;y+=2)        
       for(z=0;z<Image->ZSize;z+=2)
        {
         Sum=0;
         for(i=0;i<2;i++)          
         for(j=0;j<2;j++) 
         for(k=0;k<2;k++) Sum+=Image->Map[x+i][y+j][z+k];
         FindPool_SampleImg3D->Map[x>>1][y>>1][z>>1]=Sum>>3;
        }
      }
     
     Pool->SampledDomainBitmap3D=FindPool_SampleImg3D;

     vprintf(stderr,"\n   Domain ");
     Pool->DomainNodes=FindNodes3D(FindPool_SampleImg3D,Pool,B<<1,delta,
                                   FeatureDimensions,(float)TShadeEdge,TRUE);
     
     Pool->DomainSpace=FindSpace3D(Pool,FeatureDimensions,GridRes,(float)TShadeEdge);
 
     return Pool;
    }
   else return (PoolStructure *)NULL;
  }