/* 
Description: different test routines 

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.


Institution: Surgical Planning Laboratory
             Department of Radiology
             Brigham and Women's Hospital
             Harvard Medical School
             MA 02115
             USA

Date:        11.2000 - 3.2001

Modifications:
<Date>   <Init>    <Version>    <Description>

 */

#include <iostream.h>
#include <stdlib.h>
#include <time.h>
#include "tensorfield.h" 
#include "eigenfield.h"
#include "display.h"

#include "andi.h"

#include "tensorfunctions.h"
#include "kfunctions.h"
#include "rsfunctions.h"

extern "C"{
#include "memfuns.h"
#include "fileops.h" 
} 

void usage()
{
  cout << 
    "\n------------------------------------------------------\n"
    "Usage: \n"
    "./test number [options]\n"
    "number represents the test, options are testspecific:\n"
    "1 testkrigingsynthetic\n"
    "2 testkrigingMRI; MRI image interpolated with kriging\n"
    "3 testmatch\n"
    "4 testsort\n"
    "5 tensorfieldtest\n"
    "6 testSynthTensorWarp; different warpings of synthetic tensorfields \n"
    "7 testPointSelection\n"
    "8 testSingleValueDecomp\n"
    "9 displaysingetensor\n"
    "10 rotateTensor\n"
    "11 generateRandomTensorfield\n"
    "12 tensortest \n"
    "13 rigidTransformSynthetic; synthetic field interpolated with kriging \n"
    "14 testGaussLinear; Produce gauss and linear weights into stdout\n"
    "15 testSVD; test single value decomposition\n"
    "16 testFindPointsInTensorfield\n"
    "\n------------------------------------------------------\n";
}

void testkrigingsynthetic(int argc, char *argv[]);
void testkrigingMRI(int argc, char *argv[]);
void testmatch(int argc, char *argv[]);
void testsort(int argc, char *argv[]);
void tensorfieldtest(int argc, char *argv[]);
void tensortest(int argc, char *argv[]);
void testSynthTensorWarp(int argc, char *argv[]);
void testPointSelection(int argc, char *argv[]);
void testSingleValueDecomp(int argc, char *argv[]);
void displaysingetensor(int argc, char *argv[]);
void rotateTensor(int argc, char *argv[]);
void generateRandomTensorfield(int argc, char *argv[]);
void rigidTransformSynthetic(int argc, char *argv[]);
void testGaussLinear(int argc, char *argv[]);
void testSVD(int argc, char *argv[]);
void testFindPointsInTensorfield(int argc, char *argv[]);

int main(int argc, char *argv[]){

  if(argc<=1){ 
    usage();
    exit(0);
  }
  else{
    switch(atoi(&argv[1][0])){
    case 1: testkrigingsynthetic(argc, argv); break;
    case 2: testkrigingMRI(argc, argv);       break;
    case 3: testmatch(argc, argv);            break;
    case 4: testsort(argc, argv);             break;
    case 5: tensorfieldtest(argc, argv);      break;
    case 6: testSynthTensorWarp(argc, argv);  break;
    case 7: testPointSelection(argc, argv);   break;
    case 8: testSingleValueDecomp(argc, argv);break;
    case 9: displaysingetensor(argc, argv);   break;
    case 10: rotateTensor(argc, argv);        break;
    case 11: generateRandomTensorfield(argc, argv);break;
    case 12: tensortest(argc, argv);          break;
    case 13: rigidTransformSynthetic(argc, argv);  break;
    case 14: testGaussLinear(argc, argv);     break;
    case 15: testSVD(argc, argv);             break;
    case 16: testFindPointsInTensorfield(argc, argv);  break;
    default: usage;
    }
  }
};

void testGaussLinear(int argc, char *argv[]){
 
  float * array = new float[1024];
  float * array1 = new float[1024];
  for(int i=0; i<1024; i++){
    array[i] = 1;
    array1[i] = 1;
  }
  int dim = 0; //atoi(&argv[2][0]);
  cout <<"Enter dimension to test weightGauss: e.g. 3 or 5 "; 
  cin  >> dim;
  weightGauss(array, dim);
  cout << "Gauss field: \n" << flush;
  for(int j=0; j<dim; j++){
    for(int i=0; i<dim; i++){
      cout << array[i + j*dim] <<" ";
    }
    cout <<endl;
  }
  weightLinear(array1, dim);
  cout << "Linear field: \n" << flush;
  for(int j=0; j<dim; j++){
    for(int i=0; i<dim; i++){
      cout << array1[i + j*dim] <<" ";
    }
    cout <<endl;
  }
  float equalize = 1;
  cout <<"Equalize arrays gauss and linear  to value : e.g. 1 "; 
  cin  >> equalize;
  
  equalizeArrays(array, array1, dim*dim, equalize);
  cout << "Gauss field: \n" << flush;
  for(int j=0; j<dim; j++){
    for(int i=0; i<dim; i++){
      cout << array[i + j*dim] <<" ";
    }
    cout <<endl;
  }
  cout << "Linear field: \n" << flush;
  for(int j=0; j<dim; j++){
    for(int i=0; i<dim; i++){
      cout << array1[i + j*dim] <<" ";
    }
    cout <<endl;
  }
};

void testkrigingsynthetic(int argc, char *argv[]){
 
  char a;
  int x   = 256;
  int y   = 256;
  x = 64;
  y = 64;
  int z   = 1;
  int volume = x*y*z;
  int kriginglength = 5;//atoi(&argv[3][0]);
  int krigingmethod = 0;//atoi(&argv[4][0]);
  float range       = 20;//atof(&argv[5][0]); // 20
  float nugget      = 0.0;//atof(&argv[6][0]); // 0.0
  float sill        = 80;//atof(&argv[7][0]); // 80
  float sigma       = 10.0;//atof(&argv[8][0]); // 10.0
  float tau         = 0.0; //atof(&argv[9][0]); // 0.0

  cout <<"Enter values for kriging: kriginglength, krigingmethod, range, nugget, sill, sigma, tau\n";
  cout <<"e.g. 5 0 20 0.0 80 10.0 0.0 " ;
  cin >> kriginglength >> krigingmethod >> range >> nugget >>  sill>> sigma>> tau;
  float *origdata           = NULL;
  float *movingdata         = NULL;
  
  unsigned short *tmporig   = NULL;
  unsigned char  *orighdr   = NULL;
  int orighsize             =   -1;
  int orignumpixels         =   -1;

  display display(x, y, 1);
  float * testdata = new float[x*y];
  testImage1(testdata, 16, 16, x, y);
  display.greyarray(testdata, x, y, z);
  selectRandomPoints(testdata, x*y, 10);
  display.greyarray(testdata, x, y, z);
  interpolateKriging(testdata, x, y, z, 1, 1, 1, kriginglength, krigingmethod, range, nugget, sill, sigma, tau);
  display.greyarray(testdata, x, y, z);
  cout << "Enter a char to end "; 
  cin >> a;
}  
 
