/* 

for documentation see tensorfunctions.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 "tensorfunctions.h"

void creatOuterProdDerivative(float * inarray, tensorfield & correlationMatrix, const int dimx=256, const int dimy=256, const int dimz=1, const int neighbourhood=0)
{
  int volume    = dimx*dimy*dimz;
  float * dxT1  = new float[volume];
  float * dyT1  = new float[volume];
  float * dzT1  = new float[volume];
  float * dxxT1 = new float[volume];
  float * dyxT1 = new float[volume];
  float * dyyT1 = new float[volume];
  float * dzxT1 = new float[volume];
  float * dzyT1 = new float[volume];
  float * dzzT1 = new float[volume];

  deriveFloatArray(inarray, dxT1, dyT1, dzT1, dimx, dimy, dimz);

  for(int i=0; i<volume; i++){
    dxxT1[i] = dxT1[i]*dxT1[i];
    dyxT1[i] = dxT1[i]*dyT1[i];
    dyyT1[i] = dyT1[i]*dyT1[i];
    dzxT1[i] = dzT1[i]*dxT1[i];
    dzyT1[i] = dzT1[i]*dyT1[i];
    dzzT1[i] = dzT1[i]*dzT1[i];
  }
  if(dimz==1){  // we have a 2D volume, setting dzzT1 to 1 will make roundness = det()/trace() 2 dimensional
    for(int i=0; i<volume; i++){
      dzzT1[i] = 1;
    } 
  }

  if(neighbourhood>1){
    /*
      GaussSmoothFloatArray( dxxT1, dimx, dimy, dimz, neighbourhood);
      GaussSmoothFloatArray( dyxT1, dimx, dimy, dimz, neighbourhood);
      GaussSmoothFloatArray( dyyT1, dimx, dimy, dimz, neighbourhood);
      GaussSmoothFloatArray( dzxT1, dimx, dimy, dimz, neighbourhood);
      GaussSmoothFloatArray( dzyT1, dimx, dimy, dimz, neighbourhood);
      GaussSmoothFloatArray( dzzT1, dimx, dimy, dimz, neighbourhood);
    */
    MeanFloatArray( dxxT1, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dyxT1, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dyyT1, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dzxT1, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dzyT1, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dzzT1, dimx, dimy, dimz, neighbourhood);
  }

  correlationMatrix.merge(dxxT1, dyyT1, dzzT1, dyxT1, dzxT1, dzyT1);
  
  delete[] dxT1;
  delete[] dyT1;
  delete[] dzT1; 
  delete[] dxxT1;
  delete[] dyxT1;
  delete[] dyyT1;
  delete[] dzxT1;
  delete[] dzyT1;
  delete[] dzzT1;
}

