// DLMSimilarity.C

# include "nsl_include.h"
# include "DLMwin.h"
# include "DLMSimilarity.h"
# include "DLM.h"

int maxGallerySize = -1; // actual maximal simulated gallery size ;
                                 // in order to avoid multiple mem allocation ;
     
DLMSimilarity::DLMSimilarity(nsl_string str,NslModule* parent) :
	NslModule(str,parent),
	S21("S21",this),
	showS21("showS21",this), 
	ObjectSelectionMode("ObjectSelectionMode",this),
	preferredObject("preferredObject",this),
	alpha_S("alpha_S",this)
{
	initModule();
}

int DLMSimilarity::memAlloc(int _gallerySizeMax,int _frame,int _i1max,int _j1max,
	int _i2max,int _j2max,int _i1Rmax,int _j1Rmax)
{
	gallerySizeMax = _gallerySizeMax;
	frame = _frame;
	i1max = _i1max;
	j1max = _j1max;
	i2max = _i2max;
	j2max = _j2max;
	i1Rmax = _i1Rmax;
	j1Rmax = _j1Rmax;
	
  	S21.memAlloc(gallerySizeMax,i2max*j2max,i1Rmax*j1Rmax);
  	showS21.memAlloc(i2max*j2max,i1Rmax*j1Rmax);
  
  return 1;
}
int DLMSimilarity::initModule()
{
	selectedObject=0;

	strcpy(objectsFileName,"DLMobjects");
	strcpy(modelsFileName,"DLMmodels");

	kLevels=5; kDirections=8;
	kMaximum=1.5708; kFactor=0.707107; sigma=6.28318;
	tablesCreated=0;
	kxArr = new float[kLevels*kDirections];
	kyArr = new float[kLevels*kDirections];
	confidenceBak = new float[kLevels*kDirections];
	deltaPhaseBak = new float[kLevels*kDirections];
	
	return 1;
}

// ---- function to create tables for the similarity function ;

void DLMSimilarity::createTables()
{
  	float k0, k0Angle;
  	int kLevel, kDirection, kIndex;
  	for (kLevel=0; kLevel<kLevels; kLevel++) {
    	   k0 = kMaximum * pow(kFactor, kLevel);
    	   for (kDirection=0; kDirection<kDirections; kDirection++) {
      		kIndex = (kLevel*kDirections+kDirection);
      		k0Angle = kDirection*M_PI/kDirections;
      		kxArr[kIndex] = k0*cos(k0Angle);
      		kyArr[kIndex] = k0*sin(k0Angle);
    	   }
  	}
  	tablesCreated=1;
}

/* The similarity function takes two jets and provides similarity and
   distance information in fsData. The neural system is not dependent
   on the concrete form of the features and comparison function. And
   since this comparison function is a bit complicated, the precise
   algorithm will be described in a different publication. But you can
   choose a simpler comparison function that does not use the phase
   information, see the <#define PHASE> statement above. */

float DLMSimilarity::similarity(float *trafoData1, float *trafoData2)
{
  	int kLevel, kIndex, kIndex2;
  	float k0x, k0y, deltaPhase; 
  	float dispX=0, dispY=0, confidence, meanError=0;
  	float PhiX=0, PhiY=0, GammaXX=0, GammaXY=0, GammaYY=0;

  	for (kLevel=kLevels-1; kLevel>=0; kLevel--) {
    	   for (kIndex=kLevel*kDirections; kIndex<(kLevel+1)*kDirections; kIndex++) {
      		kIndex2 = kIndex*2;
      		confidence = trafoData1[kIndex2]*trafoData2[kIndex2];
      		confidenceBak[kIndex] = confidence;
      
      		if (confidence!=0) {
		   k0x = kxArr[kIndex];
		   k0y = kyArr[kIndex];
		   deltaPhase = (trafoData1[kIndex2+1]-trafoData2[kIndex2+1]);
		   while (deltaPhase< dispX*k0x+dispY*k0y-M_PI) 
			{ deltaPhase += 2*M_PI; }
		   while (deltaPhase>=dispX*k0x+dispY*k0y+M_PI) 
		   	{ deltaPhase -= 2*M_PI; }
		   deltaPhaseBak[kIndex] = deltaPhase;

		   PhiX    += confidence*k0x*deltaPhase;
		   PhiY    += confidence*k0y*deltaPhase;
		   GammaXX += confidence*k0x*k0x;
		   GammaXY += confidence*k0x*k0y;
		   GammaYY += confidence*k0y*k0y;
      		}
    	   }

    	   dispX = (+PhiX*GammaYY-PhiY*GammaXY)/(GammaXX*GammaYY-GammaXY*GammaXY);
    	   dispY = (-PhiX*GammaXY+PhiY*GammaXX)/(GammaXX*GammaYY-GammaXY*GammaXY);

  	}

  	for (kIndex=0; kIndex<kLevels*kDirections; kIndex++) {
    	   deltaPhase = deltaPhaseBak[kIndex]-dispX*kxArr[kIndex]-dispY*kyArr[kIndex];
    	   meanError += confidenceBak[kIndex]*(2.-deltaPhase*deltaPhase);
  	}

 	 return 0.5*meanError;
}