void testkrigingMRI(int argc, char *argv[]){

  
  char a;
  int x   = 256;
  int y   = 256;
  int z   = 1;
  int volume = x*y*z;
  int kriginglength = 5;//atoi(&argv[3][0]);
  int krigingmethod = 0;//atoi(&argv[4][0]);
  float range       = 20;//atof(&argv[5][0]); // 20
  float nugget      = 0.0;//atof(&argv[6][0]); // 0.0
  float sill        = 80;//atof(&argv[7][0]); // 80
  float sigma       = 10.0;//atof(&argv[8][0]); // 10.0
  float tau         = 0.0; //atof(&argv[9][0]); // 0.0
  char * path = new char[1024];
  
  cout <<"Enter values for kriging: kriginglength, krigingmethod, range, nugget, sill, sigma, tau\n";
  cout <<"e.g. 5 0 20 0.0 80 10.0 0.0 " ;
  cin >> kriginglength >> krigingmethod >> range >> nugget >>  sill>> sigma>> tau;
  cout <<"Enter location of MRI file: "; cin >> path;

  float *origdata           = NULL;
  float *movingdata         = NULL;
  
  unsigned short *tmporig   = NULL;
  unsigned char  *orighdr   = NULL;
  int orighsize             =   -1;
  int orignumpixels         =   -1;

  readMRIfile(path, &orighdr, &orighsize, &tmporig, &orignumpixels);
  assert(tmporig != NULL);
  origdata  = new float[volume];
  for (int i =0; i<volume; i++){
    origdata[i]  = float(tmporig[i]);
  }
  
  display display(256, 256, 1);
  display.greyarray(origdata, x, y, z);
  for(int i =0; i<volume; i++){
    if(rand() > RAND_MAX*0.1){
      origdata[i] = 1000;
    }
  }
  display.greyarray(origdata, x, y, z);
  interpolateKriging(origdata, x, y, z, 1, 1, 1, kriginglength, krigingmethod, range, nugget, sill, 1000);
  display.greyarray(origdata, x, y, z);
  
  cout << "Enter a char to end "; 
  cin >> a;
}