void findPointsInFloatArray(float * inarray, float * pointarray, const int dimx=256, const int dimy=256, const int dimz=1, const int neighbourhood=3, const float sigmathresh=0.01, const int pointmethod=0)
{
  int volume    = dimx*dimy*dimz;
  float * dxT1  = new float[volume];
  float * dyT1  = new float[volume];
  float * dzT1  = new float[volume];
  float * dxxT1 = new float[volume];
  float * dyxT1 = new float[volume];
  float * dyyT1 = new float[volume];
  float * dzxT1 = new float[volume];
  float * dzyT1 = new float[volume];
  float * dzzT1 = new float[volume];

  for(int i=0; i<volume; i++){
    dxT1[i]  = 0;
    dyT1[i]  = 0;
    dzT1[i]  = 0;
    dxxT1[i] = 0;
    dyxT1[i] = 0;
    dyxT1[i] = 0;
    dyyT1[i] = 0;
    dzxT1[i] = 0;
    dzyT1[i] = 0;
    dzzT1[i] = 0;
  }

  deriveFloatArray(inarray, dxT1, dyT1, dzT1, dimx, dimy, dimz);

  for(int i=0; i<volume; i++){
    dxxT1[i] = dxT1[i]*dxT1[i];
    dyxT1[i] = dxT1[i]*dyT1[i];
    dyyT1[i] = dyT1[i]*dyT1[i];
    dzxT1[i] = dzT1[i]*dxT1[i];
    dzyT1[i] = dzT1[i]*dyT1[i];
    dzzT1[i] = dzT1[i]*dzT1[i];
    pointarray[i] = 0; 
  }
  if(dimz==1){  // we have a 2D volume, setting dzzT1 to 1 will make roundness = det()/trace() 2 dimensional
    for(int i=0; i<volume; i++){
      dzzT1[i] = 1;
    } 
  }
  
  tensorfield correlationMatrix(dimx, dimy, dimz); // not a real tensorfield but a tensorfield structure
                                                   // usefull for correlationmatrix computation
  correlationMatrix.merge(dxxT1, dyyT1, dzzT1, dyxT1, dzxT1, dzyT1);
  //display disp(dimx, dimy, dimz);
  //char a;
  //int offset[3] = {0,0,0};
  //int window[3] = {dimx, dimy, dimz};

  //correlationMatrix.trace(pointarray);
  correlationMatrix.trace(dyT1);
  //cout << "Press any key to end findPointsInFloatArray() "; cin >> a;
  //return;
  //for(int i=0; i<dimx*dimy*dimz; i++){
    //dyT1[i] = dyT1[i]-1;
    //pointarray[i] = pointarray[i] - 1;
  //}
  //return;
  //disp.greyarray(dyT1, dimx, dimy, dimz);

  if(pointmethod==2){
    correlationMatrix.trace(pointarray);
    cout << "Pointmethod is trace = |gradient|\n";
  }
  else if(pointmethod==3){
    correlationMatrix.trace(pointarray);
    localMax(pointarray, dimx, dimy, dimz, 2);
    cout << "Pointmethod is trace = |gradient| and applies local maximas in small neighbourhood\n";
  }
  else if(pointmethod==4){
    correlationMatrix.trace(pointarray);
    localMax(pointarray, dimx, dimy, dimz, 3);
    cout << "Pointmethod is trace = |gradient| and applies local maximas in 3 by 3  neighbourhood\n";
  }
  else if(pointmethod<2){
    /*
      GaussSmoothFloatArray( dxxT1, dimx, dimy, dimz, neighbourhood);
      GaussSmoothFloatArray( dyxT1, dimx, dimy, dimz, neighbourhood);
      GaussSmoothFloatArray( dyyT1, dimx, dimy, dimz, neighbourhood);
      GaussSmoothFloatArray( dzxT1, dimx, dimy, dimz, neighbourhood);
      GaussSmoothFloatArray( dzyT1, dimx, dimy, dimz, neighbourhood);
      GaussSmoothFloatArray( dzzT1, dimx, dimy, dimz, neighbourhood);
    */
    MeanFloatArray( dxxT1, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dyxT1, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dyyT1, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dzxT1, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dzyT1, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dzzT1, dimx, dimy, dimz, neighbourhood);
  
    correlationMatrix.merge(dxxT1, dyyT1, dzzT1, dyxT1, dzxT1, dzyT1);
    correlationMatrix.det(dxT1);
    //disp.greyarray(dxT1, dimx, dimy, dimz);
    localMax(dxT1, dimx, dimy, dimz);
    //disp.greyarray(dxT1, dimx, dimy, dimz);
    //disp.trace(correlationMatrix, offset, window);
    if     (pointmethod==1)  {
      correlationMatrix.roundness(pointarray);
      cout << "Pointmethod is det/trace\n";
    }
    else if(pointmethod==0)  {
      correlationMatrix.roundnessCF(pointarray, sigmathresh);
      cout << "Pointmethod is [N*det^(1/N)]/[trace+sigma*maxtrace]\n";
      cout << "Where sigma is: "<<sigmathresh<<endl;
    }
    //disp.greyarray(pointarray, dimx, dimy, dimz);
    maskFloatArray(pointarray, dyT1, dimx*dimy*dimz);
    //disp.greyarray(pointarray, dimx, dimy, dimz);
    localMax(pointarray, dimx, dimy, dimz);
  }
  else {
    correlationMatrix.roundnessCF(pointarray, sigmathresh);
    cout << "Pointmethod is [N*det^(1/N)]/[trace+sigma*maxtrace]\n";
    cout << "Where sigma is: "<<sigmathresh<<endl;
  }
  
  //float min, max;
  //findMinMax(pointarray, min, max, dimx*dimy*dimz);
  //cout <<"Minimum of selected points is: "<<min<<", max is: "<<max<<endl;

  /*
  for(int i=0; i<volume; i++){
    dzxT1[i] = dxxT1[i] + dyyT1[i];
    dzyT1[i] = dxxT1[i] * dyyT1[i];
  }
  int offset[3] = {0,0,0};
  int window[3] = {256,256,1};
  //disp.det(correlationMatrix, offset, window);
  disp.greyarray(dxxT1, dimx, dimy, dimz);
  disp.greyarray(dyxT1, dimx, dimy, dimz);
  disp.greyarray(dyyT1, dimx, dimy, dimz);
  disp.greyarray(dzxT1, dimx, dimy, dimz);
  disp.greyarray(dzyT1, dimx, dimy, dimz);
  //disp.greyarray(dzzT1, dimx, dimy, dimz);
  disp.greyarray(dxT1, dimx, dimy, dimz);
  disp.greyarray(dyT1, dimx, dimy, dimz);
  
  disp.greyarray(pointarray, dimx, dimy, dimz);
  
  cout << "Press any key to end findPointsInFloatArray() "; cin >> a;
  */
  //delete &correlationMatrix;
  //delete &disp;
  //delete[] offset;
  //delete[] window;
  
  delete[] dxT1;
  delete[] dyT1;
  delete[] dzT1; 
  delete[] dxxT1;
  delete[] dyxT1;
  delete[] dyyT1;
  delete[] dzxT1;
  delete[] dzyT1;
  delete[] dzzT1;
}

