/*
  for ducumentation see tensorfield.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 "tensorfield.h"

#ifdef debug
  int fieldcounter=0;           // counts calls to constructors
  int fielddestructorcounter=0; // counts calls to destructors
#endif

//constructors
//tensorfield::tensorfield()  // not needed sinse tensorfield( int, int, int) defaults to 256, 256, 1.


tensorfield::tensorfield(const 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'){ // reading xml-header
    cout << "Initializing tensorfield from xml-file\t\t"<< flush;
    this->read_XMLHeader(path);
    _field = new tensor[_dimx*_dimy*_dimz];
    cout << "done\n" << flush;
    length = strlen(_datafile);
    char *datapath = new char[length+5];
    strcpy(datapath, _datafile);
    datapath[length] = '.';
    datapath[length+1] = 't';
    datapath[length+2] = 'e';
    datapath[length+3] = 'n';
    datapath[length+4] = '\0';
    //cout <<datapath<<endl;
    this->load(datapath);
    return;
  }
  cout << "Initializing tensorfield \t\t\t" << flush;                      // reading raw data
  _dimx = dimx;
  _dimy = dimy;
  _dimz = dimz;
  _deltax = _deltay = _deltaz = 1;
  _field = new tensor[_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_tensorslice(tmp, (i-1));
  }
#ifdef debug
  fieldcounter++;
  cout << "tensorcounter is: "<<tensorcounter<<endl;
#endif
}

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

tensorfield::~tensorfield()
{
  delete[] _field;
  _field = NULL;
#ifdef debug
  fielddestructorcounter++;
  cout << "fielddestructorcounter is: "<<fielddestructorcounter<<endl;
  cout << "tensordestructorcounter is: "<<tensordestructorcounter<<endl;
#endif
}

tensor* 
tensorfield::get(const int x, const int y, const int z=0)
{
  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];
}

tensor*
tensorfield::get(const int position)
{
  if( position>=_dimx*_dimy*_dimz || position < 0 ){
    cerr << "ERROR: Requesting tensor out of field\nExit\n";
    exit(0);
  }
  return &_field[position];
}

void 
tensorfield::separate(float * array1, float * array2, float * array3, float * array4, float * array5, float * array6)
{
  for(int i=0; i<_dimx*_dimy*_dimz; i++){
    array1[i] = _field[i].get(0,0);
    array2[i] = _field[i].get(1,1);
    array3[i] = _field[i].get(2,2);
    array4[i] = _field[i].get(0,1);
    array5[i] = _field[i].get(0,2);
    array6[i] = _field[i].get(1,2);
  }
}

void 
tensorfield::merge(float * array1, float * array2, float * array3, float * array4, float * array5, float * array6)
{
  for(int i=0; i<_dimx*_dimy*_dimz; i++){
    _field[i].set(array1[i], array2[i], array3[i], array4[i], array5[i], array6[i]);
  }
}

void 
tensorfield::merge(float * array1, float * array2, float * array3, float * array4, float * array5, float * array6, float * array7, float * array8, float * array9)
{
  for(int i=0; i<_dimx*_dimy*_dimz; i++){
    _field[i].set(array1[i], array2[i], array3[i], array4[i], array5[i], array6[i], array7[i], array8[i], array9[i]);
  }
}

void 
tensorfield::set(const tensor t, 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 tensor out of field, do nothing\n";
    return;
  }
  _field[x+y*_dimx+z*_dimx*_dimy] = t;
}

void
tensorfield::save(const char path[], int offset[3]=NULL, int windowsize[3]=NULL)
{
  cout << "Writing tensorfield 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] = 't';
    filename[length-2] = 'e';
    filename[length-1] = 'n';
    _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] = 't';
    filename[length+2] = 'e';
    filename[length+3] = 'n';
    filename[length+4] = '\0';
  }
  tensor *tmp_ptr;
  FILE *out_file;
  out_file = fopen(filename, "w");
  if(out_file == NULL){
    cout << "\n\t Cannot open file "<<filename<< " for writing \tReturning without writing\n";
    return;
  }    
  if(offset!=NULL || windowsize!=NULL){
    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(tensor), windowsize[0], out_file);
      }
    }
  }
  else {
    fwrite(_field, sizeof(tensor), _dimx*_dimy*_dimz, out_file);
  }
  fclose(out_file);
  //this->write_XMLHeader(xmlpath);
  delete[] filename;
  delete[] xmlpath;
  cout <<"\t\t\tdone\n"<<flush;
}

void
tensorfield::print(const int x, const int y, const int z = 0) const
{
  if( x >= _dimx || y >= _dimy || z >= _dimz || x < 0 || y < 0 || z < 0){
    cout << "Requesting tensor out of field, do nothing\n";
    return;
  }
  cout <<"Tensor at position ("<<x<<", "<<y<<", "<<z<<") is: \n"<< _field[x+y*_dimx+z*_dimx*_dimy];
}

void
tensorfield::load(const char path[])
{
  cout << "Reading tensorfield from disc"<<flush;
  FILE *in_file;
  in_file = fopen(path, "r");
  if(in_file == NULL) {
    cerr << "\nERROR: Cannot open " << path << "\tExit\n";
    exit(0);
  } else {
    fseek(in_file, 0, 0);
    fread(_field ,sizeof(tensor), _dimx*_dimy*_dimz, in_file);
  }
  fclose(in_file);  
  cout << "\t\t\tdone\n"<<flush;
}
/*
short* 
tensorfield::read_pic(short *image, char path_file[], int offset=0)
{
  FILE *in_file;
  in_file = fopen(path_file, "r");
  if(in_file == NULL) {
    cerr << "\nERROR: Cannot open " << path_file << "\tExit\n";
    exit(0);
  } else {
    fseek(in_file, offset, 0);
    fread(image,sizeof(short),_dimx*_dimy,in_file);
  }
  fclose(in_file);
  return image;
}
*/
void
tensorfield::read_tensorslice(const 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 tensors 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;
  double e1x, e1y, e1z;
  double e2x, e2y, e2z;
  double e3x, e3y, e3z;
  int pos;

  tensor lam, vec, vec_inverse;
  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] + le1y[pos]*le1y[pos] + le1z[pos]*le1z[pos]); 
      normfactor2 =  double( le2x[pos]*le2x[pos] + le2y[pos]*le2y[pos] + le2z[pos]*le2z[pos]); 
      normfactor3 =  double( le3x[pos]*le3x[pos] + le3y[pos]*le3y[pos] + le3z[pos]*le3z[pos]);

      if(normfactor1!=0){
	e1x = double(le1x[pos])/sqrt(normfactor1);
	e1y = double(le1y[pos])/sqrt(normfactor1);
	e1z = double(le1z[pos])/sqrt(normfactor1);
      } else {
	e1x = e1y = e1z = 0;
      }

      if(normfactor2!=0){
	e2x = double(le2x[pos])/sqrt(normfactor2);
	e2y = double(le2y[pos])/sqrt(normfactor2);
	e2z = double(le2z[pos])/sqrt(normfactor2);      
      } else {
	e2x = e2y = e2z = 0;
      }

      if(normfactor3!=0){
	e3x = double(le3x[pos])/sqrt(normfactor3);
	e3y = double(le3y[pos])/sqrt(normfactor3);
	e3z = double(le3z[pos])/sqrt(normfactor3); 
      } else {
	e3x = e3y = e3z = 0;
      }
      vec.set(e1x, e2x, e3x, e1y, e2y, e3y, e1z, e2z, e3z);
      if(vec.det()!=0){
	lam.set(lam1[pos], 0, 0, 0, lam2[pos], 0, 0, 0, lam3[pos]);
	vec.inv(vec_inverse); 
	_field[pos + slice_no*_dimx*_dimy] = vec*lam*vec_inverse;
      } else {
	_field[pos + slice_no*_dimx*_dimy].setZero();
      }
    }
  } 
  cout <<"\t\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;
}

