/*
  for ducumentation see eigenfield.h

Author:      Raimundo Sierra
             www.rsierra.com

Copyright:   Copyright (c) 2000 Raimundo Sierra. All rights reserved.
LICENSE:     This program is free software; you can redistribute it and/or modify
             it under the terms of the GNU General Public License as published by
             the Free Software Foundation; either version 2 of the License, or
             (at your option) any later version.

             This program is distributed in the hope that it will be useful,
             but WITHOUT ANY WARRANTY; without even the implied warranty of
             MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
             GNU General Public License for more details.

             You should have received a copy of the GNU General Public License
             along with this program; if not, write to the Free Software
             Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include "eigenfield.h"

eigenfield::eigenfield(tensorfield &t, int offset[3], int window[3])
{
  cout << "Initializing eigenfield from tensorfield\t" << flush;
  _dimx   = window[0];
  _dimy   = window[1];
  _dimz   = window[2];
  _field  = new eigen[_dimx*_dimy*_dimz];
  t.getDelta( _deltax, _deltay, _deltaz);
  cout << "done\n" << flush;

  check_boundaries(offset, window);
  int ktmp, jtmp;
  cout << "Calculating eigenvalues and vectors "<<flush;
  for(int k=offset[2]; k < offset[2]+window[2]; k++){
    ktmp = k*_dimx*_dimy;
    for(int j=offset[1]; j < offset[1]+window[1]; j++){
      jtmp = j*_dimx + ktmp;
      for(int i=offset[0]; i < offset[0]+window[0]; i++){
	_field[i+jtmp].tensor2eigen((*t.get(i, j, k)));
      }
    }
  }  
  cout << "\t\tdone\n"<<flush;
}

eigenfield::eigenfield(char path[], const int dimx=256, const int dimy=256, const int dimz=1)
{
  int length = strlen(path);
  if(path[length-3] =='x' && path[length-2] =='m' && path[length-1] =='l'){
    cout << "Initializing eigenfield from xml-file\t\t"<< flush;
    this->read_XMLHeader(path);
    _field = new eigen[_dimx*_dimy*_dimz];
    cout << "done\n" << flush;
    length = strlen(_datafile);
    char *datapath = new char[length+5];
    strcpy(datapath, _datafile);
    datapath[length] = '.';
    datapath[length+1] = 'e';
    datapath[length+2] = 'i';
    datapath[length+3] = 'g';
    datapath[length+4] = '\0';
    //cout <<datapath<<endl;
    this->load(datapath);
    delete[] datapath;
  }
  else if (path[length-3] =='e' && path[length-2] =='i' && path[length-1] =='g'){
    cout << "Initializing eigenfield from eigenfile\t\t" << flush;
    _dimx = dimx;
    _dimy = dimy;
    _dimz = dimz;
    _deltax = _deltay = _deltaz = 1;
    _field = new eigen[_dimx*_dimy*_dimz];
    cout << "done\n" << flush;
    this->load(path);
  }
  else {
    cout << "Initializing eigenfield from raw data\t\t" << flush;
    _dimx = dimx;
    _dimy = dimy;
    _dimz = dimz;
    _deltax = _deltay = _deltaz = 1;
    _field = new eigen[_dimx*_dimy*_dimz];
    cout << "done\n" << flush;
    char tmp[length + 4];
    char number[4];
    _sourcefile = new char[length];
    strcpy(_sourcefile, path);
    
    for(int i=1; i<=_dimz; i++){
      if(i<100){
	number[0] = char(i/10+48);
	number[1] = char(i%10+48);
	number[2] = '\0';
      }
      else if(i<1000){
	number[0] = char(i/100+48);
	number[1] = char((i/10)%10+48);
	number[2] = char(i%100%10+48);
	number[3] = '\0';
      }
      else {
	cerr << "Too many slices, cannot process in tensorfield::tensorfield(char, int, int, int)\nEXIT\n";
	exit(0);
      }
      strcat(strcpy(tmp, path), number);
      read_eigenslice(tmp, (i-1));
    }
  }
}

void
eigenfield::read_eigenslice(char path[], const int slice_no = 0)
{
  short *lam1 = new short[_dimx*_dimy];
  short *lam2 = new short[_dimx*_dimy];
  short *lam3 = new short[_dimx*_dimy];
  short *le1x = new short[_dimx*_dimy];
  short *le2x = new short[_dimx*_dimy];
  short *le3x = new short[_dimx*_dimy];
  short *le1y = new short[_dimx*_dimy];
  short *le2y = new short[_dimx*_dimy];
  short *le3y = new short[_dimx*_dimy];
  short *le1z = new short[_dimx*_dimy];
  short *le2z = new short[_dimx*_dimy];
  short *le3z = new short[_dimx*_dimy];

  cout <<"Deriving eigensystem for slice " << slice_no << flush;

  char tmp[strlen(path)+8];

  strcat(strcpy(tmp, path), "L1e.pic");
  lam1 = read_pic(lam1, tmp);
  strcat(strcpy(tmp, path), "L2e.pic");
  lam2 = read_pic(lam2, tmp);
  strcat(strcpy(tmp, path), "L3e.pic");
  lam3 = read_pic(lam3, tmp);
  strcat(strcpy(tmp, path), "L1x.pic");
  le1x = read_pic(le1x, tmp);
  strcat(strcpy(tmp, path), "L2x.pic");
  le2x = read_pic(le2x, tmp);
  strcat(strcpy(tmp, path), "L3x.pic");
  le3x = read_pic(le3x, tmp);
  strcat(strcpy(tmp, path), "L1y.pic");
  le1y = read_pic(le1y, tmp);
  strcat(strcpy(tmp, path), "L2y.pic");
  le2y = read_pic(le2y, tmp);
  strcat(strcpy(tmp, path), "L3y.pic");
  le3y = read_pic(le3y, tmp);
  strcat(strcpy(tmp, path), "L1z.pic");
  le1z = read_pic(le1z, tmp);
  strcat(strcpy(tmp, path), "L2z.pic");
  le2z = read_pic(le2z, tmp);
  strcat(strcpy(tmp, path), "L3z.pic");
  le3z = read_pic(le3z, tmp);

  double normfactor1, normfactor2, normfactor3;
  int pos;
  
  float eigenvector[9];
  float eigenvector1[3];
  float eigenvector2[3];
  float eigenvector3[3];
  int lambda[3];

  int vectororder[3];

  for(int j=0; j < _dimx; j++){
    for(int i=0; i < _dimy; i++){ 
      // normalize eigenvectors, this is normfactor^2!!
      // since normfactor itself is not needed
      pos = i+j*_dimx;
      normfactor1 =  double( le1x[pos]*le1x[pos] ) + double( le1y[pos]*le1y[pos] ) + double( le1z[pos]*le1z[pos]); 
      normfactor2 =  double( le2x[pos]*le2x[pos] ) + double( le2y[pos]*le2y[pos] ) + double( le2z[pos]*le2z[pos]); 
      normfactor3 =  double( le3x[pos]*le3x[pos] ) + double( le3y[pos]*le3y[pos] ) + double( le3z[pos]*le3z[pos]);

      if(normfactor1!=0){
	eigenvector[0] = double(le1x[pos])/sqrt(normfactor1);
	eigenvector[1] = double(le1y[pos])/sqrt(normfactor1);
	eigenvector[2] = double(le1z[pos])/sqrt(normfactor1);
      } else {
	eigenvector[0] = eigenvector[1] = eigenvector[2] = 0;
      }

      if(normfactor2!=0){
	eigenvector[3] = double(le2x[pos])/sqrt(normfactor2);
	eigenvector[4] = double(le2y[pos])/sqrt(normfactor2);
	eigenvector[5] = double(le2z[pos])/sqrt(normfactor2);      
      } else {
	eigenvector[3] = eigenvector[4] = eigenvector[5] = 0;
      }

      if(normfactor3!=0){
	eigenvector[6] = double(le3x[pos])/sqrt(normfactor3);
	eigenvector[7] = double(le3y[pos])/sqrt(normfactor3);
	eigenvector[8] = double(le3z[pos])/sqrt(normfactor3); 
      } else {
	eigenvector[6] = eigenvector[7] = eigenvector[8] = 0;
      }

      /*
      if(pos%100==0 && lam1[pos]!=0) {
	cout << "Before rearrenging: lam1 "<<lam1[pos]<<", lam2 "<<lam2[pos]<<", lam3 "<<lam3[pos]<<"\n";
	cout << eigenvector[0] <<"\t"<< eigenvector[3] <<"\t"<< eigenvector[6] <<"\n";
	cout << eigenvector[1] <<"\t"<< eigenvector[4] <<"\t"<< eigenvector[7] <<"\n";
	cout << eigenvector[2] <<"\t"<< eigenvector[5] <<"\t"<< eigenvector[8] <<"\n";
      }
      */
      // it seems not necessary to rearrange, but not tested for all examples, so sorting kept

      lambda[0] = lam1[pos];
      lambda[1] = lam2[pos];
      lambda[2] = lam3[pos];      
      vectororder[0] = 0; 
      vectororder[1] = 1; 
      vectororder[2] = 2;
	
      QuickSort(lambda, 0, 2, vectororder);

      /*
      if(vectororder[0]!=0 || vectororder[1]!=1 || vectororder[2]!=2){
	cout << "Rearranged position"<<pos<<endl;
      }
      */

      eigenvector1[0] = eigenvector[vectororder[0]*3];
      eigenvector1[1] = eigenvector[vectororder[0]*3 +1];
      eigenvector1[2] = eigenvector[vectororder[0]*3 +2];

      eigenvector2[0] = eigenvector[vectororder[1]*3];
      eigenvector2[1] = eigenvector[vectororder[1]*3 +1];
      eigenvector2[2] = eigenvector[vectororder[1]*3 +2];

      eigenvector3[0] = eigenvector[vectororder[2]*3];
      eigenvector3[1] = eigenvector[vectororder[2]*3 +1];
      eigenvector3[2] = eigenvector[vectororder[2]*3 +2];

      /*
      if(pos%100==0 && lam1[pos]!=0) {
	cout << "After rearrenging: lam1 "<<lambda[0]<<", lam2 "<<lambda[1]<<", lam3 "<<lambda[2]<<"\n";
	cout << eigenvector1[0] <<"\t"<< eigenvector2[0] <<"\t"<< eigenvector3[0] <<"\n";
	cout << eigenvector1[1] <<"\t"<< eigenvector2[1] <<"\t"<< eigenvector3[1] <<"\n";
	cout << eigenvector1[2] <<"\t"<< eigenvector2[2] <<"\t"<< eigenvector3[2] <<"\n\n";
      }
      */

      if(lambda[0]!=0 && normfactor1!=0){  // some positions lambda[0] have values but no direction
	_field[pos + slice_no*_dimx*_dimy].set(lambda[0], lambda[1], lambda[2], eigenvector1, eigenvector2, eigenvector3); 
      } else {
	_field[pos + slice_no*_dimx*_dimy].setZero();
      }
    }
  } 
  cout <<"\t\tdone\n" << flush;
  delete[] lam1; delete[] lam2; delete[] lam3; 
  delete[] le1x; delete[] le1y; delete[] le1z; 
  delete[] le2x; delete[] le2y; delete[] le2z;
  delete[] le3x; delete[] le3y; delete[] le3z;

}

