// DlmSimilarity.C

# include "nsl_include.h"

#include "DLMwin.h"
#include "DLMdisplay.h"
# include "DlmSimilarity.h"
# include "Dlm.h"

DlmSimilarity::DlmSimilarity(nsl_string str,NslModule* parent) :
	DlmCommon(str,parent),

	DlmObjects("DlmObjects",this),
	DlmGraph("DlmGraphs",this), 
	DlmModels("DlmModel",this),

	DlmGImage("DlmGImage",this), 
	ImageNormChar("ImageNormChar",this),
	DlmGModel("DlmGModel",this),
	ModelNormFloat("ModelNormFloat",this),
	ModelNormChar("ModelNormChar",this),

	kxArr("kxArr",this),
	kyArr("kyArr",this),
	confidenceBak("confidenceBak",this),
	deltaPhaseBak("deltaPhaseBak",this),

	objectsFileName("objectsFileName",this),
	modelsFileName("modelsFileName",this),
	objectName("objectName",this),                // name of the image graph file ;

	gallerySize("gallerySize",this),   // actual size of the simmulated gallery ;
	ObjectSelectionMode("ObjectSelectionMode",this),
	preferredObject("preferredObject",this),
	alpha_S("alpha_S",this)
{
	nsl_string _str;

	for (int i=0; i<gallerySizeMax; i++) {
		strcpy(_str,"sim");
		sprintf(_str,"%s(%d)",_str,i);
		sim[i].initialize(_str,this);
		strcpy(_str,"DlmImageModelData");
		sprintf(_str,"%s(%d)",_str,i);
		DlmImageModelData[i].initialize(_str,this);
		strcpy(_str,"modelName");
		sprintf(_str,"%s(%d)",_str,i);
		modelName[i].initialize(_str,this);
	}

	initModule();
}

void DlmSimilarity::memAlloc(int _frame,int _i1max,int _j1max,
	int _i2max,int _j2max,int _i1Rmax,int _j1Rmax)
{
	frame = _frame;
	i1max = _i1max;
	j1max = _j1max;
	i2max = _i2max;
	j2max = _j2max;
	i1Rmax = _i1Rmax;
	j1Rmax = _j1Rmax;

	tablesCreated = 0;

	DlmGImage.memAlloc(i1max*j1max*kLevels*kDirections*2);
	ImageNormChar.memAlloc(i1max,j1max);
	DlmGModel.memAlloc(i1max*j1max*kLevels*kDirections*2);
	ModelNormFloat.memAlloc(i2max,j2max);
	ModelNormChar.memAlloc(i2max,j2max);

	kxArr.memAlloc(kLevels*kDirections);
	kyArr.memAlloc(kLevels*kDirections);
	confidenceBak.memAlloc(kLevels*kDirections);
	deltaPhaseBak.memAlloc(kLevels*kDirections);

	for (int i=0; i<gallerySizeMax; i++) {
  		sim[i].memAlloc(i2max,j2max,i1Rmax,j1Rmax);
		DlmImageModelData[i].memAlloc(i1max*j1max*kLevels*kDirections*2);
	}
}

void DlmSimilarity::initModule()
{
	selectedObject=0;

	objectsFileName = "Dlmobjects";
	modelsFileName = "Dlmmodels";

	kLevels=5; kDirections=8;
	kMaximum=1.5708; kFactor=0.707107; sigma=6.28318;
	tablesCreated=0;
}