void testmatch(int argc, char *argv[]){

  
  char a;
  int x   = 256;
  int y   = 256;
  int z   = 1;
  int volume = x*y*z;
  float *origdata           = NULL;
  float *movingdata         = NULL;
  movingdata  = new float[volume];
  origdata    = new float [x*y*z];
  //testImage1(origdata, 32, 64);

  int scale         = 10;
  int initrandom    = 123456;
  int neighbourhood = 3;
  float sigmathresh = 0.01;
  float threshold   = 0.005;
  int matchmethod   = 0; //0|1
  int movingwindowsize     = 9;
  int searchwindowsize     = 13;
  int correlationwindowsize = 11;
  int diffusiontimestep    = 2;
  int diffusioniterations  = 10;
  float diffusioncontrast  = 0.5;
  float diffusionnoise     = 0.8;
  int kriginglength        = 9;
  if(argc==15){
  scale             = atoi(&argv[2][0]);
  initrandom        = atoi(&argv[3][0]);
  neighbourhood     = atoi(&argv[4][0]);
  sigmathresh       = atof(&argv[5][0]);
  threshold         = atof(&argv[6][0]);
  matchmethod       = atoi(&argv[7][0]);
  movingwindowsize       = atoi(&argv[8][0]);
  searchwindowsize       = atoi(&argv[9][0]);
  diffusiontimestep      = atoi(&argv[10][0]);
  diffusioniterations    = atoi(&argv[11][0]);
  diffusioncontrast      = atof(&argv[12][0]);
  diffusionnoise         = atof(&argv[13][0]);
  kriginglength          = atoi(&argv[14][0]);
  }
  else {
    cout << "Usage: \n"
      "int scale         = 10;\n"
      "int initrandom    = 256256;\n"
      "int neighbourhood = 3;\n"
      "float sigmathresh = 0.01;\n"
      "float threshold   = 0.0005;\n"
      "int matchmethod   = 0; //0|1\n"
      "int movingwindowsize     = 7;\n"
      "int searchwindowsize     = 7;\n"
      "int diffusiontimestep    = 2;\n"
      "int diffusioniterations  = 10;\n"
      "float diffusioncontrast  = 0.5;\n"
      "float diffusionnoise     = 0.8;\n"
      "int kriginglength        = 9;\n"<<endl;
  }

  int factor = 2;
  int sx     = x/factor;
  int sy     = y/factor;
  float * smallorig  = new float[sx*sy*z];
  float * smallmoving= new float[sx*sy*z];

  float * outdx = new float[x*y*z];
  float * outdy = new float[x*y*z];
  float * outdz = new float[x*y*z];

  //tensorfield t_field(x, y, z);
  display ldisplay(x, y, z);
  float * outdata     = new float[x*y*z];

  float * pointarray1 = new float[x*y*z];
  float * pointarray2 = new float[x*y*z];
  float * pointarray3 = new float[x*y*z];

  for(int i=0; i<x*y*z; i++){
    outdx[i] = 0;
    outdy[i] = 0;
    outdz[i] = 0;
    pointarray1[i] = 0;
    pointarray2[i] = 0;
    pointarray3[i] = 0;
  }

  // jan's images test:
  
  unsigned short *tmporig   = NULL;
  unsigned short *tmpmoving = NULL;
  unsigned char  *orighdr   = NULL;
  int orighsize             =   -1;
  int orignumpixels         =   -1;
  readMRIfile("/d/hpc/ferrant/build/images/Sphere/s19.030",  &orighdr, &orighsize, &tmpmoving, &orignumpixels);
  readMRIfile("/d/hpc/ferrant/build/images/Ellipsoid/I102710.030",  &orighdr, &orighsize, &tmporig, &orignumpixels);
  assert(tmporig != NULL);
  for (int i =0; i<volume; i++){
    origdata[i]  = float(tmporig[i]);
    movingdata[i] = float(tmpmoving[i]);
  }


  ldisplay.greyarray( origdata, x, y, z);  
  ldisplay.greyarray( movingdata, x, y, z);  

  cout << "Press any key to findPointsInFloatArray "; 
  cin >> a;
  findPointsInFloatArray( movingdata, pointarray2, x, y, z, neighbourhood, sigmathresh);
  ldisplay.greyarray(pointarray2, x, y, z);
  Threshold(pointarray2, x, y, z, threshold);  

  cout << "Press any key to createSparseDisplacement "; 
  cin >> a;
  createSparseDisplacementField(origdata, movingdata, pointarray2, outdx, outdy, outdz, x, y, z, searchwindowsize, movingwindowsize, matchmethod);

  cout << "Press any key to Interpolate Kriging "; 
  cin >> a;
  interpolateKriging(outdx, x, y, z, 1, 1, 1, kriginglength);
  interpolateKriging(outdy, x, y, z, 1, 1, 1, kriginglength);
  ldisplay.greyarray(outdx, x, y, z);
  ldisplay.greyarray(outdy, x, y, z);

  displaceFloatArray(movingdata, pointarray1, outdx, outdy, outdz, x, y, z, 1, 1, 1);
  ldisplay.greyarray(pointarray1, x, y, z);

  cout << "Total difference between matched and orig: "<<totDifference(origdata, pointarray1, x*y*z)<<endl;
  ldisplay.greyarrayDifference(origdata, pointarray1, x, y, z);
 
  cout << "Press any key continue "; 
  cin >> a;
  
  // arg1 is scale=10 arg2 init randomgenerator=integer
  generateRandomDisplacement(pointarray1, pointarray2, pointarray3, x, y, z, scale, initrandom);
  
  ldisplay.greyarray(pointarray1, x, y, z);
  ldisplay.greyarray(pointarray2, x, y, z);
  //ldisplay.greyarray(pointarray3, x, y, z);
  
  interpolateKriging(pointarray1, x, y, z, 1, 1, 1, 4);
  interpolateKriging(pointarray2, x, y, z, 1, 1, 1, 4);
  
  ldisplay.greyarray(pointarray1, x, y, z);
  ldisplay.greyarray(pointarray2, x, y, z);

  //saveFloatAsFloat("pointmatcherdata/testdx.001", pointarray1, x*y*z);
  //saveFloatAsFloat("pointmatcherdata/testdy.001", pointarray2, x*y*z);
  //saveFloatAsFloat("pointmatcherdata/testdz.001", pointarray3, x*y*z);
  

  for(int i=0; i<x*y*z; i++){
    pointarray1[i] = scale;
    pointarray2[i] = scale;
  }

  displaceFloatArray(origdata, outdata, pointarray1, pointarray2, pointarray3, x, y, z, 1, 1, 1);
  saveFloatAsMRI("pointmatcherdata/distorted.001", outdata, x*y*z);

  ldisplay.greyarray(outdata, x, y, z);
  cout << "Total difference between displaced and orig: "<<totDifference(origdata, outdata, x*y*z)<<endl;
  ldisplay.greyarrayDifference(origdata, outdata, x, y, z);

  
  diffusion(&origdata, x, y, diffusiontimestep, diffusioniterations, diffusioncontrast, diffusionnoise);
  diffusion(&outdata, x, y, diffusiontimestep, diffusioniterations, diffusioncontrast, diffusionnoise);
  ldisplay.greyarray( origdata, x, y, z);
  ldisplay.greyarray( outdata, x, y, z);
  
  cout << "Press any key to findPointsInFloatArray "; 
  cin >> a;
  
  float * soutdx = new float[sx*sy*z];
  float * soutdy = new float[sx*sy*z];
  float * soutdz = new float[sx*sy*z];

  for(int i=0; i<sx*sy*z; i++){
    soutdx[i] = 0;
    soutdy[i] = 0;
    soutdz[i] = 0;
  }  

  downsampleFloatArray(origdata, smallorig,   x, y, z, factor, 0);
  downsampleFloatArray(outdata,  smallmoving, x, y, z, factor, 0);

  ldisplay.greyarray(smallorig, sx, sy, z);
  ldisplay.greyarray(smallmoving, sx, sy, z);
  findPointsInFloatArray(smallmoving, pointarray2, sx, sy, z, neighbourhood, sigmathresh);
  ldisplay.greyarray(pointarray2, sx, sy, z);
  Threshold(pointarray2, sx, sy, z, threshold);
  ldisplay.greyarray(pointarray2, sx, sy, z);
  createSparseDisplacementField(smallorig, smallmoving, pointarray2, soutdx, soutdy, soutdz, sx, sy, z, correlationwindowsize, searchwindowsize, matchmethod);

  cout << "Press any key to upsample "; 
  cin >> a;
  
  upsampleDisplacement(soutdx, outdx, sx, sy, z, factor);
  upsampleDisplacement(soutdy, outdy, sx, sy, z, factor);
  upsampleDisplacement(soutdz, outdz, sx, sy, z, factor);

  ldisplay.greyarray(outdx, x, y, z);
  ldisplay.greyarray(outdy, x, y, z);
  
  int skip;
  cout << "Apply this displacement? (1=yes, 0=no)  "; 
  cin >> skip;
  
  if(!skip){
  while(1){
    findPointsInFloatArray( outdata, pointarray2, x, y, z, neighbourhood, sigmathresh);
    //CornerDetect(origdata, pointarray2, x, y, z);
    ldisplay.greyarray(pointarray2, x, y, z);
    Threshold(pointarray2, x, y, z, threshold);
    ldisplay.greyarray(pointarray2, x, y, z);
    cout << "Done with forward processing. Enter c to continue b to break ";
    cin >> a;
    if(a!='c') break;
    cout << "Neighbourhood (int), sigmathresh (float), threshold (float) for pointextraction: ";
    cin >> neighbourhood >>  sigmathresh >> threshold;
  }
  
  createSparseDisplacementField(origdata, outdata, pointarray2, outdx, outdy, outdz, x, y, z, searchwindowsize, movingwindowsize, matchmethod);
  
  ldisplay.greyarray(outdx, x, y, z);
  ldisplay.greyarray(outdy, x, y, z);
  }

  interpolateKriging(outdx, x, y, z, 1, 1, 1, kriginglength);
  interpolateKriging(outdy, x, y, z, 1, 1, 1, kriginglength);

  
  int filterlength=0;
  cout << "Filterlength for gaussfilter on displacement field: ";
  cin >> filterlength;
  GaussSmoothFloatArray(outdx, x, y, z, filterlength);
  GaussSmoothFloatArray(outdy, x, y, z, filterlength);
  

  ldisplay.greyarray(outdx, x, y, z);
  ldisplay.greyarray(outdy, x, y, z);


  displaceFloatArray(outdata, pointarray1, outdx, outdy, outdz, x, y, z, 1, 1, 1);
  ldisplay.greyarray(pointarray1, x, y, z);

  cout << "Total difference between matched and orig: "<<totDifference(origdata, pointarray1, x*y*z)<<endl;
  ldisplay.greyarrayDifference(origdata, pointarray1, x, y, z);
  
  cout << "Press any key to end "; cin >> a;
}





  /*
  eigenfield e_field(&argv[1][0]);
  
  e_field.check();

  display display(e_field);
  
  int offset[3];
  int window[3]; 
  offset[0] = 0;
  offset[1] = 0;
  offset[2] = 0;
  window[0] = 256;
  window[1] = 256;
  window[2] = 1; 
  
  display.max_eigenval(e_field, offset, window);

  cin >> offset[0];
  */
  
