// 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 ;



#ifdef NSL_PC

static const double PI = 3.14159;

static const double M_PI = 1.0/PI;

#endif

     

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)

{

	nsl_string _str;

	for (int i=0; i<gallerySizeMax; i++) {

		strcpy(_str,"S21");

		sprintf(_str,"%s[%d]",_str,i);

		S21[i].initialize(_str,this);

	}

	initModule();

}



void 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;

	

	 showS21.memAlloc(i2max,j2max,i1Rmax,j1Rmax);

	for (int i=0; i<gallerySizeMax; i++) {

  		S21[i].memAlloc(i2max,j2max,i1Rmax,j1Rmax);

	}

}

void 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];

}



// ---- 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. */





void DLMSimilarity::initSysTemp(NslFloat0& gallerySize,NslFloat0& showLayer)

{

  // ---- variables ---------------------------------------------------- ;

  int object, objects, model, models;

  int i1,j1,i2,j2,ij1,i1R,j1R;

  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;



  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];

  NslFloat1 DLMGImage(i1max*j1max*kLevels*kDirections*2);

  float ImageNormFloat;

//  float ImageNormChar[i1max*j1max];

//  float* ImageNormChar = new float[i1max*j1max];

  NslFloat2 ImageNormChar(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[i1][j1];

	  normCharFactor = 1/ImageNormChar[i1][j1];

	  tmpChar=' ';

	  while (tmpChar!='\n')

	    DLMGraph.get(tmpChar);

	  for (k=0; k<kLevels*kDirections*2; k+=2) {

	    DLMGraph.get(CoeffUChar); 

	    DLMGraph.get(CoeffChar);

	    DLMGImage[(i1*j1max+j1)*kLevels*kDirections*2+k] 

	      = CoeffUChar*normCharFactor;

	    DLMGImage[(i1*j1max+j1)*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];

  NslFloat1 DLMGModel(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];

  NslFloat2 ModelNormFloat(i2max,j2max);

  NslFloat2 ModelNormChar(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[i2][j2]  >> ModelNormChar[i2][j2];

	  normCharFactor = 1/ModelNormChar[i2][j2];

	  tmpChar=' ';

	  while (tmpChar!='\n')

	    DLMGraph.get(tmpChar);

	  for (k=0; k<kLevels*kDirections*2; k+=2) {

	    DLMGraph.get(CoeffUChar); 

	    DLMGraph.get(CoeffChar);

	    DLMGModel[(i2*j2max+j2)*kLevels*kDirections*2+k] 

	      = CoeffUChar*normCharFactor;

	    DLMGModel[(i2*j2max+j2)*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; i2<i2max; i2++)

      for (j2=0; j2<j2max; j2++)

	for (i1R=0, i1=i1Index(i2,0,frame,i1max,i1Rmax,i2max); i1R<i1Rmax; i1R++, i1++)

	  for (j1R=0, j1=j1Index(j2,0,frame,j1max,j1Rmax,j2max); j1R<j1Rmax; j1R++, j1++) {

	    S21[model][i2][j2][i1R][j1R] 

	      = maximum(alpha_S.elem(),

			similarity(&(DLMGImage[(i1*j1max+j1)*kLevels*kDirections*2]),

				   &(DLMGModel[(i2*j2max+j2)*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();

}

void DLMSimilarity::initRunTemp(NslFloat0& gallerySize)

{

	if (gallerySize.elem()>0) {



		float S;

		if (gallerySize.elem()>0)

			for (int i2=0; i2<i2max; i2++)

				for (int j2=0; j2<j2max; j2++)

					for (int i1R=0; i1R<i1Rmax; i1R++)

						for (int j1R=0; j1R<j1Rmax; j1R++) {

							S = 0;

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

								S = maximum(S,S21[model][i2][j2][i1R][j1R]);

							S21[0][i2][j2][i1R][j1R] = S;

						}

	}

  

//  S21.send();

}