eigenfield::eigenfield(const int dimx=256, const int dimy=256, const int dimz = 1)
{
  if(dimx <= 0 || dimy <= 0 || dimz <= 0 ){
    cerr << "Initializing eigenfield of dimension 0 is not allowed\nEach dimension has to be >= 1\nEXIT\n";
    exit(0);
  }
  cout << "Initializing eigenfield of size ("<<dimx<<", "<<dimy<<", "<<dimz<<")\t" << flush;  
  _dimx   = dimx;
  _dimy   = dimy;
  _dimz   = dimz;
  _field  = new eigen[_dimx*_dimy*_dimz];
  _deltax = _deltay = _deltaz = 1.0;
  cout << "done\n" << flush;

}

eigenfield::~eigenfield()
{
  delete[] _field;
  _field = NULL;
}
  
void
eigenfield::remove_strips(const int inside=0)
{
  // take a 3*1 window and scan the field
  int   pos  =0;
  int   pos_k=0;
  int   pos_j=0;
  int   back =0;
  float euclid_0, euclid_1, euclid_2;
  if(inside==0){                                     // asumes only 1 object
    cout << "Removing strips outside object"<< flush;
    for(int k=0; k<_dimz; k++){
      pos_k = k*_dimx*_dimy;
      for(int j=0; j<_dimy; j++){
	pos_j = j*_dimx+pos_k;
	euclid_1 = _field[pos_j].value1();           // initiale values for euclid_1 and euclid_2
	euclid_2 = _field[pos_j+1].value1();
	for(int i=1; i<_dimx-1; i++){                // forward scan
	  pos = i+pos_j;
	  euclid_0 = euclid_1;                       // forward values
	  euclid_1 = euclid_2;
	  euclid_2 = _field[pos+1].value1();
	  if(euclid_0 == 0 && euclid_1 != 0 && euclid_2 == 0){
	    _field[pos].setZero();
	  } else if(euclid_0 == 0 && euclid_1 != 0 && euclid_2 != 0){ 
	                                             // inside object -> have to do backward scan
	    back = 1;
	    break;                                   // stop forward scan
	  }
	}       
	if(back==1){
	  euclid_1 = _field[pos_j+255].value1();     // initiale values for euclid_0 and euclid_1
	  euclid_0 = _field[pos_j+254].value1();	  
	  for(int i=_dimx-2; i>0; i--){              // backward scan
	    pos = i+pos_j;
	    euclid_2 = euclid_1;                     // forward values
	    euclid_1 = euclid_0;
	    euclid_0 = _field[pos-1].value1();
	    if(euclid_0 == 0 && euclid_1 != 0 && euclid_2 == 0){
	      _field[pos].setZero();
	    } else if(euclid_2 == 0 && euclid_1 != 0 && euclid_0 != 0){  
	                                             // inside object
	      break;                                 // stop backward scan
	    }
	  }
	  back = 0;
	}
      }
    }
  } else {
    cout << "Removing strips in- and outside object"<< flush;
    for(int k=0; k<_dimz; k++){
      pos_k = k*_dimx*_dimy;      
      for(int j=0; j<_dimy; j++){
	pos_j = j*_dimx+pos_k;
	euclid_1 = _field[pos_j].value1();           // initiale values for euclid_1 and euclid_2
	euclid_2 = _field[pos_j+1].value1();
	for(int i=1; i<_dimx-1; i++){
	  pos = i+pos_j;
	  euclid_0 = euclid_1;            
	  euclid_1 = euclid_2;
	  euclid_2 = _field[pos+1].value1();	
	  if(euclid_0 == 0 && euclid_1 != 0 && euclid_2 == 0){
	    _field[pos].setZero();
	  }
	}
      }
    }
  }
  cout << "\t\tdone\n"<< flush;
}
                                         