void testsort(int argc, char *argv[]){
  int *sortarray = new int[15];
  int *testarray = new int[15];
  testarray[0] = 1;
  testarray[1] = 10;
  testarray[2] = 13;
  testarray[3] = 3;
  testarray[4] = 6;
  testarray[5] = 4;
  testarray[6] = 11;
  testarray[7] = 50;
  testarray[8] = 3; 
  testarray[9] = 8;
  testarray[10] =33;
  testarray[11] =23;
  testarray[12] =32;
  testarray[13] =2;
  testarray[14] =19;

  for(int i=0; i<15; i++){
    sortarray[i] = i;
  }
  

  QuickSort(testarray, 0, 15, sortarray);

  for(int i=0; i<15; i++){
    cout << "Testarray "<<i<<"\tis: "<<testarray[i]<<" \tSortarray is: "<<sortarray[i]<<endl;
  }
}
  //tensor a(50, 100, 150, 200, 250, 300);
  //tensor b;
  //tensor c(2.0, 4.0, 5.0, 3.0, 5.0, 1.0, 9.0, 6.0, 8.0);
  //tensor a(0,0,0,0,0,0);
  /*
  float eigen, tensor;
  int counter=0;
  for (int i=0; i<256; i++){
    for (int j=0; j<256; j++){
      eigen = e_field.get(i,j,0).cosinMaxEigen(e_field.get(i,j,1));
      tensor = t_field.get(i,j,0).cosinAngle(t_field.get(i,j,1));
      if(abs(abs(eigen)-abs(tensor))<=0.2 && eigen-tensor!=0){
	cout <<"Position: (" <<i <<", "<< j<<")\tValue: " << abs(abs(eigen)-abs(tensor))<< endl;
	counter++;
      }
    }
  }
  cout << "Counter is: "<<counter<<endl;
  */
  
void tensorfieldtest(int argc, char *argv[]){
  tensorfield a1("/projects/shortterm/warfield/rsierra/examples/janet/15955693/03476/006/tensor/rs-test-S", 256, 256, 9);
  a1.save("data/tensor.ten");
  
  tensorfield b1(256, 256, 9);
  b1.load("data/tensor.ten");
  
  while(1){
    int x, y;
    int z=0;
    cout << "Enter position of a tensor x y z: ";
    cin >> x >> y >> z ;
    if(x==-1) break;
    a1.print(x,y,z);
    b1.print(x,y,z);
  } 
}

void tensortest(int argc, char *argv[]){
    
  float t11 = 50;//1000;
  float t12 = 100;//0;
  float t13 = 150;//0;
  float t21 = 200;//0;
  float t22 = 250;
  float t23 = 300;
  float t31 = 350;
  float t32 = 400;
  float t33 = 450;

  if(argc>10){
  t11 = atof(&argv[2][0]);
  t12 = atof(&argv[3][0]);
  t13 = atof(&argv[4][0]);
  t21 = atof(&argv[5][0]);
  t22 = atof(&argv[6][0]);
  t23 = atof(&argv[7][0]);
  t31 = atof(&argv[8][0]);
  t32 = atof(&argv[9][0]);
  t33 = atof(&argv[10][0]);
  }
  tensor a(50, 100, 150, 200, 250, 300);
  //tensor c(2.0, 4.0, 5.0, 3.0, 5.0, 1.0, 9.0, 6.0, 8.0);
  tensor c(t11, t12, t13, t21, t22, t23, t31, t32, t33);
  tensor b;
 
  cout << "tensor a: \n"   << a;
  cout << "tensor c: \n" << c; 
  
  cout << "a+10: \n" << a + 10;
  cout << "10+a: \n" << 10 + a;
  cout << "a+c: \n" << a+c;

  cout << "a-1: \n" << a - 1;
  cout << "1-a: \n" << 1 - a;
  cout << "a-c: \n" << a-c;

  cout << "a*10: \n" << a*10;
  cout << "10*a: \n" << 10*a;
  cout << "a*c: \n" << a*c;

  cout << "a/10: \n" << a/10;
  cout << "a/c: \n" << a/c;
  cout << "c/a: \n" << c/a;

  cout << "-a: \n" << -a;

  cout << "a.det() f = " << a.det() <<"\n";
  cout << "tensor c: " <<c;
  cout << "c.det() f = " << c.det() <<"\n";
  
  cout << "a.inv: " << a.inv(b);
  cout << "c.inv: " << c.inv(b);

  b=a;
  cout << "a==b\t"<< (a==b) << "\na==c\t"<< (a==c)<<"\n";
  cout << "a!=b\t"<< (a!=b) << "\na!=c\t"<< (a!=c)<<"\n";
  
  cout << "euclidian distance of a: "<< a.euclid()<<"\n";
  cout << "euclidian distance of c: "<< c.euclid()<<"\n";

  cout << "a smaller then c (a<c): "<< (a<c)<<"\n";
  cout << "c smaller then a (c<a): "<< (c<a)<<"\n";
  cout << "a smaller or equal a (a<=a): "<< (a<=a)<<"\n";
  cout << "a smaller then a (a<a): "<< (a<a)<<"\n";
  cout << "maximal value in a "<< a.Max()<<"\n";
  eigen u, v;
  b = a + 100;
  
  cout <<a <<b;
    
  u.tensor2eigen(a);
  v.tensor2eigen(b);

  cout <<u<<v;

  cout << "Angle of tensors a and b: " << a.cosinAngle(b)<<endl;
  cout << "Angle of eigensys u and v:" << u.cosinMaxEigen(v)<<endl;
}
  
  /*  tensorfield a1("/home/rsierra/example/nathan_cato/006/tensor/coronal-S", 256, 256, 17);
  
  //tensorfield a1(256, 256, 1);
  //a1.set(a, 10, 10);
  //a1.set(a, 10, 20);
  //a1.set(a, 10, 30);
  //a1.set(a, 10, 40);
  //a1.set(a, 10, 50);
  //a1.set(a, 10, 100);
  // a1.print(10, 10);
  int x, y, z, x0, y0, z0;
  int offset[3];
  int window[3];
  while (1){
    cout << "Enter offset x0 y0 z0 (x0=-1 to stop): ";
    cin >> x0 >> y0 >> z0 ;
    if(x0==-1) break;
    cout << "Enter windowsize x y z (x=-1 to stop): ";
    cin >> x >> y >> z ;
    if(x==-1) break;
    offset[0] = x0;
    offset[1] = y0;
    offset[2] = z0;
    window[0] = x;
    window[1] = y;
    window[2] = z;
    a1.eig(offset, window);
    system("xv data/color.ppm &");
    }*/
  //short *lam1 = new short[256*256];
  //lam1 = a1.read_pic(lam1, "/home/rsierra/example/nathan_cato/005/tensor/mydata-S01L1e.pic");
  // a1.write_single_image(lam1);
  //a1.read_tensorslice("/home/rsierra/example/nathan_cato/005/tensor/mydata-S01");
  /*int test;
  a1.write("get00", "data/im", 4);
  test = a1.write("get00", "data/image_00", 4);
  test = a1.write("get01", "data/image_01", 4);
  test = a1.write("get02", "data/image_02", 4);
  test = a1.write("get11", "data/image_11", 4);
  test = a1.write("get12", "data/image_12", 4);
  test = a1.write("get22", "data/image_22", 4);

  if(test==1){
    while (1){
      int x, y;
      int z=0;
      cout << "Enter position of a tensor x y z: ";
      cin >> x >> y >> z ;
      if(x==-1) break;
      a1.print(x,y, z);
    }
    }*/