void findPointsInTensorfield(tensorfield * t_field_ptr, float * pointarray, const int neighbourhood=3, const float sigmathresh=0.01, const int pointmethod=0, const int searchwindowsize=7, const int movingwindowsize=7, const int matchmethod=0, const int matchwindowweighting=0)
{
  int x=0, y=0, z=0;
  t_field_ptr->getDimension(x, y, z);
  int volume                 = x*y*z;
  float * pointarray1        = new float[volume];
  float * pointarray2        = new float[volume];
  float * pointarray3        = new float[volume];
  float * pointarray4        = new float[volume];
  float * pointarray5        = new float[volume];
  float * pointarray6        = new float[volume];

  for(int i=0; i<volume; i++){
    pointarray1[i] = 0;
    pointarray2[i] = 0;
    pointarray3[i] = 0;
    pointarray4[i] = 0;
    pointarray5[i] = 0;
    pointarray6[i] = 0;
  }
  display display(x, y, z);

  float * Txx                = new float [x*y*z];
  float * Txy                = new float [x*y*z];
  float * Txz                = new float [x*y*z];
  float * Tyy                = new float [x*y*z];
  float * Tyz                = new float [x*y*z];
  float * Tzz                = new float [x*y*z];
  t_field_ptr->separate(Txx, Tyy, Tzz, Txy, Txz, Tyz);

  findPointsInFloatArray(Txx, pointarray1, x, y, z, neighbourhood, sigmathresh, pointmethod); 
  findPointsInFloatArray(Tyy, pointarray2, x, y, z, neighbourhood, sigmathresh, pointmethod); 
  findPointsInFloatArray(Tzz, pointarray3, x, y, z, neighbourhood, sigmathresh, pointmethod); 
  findPointsInFloatArray(Txy, pointarray4, x, y, z, neighbourhood, sigmathresh, pointmethod); 
  findPointsInFloatArray(Txz, pointarray5, x, y, z, neighbourhood, sigmathresh, pointmethod); 
  findPointsInFloatArray(Tyz, pointarray6, x, y, z, neighbourhood, sigmathresh, pointmethod); 

  display.greyarray(pointarray1, x, y, z);
  display.greyarray(pointarray2, x, y, z);
  display.greyarray(pointarray3, x, y, z);
  display.greyarray(pointarray4, x, y, z);
  display.greyarray(pointarray5, x, y, z);
  display.greyarray(pointarray6, x, y, z);

  for(int i=0; i<x*y*z; i++){
    pointarray[i] =  pointarray1[i] +  pointarray2[i] +  pointarray3[i] +  pointarray4[i] +  pointarray5[i] +  pointarray6[i];
  }

  display.greyarray(pointarray, x, y, z);

  float * synthdx            = new float [x*y*z];
  float * synthdy            = new float [x*y*z];
  float * synthdz            = new float [x*y*z];
  float * movedTxx           = new float [x*y*z];
  float * movedTxy           = new float [x*y*z];
  float * movedTxz           = new float [x*y*z];
  float * movedTyy           = new float [x*y*z];
  float * movedTyz           = new float [x*y*z];
  float * movedTzz           = new float [x*y*z];
  float ** tmpdx             = new float * [6];
  float ** tmpdy             = new float * [6];
  float ** tmpdz             = new float * [6];

  for(int i=0; i<6; i++){
   tmpdx[i] = new float  [x*y*z];
   tmpdy[i] = new float  [x*y*z];
   tmpdz[i] = new float  [x*y*z];
  }

  generateRandomDisplacement(synthdx, synthdy, synthdz, x, y, z, 5, 123456);
  interpolateKriging(synthdx, x, y, z, 1, 1, 1, 4, 0);
  interpolateKriging(synthdy, x, y, z, 1, 1, 1, 4, 0);

  displaceFloatArray(Txx, movedTxx, synthdx, synthdy, synthdz, x, y, z, 1, 1, 1);
  displaceFloatArray(Tyy, movedTyy, synthdx, synthdy, synthdz, x, y, z, 1, 1, 1);
  displaceFloatArray(Tzz, movedTzz, synthdx, synthdy, synthdz, x, y, z, 1, 1, 1);
  displaceFloatArray(Txy, movedTxy, synthdx, synthdy, synthdz, x, y, z, 1, 1, 1);
  displaceFloatArray(Txz, movedTxz, synthdx, synthdy, synthdz, x, y, z, 1, 1, 1);
  displaceFloatArray(Tyz, movedTyz, synthdx, synthdy, synthdz, x, y, z, 1, 1, 1);
  
  createSparseDisplacementField(movedTxx, Txx, pointarray1, tmpdx[0], tmpdy[0], tmpdz[0], x, y, z, searchwindowsize, movingwindowsize, matchmethod, matchwindowweighting);  
  createSparseDisplacementField(movedTyy, Tyy, pointarray2, tmpdx[1], tmpdy[1], tmpdz[1], x, y, z, searchwindowsize, movingwindowsize, matchmethod, matchwindowweighting);  
  createSparseDisplacementField(movedTzz, Tzz, pointarray3, tmpdx[2], tmpdy[2], tmpdz[2], x, y, z, searchwindowsize, movingwindowsize, matchmethod, matchwindowweighting);  
  createSparseDisplacementField(movedTxy, Txy, pointarray4, tmpdx[3], tmpdy[3], tmpdz[3], x, y, z, searchwindowsize, movingwindowsize, matchmethod, matchwindowweighting);  
  createSparseDisplacementField(movedTxz, Txz, pointarray5, tmpdx[4], tmpdy[4], tmpdz[4], x, y, z, searchwindowsize, movingwindowsize, matchmethod, matchwindowweighting);  
  createSparseDisplacementField(movedTyz, Tyz, pointarray6, tmpdx[5], tmpdy[5], tmpdz[5], x, y, z, searchwindowsize, movingwindowsize, matchmethod, matchwindowweighting); 
  for(int i=0; i<6; i++){
    invertValues(tmpdx[i], x*y*z);
    invertValues(tmpdy[i], x*y*z);
    interpolateKriging(tmpdx[i], x, y, z, 1, 1, 1, 9, 0);
    interpolateKriging(tmpdy[i], x, y, z, 1, 1, 1, 9, 0);
    display.greyarray( tmpdx[i], x, y, z);
    display.greyarray( tmpdy[i], x, y, z);
  }


  char stop;
  cin >> stop;

  delete[] pointarray1;
  delete[] pointarray2;
  delete[] pointarray3;
  delete[] pointarray4;
  delete[] pointarray5;
  delete[] pointarray6;

  delete[] Txx;
  delete[] Tyy;
  delete[] Tzz;
  delete[] Txy;
  delete[] Txz;
  delete[] Tzz;
}