eigen*
eigenfield::get(const int x, const int y, const int z=0) const
{  
  if( x >= _dimx || y >= _dimy || z >= _dimz || x < 0 || y < 0 || z < 0 ){
    cerr << "ERROR: Requesting tensor out of field\nExit\n";
    exit(0);
  }
  return &_field[x+y*_dimx+z*_dimx*_dimy];
}
  
eigen* 
eigenfield::get(const int position) const                                        
{
  if(position>=_dimx*_dimy*_dimz || position<0){
    cerr << "ERROR: Requesting eigensystem out of field\nExit\n";
    exit(0);
  }
  return &_field[position];
}

void 
eigenfield::set(const eigen e, const int x, const int y, const int z=0)
{
  if( x>=_dimx || y>=_dimy || z>=_dimz || x < 0 || y < 0 || z < 0){
    cout << "Setting eigensystem out of field, do nothing\n";
    return;
  }
  _field[x+y*_dimx+z*_dimx*_dimy] = e;
}

void 
eigenfield::set(const eigen e, const int position)
{
  if( position>=_dimx*_dimy*_dimz || position < 0 ){
    cout << "Setting eigensystem out of field, do nothing\n";
    return;
  }
  _field[position] = e;
}

void 
eigenfield::setVectors(const float *vec1, const float *vec2, const float *vec3, const int position)
{
  if( position>=_dimx*_dimy*_dimz || position < 0 ){
    cout << "Setting eigensystem out of field, do nothing\n";
    return;
  }
  _field[position].setVectors(vec1, vec2, vec3);
}

void
eigenfield::load(const char path[])
{
  cout << "Reading eigenfield from disc"<<flush;
  FILE *in_file;
  in_file = fopen(path, "r");
  if(in_file == NULL) {
    cerr << "\n\tCannot open " << path << "\tno data loaded into the eigenfield\n";
    return;
  } else {
    fseek(in_file, 0, 0);
    fread(_field ,sizeof(eigen), _dimx*_dimy*_dimz, in_file);
  }
  fclose(in_file);  
  cout << "\t\t\tdone\n"<<flush;
}

void
eigenfield::save(const char path[], int offset[3]=NULL, int windowsize[3]=NULL)
{
  cout << "Writing eigenfield to disc"<<flush;
  int length = strlen(path);
  char * filename = new char[length+5];
  char * xmlpath  = new char[length+5];
  _datafile       = new char[length+5];
  strcpy(filename, path);
  strcpy(xmlpath,  path);
  strcpy(_datafile,path);
  if(path[length-3] =='x' && path[length-2] =='m' && path[length-1] =='l'){ //  xmlfile
    filename[length-3] = 'e';
    filename[length-2] = 'i';
    filename[length-1] = 'g';
    _datafile[length-4]= '\0';
  }
  else if(path[length-3] =='t' && path[length-2] =='e' && path[length-1] =='n'){// tensorfile
    xmlpath[length-3] = 'x';
    xmlpath[length-2] = 'm';
    xmlpath[length-1] = 'l'; 
    _datafile[length-4]= '\0';    
  }
  else { // no extensions => add extensions for tensorfile (not needed for xmlfile
    filename[length]   = '.';
    filename[length+1] = 'e';
    filename[length+2] = 'i';
    filename[length+3] = 'g';
    filename[length+4] = '\0';
  }
  eigen *tmp_ptr;
  FILE *out_file;
  out_file = fopen(filename, "w");
  if(out_file == NULL){
    cerr << "\nERROR: Cannot open " << filename << "\tExit\n";
    exit(0);
  }    
  if(offset!=NULL || windowsize!=NULL){   // write only a window starting from offset
    check_boundaries(offset, windowsize);
    for(int z=0; z<windowsize[2]; z++){
      for(int y=0; y<windowsize[1]; y++){
	tmp_ptr = &_field[offset[0]+(y+offset[1])*_dimx+(z+offset[2])*_dimx*_dimy];
	fwrite(tmp_ptr, sizeof(eigen), windowsize[0], out_file);
      }
    }
  }
  else {                                  // write all data
    fwrite(_field, sizeof(eigen), _dimx*_dimy*_dimz, out_file);
  }
  fclose(out_file);
  this->write_XMLHeader(path);
  cout <<"\t\t\tdone\n"<<flush;
  delete[] filename;
  delete[] xmlpath;
}

void
eigenfield::eigen2tensor(tensorfield &t) const
{
  cout << "Converting to tensorfield from eigenfield\t" << flush;
  tensor tmp;
  int ktmp, jtmp;
  for(int k=0; k<_dimz; k++){
    ktmp = k*_dimx*_dimy;
    for(int j=0; j<_dimy; j++){
      jtmp = ktmp + j*_dimx;
      for(int i=0; i<_dimx; i++){
	_field[i+jtmp].eigen2tensor(tmp);
	t.set(tmp, i, j, k);
      }
    }
  }
  cout << "done\n"<<flush;
}
 
void 
eigenfield::tensor2eigen(tensorfield &t)
{
  cout << "Converting to eigenfield from tensorfield\t" << flush;

  int errorcounter = 0;
  int tmp          = 0;
  int ktmp, jtmp;
  for(int k=0; k < _dimz; k++){
    ktmp = k*_dimx*_dimy;
    for(int j=0; j < _dimy; j++){
      jtmp = j*_dimx + ktmp;
      for(int i=0; i < _dimx; i++){
	tmp = _field[i+jtmp].tensor2eigen((*t.get(i, j, k)));
	if(tmp<0){
	  errorcounter++;
	}
	if(errorcounter>10){
	  cout << "WARNING: Too many errors, stopping conversion\n";
	  cout << "Last tensor had values:\n";
	  cout << *t.get(i, j, k);
	  return;
	}
      }
    }
  }  
  cout << "done\n"<<flush;
}
 
int* 
eigenfield::structure(int* labels, const float tolerance = 0.1, const float threshold = 0.8, const int similarity = 0)
{  // tolerance in similarity; threshold in threshold()

  cout << "Deriving labelmap in structure()\t\t"<<flush;
  labels = new int[_dimx*_dimy*_dimz];
  int ktmp, jtmp, position;
  int neighbour_labels, neighbour;
  int lastlabel = 2;                                       // label 1 used for restgroup
                                                           // label 0 used for background
  for(int k=0; k<_dimx*_dimy*_dimz; k++){
    labels[k] = 0;                                         // background
  }
  for(int k=0; k<_dimz; k++){                              // all z-dim
    ktmp = k*_dimx*_dimy;
    for(int j=1; j<_dimy-1; j++){                          // y-dim without border
      jtmp = ktmp + j*_dimx;
      for(int i=1; i<_dimx-1; i++){                        // x-dim without border
	position = i+jtmp;
	if(!_field[position].isZero()){                    // position is not background

	                                                   // check neighbourhood for labels
	  neighbour_labels = check_neighbours(position, labels, neighbour);

	  if(neighbour_labels == 1){                       // there is a label in neighbourhood, compare,
	                                                   // if similar set same label
	    if(_field[position].isSimilar(_field[neighbour], tolerance, similarity)){
	      labels[position] = labels[neighbour];
	    }
	    else{                                          // else = if different 
	      setLabel(labels, position, threshold, lastlabel);
	    }
	  }
	  else if(neighbour_labels > 1){                   // there are several labels
                                                 	   // if similar to only one then add to this group
	                                                   // if similar to several add to best group 
	                                                   // and merge groups (add label to similar-list)
	    if(_field[position].isSimilar(_field[position-1], tolerance, similarity)){
	      labels[position] = labels[position-1];
	    }
	    else if(_field[position].isSimilar(_field[position-1-_dimx], tolerance, similarity)){
	      labels[position] = labels[position-1-_dimx];
	    }
	    else if(_field[position].isSimilar(_field[position-_dimx], tolerance, similarity)){
	      labels[position] = labels[position-_dimx];
	    }	
	    else if(_field[position].isSimilar(_field[position+1-_dimx], tolerance, similarity)){
	      labels[position] = labels[position+1-_dimx];
	    }    
	   	    
	    else{                                          // else = if not similar to any other
	      setLabel(labels, position, threshold, lastlabel);
	    }
	  }
	  else{                                            // there is no label in neighbourhood

	    setLabel(labels, position, threshold, lastlabel);
	  }
	}
      }
    }
  }
  cout <<"done\n"<<flush;
  labels = this->reorder(labels, lastlabel);
  return labels;
}