void testSynthTensorWarp(int argc, char *argv[]){

  int field               = 0;
  int displacement        = 0;
  int vtk                 = 0;
  int localwarp           = 0;
  int degrees             = 45;
  int coloring            = 0;
  int resolution          = 8;

  cout << "0=simple field; 1=square; 2=circle ";cin >>field;
  cout << "0=random displacement; 1=homogenous displacement; 2=towards center; 3=rotation; 4=upper left and lower right stretched "; 
  cin >>displacement;
  cout << "0=vtk display original data; else display displaced data "; cin >>vtk;
  cout << "<0 no local warping of tensordata; 0 local rotation; 1 local warping without any scaling; \nelse local warping with scaling "; cin >>localwarp;

  int x=20;
  int y=20; 
  int z=1;

  float * Txx = new float[x*y*z];
  float * Tyy = new float[x*y*z];
  float * Tzz = new float[x*y*z];
  float * Txy = new float[x*y*z];
  float * Txz = new float[x*y*z];
  float * Tyz = new float[x*y*z];

  float * pointarray1 = new float[x*y*z];
  float * pointarray2 = new float[x*y*z];
  float * pointarray3 = new float[x*y*z];
  setZero(Txx, x*y*z);
  setZero(Tyy, x*y*z);
  setZero(Tzz, x*y*z);
  setZero(Txy, x*y*z);
  setZero(Txz, x*y*z);
  setZero(Tyz, x*y*z);
  setZero(pointarray1, x*y*z);
  setZero(pointarray2, x*y*z);
  setZero(pointarray3, x*y*z);

  int offset[3] = {0, 0, 0};
  int window[3] = {x, y, z};
  vtkScalars *scalars=NULL; 

  tensorfield *m_field = new tensorfield(x, y, z);// moving field
  tensorfield *s_field = new tensorfield(x, y, z);//stationary field
  tensorfield t_field(x, y, z);
  eigenfield  e_field(x, y, z);
  display     display(x, y, z);

  if     (field==1) makeSynthField2D(e_field, t_field);
  else if(field==0) makeSynthSimple2D(e_field, t_field);
  else if(field==2) makeSynthCircle2D(e_field, t_field);

  if(displacement==0){
    generateRandomDisplacement(pointarray1, pointarray2, pointarray3, x, y, z, x/4, 123456, x/5);
    interpolateKriging(pointarray1, x, y, z, 1, 1, 1, 4, 0);
    interpolateKriging(pointarray2, x, y, z, 1, 1, 1, 4, 0);
  }
  else if(displacement==1){
    for(int i=0; i<x*y*z; i++){
      pointarray1[i] = 2;
      pointarray2[i] = 3;
    }
  }
  else if(displacement==2){
    generateDisplacementToCenter(pointarray1, pointarray2, x, y);
  }
  else if(displacement==3){
    cout << "integer degrees for rotation "; cin >>degrees  ;
    generateRotationDisplacement(pointarray1, pointarray2, pointarray3, x, y, z, degrees);
  }
  else if(displacement==4){
    generateCornerStrechDisplacement(pointarray1, pointarray2, pointarray3, x, y);
  }
   
  char * path = new char[1024];
  strcpy (path, "pointmatcherdata/test_6.synthdx");
  saveFloatAsFloat(path, pointarray1, x*y*z);
  strcpy (path, "pointmatcherdata/test_6.synthdy");
  saveFloatAsFloat(path, pointarray2, x*y*z);
  setZero(pointarray3, x*y*z);
    
  t_field.separate(Txx, Tyy, Tzz, Txy, Txz, Tzz);
  display.greyarray(Txx, x,y,z);
  display.greyarray(pointarray1, x,y,z);
  display.greyarray(pointarray2, x,y,z);
  s_field = & t_field;
  displaceTensorField(s_field, m_field, pointarray1, pointarray2, pointarray3);//synthetic displacement

  m_field->separate(Txx, Tyy, Tzz, Txy, Txz, Tzz);
  display.greyarray(Txx, x,y,z);
  if(localwarp>=0){
    int allowScaling = localwarp; // 0 no scaling with warping, else warping also scales tensordata
    localWarpTensorField(m_field, pointarray1, pointarray2, pointarray3, allowScaling);
  }
  m_field->separate(Txx, Tyy, Tzz, Txy, Txz, Tzz);
  display.greyarray(Txx, x,y,z);

  cout <<"Before displaying vtk\n"<<flush;
  cout << "Background 0=black, 1=white: ";cin >>coloring;
  cout <<"Resolution for vtk: ";cin >>resolution; 
  
  if(vtk==1){
    display.myVtkTensor(*m_field, offset, window, scalars, resolution, 1, coloring);
  }
  else {
    display.myVtkTensor( t_field, offset, window, scalars, resolution, 1, coloring);
  }

  delete[] pointarray1;
  delete[] pointarray2;
  delete[] pointarray3;
  //delete s_field;
  //delete m_field;
}


void testPointSelection(int argc, char *argv[])
{
  if(argc<=7){
    cout << "Usage: ./test 7 \n"
      "(0=simple field; 1=square; 2=circle)\n"
      "(neighbourhood for pointselection)\n"
      "(sigmathresh for pointselection)\n"
      "(threshold for  pointselection)\n\n";
  }
  int x=20;
  int y=20; 
  int z=1;

  float * pointarray1      = new float[x*y*z];

  int field                = 0;
  if(argc>2) field         = atoi(&argv[2][0]);
  int neighbourhood        = 3;
  if(argc>3) neighbourhood = atoi(&argv[3][0]);
  float sigmathresh        = 0.01;
  if(argc>4) sigmathresh   = atof(&argv[4][0]);
  float threshold          = 0.005;
  if(argc>5) threshold     = atof(&argv[5][0]);
  cout <<"Field: "<<field<<", neighbourhood: "<<neighbourhood<<", sigma: "<<sigmathresh<<", threshold: "<<threshold<<endl;

  tensorfield *s_field     = new tensorfield(x, y, z);//stationary field

  tensorfield t_field(x, y, z);
  eigenfield  e_field(x, y, z);
  display     display(x, y, z);

  if(field==1)      makeSynthField2D(e_field, t_field);
  else if(field==0) makeSynthSimple2D(e_field, t_field);
  else if(field==2) makeSynthCircle2D(e_field, t_field);
  s_field              = & t_field;

  findPointsInTensorfield2(s_field, pointarray1, neighbourhood, sigmathresh);
  Threshold(pointarray1, x, y, z, threshold);

  display.greyarray(pointarray1, x,y,z);
  
  char a;
  cout << "Enter any character to end "; 
  cin >> a;
}