// ==== function to compute the similarity matrices ======================= ;


/* The objects are stored in files as 16x17 graphs the models as 10x10
   graphs, hence Si1max=17, Sj1max=16 and Si2max=Sj2max=10. With the
   variables Si1offset, Sj1offset, i1max, and j1max a subgraph can be
   selected from the stored object graph and analogously for layers
   2.

   The similarity matrices are only computed at the first
   initialization or if the relevant object or models as noted in the
   file DLMobjects and DLMmodels has changed. */


int DLMSimilarity::initSysTemp(NslFloat0& gallerySize,NslFloat0& showLayer)
{
  // ---- variables ---------------------------------------------------- ;
  int object, objects, model, models;
  int i1,j1,i2,j2,ij1,ij2,i1R,j1R,ij1R;
  int k;
  int Si2, Sj2, Si1, Sj1, Si2max, Sj2max, Si1max, Sj1max;
  char DLMObjectPath[256], DLMObjectName[256];
  char DLMModelPath[256], DLMModelName[256], DLMCorrectName[256];
  char tmpStr[256];
  ifstream DLMGraph;
  float Norm, normCharFactor;
  char CoeffChar;
  unsigned char CoeffUChar;

 if (gallerySize.elem()<=0) 
 	return 1;

  for (model=maxGallerySize+1; model<=gallerySize.elem(); model++) {
    	modelName[model] = new char[256]; 
    	strcpy(modelName[model],"nobody");
  }
   	
  // ---- initialize tables if required -------------------------------- ;

  if (!tablesCreated)
    createTables();

  // ---- read in object graph ----------------------------------------- ;
  
  // ---- open file of object names and read basic parameters ;
  ifstream DLMObjects;
  DLMObjects.open(objectsFileName,ios::in);
  if (!DLMObjects.good()) {
    cerr << "\nERROR: Cannot open file " <<objectsFileName <<" !\n";
    exit(1);
  };
  DLMObjects >> DLMObjectPath >> objects;

  // ---- check whether the preferred object index is valid ;
  if ((preferredObject.elem()<=0)||(preferredObject.elem()>objects)) {
    cout <<"\nWARNING : preferredObject (" << preferredObject.elem() 
      <<") out of range (1-" <<objects <<"), set preferredObject to 1 !!\n";
    preferredObject.elem()=1;
  }

  // ---- select object ;
  switch (int(ObjectSelectionMode.elem())) {
  case RANDOM :
    selectedObject = rand()%objects+1;
    break;
  case NEXT :
    selectedObject++;
    break;
  case PREFERRED :
    selectedObject = int(preferredObject.elem());
    break;
  default:
    cerr << "\nERROR: ObjectSelectionMode " << ObjectSelectionMode.elem() 
      << " not valid !\n";
    break;
  }
  if ((selectedObject<=0)||(selectedObject>objects)) {
    cerr << "\nERROR: selected object (" << selectedObject 
      << ") out of range [1,...," << objects <<"] !\n";
    exit(1);
  }

  // ---- read object name ;
  for (object=0; object<selectedObject; object++)
    DLMObjects >> DLMObjectName >> Oi1offset >> Oj1offset;
  if (!DLMObjects.good()) {
    cerr <<"\nERROR : Could not read DLMObjectName, Oi1offset, and Oj1offset from DLMObjects !!\n";
    exit(1);
  }
  DLMObjects.close();
  cout << "\n Object :\n " << DLMObjectName;

  // ---- check whether the object name has changed since the last init ;
  if (strcmp(DLMObjectName,objectName)!=0) {
    for (model=0; model<=gallerySize.elem(); model++)
      strcpy(modelName[model],"nobody");
    strcpy(objectName,DLMObjectName);
  };

  // ---- open object graph file and read basic parameters ;
  sprintf(tmpStr,"%s%s",DLMObjectPath,objectName);
  DLMGraph.open(tmpStr,ios::in);
  if (!DLMGraph.good()) {
    cerr << "\nERROR : Cannot open file "<<objectName<<" !!\n";
    exit(1);
  }
  float floatDummy;
  DLMGraph >> floatDummy >> floatDummy>> Sj1max
    >> floatDummy >> floatDummy>> Si1max;

  // ---- determine offsets ;
  Si1offset = Oi1offset - (i1max-2*frame-10)/2;
  if (Si1offset<0) Si1offset = 0;
  if (Si1offset>Si1max-i1max+2*frame) Si1offset=Si1max-i1max+2*frame;
  if (Si1offset<0) {
    cerr <<"\nERROR : object graph has less columns ("<<Si1max
      <<") than required ("<<i1max-2*frame<<") !!\n";
    exit(1);
  }
  Sj1offset = Oj1offset - (j1max-2*frame-10)/2;
  if (Sj1offset<0) Sj1offset = 0;
  if (Sj1offset>Sj1max-j1max+2*frame) Sj1offset=Sj1max-j1max+2*frame;
  if (Sj1offset<0) {
    cerr <<"\nERROR : object graph has less rows ("<<Sj1max
      <<") than required ("<<j1max-2*frame<<") !!\n";
    exit(1);
  }
  cout << "   \t(i = " <<Si1offset<<",...,"<<Si1offset+i1max-2*frame-1
    <<"; j = " <<Sj1offset<<",...,"<<Sj1offset+j1max-2*frame-1<<")";

  // ---- read in trafo parameter of object graph and check them ;
  DLMGraph >> kLevelsTmp >> kDirectionsTmp >> kMaximumTmp 
    >> kFactorTmp >> sigmaTmp;
  if (kLevels!=kLevelsTmp)
    cerr << "\nERROR: kLevels (" << kLevelsTmp 
      << ") in object graph != " << kLevels <<" !!\n";
  if (kDirections!=kDirectionsTmp)
    cerr << "\nERROR: kDirections (" << kDirectionsTmp 
      << ") in object graph != " << kDirections <<" !!\n";
  if (kMaximum!=kMaximumTmp)
    cerr << "\nERROR: kMaximum (" << kMaximumTmp 
      << ") in object graph != " << kMaximum <<" !!\n";
  if (kFactor!=kFactorTmp)
    cerr << "\nERROR: kFactor (" << kFactorTmp 
      << ") in object graph != " << kFactor <<" !!\n";
  if (sigma!=sigmaTmp)
    cerr << "\nERROR: sigma (" << sigmaTmp 
      << ") in object graph != " << sigma <<" !!\n";

  // ---- read in jets of object graph ;
  float *DLMGImage = new float[i1max*j1max*kLevels*kDirections*2];
  float ImageNormFloat;
//  float ImageNormChar[i1max*j1max];
  float* ImageNormChar = new float[i1max*j1max];
  for (Sj1=0, j1=-Sj1offset+frame; Sj1<Sj1max; Sj1++, j1++)
    for (Si1=Si1max-1, i1=Si1max-1-Si1offset+frame; Si1>=0; Si1--, i1--) {
	ij1 = ij1Index(i1,j1,j1max);
	// check whether the node belongs to the selected subgraph ;
	char tmpChar=' ';
	if ((i1<frame)||(i1>=i1max-frame)||(j1<frame)||(j1>=j1max-frame)) {
	  DLMGraph >> Norm  >> Norm;
	  tmpChar=' ';
	  while (tmpChar!='\n')
	    DLMGraph.get(tmpChar);
	  for (k=0; k<kLevels*kDirections; k++) {
	    DLMGraph.get(CoeffUChar); 
	    DLMGraph.get(CoeffChar);
	  }
	} else {
	  DLMGraph >> ImageNormFloat >> ImageNormChar[ij1];
	  normCharFactor = 1/ImageNormChar[ij1];
	  tmpChar=' ';
	  while (tmpChar!='\n')
	    DLMGraph.get(tmpChar);
	  for (k=0; k<kLevels*kDirections*2; k+=2) {
	    DLMGraph.get(CoeffUChar); 
	    DLMGraph.get(CoeffChar);
	    DLMGImage[ij1*kLevels*kDirections*2+k] 
	      = CoeffUChar*normCharFactor;
	    DLMGImage[ij1*kLevels*kDirections*2+k+1] 
	      = float(CoeffChar)/127*M_PI;
	  };
	};
	if (!DLMGraph.good()) {
	  cerr << "\nERROR: Cannot read coefficient from DLMGraph !\n";
	  exit(1);
	}
      }; // for (Si1=Si1max-1; Si1>=0; Si1--) ;
  DLMGraph.close();


  // ---- read in models and compute similarity matrices ----------------- ;

  // ---- variables ;
  int modelInGal;
  float *DLMGModel = new float[i1max*j1max*kLevels*kDirections*2];
//  float ModelNormFloat[i2max*j2max];
//  float ModelNormChar[i2max*j2max];
  float* ModelNormFloat = new float[i2max*j2max];
  float* ModelNormChar = new float[i2max*j2max];
  
  // ---- open file of model names and read basic parameters ;
  ifstream DLMModels;
  DLMModels.open(modelsFileName,ios::in);
  if (!DLMModels.good()) {
    cerr << "\nERROR: Cannot open file "<<modelsFileName<<" !\n";
    exit(1);
  }
  DLMModels >> DLMModelPath >> models;

  // ---- check whether enough models are in DLMModels ;
  if (models<gallerySize.elem()) {
    cerr <<"\nWARNING : not enough models in DLMModels, set gallerySize=models (" 
      << models <<") !!\n";
    gallerySize.elem() = models;
  }

  // ---- search for correct model in gallery ;
  for (modelInGal=1; modelInGal<=models; modelInGal++) {
    DLMModels >> DLMModelName;
    if (strncmp(DLMModelName,objectName,strlen(objectName)-1)==0)
      break;
  };
  if (modelInGal>models) 
    cerr <<"\nERROR : person "<< objectName <<" not in DLMModels !!\n";

  // ---- model loop ;
  cout << "\n\n Gallery : ";
  for (model=1; model<=gallerySize.elem(); model++) {

    // ---- read name of next model ;
    if (model!=1) {
      modelInGal++;
      if (modelInGal>models) {
	DLMModels.close();
	DLMModels.open(modelsFileName,ios::in);
	if (!DLMModels.good()) {
	  cerr << "\nERROR: Cannot open file "<<modelsFileName<<" !\n";
	  exit(1);
	}
	char dummyString[256];
	int dummyInt;
	DLMModels >> dummyString >> dummyInt;
	modelInGal=1;
      };
      DLMModels >> DLMModelName;
    };
    cout <<"\n "<<DLMModelName;

    // ---- check whether the model name has changed since the last init ;
    if (strcmp(DLMModelName,modelName[model])==0)
      continue;
    strcpy(modelName[model],DLMModelName);
    
    // ---- open model graph file and read basic parameters ;
    sprintf(tmpStr,"%s%s",DLMModelPath,DLMModelName);
    DLMGraph.open(tmpStr,ios::in);
    if (!DLMGraph.good()) {
	  cerr << "\nERROR: Cannot open file "
	    << sprintf(tmpStr,"%s%s",DLMModelPath,DLMModelName)<<" !\n";
	  exit(1);
	}
    DLMGraph >> floatDummy>> floatDummy >> Sj2max
      >> floatDummy>> floatDummy >> Si2max;

    // ---- determine offsets ;
    Si2offset = (Si2max-i2max)/2;
    if (Si2offset<0)
      cerr <<"\nERROR : model graph has less columns ("<<Si2max
	<<") than required ("<<i2max<<") !!\n";
    Sj2offset = (Sj2max-j2max)/2;
    if (Sj2offset<0)
      cerr <<"\nERROR : model graph has less rows ("<<Sj2max
	<<") than required ("<<j2max<<") !!\n";
    cout << "   \t(i = " <<Si2offset<<",...,"<<Si2offset+i2max-1
      <<"; j = " <<Sj2offset<<",...,"<<Sj2offset+j2max-1<<")";
    
    // ---- read trafo parameters of model graph and check them ;
    DLMGraph >> kLevelsTmp >> kDirectionsTmp 
      >> kMaximumTmp >> kFactorTmp >> sigmaTmp;
    if (kLevels!=kLevelsTmp)
      cerr << "\nERROR: kLevels (" << kLevelsTmp 
	<< ") in model graph != " << kLevels <<" !!\n";
    if (kDirections!=kDirectionsTmp)
      cerr << "\nERROR: kDirections (" << kDirectionsTmp 
	<< ") in model graph != " << kDirections <<" !!\n";
    if (kMaximum!=kMaximumTmp)
      cerr << "\nERROR: kMaximum (" << kMaximumTmp 
	<< ") in model graph != " << kMaximum <<" !!\n";
    if (kFactor!=kFactorTmp)
      cerr << "\nERROR: kFactor (" << kFactorTmp 
	<< ") in model graph != " << kFactor <<" !!\n";
    if (sigma!=sigmaTmp)
      cerr << "\nERROR: sigma (" << sigmaTmp 
	<< ") in model graph != " << sigma <<" !!\n";

    // ---- read jets of model graph ;
    for (Sj2=0, j2=-Sj2offset; Sj2<Sj2max; Sj2++, j2++)
      for (Si2=Si2max-1, i2=Si2max-1-Si2offset; Si2>=0; Si2--, i2--) {
	ij2 = ij2Index(i2,j2,j2max);
	// check whether the node belongs to the selected subgraph ;
	char tmpChar;
	if ((i2<0)||(i2>=i2max)||(j2<0)||(j2>=j2max)) {
	  DLMGraph >> Norm  >> Norm;
	  tmpChar=' ';
	  while (tmpChar!='\n')
	    DLMGraph.get(tmpChar);
	  for (k=0; k<kLevels*kDirections; k++) {
	    DLMGraph.get(CoeffUChar); 
	    DLMGraph.get(CoeffChar);
	  }
	} else {
	  DLMGraph >> ModelNormFloat[ij2]  >> ModelNormChar[ij2];
	  normCharFactor = 1/ModelNormChar[ij2];
	  tmpChar=' ';
	  while (tmpChar!='\n')
	    DLMGraph.get(tmpChar);
	  for (k=0; k<kLevels*kDirections*2; k+=2) {
	    DLMGraph.get(CoeffUChar); 
	    DLMGraph.get(CoeffChar);
	    DLMGModel[ij2*kLevels*kDirections*2+k] 
	      = CoeffUChar*normCharFactor;
	    DLMGModel[ij2*kLevels*kDirections*2+k+1] 
	      = float(CoeffChar)/127*M_PI;
	  }
	}
	if (!DLMGraph.good()) {
	  cerr << "\nERROR: Cannot read coefficient from DLMGraph !\n";
	  exit(1);
	}
      } // for (Si2=Si2max-1; Si2>=0; Si2--) ;
    DLMGraph.close();

    
    // ---- compute similarity matrix ;
    for (i2=0, ij2=0; i2<i2max; i2++)
      for (j2=0; j2<j2max; j2++, ij2++)
	for (i1R=0, i1=i1Index(i2,0,frame,i1max,i1Rmax,i2max), ij1R=0; i1R<i1Rmax; i1R++, i1++)
	  for (j1R=0, ij1=ij1Index(i1,j1Index(j2,0,frame,j1max,j1Rmax,j2max),j1max); j1R<j1Rmax; 
	       j1R++, ij1++, ij1R++) {
//	    S21[model]->elem(ij2,ij1R)
	    S21[model][ij2][ij1R] 
	      = maximum(alpha_S.elem(),
			similarity(&(DLMGImage[ij1*kLevels*kDirections*2]),
				   &(DLMGModel[ij2*kLevels*kDirections*2])));
	  }


  } // for (model=1; model<=gallerySize.elem(); model++) ;

    if (gallerySize.elem()>0) 
      showS21        = S21[int(showLayer.elem())];
    
  // ---- free memory ;
  delete[] DLMGModel;
  delete[] DLMGImage;

  // ---- close file of model names ;
  DLMModels.close();
  
  S21.send();

  return 1;

}
int DLMSimilarity::initRunTemp(NslFloat0& gallerySize)
{
  int model, k, ij1R, ij2;  // some index variables ;

  if (gallerySize.elem()>0) {

    float S;
    if (gallerySize.elem()>0)
      for (ij2=0; ij2<i2max*j2max; ij2++)
	for (ij1R=0; ij1R<i1Rmax*j1Rmax; ij1R++) {
	  S = 0;
	  for (model=1; model<=gallerySize.elem(); model++)
	    S = maximum(S,S21[model][ij2][ij1R]);
	  S21[0][ij2][ij1R] = S;
	}
  }
  
  S21.send();
  
  return 1;
}