int* 
eigenfield::reorder(int* labels, const int lastlabel) 
{
  cout << "Number of labels in labelmap is:        "<< lastlabel<<endl;
  int i=0;
  int volume = _dimx*_dimy*_dimz;
  int* histogram = new int[lastlabel+1];
  int* newlabels = new int[lastlabel+1];
  for(i=0; i<lastlabel+1; i++){                          // initialization
    histogram[i] = 0;
    newlabels[i] = i;
  }
  for(i=0; i<volume; i++){                               // create histogram
    histogram[labels[i]] = histogram[labels[i]]+1;
  }
#ifdef debug
  for(int t=0; t<30; t++){
    cout << "Initial histogram at position "<<t<<" is: "<<histogram[t]<<"\n";
  }
#endif

  // add: remove to small groups to make sort faster

  cout << "Start sorting\t\t\t\t\t"<< flush;
  QuickSort(histogram, 2, lastlabel, newlabels);

  cout << "done\nChanging values in labelmap\t\t\t"<<flush;
  for(i=0; i<volume; i++){
    if(labels[i]!=0 && labels[i]!=1){                    // background and restgroup
      labels[i] = newlabels[labels[i]];
    }
  }
  cout << "done\n"<<flush;
#ifdef debug
  for(int t=0; t<30; t++){
    cout << "Size of group with label "<<t<<" is: "<<histogram[t]<<";\tNewlabels: "<<newlabels[t]<<"\n";
  }
#endif
  delete[] histogram;
  delete[] newlabels;
  return labels;
}


int
eigenfield::check_neighbours(const int position, const int* labels, int& neighbour) const
{ // only look at the past
  int number_of_labels = 0;
  if(labels[position-1] > 1){            // before
    number_of_labels++;
    neighbour = position-1;
  }
  if(labels[position-1-_dimx] > 1){      // before and up
    number_of_labels++;
    neighbour = position-1-_dimx;
  }
  if(labels[position  -_dimx] > 1){      // up
    number_of_labels++;
    neighbour = position-_dimx;
  }
  if(labels[position+1-_dimx] > 1){      // up and after
    number_of_labels++;
    neighbour = position+1-_dimx;
  }
  return number_of_labels;
}
 
int* 
eigenfield::labelAll(int* labels, const float tolerance = 0.1, const float threshold = 0.8, const int similarity = 0)
{  // tolerance in similarity; threshold in threshold()
  /*
    neighbourhood only in past:

    A      B      C
    D   position  x
    x      x      x

   */
  cout << "Deriving labelmap in labelAll()\t\t\t"<<flush;
  labels = new int[_dimx*_dimy*_dimz];
  int ktmp, jtmp, position;
  int neighbour;
  int lastlabel = 2;                                       // label 1 used for restgroup
                                                           // label 0 used for background
  int A, B, C, D;
  for(int k=0; k<_dimx*_dimy*_dimz; k++){
    labels[k] = 0;                                         // background
  }
  for(int k=0; k<_dimz; k++){                              // all z-dim
    ktmp = k*_dimx*_dimy;
    for(int j=1; j<_dimy-1; j++){                          // y-dim without border
      jtmp = ktmp + j*_dimx;
      for(int i=1; i<_dimx-1; i++){                        // x-dim without border
	position = i+jtmp;
	if(!_field[position].isZero()){                    // position is not background
	  neighbour = neighbours(labels, position);
	  A = position-1-_dimx;
	  B = position  -_dimx;
	  C = position+1-_dimx;
	  D = position-1;
	  switch(neighbour){
	                                                  // no neighbour has label
	  case 0:  setLabel(labels, position, threshold, lastlabel);                         break;
	                                                  // one neighbour has label   
	  case 1:  setLabel1(labels, position, threshold, tolerance, lastlabel, A, similarity);          break;
	  case 2:  setLabel1(labels, position, threshold, tolerance, lastlabel, B, similarity);          break; 
	  case 4:  setLabel1(labels, position, threshold, tolerance, lastlabel, C, similarity);          break; 
	  case 8:  setLabel1(labels, position, threshold, tolerance, lastlabel, D, similarity);          break; 
	                                                  // two neighbour have label	
	  case 3:  setLabel2(labels, position, threshold, tolerance, lastlabel, A, B, similarity);       break; 
	  case 5:  setLabel2(labels, position, threshold, tolerance, lastlabel, A, C, similarity);       break;  
	  case 6:  setLabel2(labels, position, threshold, tolerance, lastlabel, B, C, similarity);       break;  
	  case 9:  setLabel2(labels, position, threshold, tolerance, lastlabel, A, D, similarity);       break; 
	  case 10: setLabel2(labels, position, threshold, tolerance, lastlabel, B, D, similarity);       break; 
	  case 12: setLabel2(labels, position, threshold, tolerance, lastlabel, C, D, similarity);       break;
	                                                  // three neighbour have label
	  case 7:  setLabel3(labels, position, threshold, tolerance, lastlabel, A, B, C, similarity);    break;
	  case 11: setLabel3(labels, position, threshold, tolerance, lastlabel, A, B, D, similarity);    break; 
	  case 13: setLabel3(labels, position, threshold, tolerance, lastlabel, A, C, D, similarity);    break;
	  case 14: setLabel3(labels, position, threshold, tolerance, lastlabel, B, C, D, similarity);    break;
	                                                  // all neighbour have label
	  case 15: setLabel4(labels, position, threshold, tolerance, lastlabel, A, B, C, D, similarity); break;
	  default: {
	      cout << "ERROR in labelAll()\nExit\n"; 
	      cout << "Neighbour is: "<<neighbour<<endl;
	      exit(0);
	    }
	  }
	}
      }
    }
  }
  cout << "done\n"<<flush;
  labels = this->reorder(labels, lastlabel);
  return labels;
}

int
eigenfield::neighbours(const int *labels, const int position) const
{
  /*
    neighbourhood only in past:

    A      B      C
    D   position  x
    x      x      x

                  label > 1:
    returnvalue:  D C B A

     0            0 0 0 0
     1            0 0 0 1
    ..            ...
    15            1 1 1 1

   */
  int returnvalue = 0;
  int A = position - 1 - _dimx;
  int B = position     - _dimx;
  int C = position + 1 - _dimx;
  int D = position - 1;
  if(labels[D]>1)  returnvalue = 8;
  if(labels[C]>1)  returnvalue = returnvalue + 4;
  if(labels[B]>1)  returnvalue = returnvalue + 2;
  if(labels[A]>1)  returnvalue = returnvalue + 1;

  return returnvalue;
}