/*
void
tensorfield::read_tensorslice(const 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 tensors 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);

  float t1, t2, t3, t4, t5, t6;
  double normfactor1, normfactor2, normfactor3;
  double t1xx, t2xx, t3xx;
  double t1yy, t2yy, t3yy;
  double t1zz, t2zz, t3zz;
  double t1xy, t2xy, t3xy;
  double t1xz, t2xz, t3xz;
  double t1yz, t2yz, t3yz;
  int pos;

  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] + le1y[pos]*le1y[pos] + le1z[pos]*le1z[pos]); 
      normfactor2 =  double( le2x[pos]*le2x[pos] + le2y[pos]*le2y[pos] + le2z[pos]*le2z[pos]); 
      normfactor3 =  double( le3x[pos]*le3x[pos] + le3y[pos]*le3y[pos] + le3z[pos]*le3z[pos]);

      if(normfactor1!=0){
	t1xx = double(le1x[pos]*le1x[pos]) / normfactor1;
	t1yy = double(le1y[pos]*le1y[pos]) / normfactor1;
	t1zz = double(le1z[pos]*le1z[pos]) / normfactor1;
	t1xy = double(le1x[pos]*le1y[pos]) / normfactor1;
	t1xz = double(le1x[pos]*le1z[pos]) / normfactor1;
	t1yz = double(le1y[pos]*le1z[pos]) / normfactor1;
	*/ /*
	if(t1xx>1 || t1yy>1 || t1zz>1 || t1xx<0 || t1yy<0 || t1zz<0 || 
	   t1xy>1 || t1xz>1 || t1yz>1 || t1xy<-1 || t1xz<-1 || t1yz<-1){
	  cout <<"tensorsize wrong\n";
	}
	  */ /*
      } else {
	t1xx = t1yy = t1zz = t1xy = t1xz = t1yz = 0;
      }

      if(normfactor2!=0){      
	t2xx = double(le2x[pos]*le2x[pos]) / normfactor2;
	t2yy = double(le2y[pos]*le2y[pos]) / normfactor2;
	t2zz = double(le2z[pos]*le2z[pos]) / normfactor2;
	t2xy = double(le2x[pos]*le2y[pos]) / normfactor2;
	t2xz = double(le2x[pos]*le2z[pos]) / normfactor2;
	t2yz = double(le2y[pos]*le2z[pos]) / normfactor2;
      } else {
	t2xx = t2yy = t2zz = t2xy = t2xz = t2yz = 0;
      }

      if(normfactor3!=0){
	t3xx = double(le3x[pos]*le3x[pos]) / normfactor3;
	t3yy = double(le3y[pos]*le3y[pos]) / normfactor3;
	t3zz = double(le3z[pos]*le3z[pos]) / normfactor3;
	t3xy = double(le3x[pos]*le3y[pos]) / normfactor3;
	t3xz = double(le3x[pos]*le3z[pos]) / normfactor3;
	t3yz = double(le3y[pos]*le3z[pos]) / normfactor3;
      } else {
	t3xx = t3yy = t3zz = t3xy = t3xz = t3yz = 0;
      }

      t1 = float( double(lam1[pos]) * t1xx + double(lam2[pos]) * t2xx + double(lam3[pos]) * t3xx);
      t2 = float( double(lam1[pos]) * t1yy + double(lam2[pos]) * t2yy + double(lam3[pos]) * t3yy);
      t3 = float( double(lam1[pos]) * t1zz + double(lam2[pos]) * t2zz + double(lam3[pos]) * t3zz);
      t4 = float( double(lam1[pos]) * t1xy + double(lam2[pos]) * t2xy + double(lam3[pos]) * t3xy);
      t5 = float( double(lam1[pos]) * t1xz + double(lam2[pos]) * t2xz + double(lam3[pos]) * t3xz);
      t6 = float( double(lam1[pos]) * t1yz + double(lam2[pos]) * t2yz + double(lam3[pos]) * t3yz);

      _field[pos + slice_no*_dimx*_dimy].set(t1, t2, t3, t4, t5, t6);
    }
  } 
  cout <<"\t\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;
}
*/
void
tensorfield::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].euclid();           // initiale values for euclid_1 and euclid_2
	euclid_2 = _field[pos_j+1].euclid();
	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].euclid();
	  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].euclid();     // initiale values for euclid_0 and euclid_1
	  euclid_0 = _field[pos_j+254].euclid();	  
	  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].euclid();
	    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].euclid();           // initiale values for euclid_1 and euclid_2
	euclid_2 = _field[pos_j+1].euclid();
	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].euclid();	
	  if(euclid_0 == 0 && euclid_1 != 0 && euclid_2 == 0){
	    _field[pos].setZero();
	  }
	}
      }
    }
  }
  cout << "\t\tdone\n"<< flush;
}