void findPointsInTensorfield2(tensorfield * t_field_ptr, float * pointarray, const int neighbourhood=3, const float sigmathresh=0.01, const int pointmethod=0, const int searchwindowsize=7, const int movingwindowsize=7, const int matchmethod=0, const int matchwindowweighting=0)
{
  int dimx=0, dimy=0, dimz=0;
  t_field_ptr->getDimension(dimx, dimy, dimz);

  int volume                 = dimx*dimy*dimz;
  float * Txx                = new float[volume];
  float * Txy                = new float[volume];
  float * Txz                = new float[volume];
  float * Tyy                = new float[volume];
  float * Tyz                = new float[volume];
  float * Tzz                = new float[volume];


  float * dxTxx              = new float[volume];
  float * dyTxx              = new float[volume];
  float * dzTxx              = new float[volume];
  float * dxTyy              = new float[volume];
  float * dyTyy              = new float[volume];
  float * dzTyy              = new float[volume];

  float * dxTzz              = new float[volume];
  float * dyTzz              = new float[volume];
  float * dzTzz              = new float[volume];
  float * dxTxy              = new float[volume];
  float * dyTxy              = new float[volume];
  float * dzTxy              = new float[volume];

  float * dxTxz              = new float[volume];
  float * dyTxz              = new float[volume];
  float * dzTxz              = new float[volume];
  float * dxTyz              = new float[volume];
  float * dyTyz              = new float[volume];
  float * dzTyz              = new float[volume];


  float * dxxTxx              = new float[volume];
  float * dxyTxx              = new float[volume];
  float * dxzTxx              = new float[volume];
  float * dxxTyy              = new float[volume];
  float * dxyTyy              = new float[volume];
  float * dxzTyy              = new float[volume];

  float * dxxTzz              = new float[volume];
  float * dxyTzz              = new float[volume];
  float * dxzTzz              = new float[volume];
  float * dxxTxy              = new float[volume];
  float * dxyTxy              = new float[volume];
  float * dxzTxy              = new float[volume];

  float * dxxTxz              = new float[volume];
  float * dxyTxz              = new float[volume];
  float * dxzTxz              = new float[volume];
  float * dxxTyz              = new float[volume];
  float * dxyTyz              = new float[volume];
  float * dxzTyz              = new float[volume];


  float * dyyTxx              = new float[volume];
  float * dyzTxx              = new float[volume];
  float * dzzTxx              = new float[volume];
  float * dyyTyy              = new float[volume];
  float * dyzTyy              = new float[volume];
  float * dzzTyy              = new float[volume];

  float * dyyTzz              = new float[volume];
  float * dyzTzz              = new float[volume];
  float * dzzTzz              = new float[volume];
  float * dyyTxy              = new float[volume];
  float * dyzTxy              = new float[volume];
  float * dzzTxy              = new float[volume];

  float * dyyTxz              = new float[volume];
  float * dyzTxz              = new float[volume];
  float * dzzTxz              = new float[volume];
  float * dyyTyz              = new float[volume];
  float * dyzTyz              = new float[volume];
  float * dzzTyz              = new float[volume];


  t_field_ptr->separate(Txx, Tyy, Tzz, Txy, Txz, Tyz);
  deriveFloatArray(Txx, dxTxx, dyTxx, dzTxx, dimx, dimy, dimz);
  deriveFloatArray(Tyy, dxTyy, dyTyy, dzTyy, dimx, dimy, dimz);
  deriveFloatArray(Tzz, dxTzz, dyTzz, dzTzz, dimx, dimy, dimz);
  deriveFloatArray(Txy, dxTxy, dyTxy, dzTxy, dimx, dimy, dimz);
  deriveFloatArray(Txz, dxTxz, dyTxz, dzTxz, dimx, dimy, dimz);
  deriveFloatArray(Tyz, dxTyz, dyTyz, dzTyz, dimx, dimy, dimz);

  outerProduct(dxTxx, dyTxx, dzTxx, dxxTxx, dyyTxx, dzzTxx, dxyTxx, dxzTxx, dyzTxx, dimx, dimy, dimz);
  outerProduct(dxTyy, dyTyy, dzTyy, dxxTyy, dyyTyy, dzzTyy, dxyTyy, dxzTyy, dyzTyy, dimx, dimy, dimz);
  outerProduct(dxTzz, dyTzz, dzTzz, dxxTzz, dyyTzz, dzzTzz, dxyTzz, dxzTzz, dyzTzz, dimx, dimy, dimz);
  outerProduct(dxTxy, dyTxy, dzTxy, dxxTxy, dyyTxy, dzzTxy, dxyTxy, dxzTxy, dyzTxy, dimx, dimy, dimz);
  outerProduct(dxTxz, dyTxz, dzTxz, dxxTxz, dyyTxz, dzzTxz, dxyTxz, dxzTxz, dyzTxz, dimx, dimy, dimz);
  outerProduct(dxTyz, dyTyz, dzTyz, dxxTyz, dyyTyz, dzzTyz, dxyTyz, dxzTyz, dyzTyz, dimx, dimy, dimz);

  for(int i=0; i<volume; i++){ 
    // don't get confused by the names dxTxx.., used same arrays so I don't have to create more arrays.
    // Better names would be meanTxx, meanTyy etc
    dxTxx[i] = dxxTxx[i] + dxxTxy[i] + dxxTxz[i] + dxxTyy[i] + dxxTyz[i] + dxxTzz[i];
    dxTyy[i] = dyyTxx[i] + dyyTxy[i] + dyyTxz[i] + dyyTyy[i] + dyyTyz[i] + dyyTzz[i];
    dxTzz[i] = dzzTxx[i] + dzzTxy[i] + dzzTxz[i] + dzzTyy[i] + dzzTyz[i] + dzzTzz[i];
    dxTxy[i] = dxyTxx[i] + dxyTxy[i] + dxyTxz[i] + dxyTyy[i] + dxyTyz[i] + dxyTzz[i];
    dxTxz[i] = dxzTxx[i] + dxzTxy[i] + dxzTxz[i] + dxzTyy[i] + dxzTyz[i] + dxzTzz[i];
    dxTzz[i] = dzzTxx[i] + dzzTxy[i] + dzzTxz[i] + dzzTyy[i] + dzzTyz[i] + dzzTzz[i];
  }

  if(neighbourhood>1){
    MeanFloatArray( dxTxx, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dxTyy, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dxTzz, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dxTxy, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dxTxz, dimx, dimy, dimz, neighbourhood);
    MeanFloatArray( dxTyz, dimx, dimy, dimz, neighbourhood);
  }

  delete[] Txx   ;  delete[] Txy   ;  delete[] Txz   ;  delete[] Tyy   ;  delete[] Tyz   ;  delete[] Tzz   ;

  delete[] dxxTxx ;  delete[] dxyTxx ;  delete[] dxzTxx ;  delete[] dxxTyy ;  delete[] dxyTyy ;  delete[] dxzTyy ;  
  delete[] dxxTzz ;  delete[] dxyTzz ;  delete[] dxzTzz ;  delete[] dxxTxy ;  delete[] dxyTxy ;  delete[] dxzTxy ;  
  delete[] dxxTxz ;  delete[] dxyTxz ;  delete[] dxzTxz ;  delete[] dxxTyz ;  delete[] dxyTyz ;  delete[] dxzTyz ;
  delete[] dyyTxx ;  delete[] dyzTxx ;  delete[] dzzTxx ;  delete[] dyyTyy ;  delete[] dyzTyy ;  delete[] dzzTyy ;  
  delete[] dyyTzz ;  delete[] dyzTzz ;  delete[] dzzTzz ;  delete[] dyyTxy ;  delete[] dyzTxy ;  delete[] dzzTxy ;  
  delete[] dyyTxz ;  delete[] dyzTxz ;  delete[] dzzTxz ;  delete[] dyyTyz ;  delete[] dyzTyz ;  delete[] dzzTyz ;

  tensorfield correlationMatrix(dimx, dimy, dimz);
  correlationMatrix.merge(dxTxx, dxTyy, dxTzz, dxTxy, dxTxz, dxTyz);
  correlationMatrix.roundnessCF(pointarray, sigmathresh);


  delete[] dxTxx ;  delete[] dyTxx ;  delete[] dzTxx ;  delete[] dxTyy ;  delete[] dyTyy ;  delete[] dzTyy ;  
  delete[] dxTzz ;  delete[] dyTzz ;  delete[] dzTzz ;  delete[] dxTxy ;  delete[] dyTxy ;  delete[] dzTxy ;  
  delete[] dxTxz ;  delete[] dyTxz ;  delete[] dzTxz ;  delete[] dxTyz ;  delete[] dyTyz ;  delete[] dzTyz ;
}