void
eigenfield::setLabel(int* labels, const int &position, const float &threshold, int &lastlabel)
{
  if(_field[position].threshold(threshold)){               // if over threshold then create new label
    labels[position] = lastlabel;                          // and set to this label
    lastlabel++;
  }
  else {
    labels[position] = 1;                                  // below threshold => set to rest group
  }
}

void
eigenfield::setLabel1(int* labels, const int &position, const float &threshold, const float &tolerance, int &lastlabel, const int &A, const int &similarity)
{
  if (_field[position].isSimilar(_field[A], tolerance, similarity)) labels[position] = labels[A];
  else  setLabel(labels, position, threshold, lastlabel);	
}

void
eigenfield::setLabel2(int* labels, const int &position, const float &threshold, const float &tolerance, int &lastlabel, const int &A, const int &B, const int &similarity)
{ 
  if(_field[position].isSimilar(_field[A], tolerance, similarity) && _field[position].isSimilar(_field[B], tolerance, similarity)){
    if(labels[A] == labels[B])         labels[position] = labels[A]; 
    else {
      labels[position] = labels[A];
      merge2(labels, A, B, position, lastlabel);
    }
  }
  else if(_field[position].isSimilar(_field[A], tolerance, similarity)) labels[position] = labels[A];
  else if(_field[position].isSimilar(_field[B], tolerance, similarity)) labels[position] = labels[B];
  else setLabel(labels, position, threshold, lastlabel);
}

void
eigenfield::setLabel3(int* labels, const int &position, const float &threshold, const float &tolerance, int &lastlabel, const int &A, const int &B, const int &C, const int &similarity)
{
  if(_field[position].isSimilar(_field[A], tolerance, similarity) && 
     _field[position].isSimilar(_field[B], tolerance, similarity) &&
     _field[position].isSimilar(_field[C], tolerance, similarity)){       // similar to all 3
    labels[position] = labels[A]; 
    if(!(labels[A] == labels[B] && labels[B] == labels[C])) { // merge all
      merge3(labels, A, B, C, position, lastlabel);
    }
  }
  else if(_field[position].isSimilar(_field[A], tolerance, similarity) &&
	  _field[position].isSimilar(_field[B], tolerance, similarity)){  // similar to 2
    if(labels[A] == labels[B])         labels[position] = labels[A]; 
    else {
      labels[position] = labels[A];
      merge2(labels, A, B, position, lastlabel);
    }
  }    
  else if(_field[position].isSimilar(_field[A], tolerance, similarity) &&
	  _field[position].isSimilar(_field[C], tolerance, similarity)){  // similar to 2
    if(labels[A] == labels[C])         labels[position] = labels[A]; 
    else {
      labels[position] = labels[A];
      merge2(labels, A, C, position, lastlabel);
    }
  }    
  else if(_field[position].isSimilar(_field[B], tolerance, similarity) &&
	  _field[position].isSimilar(_field[C], tolerance, similarity)){  // similar to 2
    if(labels[B] == labels[C])         labels[position] = labels[B]; 
    else {
      labels[position] = labels[B];
      merge2(labels, B, C, position, lastlabel);
    }
  }
                                                             // similar to one
  else if(_field[position].isSimilar(_field[A], tolerance, similarity)) labels[position] = labels[A];
  else if(_field[position].isSimilar(_field[B], tolerance, similarity)) labels[position] = labels[B];
  else if(_field[position].isSimilar(_field[C], tolerance, similarity)) labels[position] = labels[C];
  else setLabel(labels, position, threshold, lastlabel);
}

void
eigenfield::setLabel4(int* labels, const int &position, const float &threshold, const float &tolerance, int &lastlabel, const int &A, const int &B, const int &C, const int &D, const int &similarity)
{
  if(_field[position].isSimilar(_field[A], tolerance, similarity) && 
     _field[position].isSimilar(_field[B], tolerance, similarity) &&
     _field[position].isSimilar(_field[C], tolerance, similarity) &&
     _field[position].isSimilar(_field[D], tolerance, similarity)){       // similar to all 4
    labels[position] = labels[A]; 
    if(!(labels[A] == labels[B] && labels[B] == labels[C] && labels[C] == labels[D])) {   // merge all
      merge4(labels, A, B, C, D, position, lastlabel);
    }
  }
  else if(_field[position].isSimilar(_field[A], tolerance, similarity) &&
	  _field[position].isSimilar(_field[B], tolerance, similarity) &&
	  _field[position].isSimilar(_field[C], tolerance, similarity)){  // similar to 3    A B C
    labels[position] = labels[A]; 
    if(!(labels[A] == labels[B] && labels[B] == labels[C])) { // merge all
      merge3(labels, A, B, C, position, lastlabel);
    }
  } 
  else if(_field[position].isSimilar(_field[A], tolerance, similarity) &&
	  _field[position].isSimilar(_field[C], tolerance, similarity) &&
	  _field[position].isSimilar(_field[D], tolerance, similarity)){  // similar to 3    A C D   
    labels[position] = labels[A]; 
    if(!(labels[A] == labels[C] && labels[C] == labels[D])) { // merge all
      merge3(labels, A, C, D, position, lastlabel);
    }
  }
  else if(_field[position].isSimilar(_field[A], tolerance, similarity) &&
	  _field[position].isSimilar(_field[B], tolerance, similarity) &&
	  _field[position].isSimilar(_field[D], tolerance, similarity)){  // similar to 3    A B D
    labels[position] = labels[A]; 
    if(!(labels[A] == labels[B] && labels[B] == labels[D])) { // merge all
      merge3(labels, A, B, D, position, lastlabel);
    }
  }
  else if(_field[position].isSimilar(_field[B], tolerance, similarity) &&
	  _field[position].isSimilar(_field[C], tolerance, similarity) &&
	  _field[position].isSimilar(_field[D], tolerance, similarity)){  // similar to 3    B C D
    labels[position] = labels[B]; 
    if(!(labels[B] == labels[C] && labels[C] == labels[D])) { // merge all
      merge3(labels, B, C, D, position, lastlabel);
    }
  }
  else if(_field[position].isSimilar(_field[A], tolerance, similarity) &&
	  _field[position].isSimilar(_field[B], tolerance, similarity)){  // similar to 2    A B
    if(labels[A] == labels[B])         labels[position] = labels[A]; 
    else {
      labels[position] = labels[A];
      merge2(labels, A, B, position, lastlabel);
    }
  }    
  else if(_field[position].isSimilar(_field[A], tolerance, similarity) &&
	  _field[position].isSimilar(_field[C], tolerance, similarity)){  // similar to 2    A C
    if(labels[A] == labels[C])         labels[position] = labels[A]; 
    else {
      labels[position] = labels[A];
      merge2(labels, A, C, position, lastlabel);
    }
  }    
  else if(_field[position].isSimilar(_field[A], tolerance, similarity) &&
	  _field[position].isSimilar(_field[D], tolerance, similarity)){  // similar to 2    A D
    if(labels[A] == labels[D])         labels[position] = labels[A]; 
    else {
      labels[position] = labels[A];
      merge2(labels, A, D, position, lastlabel);
    }
  }    
  else if(_field[position].isSimilar(_field[B], tolerance, similarity) &&
	  _field[position].isSimilar(_field[C], tolerance, similarity)){  // similar to 2    B C
    if(labels[B] == labels[C])         labels[position] = labels[B]; 
    else {
      labels[position] = labels[B];
      merge2(labels, B, C, position, lastlabel);
    }
  }    
  else if(_field[position].isSimilar(_field[B], tolerance, similarity) &&
	  _field[position].isSimilar(_field[D], tolerance, similarity)){  // similar to 2    B D
    if(labels[B] == labels[D])         labels[position] = labels[B]; 
    else {
      labels[position] = labels[B];
      merge2(labels, B, D, position, lastlabel);
    }
  }   
  else if(_field[position].isSimilar(_field[C], tolerance, similarity) &&
	  _field[position].isSimilar(_field[D], tolerance, similarity)){  // similar to 2    C D
    if(labels[C] == labels[D])         labels[position] = labels[C]; 
    else {
      labels[position] = labels[C];
      merge2(labels, C, D, position, lastlabel);
    }
  }
                                                         // similar to one
  else if(_field[position].isSimilar(_field[A], tolerance, similarity)) labels[position] = labels[A];
  else if(_field[position].isSimilar(_field[B], tolerance, similarity)) labels[position] = labels[B];
  else if(_field[position].isSimilar(_field[C], tolerance, similarity)) labels[position] = labels[C];
  else if(_field[position].isSimilar(_field[D], tolerance, similarity)) labels[position] = labels[D];
  else setLabel(labels, position, threshold, lastlabel);
}