void testSingleValueDecomp(int argc, char *argv[]){
  tensor A(0.4447, 0.9218, 0.4057, 0.6154, 0.7382, 0.9355, 0.7919, 0.1763, 0.9169);
  tensor R, T;
  float r1, r2, r3;
  A.svd(r1, r2, r3, R, T);

  cout << A << R << r1 <<", "<< r2 <<", "<< r3 <<endl<< T << endl;

  cout << A.det();
  
}


void displaysingetensor(int argc, char *argv[])
{
  int x=1;
  int y=1; 
  int z=1;
  
  int resolution = 20;
  int coloring = 0;
  float t11 = 1000;
  float t12 = 0;
  float t13 = 0;
  float t21 = 0;
  float t22 = 500;
  float t23 = 0;
  float t31 = 0;
  float t32 = 0;
  float t33 = 100;
  cout <<"Resolution for vtk: ";cin >>resolution;
  cout << "Background 0=black, 1=white: ";cin >>coloring; 
  cout << "Enter the 9 values of the tensor to display:  \n"
    "e.g. 1000 0 0 0 500 0 0 0 100 ";cin >> t11 >> t12 >> t13 >> t21 >> t22 >> t23 >> t31 >> t32 >> t33;

  int offset[3] = {0, 0, 0};
  int window[3] = {x, y, z};
  vtkScalars *scalars=NULL; 

  tensor A(t11, t12, t13, t21, t22, t23, t31, t32, t33);

  display display(x, y, z);
  tensorfield t_field(x, y, z);
  t_field.set(A, 0, 0, 0);
  display.myVtkTensor(t_field, offset, window, scalars, resolution, 1, coloring);

}

void rotateTensor(int argc, char *argv[])
{  
  int x=10;
  int y=3; 
  int z=1;
  
  int resolution = 20;
  int coloring = 0;

  int offset[3] = {0, 0, 0};
  int window[3] = {x, y, z};
  vtkScalars *scalars=NULL; 
  float t11 = 1000;
  float t12 = 0;
  float t13 = 0;
  float t21 = 0;
  float t22 = 500;
  float t23 = 0;
  float t31 = 0;
  float t32 = 0;
  float t33 = 400;

  cout <<"Resolution for vtk: ";cin >>resolution; 
  cout << "Background 0=black, 1=white: ";cin >>coloring;
  cout << "Enter the 9 values of the tensor to display: \n"
    "e.g. 1000 0 0 0 500 0 0 0 100 ";cin >> t11 >> t12 >> t13 >> t21 >> t22 >> t23 >> t31 >> t32 >> t33;


  tensorfield t_field(x, y, z);
  eigenfield  e_field(x, y, z);
  display display(x, y, z);

  tensor t1(t11, t12, t13, t21, t22, t23, t31, t32, t33);
  tensor t2, t3;
  eigen  e1;
  e1.tensor2eigen(t1);
  for(int i=0; i<x; i++){
    e_field.set(e1, i, 0, 0);
    e1.eigen2tensor(t2);
    e1.rotate(0,0,90/(x-1));
  }
  e_field.eigen2tensor(t_field);
  for(float i=0; i<x; i++){
    t3 = i/float(x-1)*t2 + float(x-1-i)/float((x-1))*t1;
    t_field.set(t3, i, 2, 0);
  }
  //  t_field.set(t1, 0, 1, 0);
  //t_field.set(t2, 9, 1, 0);
  display.myVtkTensor(t_field, offset, window, scalars, resolution, 1, coloring);

}

void generateRandomTensorfield(int argc, char *argv[])
{  
  int x=20;
  int y=20; 
  int z=1;
  
  int resolution = 20;
  int coloring = 0;

  int offset[3] = {0, 0, 0};
  int window[3] = {x, y, z};
  vtkScalars *scalars=NULL; 
  float t11 = 1000;
  float t12 = 0;
  float t13 = 0;
  float t21 = 0;
  float t22 = 500;
  float t23 = 0;
  float t31 = 0;
  float t32 = 0;
  float t33 = 400;

  float maxrotation = 180;
  cout << "Enter the 9 values of the tensor to display:  \n"
    "e.g. 1000 0 0 0 500 0 0 0 400 ";cin >> t11 >> t12 >> t13 >> t21 >> t22 >> t23 >> t31 >> t32 >> t33;
  cout <<"Resolution for vtk: ";cin >>resolution; 
  srand(0);
  cout << "Maximum rotation of tensors: ";cin >>maxrotation;
  cout << "Background 0=black, 1=white: ";cin >>coloring;
  tensorfield t_field(x, y, z);
  eigenfield  e_field(x, y, z);
  display display(x, y, z);

  tensor t1(t11, t12, t13, t21, t22, t23, t31, t32, t33);
  eigen  e1;
  e1.tensor2eigen(t1);
  
  float randomval1 = 0;
  float randomval2 = 0;
  float randomval3 = 0;

  for(int j=0; j<y; j++){
    for(int i=0; i<x; i++){
      randomval1 = t11 * rand()/RAND_MAX;
      randomval2 = t22 * rand()/RAND_MAX;
      randomval3 = t33 * rand()/RAND_MAX;
      sort(randomval3,randomval2,randomval1);
      t1.set(randomval1, 0, 0, 0, randomval2, 0, 0, 0, randomval3);
      e1.tensor2eigen(t1);
      randomval1 = maxrotation * rand()/RAND_MAX;
      randomval2 = maxrotation * rand()/RAND_MAX;
      randomval3 = maxrotation * rand()/RAND_MAX;
      e1.rotate(randomval1, randomval2, randomval3);
      e_field.set(e1, i, j, 0);
    }
  }
  e_field.eigen2tensor(t_field);

  int selection;
  int filterlength;
  cout <<"No smoothing (0) Smooth eigenvectors (1) smooth tensors (2)\n";
  cin >>selection;
  if(selection==1){
    e_field.smooth();
  }
  if(selection==2){
    cout << "Filterlength \n";
    cin >> filterlength;
    t_field.smooth(filterlength);
  }

  display.myVtkTensor(t_field, offset, window, scalars, resolution, 1, coloring);
 
}