void displaceTensorField(tensorfield * t_field_input_ptr, tensorfield * t_field_out_ptr, float * dx, float * dy, float * dz)
{
  cout << "Displacing tensorfield\t\t\t\t"<<flush;
  int dimx=0, dimy=0, dimz=0;
  float deltax=1.0, deltay=1.0, deltaz=1.0;
  t_field_input_ptr->getDimension(dimx, dimy, dimz);
  t_field_input_ptr->getDelta(deltax, deltay, deltaz);
  // for now: set deltas to 1
  deltax=1.0; deltay=1.0; deltaz=1.0;
  //

  int volume                 = dimx*dimy*dimz;
  float * Txx                = new float[volume];
  float * Txy                = new float[volume];
  float * Txz                = new float[volume];
  float * Tyy                = new float[volume];
  float * Tyz                = new float[volume];
  float * Tzz                = new float[volume];

  float * warpedTxx          = new float[volume];
  float * warpedTxy          = new float[volume];
  float * warpedTxz          = new float[volume];
  float * warpedTyy          = new float[volume];
  float * warpedTyz          = new float[volume];
  float * warpedTzz          = new float[volume];

  t_field_input_ptr->separate(Txx, Tyy, Tzz, Txy, Txz, Tyz); 

  displaceFloatArray(Txx, warpedTxx, dx, dy, dz, dimx, dimy, dimz, deltax, deltay, deltaz); 
  displaceFloatArray(Txy, warpedTxy, dx, dy, dz, dimx, dimy, dimz, deltax, deltay, deltaz);
  displaceFloatArray(Txz, warpedTxz, dx, dy, dz, dimx, dimy, dimz, deltax, deltay, deltaz);
  displaceFloatArray(Tyy, warpedTyy, dx, dy, dz, dimx, dimy, dimz, deltax, deltay, deltaz);
  displaceFloatArray(Tyz, warpedTyz, dx, dy, dz, dimx, dimy, dimz, deltax, deltay, deltaz);
  displaceFloatArray(Tzz, warpedTzz, dx, dy, dz, dimx, dimy, dimz, deltax, deltay, deltaz);   

  t_field_out_ptr->merge(warpedTxx, warpedTyy, warpedTzz, warpedTxy, warpedTxz, warpedTyz);

  delete[] Txx; delete[] Txy; delete[] Txz; delete[] Tyy; delete[] Tyz; delete[] Tzz;
  delete[] warpedTxx; delete[] warpedTxy; delete[] warpedTxz; delete[] warpedTyy; delete[] warpedTyz; delete[] warpedTzz;
  cout << "done\n"<<flush;
}