int*
eigenfield::growLabel(int* labels, const float tolerance = 0.1, const float threshold = 0.8, const int similarity = 0)
{  // tolerance in similarity; threshold in threshold()

  cout << "Deriving labelmap in growLabel()\t\t"<<flush;
  labels = new int[_dimx*_dimy*_dimz];
  int ktmp, jtmp, position;
                                                           // label 0 used for background
  int lastlabel = 2;                                       // label 1 used for restgroup

  for(int k=0; k<_dimx*_dimy*_dimz; k++){
    labels[k] = 0;                                         // background
  }

  for(int k=0; k<_dimz; k++){                              // all z-dim
    ktmp = k*_dimx*_dimy;
    for(int j=1; j<_dimy-1; j++){                          // y-dim without border
      jtmp = ktmp + j*_dimx;
      for(int i=1; i<_dimx-1; i++){                        // x-dim without border
	position = i+jtmp;
	if(!_field[position].isZero()){                    // position is not background
	  if(labels[position]==0 && _field[position].threshold(threshold)){ // not yet visited
	    labels[position] = lastlabel;
	    lastlabel++;
	    grow(position, labels, tolerance, similarity); // grow this label recursively 
	  }
	  else if (labels[position]>=1){}                  // do nothing, there is a label (already visited)
	  else {                                           // set to restgroup since < threshold
	    labels[position] = 1;
	  }
	}
      }
    }
  }
  cout << "done\n"<<flush;
  labels = this->reorder(labels, lastlabel);
  return labels;
}


void 
eigenfield::grow(const int position, int *labels, const float& tolerance, const int &similarity)
{
  /*
    Neighbourhood in 2D:

    A         B         C
    D      position     E
    F         G         H

  */
  if(position >= _dimx+1 && position <= _dimx*_dimy*_dimz - _dimx-1){
    int A = position - 1 - _dimx;
    int B = position     - _dimx;
    int C = position + 1 - _dimx;
    int D = position - 1;
    int E = position + 1;
    int F = position - 1 + _dimx;
    int G = position     + _dimx;
    int H = position + 1 + _dimx;  
    
    if( _field[position].isSimilar(_field[A], tolerance, similarity) && labels[A]<=1 ) 
      { labels[A] = labels[position]; grow(A, labels, tolerance, similarity); }
    
    if( _field[position].isSimilar(_field[B], tolerance, similarity) && labels[B]<=1 ) 
      { labels[B] = labels[position]; grow(B, labels, tolerance, similarity); }
    
    if( _field[position].isSimilar(_field[C], tolerance, similarity) && labels[C]<=1 ) 
      { labels[C] = labels[position]; grow(C, labels, tolerance, similarity); }
    
    if( _field[position].isSimilar(_field[D], tolerance, similarity) && labels[D]<=1 ) 
      { labels[D] = labels[position]; grow(D, labels, tolerance, similarity); }
    
    if( _field[position].isSimilar(_field[E], tolerance, similarity) && labels[E]<=1 ) 
      { labels[E] = labels[position]; grow(E, labels, tolerance, similarity); }
    
    if( _field[position].isSimilar(_field[F], tolerance, similarity) && labels[F]<=1 ) 
      { labels[F] = labels[position]; grow(F, labels, tolerance, similarity); }
    
    if( _field[position].isSimilar(_field[G], tolerance, similarity) && labels[G]<=1 ) 
      { labels[G] = labels[position]; grow(G, labels, tolerance, similarity); }
    
    if( _field[position].isSimilar(_field[H], tolerance, similarity) && labels[H]<=1 ) 
      { labels[H] = labels[position]; grow(H, labels, tolerance, similarity); }
  }
}

void
eigenfield::merge2(int *labelmap, int labelA, int labelB, const int position, int &lastlabel)
{
  if(labelA == labelB){
    cout << "WARNING: nothing to be merged in merge2() in eigenfield.h, returning\n";
    return;
  }
  if(labelA > labelB) swap( labelA, labelB);
  // now labelA < labelB for sure.
  // for the past do:
  // keep labelA, decrease all labels above labelB and set labelB to labelA
  for( int i=0; i<=position; i++){
    if     (labelmap[i] >  labelB)  labelmap[i] = labelmap[i]-1;
    else if(labelmap[i] == labelB)  labelmap[i] = labelA;
    else {} // do nothing
  }
  lastlabel--;
}

void
eigenfield::merge3(int *labelmap, int labelA, int labelB, int labelC, const int position, int &lastlabel)
{
  if(labelA == labelB && labelA == labelC) return;
  if(labelA == labelB){
    merge2(labelmap, labelB, labelC, position, lastlabel);
    return;
  }
  else if(labelA == labelC){
    merge2(labelmap, labelB, labelC, position, lastlabel);
    return;
  }
  else if(labelB == labelC){
    merge2(labelmap, labelA, labelB, position, lastlabel);
    return;
  }

  sort(labelA, labelB, labelC);
  
  // now labelA < labelB < labelC for sure.
  // for the past do:
  // keep labelA, decrease all labels above labelB and set labelB to labelA
  for( int i=0; i<=position; i++){
    if     (labelmap[i] >  labelC)  labelmap[i] = labelmap[i]-2;
    else if(labelmap[i] == labelC)  labelmap[i] = labelA;
    else if(labelmap[i] >  labelB)  labelmap[i] = labelmap[i]-1;
    else if(labelmap[i] == labelB)  labelmap[i] = labelA;
    else {} // do nothing
  }
  lastlabel=lastlabel-2;
}