void 
tensorfield::smooth(const int filterlength=3)
{
  cout << "Smoothing tensorfield with filterlength "<<filterlength<<"\t"<< flush;
  float * a = new float[_dimx* _dimy* _dimz];
  float * b = new float[_dimx* _dimy* _dimz];
  float * c = new float[_dimx* _dimy* _dimz];
  float * d = new float[_dimx* _dimy* _dimz];
  float * e = new float[_dimx* _dimy* _dimz];
  float * f = new float[_dimx* _dimy* _dimz];

  this->separate(a, b, c, d, e, f);
  GaussSmoothFloatArray(a, _dimx, _dimy, _dimz, filterlength);
  GaussSmoothFloatArray(b, _dimx, _dimy, _dimz, filterlength);
  GaussSmoothFloatArray(c, _dimx, _dimy, _dimz, filterlength);
  GaussSmoothFloatArray(d, _dimx, _dimy, _dimz, filterlength);
  GaussSmoothFloatArray(e, _dimx, _dimy, _dimz, filterlength);
  GaussSmoothFloatArray(f, _dimx, _dimy, _dimz, filterlength);
  this->merge(a, b, c, d, e, f);
  
  delete[] a;
  delete[] b;
  delete[] c;
  delete[] d;
  delete[] e;
  delete[] f;

  cout << "done\n"<<flush;
}
  