void localWarpTensorField(tensorfield * t_field_out_ptr, float * dx, float * dy, float * dz, const int allowScaling=0)
{
  if(allowScaling==0)         cout << "Localy rotating tensors WITHOUT any scaling\n"<<flush;
  else if(allowScaling==1)    cout << "Localy warping tensors WITHOUT scaling (det of transformation is 1)\n"<<flush;
  else if(allowScaling==2)    cout << "Localy warping tensors WITH scaling   \n"<<flush;

  int ktmp=0;
  int dimx=0, dimy=0, dimz=0;
  float deltax=1.0, deltay=1.0, deltaz=1.0;
  t_field_out_ptr->getDimension(dimx, dimy, dimz);
  t_field_out_ptr->getDelta(deltax, deltay, deltaz);
  int volume               = dimx*dimy*dimz;

  float * dxx              = new float[volume];
  float * dyx              = new float[volume];
  float * dzx              = new float[volume];
  float * dxy              = new float[volume];
  float * dyy              = new float[volume];
  float * dzy              = new float[volume];
  float * dxz              = new float[volume];
  float * dyz              = new float[volume];
  float * dzz              = new float[volume];

  invertValues(dx, volume);
  invertValues(dy, volume);
  invertValues(dz, volume);

  tensorfield warptensor(dimx, dimy, dimz);
  deriveFloatArray(dx, dxx, dxy, dxz, dimx, dimy, dimz); // how to treat the deltas?
  deriveFloatArray(dy, dyx, dyy, dyz, dimx, dimy, dimz);
  deriveFloatArray(dz, dzx, dzy, dzz, dimx, dimy, dimz);

  for(int i=0; i<volume; i++){
    dxx[i] = dxx[i] +1;
    dyy[i] = dyy[i] +1;
    dzz[i] = dzz[i] +1;
  }
  /* 
  if(allowScaling==0){ // only rotate data
    double * Axx = new double[volume];
    double * Axy = new double[volume];
    double * Axz = new double[volume];
    double * Ayx = new double[volume];
    double * Ayy = new double[volume];
    double * Ayz = new double[volume];
    double * Azx = new double[volume];
    double * Azy = new double[volume];
    double * Azz = new double[volume];
    
    for(int i=0; i<volume; i++){
    
    }
    t_field_out_ptr->merge(resultxx, resultyy, resultzz, resultxy, resultxz, resultyz);
    delete[] Axx; 
    delete[] Ayx; 
    delete[] Azx; 
    delete[] Axy; 
    delete[] Ayy; 
    delete[] Azy; 
    delete[] Axz;
    delete[] Ayz; 
    delete[] Azz;
  }
  else {*/
  warptensor.merge(dxx, dxy, dxz, dyx, dyy, dyz, dzx, dzy, dzz);

  //float det = 0;
  double scale    = 1;
  double tmpscale = 1;
  tensor orig, result, warp, transpose;
  tensor A, AT, U, V, VT;
  tensor test, temp;
  float r1, r2, r3;
  for(int k=0; k<dimz; k++){
    ktmp = k*dimx*dimy;
    for(int j=0; j<dimy; j++){
      for(int i=0; i<dimx; i++){
	orig = (*t_field_out_ptr->get(i+j*dimx + ktmp));
	test = orig;
	if(!orig.isZero()){
	  warp      = (*warptensor.get(i+j*dimx + ktmp));
	  warp.trans(transpose);
	  //cout << "Determinants: warp.det "<<warp.det() <<" and transpose.det "<< transpose.det()<<endl;

	  if     (allowScaling==0){ // only rotate data
	    warp.svd(r1, r2, r3, U, VT);
	    A         = U*VT;
	    A.trans(AT);
	    result    = A*orig*AT;
	    if(A.det()>1.01 || A.det()<0.99) cout <<"A.det is larger than 1.01 or smaller than 0.99!\n";/*
	    if(orig.det()!=0 && (fabs(result.det()*0.99)>fabs(orig.det()) || fabs(result.det()*1.01)<fabs(orig.det()))){
	      //cout << "warp.det   "<< warp.det()<<" transpose.det "<<transpose.det()<<endl;
	      cout << "result.det "<< result.det()<<" and orig.det:      "<< orig.det()<<endl;
	      cout << "A.det "<< A.det()<<" and AT.det:      "<< AT.det()<<endl;
	      cout << "Difference between result.det and orig.det:    "<< result.det() - orig.det()<<endl;
	      cout << "SVD tensor A=U*VT "<<A << "SVD tensor U "<<U <<"r matrix ["<< r1 <<" 0 0;0 "<< r2 <<" 0; 0 0 "<< r3 <<"];\nSVD tensor VT "<< VT << "Transpose of A " << AT<< "orig is " <<orig<<"result is "<<result<<"----------------\n";
	      }*/
	  }
	  else if(allowScaling==10){
	    warp.svd(r1, r2, r3, U, VT);
	    VT.trans(V);
	    A = U*VT;
	    A.trans(AT);
	    result = AT*orig*A;
	    //cout <<"det(A): "<<A.det()<<" det(AT): "<< AT.det() << " det(result): "<<result.det()<<" det(orig): "<<orig.det()<<endl;
	  }
	  else if(allowScaling==1){ // warp data and divide by determinant of transformation
	    tmpscale  = double(warp.det());
	    scale     = pow(tmpscale, double(-1.0)/double(3.0));
	    if(scale == 0 || scale != scale){   // but why should it be zero here?
	      scale = 1;                        // do not want to divide by zero
	      cout << "Infinite scale or scale == 0; transpose.det()*transpose.det() is: "
		   <<tmpscale<< " pow(scale,1.0/3.0) is: " <<scale<<endl;
	    }
	    warp      = warp*scale;
	    warp.trans(transpose);
	    result    = transpose*orig*warp;
	    //result    = warp*orig*transpose;
	    //cout <<"transpose*orig*warp=result is: "<<transpose<<orig<<warp<<result;
	    //result.divide(p, float(scale));
	    //result = transpose*orig*warp / float(scale);
	    //cout <<"transpose*orig*warp/scale is: "<<result;	
	    //if(test != orig) cout << "Orig has been changed from "<<test<< "to"<< orig;
	    if(fabs(result.det()*0.99)>=fabs(orig.det()) || fabs(result.det()*1.01)<=fabs(orig.det())){
	      cout << "Scale is: "<<scale<<endl;
	      cout << "result.det "<< result.det()<<" and orig.det:      "<< orig.det()<<endl;
	      cout << "warp.det   "<< warp.det()  <<" and transpose.det: "<< transpose.det()<<endl;
	    }
	  }
	  else if(allowScaling==11){ // warp data and divide by determinant of transformation
	    tmpscale  = double(warp.det());
	    scale     = pow(tmpscale, double(-1.0)/double(3.0));
	    if(scale == 0 || scale != scale){   // but why should it be zero here?
	      scale = 1;                        // do not want to divide by zero
	      cout << "Infinite scale or scale == 0; transpose.det()*transpose.det() is: "
		   <<tmpscale<< " pow(scale,1.0/3.0) is: " <<scale<<endl;
	    }
	    warp      = warp*scale;
	    warp.trans(transpose);
	    result    = warp*orig*transpose;
	  }
	  else if(allowScaling==2){ // warp data completely
	    result    = transpose*orig*warp;
	  }
	  else if(allowScaling==12){ // warp data completely
	    result    = warp*orig*transpose;
	  }

	  if(result.get(0,0) !=result.get(0,0)){
	    cout << "Result has non valid float number:";
	    cout << "Transpose is "<<transpose <<"Orig is "<< orig  <<"Warp is "<< warp << "orig*warp is "<<orig*warp <<"transpose*orig*warp is "<< transpose*orig*warp << "result is "<<result <<"Scale is "<< scale <<"----------------\n";
	  }

	  scale    = 0;
	  tmpscale = 0;
	  t_field_out_ptr->set(result, i, j, k);
	  //cout << "---------------\n";
	}
      }
    }
  } 
  //}
  cout << "\t\t\t\t\t\tdone\n"<<flush;
  //delete warptensor;
  delete[] dxx; 
  delete[] dyx; 
  delete[] dzx; 
  delete[] dxy; 
  delete[] dyy; 
  delete[] dzy; 
  delete[] dxz;
  delete[] dyz; 
  delete[] dzz;
 
}