void rigidTransformSynthetic(int argc, char *argv[]){

  int numberOfDim = 2;
  cout <<"Rigid transform synthetic field in 2=2D or 3=3D: ";cin >> numberOfDim;
  int xdim = 20;
  int ydim = 20;
  int zdim = 1;
  if(numberOfDim == 3) zdim = 10;
  float deltax = 1.0;
  float deltay = 1.0;
  float deltaz = 1.0;

  int out_xdim = xdim;
  int out_ydim = ydim;
  int out_zdim = zdim;

  tensorfield t_field(xdim, ydim, zdim);
  eigenfield  e_field(xdim, ydim, zdim);
  display     display(t_field);
  vtkScalars *scalars = vtkScalars::New();
  scalars = NULL;

  int offset[3] = {0,0,0};
  int window[3] = {xdim, ydim, zdim};

  // Resample a data set according to a given transform.
  int i = 0;

  int nStatVoxels = 0;

  int startNumber;
  int endNumber;
  int increment;

  int nrow;
  int ncol;
  int nslice;

  float drow;
  float dcol;
  float dslice;

  float transform[12];
  float invRotation[9];
  float invTranslate[3];

  double parameters[9] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0};
  int numparms = 9;

  char *outPrefix = NULL;
  int outnrow;
  int outncol;
  int outnslice;

  float outdrow;
  float outdcol;
  float outdslice;

  int interpStyle = 2;

  float targetOriginRow;
  float targetOriginCol;
  float targetOriginSlice;

  if(numberOfDim==3)  makeSynthField3D( e_field, t_field);
  else                makeSynthField2D( e_field, t_field);
  //display.max_eigenval(e_field, offset, window);

  startNumber = 1;//atoi(argv[2]);
  endNumber   = zdim;//atoi(argv[3]);
  increment   = 1;//atoi(argv[4]);

  nrow        = xdim;//atoi(argv[5]);
  ncol        = ydim;//atoi(argv[6]);
  nslice      = zdim;//atoi(argv[7]);

  drow        = deltax;//atof(argv[8]);
  dcol        = deltay;//atof(argv[9]);
  dslice      = deltaz;//atof(argv[10]);

  char * paramfile = new char[1024];
  cout << "enter location of the parameterfile (eg syntheticfield/final-param-file) : "; cin >> paramfile;
  numparms = readParametersFromFile(paramfile ,parameters, 9); // argv was 11
  if (numparms <= 0) {
    fprintf(stderr,"Failed to read any parameters from '%s'\n", paramfile); // argv was 11
    exit(1);
  }
  cout << "Parameters are: "<<parameters[0]<<", "<<parameters[1]<<", "<<parameters[2]<<"; "
       <<parameters[3]<<", "<<parameters[4]<<", "<<parameters[5]<<"; "<<parameters[6]<<", "<<parameters[7]<<", "<<parameters[8]<<endl;

  convertParmsToTransform(parameters, transform, numparms, 0);

  //outPrefix   = argv[12];

  outnrow     = out_xdim;//atoi(argv[13]);
  outncol     = out_ydim;//atoi(argv[14]);
  outnslice   = out_zdim;//atoi(argv[15]);

  outdrow     = deltax;  //atof(argv[16]);
  outdcol     = deltay;  //atof(argv[17]);
  outdslice   = deltaz;  //atof(argv[18]);

  cout << "enter interpolation style (2=Nearest Neighbour|1=linear): "; cin >> interpStyle;
  //interpStyle = atoi(argv[2]); // argv was 19

  targetOriginRow = out_xdim;  //atoi(argv[3]); // argv was 20
  targetOriginCol = out_ydim;  //atoi(argv[4]); // argv was 21
  targetOriginSlice = out_zdim;//atoi(argv[5]); // argv was 22

  /* Compute the inverse of the source to target transform */
  if (fast_invert_3x3_matrix(transform, invRotation) != 0) {
    fprintf(stderr,"Can't invert transform rotation\n");
    exit(1);
  }
  invTranslate[0] = -transform[9];
  invTranslate[1] = -transform[10];
  invTranslate[2] = -transform[11];

  float *in1  = new float[xdim*ydim*zdim];
  float *in2  = new float[xdim*ydim*zdim];
  float *in3  = new float[xdim*ydim*zdim];
  float *in4  = new float[xdim*ydim*zdim];
  float *in5  = new float[xdim*ydim*zdim];
  float *in6  = new float[xdim*ydim*zdim];

  float *out1 = new float[out_xdim*out_ydim*out_zdim];
  float *out2 = new float[out_xdim*out_ydim*out_zdim];
  float *out3 = new float[out_xdim*out_ydim*out_zdim];
  float *out4 = new float[out_xdim*out_ydim*out_zdim];
  float *out5 = new float[out_xdim*out_ydim*out_zdim];
  float *out6 = new float[out_xdim*out_ydim*out_zdim];

  t_field.separate(in1, in2, in3, in4, in5, in6);

  if (resampleFromSourceIntoTargetFloat(
    0, outnrow - 1, 0, outncol - 1, 0, outnslice - 1, invRotation, invTranslate, nrow, ncol, nslice, drow, dcol, dslice,
    in1, outnrow, outncol, outnslice, outdrow, outdcol, outdslice, targetOriginRow, targetOriginCol, targetOriginSlice,
    out1, interpStyle) != 0) { fprintf(stderr, "Failed to resample the volume\n");    exit(1);
  }
  if (resampleFromSourceIntoTargetFloat(
    0, outnrow - 1, 0, outncol - 1, 0, outnslice - 1, invRotation, invTranslate, nrow, ncol, nslice, drow, dcol, dslice,
    in2, outnrow, outncol, outnslice, outdrow, outdcol, outdslice, targetOriginRow, targetOriginCol, targetOriginSlice,
    out2, interpStyle) != 0) { fprintf(stderr, "Failed to resample the volume\n");    exit(1);
  }
  if (resampleFromSourceIntoTargetFloat(
    0, outnrow - 1, 0, outncol - 1, 0, outnslice - 1, invRotation, invTranslate, nrow, ncol, nslice, drow, dcol, dslice,
    in3, outnrow, outncol, outnslice, outdrow, outdcol, outdslice, targetOriginRow, targetOriginCol, targetOriginSlice,
    out3, interpStyle) != 0) { fprintf(stderr, "Failed to resample the volume\n");    exit(1);
  }
  if (resampleFromSourceIntoTargetFloat(
    0, outnrow - 1, 0, outncol - 1, 0, outnslice - 1, invRotation, invTranslate, nrow, ncol, nslice, drow, dcol, dslice,
    in4, outnrow, outncol, outnslice, outdrow, outdcol, outdslice, targetOriginRow, targetOriginCol, targetOriginSlice,
    out4, interpStyle) != 0) { fprintf(stderr, "Failed to resample the volume\n");    exit(1);
  }
  if (resampleFromSourceIntoTargetFloat(
    0, outnrow - 1, 0, outncol - 1, 0, outnslice - 1, invRotation, invTranslate, nrow, ncol, nslice, drow, dcol, dslice,
    in5, outnrow, outncol, outnslice, outdrow, outdcol, outdslice, targetOriginRow, targetOriginCol, targetOriginSlice,
    out5, interpStyle) != 0) { fprintf(stderr, "Failed to resample the volume\n");    exit(1);
  }
  if (resampleFromSourceIntoTargetFloat(
    0, outnrow - 1, 0, outncol - 1, 0, outnslice - 1, invRotation, invTranslate, nrow, ncol, nslice, drow, dcol, dslice,
    in6, outnrow, outncol, outnslice, outdrow, outdcol, outdslice, targetOriginRow, targetOriginCol, targetOriginSlice,
    out6, interpStyle) != 0) { fprintf(stderr, "Failed to resample the volume\n");    exit(1);
  }

  t_field.merge( out1, out2, out3, out4, out5, out6);

  int resample;
  cout <<"Resample tensor data 0=no, 1=yes: ";cin >>resample;
  if(resample==1){
    resampleTensorData( t_field, 0, 0, 0, -parameters[3], -parameters[4], -parameters[5]);
    //resampleTensorData( t_field, 0, 0, 0, 0, 0, 45);
  }

  //display.greyarray(in1, xdim, ydim, zdim);
  //display.greyarray(out1, xdim, ydim, zdim);

  e_field.tensor2eigen(t_field);
  //display.max_eigenval(e_field, offset, window);
  //e_field.rotate(0, 0, 45);
  //display.max_eigenval(e_field, offset, window);

  //e_field.rotate(parameters[4], parameters[3], parameters[5]);
  //e_field.scale( parameters[6], parameters[7], parameters[8]);
  //e_field.eigen2tensor(t_field);
  
  int resolution, coloring;
  cout <<"Resolution for vtk: ";cin >>resolution; 
  cout << "Background 0=black, 1=white: ";cin >>coloring;
  display.myVtkTensor(t_field, offset, window, scalars, resolution, 1, coloring);
  //display.myVtkTensor(t_field, offset, window, scalars, 6); 

  char a;
  cout << "Press any key to end "; cin >> a;
  
  cout << "\n------------------------------------------------ bye\n\n";
  
  delete[] in1;
  delete[] in2;
  delete[] in3;
  delete[] in4;
  delete[] in5;
  delete[] in6;
  delete[] out1;
  delete[] out2;
  delete[] out3;
  delete[] out4;
  delete[] out5;
  delete[] out6;
  
}