void
eigenfield::merge4(int *labelmap, int labelA, int labelB, int labelC, int labelD, const int position, int &lastlabel)
{
  if(labelA == labelB && labelA == labelC && labelA == labelD) return;
  if(labelA == labelB){
    merge3(labelmap, labelB, labelC, labelD, position, lastlabel);
    return;
  }
  else if(labelA == labelC){
    merge3(labelmap, labelB, labelC, labelD, position, lastlabel);
    return;
  }
  else if(labelA == labelD){
    merge3(labelmap, labelA, labelB, labelC, position, lastlabel);
    return;
  }
  else if(labelB == labelC){
    merge3(labelmap, labelA, labelC, labelD, position, lastlabel);
    return;
  }
  else if(labelB == labelD){
    merge3(labelmap, labelA, labelB, labelC, position, lastlabel);
    return;
  }
  else if(labelC == labelD){
    merge3(labelmap, labelA, labelB, labelC, position, lastlabel);
    return;
  }

  sort(labelA, labelB, labelC, labelD);
  
  // now labelA < labelB < labelC for sure.
  // for the past do:
  // keep labelA, decrease all labels above labelB and set labelB to labelA
  for( int i=0; i<=position; i++){
    if     (labelmap[i] >  labelD)  labelmap[i] = labelmap[i]-3;
    else if(labelmap[i] == labelD)  labelmap[i] = labelA;
    else if(labelmap[i] >  labelC)  labelmap[i] = labelmap[i]-2;
    else if(labelmap[i] == labelC)  labelmap[i] = labelA;
    else if(labelmap[i] >  labelB)  labelmap[i] = labelmap[i]-1;
    else if(labelmap[i] == labelB)  labelmap[i] = labelA;
    else {} // do nothing
  }
  lastlabel=lastlabel-3;
}

void 
eigenfield::median(const int filterlength)
{
  int ktmp, jtmp, position;

  int halffilter     = filterlength/2;
  float * lam1_values  = new float[filterlength];
  float * lam2_values  = new float[filterlength];
  float * lam3_values  = new float[filterlength];

  cout << "Applying medianfilter on eigenfield\t\t"<< flush;
  for(int k=0; k<_dimz; k++){
    ktmp = k*_dimx*_dimy;
    for(int j=0; j<_dimy; j++){
      jtmp = ktmp + j*_dimx;
      for(int i=halffilter; i<_dimx-halffilter; i++){
	if(!_field[i + jtmp].isZero()){
	  position = (i-halffilter) + jtmp;
	  for(int l=0; l<filterlength; l++){
	    lam1_values[l] = _field[position+l].value1();
	    lam2_values[l] = _field[position+l].value2();
	    lam3_values[l] = _field[position+l].value3();
	  }
	  QuickSort(lam1_values, 0, filterlength-1);
	  QuickSort(lam2_values, 0, filterlength-1);
	  QuickSort(lam3_values, 0, filterlength-1);
	  _field[position+halffilter].set(lam1_values[halffilter], lam2_values[halffilter], lam3_values[halffilter] );
	}
      }
    }
  }
  cout << "done\n"<<flush;
}

void
eigenfield::setZero()
{
  int ktmp, jtmp, position;

  cout << "Setting eigensystem to zero if any lambda==0\t"<< flush;
  for(int k=0; k<_dimz; k++){
    ktmp = k*_dimx*_dimy;
    for(int j=0; j<_dimy; j++){
      jtmp = ktmp + j*_dimx;
      for(int i=0; i<_dimx; i++){
	position = i+jtmp;
	if((_field[position].value1() * _field[position].value2() * _field[position].value3()) == 0){
	  _field[position].setZero();
	}/*
	norm1 = _field[position].vector1(0)*_field[position].vector1(0) + 
	        _field[position].vector1(1)*_field[position].vector1(1) + 
	        _field[position].vector1(2)*_field[position].vector1(2);
	norm2 = _field[position].vector2(0)*_field[position].vector2(0) + 
	        _field[position].vector2(1)*_field[position].vector2(1) + 
	        _field[position].vector2(2)*_field[position].vector2(2);
	norm3 = _field[position].vector3(0)*_field[position].vector3(0) + 
	        _field[position].vector3(1)*_field[position].vector3(1) + 
	        _field[position].vector3(2)*_field[position].vector3(2);
	if(norm1==0 || norm2==0 || norm3==0){
	  _field[position].setZero();
	}*/     
      }
    }
  }
  cout << "done\n"<<flush;
}

void 
eigenfield::smooth()
{
  /*
    Neighbourhood in 2D:

    A         B         C
    D      position     E
    F         G         H

    Gaussfilter for all components:
    
    1         2         1
    2         4         2        : 16
    1         2         1

  */
  int A, B, C, D, E, F, G, H;
  int ktmp, jtmp, position;
  float lambda1, lambda2, lambda3;

  cout << "Smoothing eigenfield \t\t\t\t"<< flush;
  for(int k=0; k<_dimz; k++){
    ktmp = k*_dimx*_dimy;
    for(int j=1; j<_dimy-1; j++){
      jtmp = ktmp + j*_dimx;
      for(int i=1; i<_dimx-1; i++){
	position = i+jtmp;
	if(!_field[position].isZero()){

	  A = position - 1 - _dimx;
	  B = position     - _dimx;
	  C = position + 1 - _dimx;
	  D = position - 1;
	  E = position + 1;
	  F = position - 1 + _dimx;
	  G = position     + _dimx;
	  H = position + 1 + _dimx; 
	  
	  lambda1 = _field[A].value1() + 2*_field[B].value1() + _field[C].value1() +
	    2*_field[D].value1() + 4*_field[position].value1() + 2*_field[E].value1() +
	    _field[F].value1() + 2*_field[G].value1() + _field[H].value1();
	  lambda2 = _field[A].value2() + 2*_field[B].value2() + _field[C].value2() +
	    2*_field[D].value2() + 4*_field[position].value2() + 2*_field[E].value2() +
	    _field[F].value2() + 2*_field[G].value2() + _field[H].value2();
	  lambda3 = _field[A].value3() + 2*_field[B].value3() + _field[C].value3() +
	    2*_field[D].value3() + 4*_field[position].value3() + 2*_field[E].value3() +
	    _field[F].value3() + 2*_field[G].value3() + _field[H].value3();

	  _field[position].set(lambda1/16, lambda2/16, lambda3/16);

	}
      }
    }
  }
  cout << "done\n"<<flush;
}



void 
eigenfield::measure2vtkScalars(int offset[3], int window[3], vtkScalars *scalars, tensorfield &t)  
{
  int pos;
  float tmp;
  check_boundaries(offset, window);
  scalars->Allocate( window[0]*window[1]*window[2]);

  for(int k=0; k<window[2]; k++){
    for(int j=0; j<window[1]; j++){
      for(int i=0; i<window[0]; i++){
	pos = (i+offset[0]) + (j+offset[1])*_dimx + (k+offset[2])*_dimx*_dimy;

	if(_field[pos].close_line()>=_field[pos].close_plane() &&
	   _field[pos].close_line()>=_field[pos].close_sphere()) tmp = 2;
	else if(_field[pos].close_plane()>=_field[pos].close_line() &&
		_field[pos].close_plane()>=_field[pos].close_sphere()) tmp = 1;
	else tmp = 0;
	/*
	if     (_field[pos].close_line()>0.6 ||
		_field[pos].close_plane()>0.9)   tmp = 2; 
	else if(_field[pos].close_sphere()<0.5)  tmp = 1;
	else {
	  tmp=0;
	  //(*t.get(pos)).setZero();
	}
	*/
	scalars->InsertNextScalar(tmp);
      }
    }
  }
}

//------------------------------------