void resampleTensorData( tensorfield & t_field, const float dx, const float dy, const float dz, const float pitch, const float roll, const float yaw)
{
  char a;
  int _dimx, _dimy, _dimz, volume;
  //int _deltax = 1;
  //int _deltay = 1;
  //int _deltaz = 1;
  int ktmp;

  t_field.getDimension(_dimx, _dimy, _dimz);
  //t_field.getDelta( _deltax, _deltay, _deltaz);
  volume = _dimx* _dimy* _dimz;

  tensorfield transform(_dimx, _dimy, _dimz);
  tensor temp, transtemp;
  //float dTxx, dTxy, dTxz, dTyx, dTyy, dTyz, dTzx, dTzy, dTzz;

  //cout << "x-Dim: "<< _dimx << ", y-Dim: "<< _dimy << ", z-Dim: "<<_dimz<<endl;

  float * Tx = new float[volume];
  float * Ty = new float[volume];
  float * Tz = new float[volume];

  float * dxTx = new float[volume];
  float * dyTx = new float[volume];
  float * dzTx = new float[volume];
  float * dxTy = new float[volume];
  float * dyTy = new float[volume];
  float * dzTy = new float[volume];
  float * dxTz = new float[volume];
  float * dyTz = new float[volume];
  float * dzTz = new float[volume];
  
  double PI       = 3.141592653589793;
  //double radPitch = double(pitch)/180.0*PI;
  //double radRoll  = double(roll )/180.0*PI;
  double radYaw   = double(yaw  )/180.0*PI;

  // 2D rotation 
  for(int k=0; k<_dimz; k++){
    ktmp = k*_dimx*_dimy;
    for(int j=0; j<_dimy; j++){
      for(int i=0; i<_dimx; i++){
	Tx[i+j*_dimx + ktmp] =  cos(radYaw)*((i+1)-_dimx/2) + sin(radYaw)*((j+1)-_dimy/2) + dx  ;
	Ty[i+j*_dimx + ktmp] = -sin(radYaw)*((i+1)-_dimx/2) + cos(radYaw)*((j+1)-_dimy/2) + dy  ;
	Tz[i+j*_dimx + ktmp] = (k+1) + dz;//sqrt(Tx[i+j*_dimx]*Tx[i+j*_dimx] + Ty[i+j*_dimx]*Ty[i+j*_dimx]) ;
      }
    }
  }
  //display display(t_field);
  //display.greyarray(Tx, _dimx, _dimy, _dimz);
  //display.greyarray(Ty, _dimx, _dimy, _dimz);
  //display.greyarray(Tz, _dimx, _dimy, _dimz);

  deriveFloatArray(Tx, dxTx, dyTx, dzTx, _dimx, _dimy, _dimz);
  deriveFloatArray(Ty, dxTy, dyTy, dzTy, _dimx, _dimy, _dimz);
  deriveFloatArray(Tz, dxTz, dyTz, dzTz, _dimx, _dimy, _dimz);

  if(_dimz==1){
    for(int i=0; i< _dimx*_dimy*_dimz; i++){
      dzTz[i] = 1;
    }
  }

  transform.merge( dxTx, dyTx, dzTx, dxTy, dyTy, dzTy, dxTz, dyTz, dzTz);

  //int offset[3] = {0,0,0};
  //int window[3] = {_dimx, _dimy, _dimz};
  //display.greyarray(Tz, _dimx, _dimy, _dimz);
  //display.element( transform, offset, window, 0, 0);
  //display.element( transform, offset, window, 0, 1);
  //display.element( transform, offset, window, 1, 0);
  //display.element( transform, offset, window, 1, 1);

  for(int k=0; k<_dimz; k++){
    ktmp = k*_dimx*_dimy;
    for(int j=0; j<_dimy; j++){
      for(int i=0; i<_dimx; i++){
	if(!(*transform.get(i+j*_dimx + ktmp)).isZero()){
	  //if(! (*t_field.get(i+j*_dimx + ktmp)).isZero())  cout << (*t_field.get(i+j*_dimx + ktmp));
	  transtemp = (*transform.get(i+j*_dimx + ktmp)).trans(transtemp);
	  t_field.set(transtemp*(*t_field.get(i+j*_dimx + ktmp))*(*transform.get(i+j*_dimx + ktmp)), i, j, k);
	  //if(! (*t_field.get(i+j*_dimx + ktmp)).isZero())  cout << (*t_field.get(i+j*_dimx + ktmp));
	}
      }
    }
  }
  delete[] dxTx;
  delete[] dyTx;
  delete[] dzTx;
  delete[] dxTy;
  delete[] dyTy;
  delete[] dzTy;
  delete[] dxTz;
  delete[] dyTz;
  delete[] dzTz;
  delete[] Tx;
  delete[] Ty;
  delete[] Tz;

  //cout << "End of function resampleTensorData\nPress any key to continue "; cin >> a; // block so images can be displayed
}