void testSVD(int argc, char *argv[]){

  float t11 = 1000;
  float t12 = 0;
  float t13 = 0;
  float t21 = 0;
  float t22 = 500;
  float t23 = 0;
  float t31 = 0;
  float t32 = 0;
  float t33 = 400;

  float r1  = 0;
  float r2  = 0;
  float r3  = 0;

  tensor orig, A, AT, U, VT, V, Sigma, result, UVT, UVTT;
  cout << "Enter the 9 values of the tensor:  \n"
    "e.g. 1000 0 0 0 500 0 0 0 400 ";
  cin >> t11 >> t12 >> t13 >> t21 >> t22 >> t23 >> t31 >> t32 >> t33;

  orig.set(t11, t12, t13, t21, t22, t23, t31, t32, t33);
  orig.svd(r1, r2, r3, U, VT);
  VT.trans(V);
  Sigma.set(r1, r2, r3, 0, 0, 0);
  //cout << temp;
  A = V*Sigma*VT;
  A.trans(AT);
  result = AT*orig*A;
  cout <<"det(A): "<<A.det()<<" det(AT): "<< AT.det() << " det(AT*orig*A): "<<result.det()<<" det(orig): "<<orig.det()<<endl;
  cout <<"det(U): "<<U.det()<<endl;
  cout <<"det(V): "<<V.det()<<" det(VT): "<< VT.det() <<endl;
  UVT = U*VT;
  cout <<"det(U*VT): "<<UVT.det()<< endl;

  cout << "orig: "<<orig;
  cout << "U: "<<U;
  cout << "Sigma: "<<Sigma;
  cout << "V: "<<V;
  cout << "VT: "<<VT;
  cout << "A = V*Sigma*VT: "<<A;
  cout << "AT: "<<AT;
  cout << "U*VT: "<<U*VT;
  cout << "result = AT*orig*A: "<<AT*orig*A;
  cout << "orig = U*Sigma*VT: "<<U*Sigma*VT;
  UVT.trans(UVTT);
  cout << "result = (U*VT)'*orig*(U*VT): "<<UVTT*orig*UVT;
  cout << "det(result = (U*VT)'*orig*(U*VT)): "<<result.det();

}




void testFindPointsInTensorfield(int argc, char *argv[])
{
  char stationarypath[1024];
  cout << "Enter path to tensor data set: "; cin >>  stationarypath;

  float ** stationary         = NULL;
  float ** moving             = NULL;
  tensorfield * stationary_t_field;
  eigenfield  * stationary_e_field;
  stationary_t_field        = new tensorfield(stationarypath);
  stationary_e_field        = new eigenfield(stationarypath);

  int scales = 1;
  int x, y, z;
  stationary_t_field->getDimension(x, y, z);

  stationary                  = new float * [scales];
  moving                      = new float * [scales];

  stationary[0]   = new float[x*y*z];
  moving[0]       = new float[x*y*z];
  
  display display(x, y, z);
  float * pointarray1 = new float[x*y*z];

  for(int i=0; i<x*y*z; i++){                                          // generate the grayarrays to perform the matching on them
    stationary[0][i] = 200*(*stationary_e_field->get(i)).anisotropy();  // anisotropy goes from 0..1 -> scale to MRI-range
    //moving[0][i] = 200*(*moving_e_field->get(i)).anisotropy(); 
  }

  int neighbourhood, pointmethod;
  float sigmathresh, threshold;
  cout <<"Enter neighbourhood, sigmathresh, pointmethod for pointselection e.g. 3 0.01 0: "; cin >> neighbourhood >> sigmathresh >> pointmethod;
  cout <<"Enter threshold e.g. 0.0005: "; cin >> threshold;
  findPointsInFloatArray(stationary[0], pointarray1, x, y, z, neighbourhood, sigmathresh, pointmethod); 
  Threshold(pointarray1, x, y, z, threshold);


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

  char a;
  cout <<"Enter a character to end: "; cin >> a;

}