void 
eigenfield::correctVectors()
{
  double e1x, e1y, e1z, e2x, e2y, e2z, e3x, e3y, e3z;
  double alpha, scale;
#ifdef debug
  this->check();
#endif
  int    count  = 0;
  double error2 = 0;
  double error3 = 0;

  cout << "Correcting eigenvectors\t\t\t\t"<< flush;
  for(int i=0; i<_dimx*_dimy*_dimz; i++){
    if(!_field[i].isZero()){
      e1x = _field[i].vector1(0);
      e1y = _field[i].vector1(1);
      e1z = _field[i].vector1(2);
      e2x = _field[i].vector2(0);
      e2y = _field[i].vector2(1);
      e2z = _field[i].vector2(2);
      //cout << "\ne1_b = ["<<e1x<<"; "<<e1y<<"; "<<e1z<<"];\n";
      //cout <<   "e2_b = ["<<e2x<<"; "<<e2y<<"; "<<e2z<<"];\n";

      alpha = - (e1x*e2x + e1y*e2y + e1z*e2z) / (e1x*e1x + e1y*e1y + e1z*e1z);

      e2x = alpha*e1x + e2x;
      e2y = alpha*e1y + e2y;
      e2z = alpha*e1z + e2z;
      
      scale = sqrt(e2x*e2x + e2y*e2y + e2z*e2z);

      e3x = _field[i].vector3(0);
      e3y = _field[i].vector3(1);
      e3z = _field[i].vector3(2);
      
      _field[i].setVector2(float(e2x/scale), float(e2y/scale), float(e2z/scale));
      _field[i].resetVector3();
      
      count++;
      //cout << "e2_a = ["<<_field[i].vector2(0)<<"; "<<_field[i].vector2(1)<<"; "<<_field[i].vector2(2)<<"];\n";
      //cout << "e3_b = ["<<e3x<<"; "<<e3y<<"; "<<e3z<<"];\n";
      //cout << "e3_a = ["<<_field[i].vector3(0)<<"; "<<_field[i].vector3(1)<<"; "<<_field[i].vector3(2)<<"];\n";
      //cout << "scale is: "<<scale<<endl;
      
      error3 = error3 + sqrt((_field[i].vector3(0)-e3x)*(_field[i].vector3(0)-e3x)+
			     (_field[i].vector3(1)-e3y)*(_field[i].vector3(1)-e3y)+
			     (_field[i].vector3(2)-e3z)*(_field[i].vector3(2)-e3z));
      error2 = error2+ sqrt(alpha*alpha + (1-scale)*(1-scale));

    }
  }
  cout << "done\n"<<flush;
  cout << "Mean error of second eigenvector mean( |e2Before-e2After| ) : "<< error2/double(count)<<endl;
  cout << "Mean error of third  eigenvector mean( |e3Before-e3After| ) : "<< error3/double(count)<<endl;
#ifdef debug
  this->check();
#endif
}

void
eigenfield::check()
{
  double scalarprod12;
  double scalarprod23;
  double scalarprod13;
  double length1, length2, length3;
  double test;
  
  int count     = 0;
  int zerocount = 0;
  int similar   = 0;
  double max    = 0;

  tensor t_tmp;
  tensor inverse;
  eigen e_tmp;
  for(int i=0; i<_dimx*_dimy*_dimz; i++){
    if(!_field[i].isZero()){
      scalarprod12 = _field[i].vector1(0)*_field[i].vector2(0) + 
	             _field[i].vector1(1)*_field[i].vector2(1) +
	             _field[i].vector1(2)*_field[i].vector2(2);
      scalarprod13 = _field[i].vector1(0)*_field[i].vector3(0) + 
	             _field[i].vector1(1)*_field[i].vector3(1) +
	             _field[i].vector1(2)*_field[i].vector3(2);
      scalarprod23 = _field[i].vector2(0)*_field[i].vector3(0) + 
	             _field[i].vector2(1)*_field[i].vector3(1) +
	             _field[i].vector2(2)*_field[i].vector3(2);
      
      length1 = sqrt(_field[i].vector1(0)*_field[i].vector1(0) + 
	        _field[i].vector1(1)*_field[i].vector1(1) +
	        _field[i].vector1(2)*_field[i].vector1(2));
      length2 = sqrt(_field[i].vector2(0)*_field[i].vector2(0) + 
	        _field[i].vector2(1)*_field[i].vector2(1) +
	        _field[i].vector2(2)*_field[i].vector2(2));
      length3 = sqrt(_field[i].vector3(0)*_field[i].vector3(0) + 
	        _field[i].vector3(1)*_field[i].vector3(1) +
	        _field[i].vector3(2)*_field[i].vector3(2));

      test = scalarprod12 + scalarprod13 + scalarprod23;
      
      
      
      if(abs(test)>max)  max = test;
      if(abs(test)>0.01){
	count++;
#ifdef debug	
	if(count%250==0){
	  cout << "\n-------------------------------------------------------\n\n";
	  cout << _field[i];
	  _field[i].eigen2tensor(t_tmp);
	  cout << t_tmp;
	  e_tmp.tensor2eigen(t_tmp);
	  cout << e_tmp;
	  e_tmp.eigen2tensor(t_tmp);
	  cout << t_tmp;
	  
	  float pi= 3.1416;
	  cout <<"Angles are: (lam1-lam2) "<< acos(scalarprod12/length1/length2)*180/pi << ",   (lam1-lam3) "<< acos(scalarprod13/length1/length3)*180/pi << ",   (lam2-lam3) "<< acos(scalarprod23/length3/length2)*180/pi<<endl;
	}
#endif	
      }
      else similar++;
    }
    else zerocount++;
  }
  cout << endl;
  cout << "Count of vectors that are in 90 deg:     " << similar<<endl;
  cout << "Count of vectors that are not in 90 deg: " << count<<endl;
  cout << "The largest value was:                   " << max<<endl;
  cout << "Number of zero's:                        " << zerocount <<endl;
  cout << endl;
}

void 
eigenfield::mask(background &bg)
{
  for(int i=0; i<_dimx*_dimy*_dimz; i++){
    if(bg.isZero(i)) _field[i].setZero();
  }
}

float *
eigenfield::neighbours_direction(float *results)
{  /*
    Neighbourhood in 2D:

    A         B         C
    D      position     E
    F         G         H

  */
  int A, B, C, D, E, F, G, H;
  int ktmp, jtmp, position;

  for(int k=0; k<_dimz; k++){
    ktmp = k*_dimx*_dimy;
    for(int j=1; j<_dimy-1; j++){
      jtmp = ktmp + j*_dimx;
      for(int i=1; i<_dimx-1; i++){
	position = i+jtmp;
	if(!_field[position].isZero()){
	  A = position - 1 - _dimx;
	  B = position     - _dimx;
	  C = position + 1 - _dimx;
	  D = position - 1;
	  E = position + 1;
	  F = position - 1 + _dimx;
	  G = position     + _dimx;
	  H = position + 1 + _dimx; 	
	  results[position] = _field[position].cosinMaxEigen(_field[A]) +
	     	   	      _field[position].cosinMaxEigen(_field[B]) +
	     	   	      _field[position].cosinMaxEigen(_field[C]) +
	     	   	      _field[position].cosinMaxEigen(_field[D]) +
	     	   	      _field[position].cosinMaxEigen(_field[E]) +
	     	   	      _field[position].cosinMaxEigen(_field[F]) +
	     	   	      _field[position].cosinMaxEigen(_field[G]) +
	     	   	      _field[position].cosinMaxEigen(_field[H]) + 10;
	}
	else results[position] = 0;
      }
    }
  }
  return results;
}

void 
eigenfield::scale( const float scalex, const float scaley, const float scalez)
{
  for(int i=0; i<_dimx*_dimy*_dimz; i++){
    if(!_field[i].isZero()){
      _field[i].scale(scalex, scaley, scalez);
    }
  }
}

void 
eigenfield::rotate(const float pitch, const float roll, const float yaw)
{
  for(int i=0; i<_dimx*_dimy*_dimz; i++){
    if(!_field[i].isZero()){
      _field[i].rotate(pitch, roll, yaw);
    }
  }
}