void makeSynthField3D( eigenfield & e_field, tensorfield &t_field)
{
  float vec1a[3] = {1, 0, 0};
  float vec2a[3] = {0, 1, 0};
  float vec3a[3] = {0, 0, 1};

  float vec1b[3] = {0, 1, 0};
  float vec2b[3] = {1, 0, 0};
  float vec3b[3] = {0, 0, -1};

  float vec1c[3] = {0, 0, 1};
  float vec2c[3] = {1, 0, 0};
  float vec3c[3] = {0, 1, 0};
  
  eigen  eigen1(1000, 300, 100, vec1a, vec2a, vec3a);
  eigen  eigen2(1000, 300, 200, vec1b, vec2b, vec3b);
  eigen  eigen3(1000, 500, 50,  vec1c, vec2c, vec3c);

  int z1 = 2;
  int z2 = 6;

  for(int i=5; i<15; i++){ 
    for(int j=5; j<15; j++){
      e_field.set(eigen1, i, j, z1);
    }
  }
  for(int i=6; i<14; i++){
    e_field.set(eigen2, 5, i, z1);
    e_field.set(eigen2, 14, i, z1);
    e_field.set(eigen2, 5, i, z2);
    e_field.set(eigen2, 14, i, z2);
  }
  for(int i=7; i<13; i++){
    e_field.set(eigen2, 6, i, z1);
    e_field.set(eigen2, 13, i, z1);
    e_field.set(eigen2, 6, i, z2);
    e_field.set(eigen2, 13, i, z2);
  }
  for(int i=8; i<12; i++){
    e_field.set(eigen2, 7, i, z1);
    e_field.set(eigen2, 12, i, z1);
    e_field.set(eigen2, 7, i, z2);
    e_field.set(eigen2, 12, i, z2);
  }
  for(int i=9; i<11; i++){
    e_field.set(eigen2, 8, i, z1);
    e_field.set(eigen2, 11, i, z1);
    e_field.set(eigen2, 8, i, z2);
    e_field.set(eigen2, 11, i, z2);
  }    
  for(int k=z1+1; k<z2; k++){   
    for(int i=5; i<15; i++){
      e_field.set(eigen3, i, 5, k);
      e_field.set(eigen3, i, 5, k);
      e_field.set(eigen3, i, 14, k);
      e_field.set(eigen3, i, 14, k);
    }
  }
  e_field.eigen2tensor(t_field);

  tensor tensor1, tensor2, tensor3;
  eigen1.eigen2tensor(tensor1);
  eigen2.eigen2tensor(tensor2);

  // line from one tensor to another
  for(int i=0; i<10; i++){
    tensor3 = tensor1*(float(i)/9.0)+tensor2*(1-float(i)/9.0);
    //cout << tensor3;
    t_field.set(tensor3, i+5, 3, 5);
  }
  e_field.tensor2eigen(t_field);

  /*
  delete[] vec1a;
  delete[] vec2a;
  delete[] vec3a;
  delete[] vec1b;
  delete[] vec2b;
  delete[] vec3b;
  delete[] vec1c;
  delete[] vec2c;
  delete[] vec3c;
  */
}

void makeSynthField2D( eigenfield & e_field, tensorfield &t_field)
{
  float vec1a[3] = {1, 0, 0};
  float vec2a[3] = {0, 1, 0};
  float vec3a[3] = {0, 0, 1};

  float vec1b[3] = {0, 1, 0};
  float vec2b[3] = {1, 0, 0};
  float vec3b[3] = {0, 0, -1};
  
  eigen  eigen1(1000, 300, 100, vec1a, vec2a, vec3a);
  eigen  eigen2(1000, 300, 200, vec1b, vec2b, vec3b);

  for(int i=5; i<15; i++){ 
    for(int j=5; j<15; j++){
      e_field.set(eigen1, i, j, 0);
    }
  }
  for(int i=6; i<14; i++){
    e_field.set(eigen2, 5, i, 0);
    e_field.set(eigen2, 14, i, 0);
  }
  for(int i=7; i<13; i++){
    e_field.set(eigen2, 6, i, 0);
    e_field.set(eigen2, 13, i, 0);
  }
  for(int i=8; i<12; i++){
    e_field.set(eigen2, 7, i, 0);
    e_field.set(eigen2, 12, i, 0);
  }
  for(int i=9; i<11; i++){
    e_field.set(eigen2, 8, i, 0);
    e_field.set(eigen2, 11, i, 0);
  }    
  e_field.eigen2tensor(t_field);

  tensor tensor1, tensor2, tensor3;
  eigen1.eigen2tensor(tensor1);
  eigen2.eigen2tensor(tensor2);

  // line from one tensor to another
  for(int i=0; i<10; i++){
    tensor3 = tensor1*(float(i)/9.0)+tensor2*(1-float(i)/9.0);
    //cout << tensor3;
    t_field.set(tensor3, i+5, 3, 0);
  }
  e_field.tensor2eigen(t_field);

}

void makeSynthSimple2D(eigenfield & e_field, tensorfield &t_field)
{
  float vec1a[3] = {1, 0, 0};
  float vec2a[3] = {0, 1, 0};
  float vec3a[3] = {0, 0, 1};

  eigen  eigen1(1000, 300, 100, vec1a, vec2a, vec3a);

  for(int i=5; i<15; i++){ 
    for(int j=5; j<15; j++){
      e_field.set(eigen1, i, j, 0);
    }
  }

  e_field.eigen2tensor(t_field);
}

void makeSynthCircle2D(eigenfield & e_field, tensorfield &t_field)
{
  float vec1a[3] = {1, 0, 0};
  float vec2a[3] = {0, 1, 0};
  float vec3a[3] = {0, 0, 1};

  eigen  eigen1(1000, 300, 100, vec1a, vec2a, vec3a);

  e_field.set(eigen1, 10, 5, 0);
  e_field.set(eigen1, 10, 11, 0);

  cout << "Not yet implemented\n";
}