//tensorfield * 
void
tensorfield::downsampleSlices(tensorfield &smallfield, const int factor=2, const int smoothlength=3)
{
  int dimx, dimy, dimz;
  smallfield.getDimension(dimx, dimy, dimz);
  if(dimx*factor!=_dimx || dimy*factor!=_dimy || dimz!=_dimz){
    cout << "WARNING: In tensorfield.h downsampleSlices(): \n"
      "Dimensions of smallfield and factor don't match with data \n"
      "Returning smallfield unchanged\n";
    //return &smallfield;
  }

  this->smooth(smoothlength);
  for(int k=0; k<_dimz; k++){
    for(int j=0; j<_dimy; j++){
      for(int i=0; i<_dimx; i++){
	if(i%factor==0 && j%factor==0){
	  smallfield.set(_field[i+j*_dimx+k*_dimx*_dimy], i/factor, j/factor, k);
	  //cout << (*smallfield.get(x/factor, y/factor, k));
	}
      }
    }
  }
  //return &smallfield;
}
  
void
tensorfield::trace(float * array) const
{  
  for(int i=0; i<_dimx*_dimy*_dimz; i++){
    array[i] = _field[i].trace();
  }
}
  
void
tensorfield::det(float * array) const
{  
  for(int i=0; i<_dimx*_dimy*_dimz; i++){
    array[i] = _field[i].det();
  }
}

void 
tensorfield::roundnessCF(float * pointarray, const float thresh=0.01)
{
  float maxtrace = 0;
  for(int i=0; i<_dimx*_dimy*_dimz; i++){
    if(fabs(_field[i].trace()) > maxtrace) maxtrace = fabs(_field[i].trace());
    pointarray[i] = 0;
    /*
    if(_field[i].det()!=0){
      cout << "Field position "<<i<<" has det "<<_field[i].det()<<" and trace "<<_field[i].trace()<<endl;
    }
    */
  }
  if(_dimz==1){
    for(int i=0; i<_dimx*_dimy*_dimz; i++){
      pointarray[i] = 2*sqrt(fabs(_field[i].det()))/(fabs(_field[i].trace()) + thresh*maxtrace);
    }
  }
  else {
    for(int i=0; i<_dimx*_dimy*_dimz; i++){
      pointarray[i] = 3*pow(_field[i].det(), 1.0/3.0)/(_field[i].trace() + thresh*maxtrace);
    }
  }
}
  
void 
tensorfield::roundness(float * pointarray)
{
  for(int i=0; i<_dimx*_dimy*_dimz; i++){
    pointarray[i] = _field[i].det()/_field[i].trace();
  }
}

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

void
tensorfield::tensorField2vtkField(const int offset[3], const int window[3], vtkTensors * newTensors) const
{
  vtkTensor  *tensor                = vtkTensor::New();
  newTensors->Allocate( window[0]*window[1]*window[2]);

  int pos = 0;
  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;
	tensor->SetComponent(0,0, _field[pos].get(0,0));
	tensor->SetComponent(0,1, _field[pos].get(0,1));
	tensor->SetComponent(0,2, _field[pos].get(0,2));
	tensor->SetComponent(1,0, _field[pos].get(1,0));
	tensor->SetComponent(1,1, _field[pos].get(1,1));
	tensor->SetComponent(1,2, _field[pos].get(1,2));
	tensor->SetComponent(2,0, _field[pos].get(2,0));
	tensor->SetComponent(2,1, _field[pos].get(2,1));
	tensor->SetComponent(2,2, _field[pos].get(2,2));
	newTensors->InsertNextTensor(tensor);
      }
    }
  }
  tensor->Delete();
}

void 
tensorfield::det2vtkScalars(const int offset[3], const int window[3], vtkScalars *newScalars) const
{
  newScalars->Allocate( window[0]*window[1]*window[2]);

  float tmp;
  int pos = 0;
  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;
	tmp = _field[pos].det();
	newScalars->InsertNextScalar(tmp);
      }
    }
  }
}