// ---- 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(NslFloat1& trafoData1,int i1, int j1, NslFloat1& trafoData2,int i2, int j2)
{
  	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[(i1*j1max+j1)*kLevels*kDirections*2+kIndex2]*
				trafoData2[(i2*j2max+j2)*kLevels*kDirections*2+kIndex2];
      		confidenceBak[kIndex] = confidence;
      		if (confidence!=0) {
				k0x = kxArr[kIndex];
				k0y = kyArr[kIndex];
				deltaPhase = (trafoData1[(i1*j1max+j1)*kLevels*kDirections*2+kIndex2+1]-
					trafoData2[(i2*j2max+j2)*kLevels*kDirections*2+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::initSys()
{
  // ---- variables ---------------------------------------------------- ;

  int object, objects, model, models;
  int i1,j1,i2,j2,i1R,j1R;
  int k;
  int Si2, Sj2, Si1, Sj1, Si2max, Sj2max, Si1max, Sj1max;
  NslString0 DlmObjectPath, DlmObjectName;
  NslString0 DlmModelPath, DlmModelName; 
  NslString0 tmpStr; 
  char dummyString[256];
  int dummyInt;
  float floatDummy;
  float Norm, normCharFactor;
  char CoeffChar;
  unsigned char CoeffUChar;

  if (gallerySize<=0) 
 	return;

  for (model=0; model<=gallerySize; model++) {
    	modelName[model] = "nobody";
  }

  // ---- initialize tables if required -------------------------------- ;

  if (!tablesCreated)
    createTables();

  // ---- read in object graph ----------------------------------------- ;

  // ---- open file of object names and read basic parameters ;

//  DlmObjects.init_file(objectsFileName,""); // join with next call
  if (DlmObjects.new_file(objectsFileName,NSL_INPUT) == -1) {
  	NSLoutput("Bad file name: ");
	objectsFileName.nslPrint();
  	exit(1);

  } 
  DlmObjects.read(DlmObjectPath);
  DlmObjects.read(objects);

  // ---- check whether the preferred object index is valid ;

  if ((preferredObject<=0)||(preferredObject>objects)) {
    NSLoutput("\nWARNING : preferredObject (",preferredObject.elem()); 
    NSLoutput(") out of range (1-",objects);
	NSLoutput("), set preferredObject to 1 !!\n");
    preferredObject=1;
  }

  // ---- select object ;

  switch (int(ObjectSelectionMode.elem())) {
  case 1: // PREFERRED :
    selectedObject = int(preferredObject.elem());
    break;
  case 2: // NEXT :
    selectedObject++;
    break;
  case 3: // RANDOM :
    selectedObject = rand()%objects+1;
    break;
  default:
    NSLoutput("\nERROR: ObjectSelectionMode ",ObjectSelectionMode.elem());
	NSLoutput(" not valid !\n");
    break;
  }

  if ((selectedObject<=0)||(selectedObject>objects)) {
    NSLoutput("\nERROR: selected object (",selectedObject); 
    NSLoutput(") out of range [1,...,",objects);
	NSLoutput("] !\n");
    exit(1);
  }

  // ---- read object name 
  
  for (object=0; object<selectedObject; object++)
  {
    DlmObjects.read(DlmObjectName);
    DlmObjects.read(Oi1offset);
    DlmObjects.read(Oj1offset);
  }
        
  DlmObjects.close_file();   
  
  NSLoutput("\n Object :\n ");
  DlmObjectName.nslPrint();

  // ---- check whether the object name has changed since the last init ;

//  if (strcmp(DlmObjectName.get_value(),objectName.get_value())!=0) {
  if (DlmObjectName != objectName) {
    for (model=0; model<=gallerySize; model++)
      	modelName[model]="nobody";
    objectName=DlmObjectName;
  }

  // ---- open object graph file and read basic parameters ;

  tmpStr = DlmObjectPath + objectName;

//  DlmGraph.init_file(tmpStr,""); // join with next call
  if (DlmGraph.new_file(tmpStr,NSL_INPUT,NSL_BINARY) == -1) {
  	NSLoutput("Bad file name: ");
	tmpStr.nslPrint();
  	exit(1);
  }  
  
  DlmGraph.read(floatDummy);
  DlmGraph.read(floatDummy);
  DlmGraph.read(Sj1max);
  DlmGraph.read(floatDummy);
  DlmGraph.read(floatDummy);
  DlmGraph.read(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) {
    NSLoutput("\nERROR : object graph has less columns (",Si1max);
    NSLoutput(") than required (",i1max-2*frame);
	NSLoutput(") !!\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) {
    NSLoutput("\nERROR : object graph has less rows (",Sj1max);
    NSLoutput(") than required (",j1max-2*frame);
	NSLoutput(") !!\n");
    exit(1);
  }

  NSLoutput("   \t(i = ",Si1offset);
  NSLoutput(",...,",Si1offset+i1max-2*frame-1);
  NSLoutput("; j = ",Sj1offset);
  NSLoutput(",...,",Sj1offset+j1max-2*frame-1);
  NSLoutput(")\n");

  // ---- read in trafo parameter of object graph and check them ;

  DlmGraph.read(kLevelsTmp);
  DlmGraph.read(kDirectionsTmp);
  DlmGraph.read(kMaximumTmp);
  DlmGraph.read(kFactorTmp);
  DlmGraph.read(sigmaTmp);

  if (kLevels!=kLevelsTmp) {
    NSLoutput("\nERROR: kLevels (",kLevelsTmp);
    NSLoutput(") in object graph != ",kLevels);
	NSLoutput(" !!\n");
  }
  if (kDirections!=kDirectionsTmp) {
    NSLoutput("\nERROR: kDirections (",kDirectionsTmp);
    NSLoutput(") in object graph != ",kDirections);
	NSLoutput(" !!\n");
  }
  if (kMaximum!=kMaximumTmp) {
    NSLoutput("\nERROR: kMaximum (",kMaximumTmp);
    NSLoutput(") in object graph != ",kMaximum);
	NSLoutput(" !!\n");
  }
  if (kFactor!=kFactorTmp) {
    NSLoutput("\nERROR: kFactor (",kFactorTmp); 
    NSLoutput(") in object graph != ",kFactor);
	NSLoutput(" !!\n");
  }
  if (sigma!=sigmaTmp){
    NSLoutput("\nERROR: sigma (",sigmaTmp);
    NSLoutput(") in object graph != ",sigma);
	NSLoutput(" !!\n");
  }

  // ---- read in jets of object graph ;

  float ImageNormFloat;

  for (Sj1=0, j1=-Sj1offset+frame; Sj1<Sj1max; Sj1++, j1++)
    for (Si1=Si1max-1, i1=Si1max-1-Si1offset+frame; Si1>=0; Si1--, i1--) {

	// check whether the node belongs to the selected subgraph ;

	char tmpChar=' ';

	if ((i1<frame)||(i1>=i1max-frame)||(j1<frame)||(j1>=j1max-frame)) {

	  DlmGraph.read(Norm);  
	  DlmGraph.read(Norm);  

	  tmpChar=' ';

	  while (tmpChar!='\n')
		DlmGraph.read(tmpChar);

	  for (k=0; k<kLevels*kDirections; k++) {
	    DlmGraph.read(CoeffUChar);  
	    DlmGraph.read(CoeffChar); 
	  }

	} else {

	  DlmGraph.read(ImageNormFloat); 
	  DlmGraph.read(ImageNormChar[i1][j1]); 

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

	  tmpChar=' ';

	  while (tmpChar!='\n')
	    DlmGraph.read(tmpChar); 

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

	    DlmGraph.read(CoeffUChar);  
	    DlmGraph.read(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;
	  }
	}
  } // for (Si1=Si1max-1; Si1>=0; Si1--) 

  DlmGraph.close_file();

  DlmImageModelData[0] = DlmGImage;

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

  // ---- variables ;

  int modelInGal;

  // ---- open file of model names and read basic parameters ;

//  DlmModels.init_file(modelsFileName,""); // join with next call
  if (DlmModels.new_file(modelsFileName,NSL_INPUT) == -1) {
  	NSLoutput("Bad file name: ");
	modelsFileName.nslPrint();
  	exit(1);
  }  
  
  DlmModels.read(DlmModelPath);
  DlmModels.read(models);  

  // ---- check whether enough models are in DlmModels ;

  if (models<gallerySize) {
    NSLoutput("\nWARNING : not enough models in DlmModels, set gallerySize=models (",models);
	NSLoutput(") !!\n");
    gallerySize = models;
  }

  // ---- search for correct model in gallery ;

  for (modelInGal=1; modelInGal<=models; modelInGal++) {

    DlmModels.read(DlmModelName);  
    if (nslEqual(DlmModelName,objectName,objectName.get_size()-1))
      break;
  }
  if (modelInGal>models) {
    NSLoutput("\nERROR : person ");
	objectName.nslPrint();
	NSLoutput(" not in DlmModels !!\n");
  }

  // ---- model loop ;

  NSLoutput("\n\n Gallery : ");

  for (model=1; model<=gallerySize; model++) {
    // ---- read name of next model 
    if (model!=1) {
      modelInGal++;
      if (modelInGal>models) {

		DlmModels.close_file();
//		DlmModels.init_file(modelsFileName,""); // join with next call
  		if (DlmModels.new_file(modelsFileName,NSL_INPUT) == -1) {
  			NSLoutput("Bad file name: ");
			modelsFileName.nslPrint();
  			exit(1);
  		} 
  	
  		DlmModels.read(dummyString);
		DlmModels.read(dummyInt); 

		modelInGal=1;
      }
      DlmModels.read(DlmModelName);
    }
    NSLoutput("\n ");
	DlmModelName.nslPrint();

    // ---- check whether the model name has changed since the last init ;

    if (DlmModelName == modelName[model])
      continue;

    modelName[model] = DlmModelName;

    // ---- open model graph file and read basic parameters ;

    tmpStr = DlmModelPath + DlmModelName;

 //   DlmGraph.init_file(tmpStr,""); // join with next call
    if (DlmGraph.new_file(tmpStr,NSL_INPUT,NSL_BINARY) == -1) {
  		NSLoutput("Bad file name: ");
		tmpStr.nslPrint();
  		exit(1);
    }  
    
	DlmGraph.read(floatDummy);
	DlmGraph.read(floatDummy);
	DlmGraph.read(Sj2max);
	DlmGraph.read(floatDummy);
	DlmGraph.read(floatDummy);
	DlmGraph.read(Si2max);
  
    // ---- determine offsets ;

    Si2offset = (Si2max-i2max)/2;
    if (Si2offset<0) {
		NSLoutput("\nERROR : model graph has less columns (",Si2max);
		NSLoutput(") than required (",i2max);
		NSLoutput(") !!\n");
	}
    Sj2offset = (Sj2max-j2max)/2;
    if (Sj2offset<0){
		NSLoutput("\nERROR : model graph has less rows (",Sj2max);
		NSLoutput(") than required (",j2max);
		NSLoutput(") !!\n");
	}
    
	NSLoutput("   \t(i = ",Si2offset);
	NSLoutput(",...,",Si2offset+i2max-1);
    NSLoutput("; j = ",Sj2offset);
	NSLoutput(",...,",Sj2offset+j2max-1);
	NSLoutput(")");

    // ---- read trafo parameters of model graph and check them 

	DlmGraph.read(kLevelsTmp);
	DlmGraph.read(kDirectionsTmp);
	DlmGraph.read(kMaximumTmp);
	DlmGraph.read(kFactorTmp);
	DlmGraph.read(sigmaTmp);

	if (kLevels!=kLevelsTmp){
		NSLoutput("\nERROR: kLevels (",kLevelsTmp); 
		NSLoutput(") in model graph != ",kLevels);
		NSLoutput(" !!\n");
	}
	if (kDirections!=kDirectionsTmp){
		NSLoutput("\nERROR: kDirections (",kDirectionsTmp); 
		NSLoutput(") in model graph != ",kDirections);
		NSLoutput(" !!\n");
	}
	if (kMaximum!=kMaximumTmp){
		NSLoutput("\nERROR: kMaximum (",kMaximumTmp);
		NSLoutput(") in model graph != ",kMaximum);
		NSLoutput(" !!\n");
	}
	if (kFactor!=kFactorTmp){
		NSLoutput("\nERROR: kFactor (",kFactorTmp);
		NSLoutput(") in model graph != ",kFactor);
		NSLoutput(" !!\n");
	}
	if (sigma!=sigmaTmp){
		NSLoutput("\nERROR: sigma (",sigmaTmp);
		NSLoutput(") in model graph != ",sigma);
		NSLoutput(" !!\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--) {

	// check whether the node belongs to the selected subgraph ;

	char tmpChar;

	if ((i2<0)||(i2>=i2max)||(j2<0)||(j2>=j2max)) {

	  DlmGraph.read(Norm);
	  DlmGraph.read(Norm);

	  tmpChar=' ';

	  while (tmpChar!='\n')
	    DlmGraph.read(tmpChar); 

	  for (k=0; k<kLevels*kDirections; k++) {
	    DlmGraph.read(CoeffUChar);  
	    DlmGraph.read(CoeffChar); 
	  }

	} else {

	  DlmGraph.read(ModelNormFloat[i2][j2]);
	  DlmGraph.read(ModelNormChar[i2][j2]);

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

	  while (tmpChar!='\n')
	    DlmGraph.read(tmpChar); 

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

	    DlmGraph.read(CoeffUChar);  
	    DlmGraph.read(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;
	  }
	}

   } // for (Si2=Si2max-1; Si2>=0; Si2--) 

	DlmGraph.close_file();
 
	DlmImageModelData[model] = DlmGModel;

    // ---- 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++) {
					sim[model][i2][j2][i1R][j1R] = NSLmax(alpha_S.elem(),
						similarity(DlmGImage,i1,j1,DlmGModel,i2,j2));
				}
  } // for (model=1; model<=gallerySize; model++) 

  DlmModels.close_file();

  // ---- close file of model names 
  	float S;
	if (gallerySize>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; model++)
					S = NSLmax(S,sim[model][i2][j2][i1R][j1R]);
			   sim[0][i2][j2][i1R][j1R] = S;
			}
}

// DlmCommon

DlmCommon::DlmCommon(nsl_string str,NslModule* parent) : 
	NslModule(str,parent)
{
}
int DlmCommon::i1Index(int i2, int i1R, int frame,int i1max,int i1Rmax,int i2max) 
{
		return i1R + frame + int((i1max-2*frame-i1Rmax)*i2/float(i2max-1)+0.5);
}
int DlmCommon::j1Index(int j2, int j1R, int frame,int j1max,int j1Rmax,int j2max) 
{
	return j1R + frame + int((j1max-2*frame-j1Rmax)*j2/float(j2max-1)+0.5);
}

int DlmCommon::i1RIndex(int i2, int i1, int frame,int i1max,int i1Rmax,int i2max) 
{
	return i1 - frame - int((i1max-2*frame-i1Rmax)*i2/float(i2max-1)+0.5);
}
int DlmCommon::j1RIndex(int j2, int j1, int frame,int j1max,int j1Rmax,int j2max) 
{
	return j1 - frame - int((j1max-2*frame-j1Rmax)*j2/float(j2max-1)+0.5);
}