/**********************************************************************************
 * Copyright 2010 Christoph Pflaum 
 * 		Department Informatik Lehrstuhl 10 - Systemsimulation
 *		Friedrich-Alexander Universität Erlangen-Nürnberg
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 **********************************************************************************/


#include "../mympi.h"
#include "../abbrevi.h"
#include "../parameter.h"
#include "../math_lib/math_lib.h"
#include "../basics/basic.h"
#include "../grid/elements.h"
#include "../grid/parti.h"
#include "../grid/ug.h"
#include "../grid/examples_ug.h"
#include "../grid/blockgrid.h"
#include "../grid/marker.h"
#include "../extemp/extemp.h"
#include "../extemp/parallel.h"
#include "../extemp/variable.h"
#include "../extemp/cellvar.h"
#include "../extemp/co_fu.h"
#include "../extemp/functor.h"
#include "interpol.h"
//#include "customtime.h"
#include <iomanip>
#include "assert.h"


/////////////////////////////////////////////////////////////
// 1. Interpolate from  blockgrid to rectangular blockgrid
/////////////////////////////////////////////////////////////


bool contained_in_tet(D3vector lam) {
  if(lam.x < -0.1)                 return false;
  if(lam.y < -0.1)                 return false;
  if(lam.z < -0.1)                 return false;
  if(lam.x + lam.y + lam.z > 1.1)  return false;
  return true;
}

bool contained_in_tet_strong(D3vector lam) {
    double limit = 0.2; // 0.2
  if(lam.x < -limit)                 return false;
  if(lam.y < -limit)                 return false;
  if(lam.z < -limit)                 return false;
  if(lam.x + lam.y + lam.z > 1+limit)  return false;
  return true;
}

bool new_lam_better(D3vector lam_old, D3vector lam_new) {

    if (MIN(lam_new ) < -0.2 || MAX(lam_new) > 1.2) return false;
    if (MIN(lam_old ) < -0.2 || MAX(lam_old) > 1.2) return true;

    if( (MIN(lam_new ) < 0 || MAX(lam_new) >1 )&&  (MIN(lam_old ) >= 0 || MAX(lam_old) <=1 )) return false;
    if (MIN(lam_new) > MIN(lam_old)) return true;
    if (MAX(lam_new) < MAX(lam_old)) return true;
    return false;
}

bool new_lam_worse(D3vector lam_old, D3vector lam_new) {
  if(MIN(lam_new) < MIN(lam_old) &&  MIN(lam_old) < -0.2) return true;
  if(MAX(lam_new) > MAX(lam_old) &&  MAX(lam_old) >  1.2) return true;
  return false;
}

/*

Intermadiate_grid_for_PointInterpolator::Intermadiate_grid_for_PointInterpolator(int nx_, int ny_, int nz_, Variable<double>* U_from)
{
    nx = nx_;
    ny = ny_;
    nz = nz_;
 
    if(nx<=2) nx = 3;
    if(ny<=2) ny = 3;
    if(nz<=2) nz = 3;
     
    Blockgrid* blockgrid_from = U_from->Give_blockgrid();
    
    //Variable<double> coordXYZ(*blockgrid);
    X_coordinate Xc(*blockgrid_from);
    Y_coordinate Yc(*blockgrid_from);
    Z_coordinate Zc(*blockgrid_from);
    pWSD.x = Minimum(Xc);    pWSD.y = Minimum(Yc);    pWSD.z = Minimum(Zc);
    pENT.x = Maximum(Xc);    pENT.y = Maximum(Yc);    pENT.z = Maximum(Zc);  

    
    interpolatorStructured = new Interpolate_on_structured_grid(nx,ny,nz, pWSD, pENT, *blockgrid_from);
    
}
*/
Interpolate_on_structured_grid::~Interpolate_on_structured_grid() {
  delete[] ids_hex;
  delete[] ids_i;
  delete[] ids_j;
  delete[] ids_k;

  delete[] typ_tet;
    delete[] lambda;
}

void Interpolate_on_structured_grid::update_Interpolate_on_structured_grid(Blockgrid &blockgrid_)
{
        int Nx, Ny, Nz;
        int typ;

        int ilmin, jlmin, klmin;
        int ilmax, jlmax, klmax;

        double factor = 0.1;
        //  double factor = 0.00001;

        D3vector lam;



          //Variable<double> coordXYZ(*blockgrid);
          X_coordinate Xc(blockgrid_);
          Y_coordinate Yc(blockgrid_);
          Z_coordinate Zc(blockgrid_);
          //D3vector pWSD, pENT;
          pWSD.x = Minimum(Xc);    pWSD.y = Minimum(Yc);    pWSD.z = Minimum(Zc);
          pENT.x = Maximum(Xc);    pENT.y = Maximum(Yc);    pENT.z = Maximum(Zc);

        blockgrid = &blockgrid_;
        ug = blockgrid->Give_unstructured_grid();



        if(nx>1)
          hx = (pENT.x - pWSD.x) / (nx-1);
        else
          hx = 1.0;
        if(ny>1)
          hy = (pENT.y - pWSD.y) / (ny-1);
        else
          hy = 1.0;
        if(nz>1)
          hz = (pENT.z - pWSD.z) / (nz-1);
        else
          hz = 1.0;

        int num_total = nx * ny * nz;

        D3vector cWSD, cESD;
        D3vector cWND, cEND;

        D3vector cWST, cEST;
        D3vector cWNT, cENT;

        D3vector boxWSD, boxENT;

        D3vector ploc;

//        ids_hex = new int[num_total];

//        ids_i = new int[num_total];
//        ids_j = new int[num_total];
//        ids_k = new int[num_total];

//        typ_tet = new int[num_total];

//        lambda = new D3vector[num_total];

        for(int i=0;i<num_total;++i) ids_hex[i] = -1;

        for(int id_hex=0;id_hex<ug->Give_number_hexahedra();++id_hex) {
            Nx = blockgrid->Give_Nx_hexahedron(id_hex);
            Ny = blockgrid->Give_Ny_hexahedron(id_hex);
            Nz = blockgrid->Give_Nz_hexahedron(id_hex);

            for(int k=0;k<Nz;++k)
          for(int j=0;j<Ny;++j)
            for(int i=0;i<Nx;++i) {
                  // corner points of general hex-cell
              cWSD = blockgrid->Give_coord_hexahedron(id_hex,i,  j,  k  );
              cESD = blockgrid->Give_coord_hexahedron(id_hex,i+1,j  ,k  );
              cWND = blockgrid->Give_coord_hexahedron(id_hex,i,  j+1,k  );
              cEND = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k  );

              cWST = blockgrid->Give_coord_hexahedron(id_hex,i,  j,  k+1);
              cEST = blockgrid->Give_coord_hexahedron(id_hex,i+1,j  ,k+1);
              cWNT = blockgrid->Give_coord_hexahedron(id_hex,i,  j+1,k+1);
              cENT = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k+1);

                  // bounding box calculation
              boxWSD.x = MIN(MIN(MIN(cWSD.x,cESD.x),MIN(cWND.x,cEND.x)),
                     MIN(MIN(cWST.x,cEST.x),MIN(cWNT.x,cENT.x))) - factor *hx;
              boxWSD.y = MIN(MIN(MIN(cWSD.y,cESD.y),MIN(cWND.y,cEND.y)),
                     MIN(MIN(cWST.y,cEST.y),MIN(cWNT.y,cENT.y))) - factor *hy;
              boxWSD.z = MIN(MIN(MIN(cWSD.z,cESD.z),MIN(cWND.z,cEND.z)),
                     MIN(MIN(cWST.z,cEST.z),MIN(cWNT.z,cENT.z))) - factor *hz;

              boxENT.x = MAX(MAX(MAX(cWSD.x,cESD.x),MAX(cWND.x,cEND.x)),
                     MAX(MAX(cWST.x,cEST.x),MAX(cWNT.x,cENT.x))) + factor *hx;
              boxENT.y = MAX(MAX(MAX(cWSD.y,cESD.y),MAX(cWND.y,cEND.y)),
                     MAX(MAX(cWST.y,cEST.y),MAX(cWNT.y,cENT.y))) + factor *hy;
              boxENT.z = MAX(MAX(MAX(cWSD.z,cESD.z),MAX(cWND.z,cEND.z)),
                     MAX(MAX(cWST.z,cEST.z),MAX(cWNT.z,cENT.z))) + factor *hz;

              // calculation of indices of a collection of cells of structured grid which contains bounding box
              ilmin = Ganzzahliger_Anteil((boxWSD.x - pWSD.x) / hx);
              jlmin = Ganzzahliger_Anteil((boxWSD.y - pWSD.y) / hy);
              klmin = Ganzzahliger_Anteil((boxWSD.z - pWSD.z) / hz);


              ilmax = Ganzzahliger_Anteil((boxENT.x - pWSD.x) / hx);
              jlmax = Ganzzahliger_Anteil((boxENT.y - pWSD.y) / hy);
              klmax = Ganzzahliger_Anteil((boxENT.z - pWSD.z) / hz);

              /*
              cout << " indices: "
               << " ilmin: " << ilmin
               << " jlmin: " << jlmin
               << " klmin: " << klmin
               << " ilmax: " << ilmax
               << " jlmax: " << jlmax
               << " klmax: " << klmax
               << " boxWSD.x: " << boxWSD.x
               << " cWSD.x: " << cWSD.x
               << " Nx: " <<  Nx
               << endl;
              */
              /*
      bool now;
      if(boxWSD.z < 0 && boxENT.z > 0.0 && boxWSD.y < 0.5 && boxENT.y > 0.5 && boxWSD.x < 1.0 && boxENT.x > 1.0 ) {
        cout << "\n \n WSD   : ";  boxWSD.Print();
        cout << "\n ENT  : ";  boxENT.Print();
        now = true;
       }
       else now = false;


       cout << " tt: " << boxWSD.x << " " << pWSD.x << " " << hx << endl;
       cout << (boxWSD.x - pWSD.x) << endl;

       cout << " z: " << 0.1 << " g: " << Ganzzahliger_Anteil(0.1) << endl;
       cout << " z: " << -0.1 << " g: " << Ganzzahliger_Anteil(-0.1) << endl;

       cout << " z: " << 5.1 << " g: " << Ganzzahliger_Anteil(5.1) << endl;
       cout << " z: " << -5.1 << " g: " << Ganzzahliger_Anteil(-5.1) << endl;
              */

              if(ilmin<0) ilmin=0;
              if(jlmin<0) jlmin=0;
              if(klmin<0) klmin=0;

              for(int il = ilmin; (il <= ilmax) && (il < nx);++il)
                for(int jl = jlmin; (jl <= jlmax) && (jl < ny);++jl)
              for(int kl = klmin; (kl <= klmax) && (kl < nz);++kl) {

                ploc = D3vector(il * hx, jl * hy, kl * hz) + pWSD;

                //		  cout << "HI" << endl;

                typ = -1;

                lam = lambda_of_p_in_tet(ploc,cWND,cWNT,cWST,cEST);
                if(contained_in_tet(lam)) typ=0;
                else {
                  lam = lambda_of_p_in_tet(ploc,cEST,cWND,cWST,cESD);
                  if(contained_in_tet(lam)) typ=1;
                  else {
                    lam = lambda_of_p_in_tet(ploc,cWND,cWSD,cWST,cESD);
                    if(contained_in_tet(lam)) typ=2;
                    else {
                  lam = lambda_of_p_in_tet(ploc,cEST,cWND,cESD,cEND);
                  if(contained_in_tet(lam)) typ=3;
                  else {
                    lam = lambda_of_p_in_tet(ploc,cENT,cWNT,cEST,cEND);
                    if(contained_in_tet(lam)) typ=4;
                    else {
                      lam = lambda_of_p_in_tet(ploc,cWNT,cWND,cEST,cEND);
                      if(contained_in_tet(lam)) typ=5;
                    }
                  }
                    }
                  }
                }

                /*
                cout << " typ " << typ << id_hex
                     << " il: " << il
                     << " jl: " << jl
                     << " kl: " << kl
                     << endl;
                */

                if(typ!=-1) {
                  int ind_global;
                  ind_global = il+nx*(jl+ny*kl);
                  bool stop;
                  stop=false;

                  if(ids_hex[ind_global]!=-1) {
                    stop=new_lam_worse(lambda[ind_global],lam);
                  }

                  if(stop==false) {
                    ids_hex[ind_global] = id_hex;
                    ids_i[ind_global] = i;
                    ids_j[ind_global] = j;
                    ids_k[ind_global] = k;

                    typ_tet[ind_global] = typ;

                    lambda[ind_global] = lam;
                  }
                  //go_on = false;
                }


              }
            }
        }


        for(int i=0;i<num_total;++i) {
          if(ids_hex[i]==-1) {
            // wir nehmen default value!!
            /*
            cout << i
             << " Error: Interpolate_on_structured_grid: I cannot interpolate all data!"
             << endl;
            ids_hex[i] = 0;
            */
          }
          else {
            //cout << i << " Interpolate_on_structured_grid: o.k.!" << endl;
          }
        }
      }

Interpolate_on_structured_grid::Interpolate_on_structured_grid(int nx_, int ny_, int nz_,
                                   D3vector pWSD, D3vector pENT,
                                   Blockgrid& blockgrid_) {
  int Nx, Ny, Nz;
  // int typ;

  assert(nx_ > 1);
  assert(ny_ > 1);
  assert(nz_ > 1);

  //int ilmin, jlmin, klmin;
  //int ilmax, jlmax, klmax;

  double factor = 0.1;
  //  double factor = 0.00001;

  // D3vector lam;

  blockgrid = &blockgrid_;
  ug = blockgrid->Give_unstructured_grid();

  nx = nx_;
  ny = ny_;
  nz = nz_;

  if(nx_>1)
    hx = (pENT.x - pWSD.x) / (nx_-1);
  else
    hx = 1.0;
  if(ny_>1)
    hy = (pENT.y - pWSD.y) / (ny_-1);
  else
    hy = 1.0;
  if(nz_>1)
    hz = (pENT.z - pWSD.z) / (nz_-1);
  else
    hz = 1.0;

  int num_total = nx * ny * nz;

  /*
  D3vector cWSD, cESD;
  D3vector cWND, cEND;

  D3vector cWST, cEST;
  D3vector cWNT, cENT;
*/

  // D3vector boxWSD, boxENT;

  // D3vector ploc;

  ids_hex = new int[num_total];

  ids_i = new int[num_total];
  ids_j = new int[num_total];
  ids_k = new int[num_total];

  typ_tet = new int[num_total];

  lambda = new D3vector[num_total];

  for(int i=0;i<num_total;++i) ids_hex[i] = -1;

  for(int id_hex=0;id_hex<ug->Give_number_hexahedra();++id_hex) {
      Nx = blockgrid->Give_Nx_hexahedron(id_hex);
      Ny = blockgrid->Give_Ny_hexahedron(id_hex);
      Nz = blockgrid->Give_Nz_hexahedron(id_hex);

#pragma omp parallel for num_threads(UGBlocks::numThreadsToTake) if(UGBlocks::useOpenMP)
      for(int k=0;k<Nz;++k)
    for(int j=0;j<Ny;++j)
      for(int i=0;i<Nx;++i) {
            // corner points of general hex-cell
        D3vector cWSD = blockgrid->Give_coord_hexahedron(id_hex,i,  j,  k  );
        D3vector cESD = blockgrid->Give_coord_hexahedron(id_hex,i+1,j  ,k  );
        D3vector cWND = blockgrid->Give_coord_hexahedron(id_hex,i,  j+1,k  );
        D3vector cEND = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k  );

        D3vector cWST = blockgrid->Give_coord_hexahedron(id_hex,i,  j,  k+1);
        D3vector cEST = blockgrid->Give_coord_hexahedron(id_hex,i+1,j  ,k+1);
        D3vector cWNT = blockgrid->Give_coord_hexahedron(id_hex,i,  j+1,k+1);
        D3vector cENT = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k+1);

            // bounding box calculation
        D3vector boxWSD, boxENT;
        boxWSD.x = MIN(MIN(MIN(cWSD.x,cESD.x),MIN(cWND.x,cEND.x)),
               MIN(MIN(cWST.x,cEST.x),MIN(cWNT.x,cENT.x))) - factor *hx;
        boxWSD.y = MIN(MIN(MIN(cWSD.y,cESD.y),MIN(cWND.y,cEND.y)),
               MIN(MIN(cWST.y,cEST.y),MIN(cWNT.y,cENT.y))) - factor *hy;
        boxWSD.z = MIN(MIN(MIN(cWSD.z,cESD.z),MIN(cWND.z,cEND.z)),
               MIN(MIN(cWST.z,cEST.z),MIN(cWNT.z,cENT.z))) - factor *hz;

        boxENT.x = MAX(MAX(MAX(cWSD.x,cESD.x),MAX(cWND.x,cEND.x)),
               MAX(MAX(cWST.x,cEST.x),MAX(cWNT.x,cENT.x))) + factor *hx;
        boxENT.y = MAX(MAX(MAX(cWSD.y,cESD.y),MAX(cWND.y,cEND.y)),
               MAX(MAX(cWST.y,cEST.y),MAX(cWNT.y,cENT.y))) + factor *hy;
        boxENT.z = MAX(MAX(MAX(cWSD.z,cESD.z),MAX(cWND.z,cEND.z)),
               MAX(MAX(cWST.z,cEST.z),MAX(cWNT.z,cENT.z))) + factor *hz;

        // calculation of indices of a collection of cells of structured grid which contains bounding box
        int ilmin = Ganzzahliger_Anteil((boxWSD.x - pWSD.x) / hx);
        int jlmin = Ganzzahliger_Anteil((boxWSD.y - pWSD.y) / hy);
        int klmin = Ganzzahliger_Anteil((boxWSD.z - pWSD.z) / hz);


        int ilmax = Ganzzahliger_Anteil((boxENT.x - pWSD.x) / hx);
        int jlmax = Ganzzahliger_Anteil((boxENT.y - pWSD.y) / hy);
        int klmax = Ganzzahliger_Anteil((boxENT.z - pWSD.z) / hz);

        /*
        cout << " indices: "
         << " ilmin: " << ilmin
         << " jlmin: " << jlmin
         << " klmin: " << klmin
         << " ilmax: " << ilmax
         << " jlmax: " << jlmax
         << " klmax: " << klmax
         << " boxWSD.x: " << boxWSD.x
         << " cWSD.x: " << cWSD.x
         << " Nx: " <<  Nx
         << endl;
        */
        /*
bool now;
if(boxWSD.z < 0 && boxENT.z > 0.0 && boxWSD.y < 0.5 && boxENT.y > 0.5 && boxWSD.x < 1.0 && boxENT.x > 1.0 ) {
  cout << "\n \n WSD   : ";  boxWSD.Print();
  cout << "\n ENT  : ";  boxENT.Print();
  now = true;
 }
 else now = false;


 cout << " tt: " << boxWSD.x << " " << pWSD.x << " " << hx << endl;
 cout << (boxWSD.x - pWSD.x) << endl;

 cout << " z: " << 0.1 << " g: " << Ganzzahliger_Anteil(0.1) << endl;
 cout << " z: " << -0.1 << " g: " << Ganzzahliger_Anteil(-0.1) << endl;

 cout << " z: " << 5.1 << " g: " << Ganzzahliger_Anteil(5.1) << endl;
 cout << " z: " << -5.1 << " g: " << Ganzzahliger_Anteil(-5.1) << endl;
        */

        if(ilmin<0) ilmin=0;
        if(jlmin<0) jlmin=0;
        if(klmin<0) klmin=0;

        for(int il = ilmin; (il <= ilmax) && (il < nx_);++il)
          for(int jl = jlmin; (jl <= jlmax) && (jl < ny_);++jl)
        for(int kl = klmin; (kl <= klmax) && (kl < nz_);++kl) {

          D3vector ploc = D3vector(il * hx, jl * hy, kl * hz) + pWSD;

          //		  cout << "HI" << endl;

          int typ = -1;

          D3vector lam = lambda_of_p_in_tet(ploc,cWND,cWNT,cWST,cEST);
          if(contained_in_tet(lam)) typ=0;
          else {
            lam = lambda_of_p_in_tet(ploc,cEST,cWND,cWST,cESD);
            if(contained_in_tet(lam)) typ=1;
            else {
              lam = lambda_of_p_in_tet(ploc,cWND,cWSD,cWST,cESD);
              if(contained_in_tet(lam)) typ=2;
              else {
            lam = lambda_of_p_in_tet(ploc,cEST,cWND,cESD,cEND);
            if(contained_in_tet(lam)) typ=3;
            else {
              lam = lambda_of_p_in_tet(ploc,cENT,cWNT,cEST,cEND);
              if(contained_in_tet(lam)) typ=4;
              else {
                lam = lambda_of_p_in_tet(ploc,cWNT,cWND,cEST,cEND);
                if(contained_in_tet(lam)) typ=5;
              }
            }
              }
            }
          }

          /*
          cout << " typ " << typ << id_hex
               << " il: " << il
               << " jl: " << jl
               << " kl: " << kl
               << endl;
          */

          if(typ!=-1) {
            int ind_global;
            ind_global = il+nx*(jl+ny*kl);
            bool stop;
            stop=false;

            if(ids_hex[ind_global]!=-1) {
              stop=new_lam_worse(lambda[ind_global],lam);
            }

            #pragma omp critical
            if(stop==false) {
              ids_hex[ind_global] = id_hex;
              ids_i[ind_global] = i;
              ids_j[ind_global] = j;
              ids_k[ind_global] = k;

              typ_tet[ind_global] = typ;

              lambda[ind_global] = lam;
            }
            //go_on = false;
          }

          /*
          cout << " out "
               << " ilmin: " << ilmin
               << " ilmax: " << ilmax
               << " jlmin: " << jlmin
               << " jlmax: " << jlmax
               << " klmin: " << klmin
               << " klmax: " << klmax;
          cout << "\n   "; cWSD.Print();
          cout << "\n   "; cESD.Print();
          cout << "\n   "; cWND.Print();
          cout << "\n   "; cEND.Print();
          cout << "\n   "; cWST.Print();
          cout << "\n   "; cEST.Print();
          cout << "\n   "; cWNT.Print();
          cout << "\n   "; cENT.Print();
          cout << "\n   p: "; ploc.Print();

          cout << "\n   : ";  boxWSD.Print();
          cout << "\n   : ";  boxENT.Print();

          cout << endl;
          */
        }
      }
  }


  for(int i=0;i<num_total;++i) {
    if(ids_hex[i]==-1) {
      // wir nehmen default value!!
      /*
      cout << i
       << " Error: Interpolate_on_structured_grid: I cannot interpolate all data!"
       << endl;
      ids_hex[i] = 0;
      */
    }
    else {
      //cout << i << " Interpolate_on_structured_grid: o.k.!" << endl;
    }
  }
}

/////////////////////////////////////////////////////////////
// 2. Interpolate from  blockgrid  to  blockgrid
/////////////////////////////////////////////////////////////


Interpolate_on_structured_grid::Interpolate_on_structured_grid(int nx_, int ny_, int nz_,
							       Blockgrid& blockgrid_) {
  int Nx, Ny, Nz;
  int typ;

  
  
  
  assert(nx_ > 1);
  assert(ny_ > 1);
  assert(nz_ > 1);

  int ilmin, jlmin, klmin;
  int ilmax, jlmax, klmax;

  double factor = 0.1;
  //  double factor = 0.00001;

  D3vector lam;

     
    
    //Variable<double> coordXYZ(*blockgrid);
    X_coordinate Xc(blockgrid_);
    Y_coordinate Yc(blockgrid_);
    Z_coordinate Zc(blockgrid_);
    //D3vector pWSD, pENT;
    pWSD.x = Minimum(Xc);    pWSD.y = Minimum(Yc);    pWSD.z = Minimum(Zc);
    pENT.x = Maximum(Xc);    pENT.y = Maximum(Yc);    pENT.z = Maximum(Zc);  
  
  blockgrid = &blockgrid_;
  ug = blockgrid->Give_unstructured_grid();

  nx = nx_;
  ny = ny_;
  nz = nz_;

  if(nx_>1)
    hx = (pENT.x - pWSD.x) / (nx_-1);
  else  
    hx = 1.0;
  if(ny_>1)
    hy = (pENT.y - pWSD.y) / (ny_-1);
  else  
    hy = 1.0;
  if(nz_>1)
    hz = (pENT.z - pWSD.z) / (nz_-1);
  else 
    hz = 1.0;

  int num_total = nx * ny * nz;

  D3vector cWSD, cESD;
  D3vector cWND, cEND;

  D3vector cWST, cEST;
  D3vector cWNT, cENT;

  D3vector boxWSD, boxENT;

  D3vector ploc;

  ids_hex = new int[num_total];

  ids_i = new int[num_total];
  ids_j = new int[num_total];
  ids_k = new int[num_total];

  typ_tet = new int[num_total];

  lambda = new D3vector[num_total];

  for(int i=0;i<num_total;++i) ids_hex[i] = -1;

  for(int id_hex=0;id_hex<ug->Give_number_hexahedra();++id_hex) {
      Nx = blockgrid->Give_Nx_hexahedron(id_hex);
      Ny = blockgrid->Give_Ny_hexahedron(id_hex);
      Nz = blockgrid->Give_Nz_hexahedron(id_hex);

      for(int k=0;k<Nz;++k)
	for(int j=0;j<Ny;++j)
	  for(int i=0;i<Nx;++i) {
            // corner points of general hex-cell
	    cWSD = blockgrid->Give_coord_hexahedron(id_hex,i,  j,  k  );
	    cESD = blockgrid->Give_coord_hexahedron(id_hex,i+1,j  ,k  );
	    cWND = blockgrid->Give_coord_hexahedron(id_hex,i,  j+1,k  );
	    cEND = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k  );

	    cWST = blockgrid->Give_coord_hexahedron(id_hex,i,  j,  k+1);
	    cEST = blockgrid->Give_coord_hexahedron(id_hex,i+1,j  ,k+1);
	    cWNT = blockgrid->Give_coord_hexahedron(id_hex,i,  j+1,k+1);
	    cENT = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k+1);

            // bounding box calculation 
	    boxWSD.x = MIN(MIN(MIN(cWSD.x,cESD.x),MIN(cWND.x,cEND.x)),
			   MIN(MIN(cWST.x,cEST.x),MIN(cWNT.x,cENT.x))) - factor *hx;
	    boxWSD.y = MIN(MIN(MIN(cWSD.y,cESD.y),MIN(cWND.y,cEND.y)),
			   MIN(MIN(cWST.y,cEST.y),MIN(cWNT.y,cENT.y))) - factor *hy;
	    boxWSD.z = MIN(MIN(MIN(cWSD.z,cESD.z),MIN(cWND.z,cEND.z)),
			   MIN(MIN(cWST.z,cEST.z),MIN(cWNT.z,cENT.z))) - factor *hz;

	    boxENT.x = MAX(MAX(MAX(cWSD.x,cESD.x),MAX(cWND.x,cEND.x)),
			   MAX(MAX(cWST.x,cEST.x),MAX(cWNT.x,cENT.x))) + factor *hx;
	    boxENT.y = MAX(MAX(MAX(cWSD.y,cESD.y),MAX(cWND.y,cEND.y)),
			   MAX(MAX(cWST.y,cEST.y),MAX(cWNT.y,cENT.y))) + factor *hy;
	    boxENT.z = MAX(MAX(MAX(cWSD.z,cESD.z),MAX(cWND.z,cEND.z)),
			   MAX(MAX(cWST.z,cEST.z),MAX(cWNT.z,cENT.z))) + factor *hz;

	    // calculation of indices of a collection of cells of structured grid which contains bounding box
	    ilmin = Ganzzahliger_Anteil((boxWSD.x - pWSD.x) / hx);
	    jlmin = Ganzzahliger_Anteil((boxWSD.y - pWSD.y) / hy);
	    klmin = Ganzzahliger_Anteil((boxWSD.z - pWSD.z) / hz);
	    

	    ilmax = Ganzzahliger_Anteil((boxENT.x - pWSD.x) / hx);
	    jlmax = Ganzzahliger_Anteil((boxENT.y - pWSD.y) / hy);
	    klmax = Ganzzahliger_Anteil((boxENT.z - pWSD.z) / hz);

	    /*
	    cout << " indices: "
		 << " ilmin: " << ilmin 
		 << " jlmin: " << jlmin 
		 << " klmin: " << klmin 
		 << " ilmax: " << ilmax 
		 << " jlmax: " << jlmax 
		 << " klmax: " << klmax 
		 << " boxWSD.x: " << boxWSD.x
		 << " cWSD.x: " << cWSD.x
		 << " Nx: " <<  Nx
		 << endl;
	    */
	    /*	    
bool now;
if(boxWSD.z < 0 && boxENT.z > 0.0 && boxWSD.y < 0.5 && boxENT.y > 0.5 && boxWSD.x < 1.0 && boxENT.x > 1.0 ) {
  cout << "\n \n WSD   : ";  boxWSD.Print();
  cout << "\n ENT  : ";  boxENT.Print();
  now = true;
 }
 else now = false;

 
 cout << " tt: " << boxWSD.x << " " << pWSD.x << " " << hx << endl;
 cout << (boxWSD.x - pWSD.x) << endl;

 cout << " z: " << 0.1 << " g: " << Ganzzahliger_Anteil(0.1) << endl;
 cout << " z: " << -0.1 << " g: " << Ganzzahliger_Anteil(-0.1) << endl;

 cout << " z: " << 5.1 << " g: " << Ganzzahliger_Anteil(5.1) << endl;
 cout << " z: " << -5.1 << " g: " << Ganzzahliger_Anteil(-5.1) << endl;
	    */

	    if(ilmin<0) ilmin=0;
	    if(jlmin<0) jlmin=0;
	    if(klmin<0) klmin=0;

	    for(int il = ilmin; (il <= ilmax) && (il < nx_);++il)
	      for(int jl = jlmin; (jl <= jlmax) && (jl < ny_);++jl)
		for(int kl = klmin; (kl <= klmax) && (kl < nz_);++kl) {

		  ploc = D3vector(il * hx, jl * hy, kl * hz) + pWSD;

		  //		  cout << "HI" << endl;

		  typ = -1;

		  lam = lambda_of_p_in_tet(ploc,cWND,cWNT,cWST,cEST);
		  if(contained_in_tet(lam)) typ=0;
		  else {
		    lam = lambda_of_p_in_tet(ploc,cEST,cWND,cWST,cESD);
		    if(contained_in_tet(lam)) typ=1;
		    else {
		      lam = lambda_of_p_in_tet(ploc,cWND,cWSD,cWST,cESD);
		      if(contained_in_tet(lam)) typ=2;
		      else {
			lam = lambda_of_p_in_tet(ploc,cEST,cWND,cESD,cEND);
			if(contained_in_tet(lam)) typ=3;
			else {
			  lam = lambda_of_p_in_tet(ploc,cENT,cWNT,cEST,cEND);
			  if(contained_in_tet(lam)) typ=4;
			  else {
			    lam = lambda_of_p_in_tet(ploc,cWNT,cWND,cEST,cEND);
			    if(contained_in_tet(lam)) typ=5;
			  }
			}
		      }
		    }
		  }

		  /*		  
		  cout << " typ " << typ << id_hex 
		       << " il: " << il 
		       << " jl: " << jl 
		       << " kl: " << kl 
		       << endl; 
		  */

		  if(typ!=-1) {
		    int ind_global;
		    ind_global = il+nx*(jl+ny*kl);      
		    bool stop;
		    stop=false;

		    if(ids_hex[ind_global]!=-1) {
		      stop=new_lam_worse(lambda[ind_global],lam);
		    }

		    if(stop==false) {
		      ids_hex[ind_global] = id_hex;
		      ids_i[ind_global] = i;
		      ids_j[ind_global] = j;
		      ids_k[ind_global] = k;
		      
		      typ_tet[ind_global] = typ;
		      
		      lambda[ind_global] = lam;
		    }
		    //go_on = false;
		  }

		  /*
		  cout << " out "
		       << " ilmin: " << ilmin
		       << " ilmax: " << ilmax
		       << " jlmin: " << jlmin
		       << " jlmax: " << jlmax
		       << " klmin: " << klmin
		       << " klmax: " << klmax;
		  cout << "\n   "; cWSD.Print();
		  cout << "\n   "; cESD.Print();
		  cout << "\n   "; cWND.Print();
		  cout << "\n   "; cEND.Print();
		  cout << "\n   "; cWST.Print();
		  cout << "\n   "; cEST.Print();
		  cout << "\n   "; cWNT.Print();
		  cout << "\n   "; cENT.Print();
		  cout << "\n   p: "; ploc.Print();

		  cout << "\n   : ";  boxWSD.Print();
		  cout << "\n   : ";  boxENT.Print();

		  cout << endl;
		  */
		}
	  }
  }


  for(int i=0;i<num_total;++i) {
    if(ids_hex[i]==-1) {
      // wir nehmen default value!!
      /*
      cout << i 
	   << " Error: Interpolate_on_structured_grid: I cannot interpolate all data!"
	   << endl;
      ids_hex[i] = 0;
      */
    }
    else {
      //cout << i << " Interpolate_on_structured_grid: o.k.!" << endl;
    }
  }
}

/////////////////////////////////////////////////////////////
// 2. Interpolate from  blockgrid  to  blockgrid
/////////////////////////////////////////////////////////////


Interpolate_on_block_grid::Interpolate_on_block_grid(int nx_, int ny_, int nz_,
				                     Blockgrid* blockgrid_from, Blockgrid* blockgrid_to_) {
    nx = nx_;
    ny = ny_;
    nz = nz_;
 
    if(nx<=2) nx = 3;
    if(ny<=2) ny = 3;
    if(nz<=2) nz = 3;
     
    blockgrid_to = blockgrid_to_;
    
    //Variable<double> coordXYZ(*blockgrid);
    X_coordinate Xc(*blockgrid_to);
    Y_coordinate Yc(*blockgrid_to);
    Z_coordinate Zc(*blockgrid_to);
    
    pWSD.x = Minimum(Xc);    pWSD.y = Minimum(Yc);    pWSD.z = Minimum(Zc);
    pENT.x = Maximum(Xc);    pENT.y = Maximum(Yc);    pENT.z = Maximum(Zc);  

    interpolatorStructured = new Interpolate_on_structured_grid(nx,ny,nz, pWSD, pENT, *blockgrid_from);
    data = new double[nx*ny*nz];
    
    
    hx = (pENT.x - pWSD.x) / (nx-1);
    hy = (pENT.y - pWSD.y) / (ny-1);
    hz = (pENT.z - pWSD.z) / (nz-1);
    
    
    /*
    
    // test GGGG
    cout << "\n WSD: " ; pWSD.Print();
    cout << "\n ENT: " ; pENT.Print();
    cout << "nx: " << nx << " ny: " << ny << " nz: " << nz << endl;
    */
}
  


  
void Interpolate_on_block_grid::interpolate(Variable<double>* U_from, Variable<double>* U_to,
					    double defaultInterpolation) {
/*
   //test GGGG
   X_coordinate Xfrom(*U_from->Give_blockgrid());
  (*U_from) = Xfrom;  
*/  
    interpolatorStructured->interpolate<double>(*U_from,data,defaultInterpolation);

    /*
 //test GGGG
    for(int i=0;i<Nx;++i) for(int j=0;j<nz;++j) for(int k=0;k<nz;++k) 
       data[i    +nx*(j    +ny* k)] = hx * i;
      */
      
    Functor3<double,double,Interpolate_on_block_grid> myFunctor(this);
    
    X_coordinate Xc(*blockgrid_to);
    Y_coordinate Yc(*blockgrid_to);
    Z_coordinate Zc(*blockgrid_to);
    
    (*U_to) = myFunctor(Xc,Yc,Zc);
}


 
double Interpolate_on_block_grid::evaluate(double coord_x, double coord_y, double coord_z) {  
  if(coord_x > pENT.x) return 0.0;
  if(coord_x < pWSD.x) return 0.0;
  if(coord_y > pENT.y) return 0.0;
  if(coord_y < pWSD.y) return 0.0;
  if(coord_z > pENT.z) return 0.0;
  if(coord_z < pWSD.z) return 0.0;
  
  int i = (coord_x - pWSD.x) / hx;   
  int j = (coord_y - pWSD.y) / hy;   
  int k = (coord_z - pWSD.z) / hz;   
    
  if(i < 0)   i=0;     if(j <0   ) j=0;     if(k<0)     k=0;
  if(i>=nx-1) i=nx-2;  if(j>=ny-1) j=ny-2;  if(k>=nz-1) k=nz-2;
  
  //cout << "i: " << i << " j: " << j << " k: " << k << endl;

  double uWSD = data[i    +nx*(j    +ny* k)];
  double uESD = data[(i+1)+nx*(j    +ny* k)];
  double uWND = data[i    +nx*((j+1)+ny* k)];
  double uEND = data[(i+1)+nx*((j+1)+ny* k)];
  double uWST = data[i    +nx*(j    +ny*(k+1))];
  double uEST = data[(i+1)+nx*(j    +ny*(k+1))];
  double uWNT = data[i    +nx*((j+1)+ny*(k+1))];
  double uENT = data[(i+1)+nx*((j+1)+ny*(k+1))];
  
  
  // assert( (i+1)+nx*((j+1)+ny*(k+1)) < nx*ny*nz);
  
  double locX = (coord_x - pWSD.x) / hx - i;
  double locY = (coord_y - pWSD.y) / hy - j;
  double locZ = (coord_z - pWSD.z) / hz - k;
  

  return uWSD * (1.0 - locX) * (1.0 - locY) * (1.0 - locZ) +
         uESD *        locX  * (1.0 - locY) * (1.0 - locZ) +
         uWND * (1.0 - locX) *        locY  * (1.0 - locZ) +
         uEND *        locX  *        locY  * (1.0 - locZ) +
         uWST * (1.0 - locX) * (1.0 - locY) *        locZ  +
         uEST *        locX  * (1.0 - locY) *        locZ  +
         uWNT * (1.0 - locX) *        locY  *        locZ  +
         uENT *        locX  *        locY  *        locZ;
}
 
Interpolate_on_block_grid::~Interpolate_on_block_grid() {
    delete interpolatorStructured;
    delete[] data;
}


/////////////////////////////////////////////////////////////
// 3. Interpolate from Variable on a blockgrid to any point using structured intermediate grid  
/////////////////////////////////////////////////////////////

   

PointInterpolator::PointInterpolator(int nx_, int ny_, int nz_,
				     Variable<double>* U_from, double defaultInterpolation_) {
    defaultInterpolation = defaultInterpolation_;
    shiftx = 0.0;
    shifty = 0.0;
    shiftz = 0.0;
    nx = nx_;
    ny = ny_;
    nz = nz_;
 
    if(nx<=2) nx = 3;
    if(ny<=2) ny = 3;
    if(nz<=2) nz = 3;
     
    Blockgrid* blockgrid_from = U_from->Give_blockgrid();
    
    //Variable<double> coordXYZ(*blockgrid);
    X_coordinate Xc(*blockgrid_from);
    Y_coordinate Yc(*blockgrid_from);
    Z_coordinate Zc(*blockgrid_from);
    pWSD.x = Minimum(Xc);    pWSD.y = Minimum(Yc);    pWSD.z = Minimum(Zc);
    pENT.x = Maximum(Xc);    pENT.y = Maximum(Yc);    pENT.z = Maximum(Zc);  


    interpolatorStructured = new Interpolate_on_structured_grid(nx,ny,nz, pWSD, pENT, *blockgrid_from);

    data = new double[nx*ny*nz];
    
    
    hx = (pENT.x - pWSD.x) / (nx-1);
    hy = (pENT.y - pWSD.y) / (ny-1);
    hz = (pENT.z - pWSD.z) / (nz-1);

    interpolatorStructured->interpolate<double>(*U_from,data,defaultInterpolation_);

    
    /*
    
    // test GGGG
    cout << "\n WSD: " ; pWSD.Print();
    cout << "\n ENT: " ; pENT.Print();
    cout << "nx: " << nx << " ny: " << ny << " nz: " << nz << endl;
    */
}
  
PointInterpolator::PointInterpolator(int nx_, int ny_, int nz_,
				     D3vector pWSD_, D3vector pENT_,
				     Variable<double>* U_from, double defaultInterpolation_) {
    defaultInterpolation = defaultInterpolation_;
  
    shiftx = 0.0;
    shifty = 0.0;
    shiftz = 0.0;
    nx = nx_;
    ny = ny_;
    nz = nz_;
 
    if(nx<=2) nx = 3;
    if(ny<=2) ny = 3;
    if(nz<=2) nz = 3;
     
    Blockgrid* blockgrid_from = U_from->Give_blockgrid();
        
    pWSD = pWSD_;
    pENT = pENT_; 

    interpolatorStructured = new Interpolate_on_structured_grid(nx,ny,nz, pWSD, pENT, *blockgrid_from);
    data = new double[nx*ny*nz];
    
    
    hx = (pENT.x - pWSD.x) / (nx-1);
    hy = (pENT.y - pWSD.y) / (ny-1);
    hz = (pENT.z - pWSD.z) / (nz-1);
    
    
    interpolatorStructured->interpolate<double>(*U_from,data,defaultInterpolation);
    
   
    
    
    /*
    
    // test GGGG
    cout << "\n WSD: " ; pWSD.Print();
    cout << "\n ENT: " ; pENT.Print();
    cout << "nx: " << nx << " ny: " << ny << " nz: " << nz << endl;
    */
}

PointInterpolator::PointInterpolator(Interpolate_on_structured_grid* intermediateGrid, Variable<double>* U_from, double defaultInterpolation_)
{
    defaultInterpolation = defaultInterpolation_;
  
    nx = intermediateGrid->nx;
    ny = intermediateGrid->ny;
    nz = intermediateGrid->nz;
    
    data = new double[nx*ny*nz];
    
    pENT = intermediateGrid->pENT;
    pWSD = intermediateGrid->pWSD;
    
    shiftx = 0.0;
    shifty = 0.0;
    shiftz = 0.0;
    hx = (pENT.x - pWSD.x) / (nx-1);
    hy = (pENT.y - pWSD.y) / (ny-1);
    hz = (pENT.z - pWSD.z) / (nz-1);

    intermediateGrid->interpolate<double>(*U_from,data,defaultInterpolation_);
}

PointInterpolator::PointInterpolator(Interpolate_on_structured_grid *intermediateGrid, double defaultInterpolation_)
{
    defaultInterpolation = defaultInterpolation_;

    nx = intermediateGrid->nx;
    ny = intermediateGrid->ny;
    nz = intermediateGrid->nz;

    data = new double[nx*ny*nz];
    dataCounter = new int[nx*ny*nz];
    for (int iter = 0 ; iter < nx*ny*nz;iter++)
    {
        data[iter]=0.0;
        dataCounter[iter]=0;
    }

    pENT = intermediateGrid->pENT;
    pWSD = intermediateGrid->pWSD;

    hx = intermediateGrid->getHx();
    hy = intermediateGrid->getHy();
    hz = intermediateGrid->getHz();

}
 
 /*
Interpolate_on_structured_grid* PointInterpolator::intermediateGrid(int nx_, int ny_, int nz_, Variable<double>* U_from)
{
    nx = nx_;
    ny = ny_;
    nz = nz_;
 
    if(nx<=2) nx = 3;
    if(ny<=2) ny = 3;
    if(nz<=2) nz = 3;
     
    Blockgrid* blockgrid_from = U_from->Give_blockgrid();
    
    //Variable<double> coordXYZ(*blockgrid);
    X_coordinate Xc(*blockgrid_from);
    Y_coordinate Yc(*blockgrid_from);
    Z_coordinate Zc(*blockgrid_from);
    pWSD.x = Minimum(Xc);    pWSD.y = Minimum(Yc);    pWSD.z = Minimum(Zc);
    pENT.x = Maximum(Xc);    pENT.y = Maximum(Yc);    pENT.z = Maximum(Zc);  

    
    interpolatorStructured = new Interpolate_on_structured_grid(nx,ny,nz, pWSD, pENT, *blockgrid_from);
    
    return interpolatorStructured;
}
*/


 
double PointInterpolator::evaluate(double coord_x, double coord_y, double coord_z) {  

    coord_x-=shiftx;
    coord_y-=shifty;
    coord_z-=shiftz;

  if(coord_x > pENT.x) return defaultInterpolation;
  if(coord_x < pWSD.x) return defaultInterpolation;
  if(coord_y > pENT.y) return defaultInterpolation;
  if(coord_y < pWSD.y) return defaultInterpolation;
  if(coord_z > pENT.z) return defaultInterpolation;
  if(coord_z < pWSD.z) return defaultInterpolation;
  
  //cout << "coord_z " << coord_z << " pWSD.z " << pWSD.z << endl;
    /*
  int i = (coord_x - pWSD.x) / hx;
  int j = (coord_y - pWSD.y) / hy;
  int k = (coord_z - pWSD.z) / hz;
  */
  double id = (coord_x - pWSD.x) / hx;
  double jd = (coord_y - pWSD.y) / hy;
  double kd = (coord_z - pWSD.z) / hz;


  int i = int(id);
  int j = int(jd);
  int k = int(kd);

  if(i < 0)   i=0;     if(j <0   ) j=0;     if(k<0)     k=0;
  if(i>=nx-1) i=nx-2;  if(j>=ny-1) j=ny-2;  if(k>=nz-1) k=nz-2;



  //cout << "hx " << hx << " hy "<< hy << " hz " << hz << endl;
  //cout << "id: " << id << " jd: " << jd << " kd: " << kd << endl;
  //cout << "i: " << i << " j: " << j << " k: " << k << endl;
  //cout << "nx: " << nx << " ny: " << ny << " nz: " << nz << endl;


  double uWSD = data[i    +nx*(j    +ny* k)];
  double uESD = data[(i+1)+nx*(j    +ny* k)];
  double uWND = data[i    +nx*((j+1)+ny* k)];
  double uEND = data[(i+1)+nx*((j+1)+ny* k)];
  double uWST = data[i    +nx*(j    +ny*(k+1))];
  double uEST = data[(i+1)+nx*(j    +ny*(k+1))];
  double uWNT = data[i    +nx*((j+1)+ny*(k+1))];
  double uENT = data[(i+1)+nx*((j+1)+ny*(k+1))];
  //i++;
  //j++;
  //k++;
  //k++;
  //cout << "uWSD " << uWSD << endl;
  //cout << "uESD " << uESD << endl;
  //cout << "uWND " << uWND << endl;
  //cout << "uEND " << uEND << endl;
  //cout << "uWST " << uWST << endl;
  //cout << "uEST " << uEST << endl;
  //cout << "uWNT " << uWNT << endl;
  //cout << "uENT " << uENT << endl;


  //cout << "x+1 "<< data[(i+2)+nx*(j    +ny* k)] << endl;
  //cout << "x-1 " <<data[i-1    +nx*(j    +ny* k)] << endl;
  //cout << "x-1, y-1 " <<data[i-1    +nx*(j-1    +ny* k)] << endl;

  // assert( (i+1)+nx*((j+1)+ny*(k+1)) < nx*ny*nz);
  
  double posX = (coord_x - pWSD.x) ;
  double locX = posX / hx - i;
  double posY = (coord_y - pWSD.y);
  double locY = posY / hy - j;
  double posZ = (coord_z - pWSD.z);
  double locZ = posZ / hz - k;



  //cout << "locX, Y, Z: " << locX << " " << locY << " " << locZ << endl;
  //return uWSD;
  
  
  //cout << "uPOS : " << uWSD << " , " << uESD << " , " << uWND << " , " << uEND << " , " << uWST << " , " << uEST << " , " << uWNT << " , " << uENT << endl;
  double uTOT(0);
  double uET, uWT, uWD, uED;
  double uT, uD;

  if      ( (uEST != defaultInterpolation) == (uENT != defaultInterpolation) ) { uET = uEST * (1.0 - locY) + uENT * locY ;}
  else if ( (uEST != defaultInterpolation) && (uENT == defaultInterpolation) ) { uET = uEST;}
  else     								       { uET = uENT;}
  
  if      ( (uWST != defaultInterpolation) == (uWNT != defaultInterpolation) ) {uWT = uWST * (1.0 - locY) + uWNT * locY ;}
  else if ( (uWST != defaultInterpolation) && (uWNT == defaultInterpolation) ) {uWT = uWST;}
  else     								       {uWT = uWNT;}
  
  if      ( (uESD != defaultInterpolation) == (uEND != defaultInterpolation) ) {uED = uESD * (1.0 - locY) + uEND * locY ;}
  else if ( (uESD != defaultInterpolation) && (uEND == defaultInterpolation) ) {uED = uESD;}
  else  								       {uED = uEND;}
  
  if      ( (uWSD != defaultInterpolation) == (uWND != defaultInterpolation) ) {uWD = uWSD * (1.0 - locY) + uWND * locY ;}
  else if ( (uWSD != defaultInterpolation) && (uWND == defaultInterpolation) ) {uWD = uWSD;}
  else     								       {uWD = uWND;}
    
  if      ( (uET != defaultInterpolation)  == (uWT != defaultInterpolation)  ) {uT = uWT  * (1.0 - locX) + uET  * locX ;}
  else if ( (uET != defaultInterpolation)  && (uWT == defaultInterpolation)  ) {uT = uET;}
  else     								       {uT = uWT;}
  
  if      ( (uED != defaultInterpolation)  == (uWD != defaultInterpolation)  ) {uD = uWD  * (1.0 - locX) + uED  * locX ;}
  else if ( (uED != defaultInterpolation)  && (uWD == defaultInterpolation)  ) {uD = uED;}
  else     								       {uD = uWD;}
  
  if      ( (uT != defaultInterpolation)   == (uD != defaultInterpolation)   ) {uTOT = uD   * (1.0 - locZ) + uT   * locZ ;}
  else if ( (uT != defaultInterpolation)   && (uD == defaultInterpolation)   ) {uTOT = uT;}
  else    								       {uTOT = uD;}



//  if (uWSD != defaultInterpolation)
//  {    uTOT += uWSD * (1.0 - locX) * (1.0 - locY) * (1.0 - locZ);  }
//  if (uESD != defaultInterpolation)
//  {    uTOT += uESD *        locX  * (1.0 - locY) * (1.0 - locZ);  }
//  if (uWND != defaultInterpolation)
//  {    uTOT += uWND * (1.0 - locX) *        locY  * (1.0 - locZ);  }
//  if (uEND != defaultInterpolation)
//  {    uTOT += uEND *        locX  *        locY  * (1.0 - locZ);  }
//  if (uWST != defaultInterpolation)
//  {    uTOT += uWST * (1.0 - locX) * (1.0 - locY) *        locZ;  }
//  if (uEST != defaultInterpolation)
//  {    uTOT += uEST *        locX  * (1.0 - locY) *        locZ;  }
//  if (uWNT != defaultInterpolation)
//  {    uTOT += uWNT * (1.0 - locX) *        locY  *        locZ;  }
//  if (uENT != defaultInterpolation)
//  {    uTOT += uENT *        locX  *        locY  *        locZ;  }

    //cout << "my method, other method " << uTOT << " , " << uWSD * (1.0 - locX) * (1.0 - locY) * (1.0 - locZ) +
    //     uESD *        locX  * (1.0 - locY) * (1.0 - locZ) +
   //      uWND * (1.0 - locX) *        locY  * (1.0 - locZ) +
    //     uEND *        locX  *        locY  * (1.0 - locZ) +
   //      uWST * (1.0 - locX) * (1.0 - locY) *        locZ  +
   //      uEST *        locX  * (1.0 - locY) *        locZ  +
    //     uWNT * (1.0 - locX) *        locY  *        locZ  +
    //     uENT *        locX  *        locY  *        locZ << endl;


  //cout <<endl<< "RESULT: " << uTOT<<endl<<endl;
  return uTOT;


  

  /*
  return uWSD * (1.0 - locX) * (1.0 - locY) * (1.0 - locZ) +
         uESD *        locX  * (1.0 - locY) * (1.0 - locZ) +
         uWND * (1.0 - locX) *        locY  * (1.0 - locZ) +
         uEND *        locX  *        locY  * (1.0 - locZ) +
         uWST * (1.0 - locX) * (1.0 - locY) *        locZ  +
         uEST *        locX  * (1.0 - locY) *        locZ  +
         uWNT * (1.0 - locX) *        locY  *        locZ  +
         uENT *        locX  *        locY  *        locZ;
  */
}

std::vector<double> PointInterpolator::evaluateGradient(double coord_x, double coord_y, double coord_z){
    coord_x-=shiftx;
    coord_y-=shifty;
    coord_z-=shiftz;
    //cout << "coord_x y z " << coord_x << " " << coord_y << " " << coord_z << endl;
    if(coord_x > pENT.x) return std::vector<double>({0.0 , 0.0 , 0.0});
    if(coord_x < pWSD.x) return std::vector<double>({0.0 , 0.0 , 0.0});
    if(coord_y > pENT.y) return std::vector<double>({0.0 , 0.0 , 0.0});
    if(coord_y < pWSD.y) return std::vector<double>({0.0 , 0.0 , 0.0});
    if(coord_z > pENT.z) return std::vector<double>({0.0 , 0.0 , 0.0});
    if(coord_z < pWSD.z) return std::vector<double>({0.0 , 0.0 , 0.0});


    double id = (coord_x - pWSD.x) / hx;
    double jd = (coord_y - pWSD.y) / hy;
    double kd = (coord_z - pWSD.z) / hz;


    int i = int(id);
    int j = int(jd);
    int k = int(kd);

    if(i < 0)   i=0;     if(j <0   ) j=0;     if(k<0)     k=0;
    if(i>=nx-1) i=nx-2;  if(j>=ny-1) j=ny-2;  if(k>=nz-1) k=nz-2;


    //cout << "coord_i j k " << i << " " << j << " " << k << endl;





    double uWSD = data[i    +nx*(j    +ny* k)];
    double uESD = data[(i+1)+nx*(j    +ny* k)];
    double uWND = data[i    +nx*((j+1)+ny* k)];
    double uEND = data[(i+1)+nx*((j+1)+ny* k)];
    double uWST = data[i    +nx*(j    +ny*(k+1))];
    double uEST = data[(i+1)+nx*(j    +ny*(k+1))];
    double uWNT = data[i    +nx*((j+1)+ny*(k+1))];
    double uENT = data[(i+1)+nx*((j+1)+ny*(k+1))];

    double posX = (coord_x - pWSD.x) ;
    double locX = posX / hx - i;
    double posY = (coord_y - pWSD.y);
    double locY = posY / hy - j;
    double posZ = (coord_z - pWSD.z);
    double locZ = posZ / hz - k;



    //cout << "coord_locX locY locZ " << locX << " " << locY << " " << locZ << endl;

    if (uWSD == 0.0 || uESD == 0.0 || uWND == 0.0 || uEND == 0.0 || uWST == 0.0 || uEST == 0.0 || uWNT == 0.0 || uENT == 0.0)
    {
        std::vector<double> gradient = { 0.0 , 0.0 , 0.0};
        return gradient;
    }
    double uTOT(0);
    double uET, uWT, uWD, uED;
    double uT, uD, uN, uS, uW, uE;

    double uGradientZ, uGradientX, uGradientY;

    //assume: all values are != defaultInterpolation
    //does not hold for curved interfaces
    /*
    return uWSD * (1.0 - locX) * (1.0 - locY) * (1.0 - locZ) +
           uESD *        locX  * (1.0 - locY) * (1.0 - locZ) +
           uWND * (1.0 - locX) *        locY  * (1.0 - locZ) +
           uEND *        locX  *        locY  * (1.0 - locZ) +
           uWST * (1.0 - locX) * (1.0 - locY) *        locZ  +
           uEST *        locX  * (1.0 - locY) *        locZ  +
           uWNT * (1.0 - locX) *        locY  *        locZ  +
           uENT *        locX  *        locY  *        locZ;
    */
    uGradientX =    uWSD * (0.0 - 1.0) * (1.0 - locY) * (1.0 - locZ) +
                    uESD *        1.0  * (1.0 - locY) * (1.0 - locZ) +
                    uWND * (0.0 - 1.0) *        locY  * (1.0 - locZ) +
                    uEND *        1.0  *        locY  * (1.0 - locZ) +
                    uWST * (0.0 - 1.0) * (1.0 - locY) *        locZ  +
                    uEST *        1.0  * (1.0 - locY) *        locZ  +
                    uWNT * (0.0 - 1.0) *        locY  *        locZ  +
                    uENT *        1.0  *        locY  *        locZ;
    uGradientY =    uWSD * (1.0 - locX) * (0.0 - 1.0) * (1.0 - locZ) +
                    uESD *        locX  * (0.0 - 1.0) * (1.0 - locZ) +
                    uWND * (1.0 - locX) *        1.0  * (1.0 - locZ) +
                    uEND *        locX  *        1.0  * (1.0 - locZ) +
                    uWST * (1.0 - locX) * (0.0 - 1.0) *        locZ  +
                    uEST *        locX  * (0.0 - 1.0) *        locZ  +
                    uWNT * (1.0 - locX) *        1.0  *        locZ  +
                    uENT *        locX  *        1.0  *        locZ;

    uGradientZ =    uWSD * (1.0 - locX) * (1.0 - locY) * (0.0 - 1.0) +
                    uESD *        locX  * (1.0 - locY) * (0.0 - 1.0) +
                    uWND * (1.0 - locX) *        locY  * (0.0 - 1.0) +
                    uEND *        locX  *        locY  * (0.0 - 1.0) +
                    uWST * (1.0 - locX) * (1.0 - locY) *        1.0  +
                    uEST *        locX  * (1.0 - locY) *        1.0  +
                    uWNT * (1.0 - locX) *        locY  *        1.0  +
                    uENT *        locX  *        locY  *        1.0;

    std::vector<double> gradient = {uGradientX / hx, uGradientY / hy, uGradientZ / hz};

    return gradient;

}

void PointInterpolator::smoothGrid()
{
    for (int i = 1 ; i < nx -1 ; i++)
    {
        for (int j = 1 ; j < ny -1 ; j++)
        {
            for (int k = 1 ; k < nz -1 ; k++)
            {
                data[i    +nx*(j    +ny* k)] =  (2.0 * data[i    +nx*(j    +ny* k)]
                                                    + data[i+1  +nx*(j    +ny* k)]
                                                    + data[i-1  +nx*(j    +ny* k)]
                                                    + data[i+1  +nx*(j+1  +ny* k)]
                                                    + data[i+1  +nx*(j-1  +ny* k)]
                                                    + data[i+1  +nx*(j    +ny* k+1)]
                                                    + data[i+1  +nx*(j    +ny* k-1)]) / 8.0;

            }
        }
    }
}

void PointInterpolator::resetInterpolator()
{
    for (int iter = 0 ; iter < nx*ny*nz;iter++)
    {
        dataCounter[iter] = 0;
        data[iter] = defaultInterpolation;
    }
}

void PointInterpolator::normToNumberOfWritings()
{
    for (int iter = 0 ; iter < nx*ny*nz;iter++)
    {
        if (dataCounter[iter] >0)
        {
            data[iter] = data[iter] / double(dataCounter[iter]);
        }
    }
}



void PointInterpolator::writeOnInterpolatedGrid(double coord_x, double coord_y, double coord_z, double value)
{
    coord_x-=shiftx;
    coord_y-=shifty;
    coord_z-=shiftz;

    if(coord_x > pENT.x) return;
    if(coord_x < pWSD.x) return;
    if(coord_y > pENT.y) return;
    if(coord_y < pWSD.y) return;
    if(coord_z > pENT.z) return;
    if(coord_z < pWSD.z) return;

    double id = (coord_x - pWSD.x) / hx;
    double jd = (coord_y - pWSD.y) / hy;
    double kd = (coord_z - pWSD.z) / hz;


    int i = int(id);
    int j = int(jd);
    int k = int(kd);

    if(i < 0)   i=0;     if(j <0   ) j=0;     if(k<0)     k=0;
    if(i>=nx-1) i=nx-2;  if(j>=ny-1) j=ny-2;  if(k>=nz-1) k=nz-2;

    double posX = (coord_x - pWSD.x) ;
    double locX = posX / hx - i;
    double posY = (coord_y - pWSD.y);
    double locY = posY / hy - j;
    double posZ = (coord_z - pWSD.z);
    double locZ = posZ / hz - k;



//    double uWSD = value * (1.0 - locX) * (1.0 - locY) * (1.0 - locZ);
//    double uESD = value *        locX  * (1.0 - locY) * (1.0 - locZ);
//    double uWND = value * (1.0 - locX) *        locY  * (1.0 - locZ);
//    double uEND = value *        locX  *        locY  * (1.0 - locZ);
//    double uWST = value * (1.0 - locX) * (1.0 - locY) *        locZ ;
//    double uEST = value *        locX  * (1.0 - locY) *        locZ ;
//    double uWNT = value * (1.0 - locX) *        locY  *        locZ ;
//    double uENT = value *        locX  *        locY  *        locZ ;

//    data[i    +nx*(j    +ny* k)] = uWSD;
//    data[(i+1)+nx*(j    +ny* k)] = uESD;
//    data[i    +nx*((j+1)+ny* k)] = uWND;
//    data[(i+1)+nx*((j+1)+ny* k)] = uEND;
//    data[i    +nx*(j    +ny*(k+1))] = uWST;
//    data[(i+1)+nx*(j    +ny*(k+1))] = uEST;
//    data[i    +nx*((j+1)+ny*(k+1))] = uWNT;
//    data[(i+1)+nx*((j+1)+ny*(k+1))] = uENT;
        data[i    +nx*(j    +ny* k)] += value;
        data[(i+1)+nx*(j    +ny* k)] += value;
        data[i    +nx*((j+1)+ny* k)] += value;
        data[(i+1)+nx*((j+1)+ny* k)] += value;
        data[i    +nx*(j    +ny*(k+1))] += value;
        data[(i+1)+nx*(j    +ny*(k+1))] += value;
        data[i    +nx*((j+1)+ny*(k+1))] += value;
        data[(i+1)+nx*((j+1)+ny*(k+1))] += value;
        dataCounter[i    +nx*(j    +ny* k)]++;
        dataCounter[(i+1)+nx*(j    +ny* k)]++;
        dataCounter[i    +nx*((j+1)+ny* k)]++;
        dataCounter[(i+1)+nx*((j+1)+ny* k)]++;
        dataCounter[i    +nx*(j    +ny*(k+1))]++;
        dataCounter[(i+1)+nx*(j    +ny*(k+1))]++;
        dataCounter[i    +nx*((j+1)+ny*(k+1))]++;
        dataCounter[(i+1)+nx*((j+1)+ny*(k+1))]++;

    return;


}

void PointInterpolator::subtractOnInterpolatedGrid(double coord_x, double coord_y, double coord_z, double value)
{
    coord_x+=shiftx;
    coord_y+=shifty;
    coord_z+=shiftz;
    if(coord_x > pENT.x) return;
    if(coord_x < pWSD.x) return;
    if(coord_y > pENT.y) return;
    if(coord_y < pWSD.y) return;
    if(coord_z > pENT.z) return;
    if(coord_z < pWSD.z) return;

    double id = (coord_x - pWSD.x) / hx;
    double jd = (coord_y - pWSD.y) / hy;
    double kd = (coord_z - pWSD.z) / hz;


    int i = int(id);
    int j = int(jd);
    int k = int(kd);

    if(i < 0)   i=0;     if(j <0   ) j=0;     if(k<0)     k=0;
    if(i>=nx-1) i=nx-2;  if(j>=ny-1) j=ny-2;  if(k>=nz-1) k=nz-2;

    double posX = (coord_x - pWSD.x) ;
    double locX = posX / hx - i;
    double posY = (coord_y - pWSD.y);
    double locY = posY / hy - j;
    double posZ = (coord_z - pWSD.z);
    double locZ = posZ / hz - k;



    double uWSD = value * (1.0 - locX) * (1.0 - locY) * (1.0 - locZ);
    double uESD = value *        locX  * (1.0 - locY) * (1.0 - locZ);
    double uWND = value * (1.0 - locX) *        locY  * (1.0 - locZ);
    double uEND = value *        locX  *        locY  * (1.0 - locZ);
    double uWST = value * (1.0 - locX) * (1.0 - locY) *        locZ ;
    double uEST = value *        locX  * (1.0 - locY) *        locZ ;
    double uWNT = value * (1.0 - locX) *        locY  *        locZ ;
    double uENT = value *        locX  *        locY  *        locZ ;

    data[i    +nx*(j    +ny* k)] -= uWSD;
    data[(i+1)+nx*(j    +ny* k)] -= uESD;
    data[i    +nx*((j+1)+ny* k)] -= uWND;
    data[(i+1)+nx*((j+1)+ny* k)] -= uEND;
    data[i    +nx*(j    +ny*(k+1))] -= uWST;
    data[(i+1)+nx*(j    +ny*(k+1))] -= uEST;
    data[i    +nx*((j+1)+ny*(k+1))] -= uWNT;
    data[(i+1)+nx*((j+1)+ny*(k+1))] -= uENT;

    return;
}

void PointInterpolator::shiftInterpolatedGrid(double shift_x, double shift_y, double shift_z)
{
    shiftx = shift_x;
    shifty = shift_y;
    shiftz = shift_z;
}

void PointInterpolator::scaleInterpolatedData(double scale, double zeroVal)
{
    if (scale == 1.0)
    {
        return;
    }
    for (int k = 0; k < nz; k++)
    {
        for (int j = 0; j < ny; j++)
        {
            for (int i = 0; i < nx; i++)
            {
                if (data[i    +nx*(j    +ny* k)] != defaultInterpolation )
                {
                    data[i    +nx*(j    +ny* k)] = (data[i    +nx*(j    +ny* k)] - zeroVal) * scale + zeroVal;
                    if (data[i    +nx*(j    +ny* k)] <= 1.0 & zeroVal != 0.0)
                    {
                        cout << "Warning:  " << data[i    +nx*(j    +ny* k)] << endl;
                    }
                }
            }
        }
    }
}

 
PointInterpolator::~PointInterpolator() {
    //delete interpolatorStructured;
    delete[] data;
}

void PointInterpolator::updatePointInterpolator(Interpolate_on_structured_grid *intermediateGrid)
{

    nx = intermediateGrid->nx;
    ny = intermediateGrid->ny;
    nz = intermediateGrid->nz;

    for (int iter = 0 ; iter < nx*ny*nz;iter++)
    {
        data[iter]=0.0;
        dataCounter[iter]=0;
    }

    pENT = intermediateGrid->pENT;
    pWSD = intermediateGrid->pWSD;

    hx = intermediateGrid->getHx();
    hy = intermediateGrid->getHy();
    hz = intermediateGrid->getHz();
}



void Interpolate_direct::init()
{
    arrayBoxWSDENT.resize(blockgrid->Give_unstructured_grid()->Give_number_hexahedra());
    array_box_boundary.resize(blockgrid->Give_unstructured_grid()->Give_number_hexahedra());

    int counter = 0;
    int counterTwoNeighbours = 0;
    int counterBoxesAtBoundary = 0;

    for (int id_hex = 0 ; id_hex < blockgrid->Give_unstructured_grid()->Give_number_hexahedra() ; id_hex++)
    {
        int Nx = blockgrid->Give_Nx_hexahedron(id_hex);
        int Ny = blockgrid->Give_Ny_hexahedron(id_hex);
        int Nz = blockgrid->Give_Nz_hexahedron(id_hex);
        arrayBoxWSDENT.at(id_hex).resize(Nx*Ny*Nz);
        array_box_boundary.at(id_hex).resize(Nx*Ny*Nz);
    }
    std::vector<std::vector<std::vector<int> > >addToIndex;
    addToIndex.resize(blockgrid->Give_unstructured_grid()->Give_number_hexahedra());


    for (int id_hex = 0 ; id_hex < blockgrid->Give_unstructured_grid()->Give_number_hexahedra() ; id_hex++)
    {
        int Nx = blockgrid->Give_Nx_hexahedron(id_hex); // now without +1, since for ( n gridpoints, only n-1 surrounding blocks exist!)
        int Ny = blockgrid->Give_Ny_hexahedron(id_hex);
        int Nz = blockgrid->Give_Nz_hexahedron(id_hex);
        addToIndex.at(id_hex).resize(blockgrid->Give_unstructured_grid()->Give_number_hexahedra());
        std::vector<std::vector<std::vector< int > > > indexAllFacesInside(6);
        std::vector<std::vector<std::vector< int > > > indexAllFacesOutside(6);
        std::vector<std::vector< int > > indicesOfFirstFaceInside(0);
        std::vector<std::vector<std::vector< int > > > correctRelation(6);
        std::vector<int> emptyVectorInt(4);
        std::vector<std::vector<int> > emptyVector(3);
        emptyVector.at(0) = emptyVectorInt; emptyVector.at(1) = emptyVectorInt; emptyVector.at(2) = emptyVectorInt;
        //iterate each side separately;
        int i = Nx;
        int j = Ny;
        int k = Nz;
        for (j = 0 ; j < Ny+1; j = j + Ny) for (k = 0 ; k < Nz+1; k = k + Nz)
        {
            std::vector<int> temp(0);
            temp.push_back(id_hex);temp.push_back(i);temp.push_back(j);temp.push_back(k);
            int indexInner = i    +(Nx+1)*(j    +(Ny+1)* k) ;
            for (int iterIndexToAdd = 0 ; iterIndexToAdd <(int)blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).size() ; iterIndexToAdd+=4 )
            {
                std::vector<int>::const_iterator first = blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).begin() + iterIndexToAdd;
                std::vector<int>::const_iterator last  = blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).begin() + 4 + iterIndexToAdd;
                //indicesOfFirstFaceOutside.push_back(std::vector<int>(first, last));
                indexAllFacesOutside.at(0).push_back(std::vector<int>(first, last));
            }
            //indicesOfFirstFaceInside.push_back(temp);
            indexAllFacesInside.at(0).push_back(temp);
        }
        //filter wrong Neighbours (from other side)
        //indicesOfFirstFaceOutside = filterCorrectNeighbours(indicesOfFirstFaceOutside);
        indexAllFacesOutside.at(0) = filterCorrectNeighbours(indexAllFacesOutside.at(0));
        if (indexAllFacesOutside.at(0).empty())
        {
            //cout << "no outer neighbours here ! " << endl;
            correctRelation.at(0) = emptyVector;
        }
        else
        {
            correctRelation.at(0) = calculateNeighbourIndexRelation(indexAllFacesInside.at(0), indexAllFacesOutside.at(0));
        }

        i = 0;
        for (j = 0 ; j < Ny+1; j = j + Ny) for (k = 0 ; k < Nz+1; k = k + Nz)
        {
            std::vector<int> temp(0);
            temp.push_back(id_hex);temp.push_back(i);temp.push_back(j);temp.push_back(k);
            int indexInner = i    +(Nx+1)*(j    +(Ny+1)* k) ;
            for (int iterIndexToAdd = 0 ; iterIndexToAdd <(int)blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).size() ; iterIndexToAdd+=4 )
            {
                std::vector<int>::const_iterator first = blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).begin() + iterIndexToAdd;
                std::vector<int>::const_iterator last  = blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).begin() + 4 + iterIndexToAdd;
                indexAllFacesOutside.at(1).push_back(std::vector<int>(first, last));
            }
            indexAllFacesInside.at(1).push_back(temp);
        }
        indexAllFacesOutside.at(1) = filterCorrectNeighbours(indexAllFacesOutside.at(1));
        if (indexAllFacesOutside.at(1).empty())
        {
            //cout << "no outer neighbours here ! " << endl;
            correctRelation.at(1) = emptyVector;
        }
        else
        {
        correctRelation.at(1) = calculateNeighbourIndexRelation(indexAllFacesInside.at(1), indexAllFacesOutside.at(1));
        }

        j = Ny;
        for (i = 0 ; i < Nx+1; i = i + Nx) for (k = 0 ; k < Nz+1; k = k + Nz)
        {
            std::vector<int> temp(0);
            temp.push_back(id_hex);temp.push_back(i);temp.push_back(j);temp.push_back(k);
            int indexInner = i    +(Nx+1)*(j    +(Ny+1)* k) ;
            for (int iterIndexToAdd = 0 ; iterIndexToAdd <(int)blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).size() ; iterIndexToAdd+=4 )
            {
                std::vector<int>::const_iterator first = blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).begin() + iterIndexToAdd;
                std::vector<int>::const_iterator last  = blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).begin() + 4 + iterIndexToAdd;
                indexAllFacesOutside.at(2).push_back(std::vector<int>(first, last));
            }
            indexAllFacesInside.at(2).push_back(temp);
        }
        indexAllFacesOutside.at(2) = filterCorrectNeighbours(indexAllFacesOutside.at(2));
        if (indexAllFacesOutside.at(2).empty())
        {
            //cout << "no outer neighbours here ! " << endl;
            correctRelation.at(2) = emptyVector;
        }
        else
        {
        correctRelation.at(2) = calculateNeighbourIndexRelation(indexAllFacesInside.at(2), indexAllFacesOutside.at(2));
        }

        j = 0;
        for (i = 0 ; i < Nx+1; i = i + Nx) for (k = 0 ; k < Nz+1; k = k + Nz)
        {
            std::vector<int> temp(0);
            temp.push_back(id_hex);temp.push_back(i);temp.push_back(j);temp.push_back(k);
            int indexInner = i    +(Nx+1)*(j    +(Ny+1)* k) ;
            for (int iterIndexToAdd = 0 ; iterIndexToAdd <(int)blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).size() ; iterIndexToAdd+=4 )
            {
                std::vector<int>::const_iterator first = blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).begin() + iterIndexToAdd;
                std::vector<int>::const_iterator last  = blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).begin() + 4 + iterIndexToAdd;
                indexAllFacesOutside.at(3).push_back(std::vector<int>(first, last));
            }
            indexAllFacesInside.at(3).push_back(temp);
        }
        indexAllFacesOutside.at(3) = filterCorrectNeighbours(indexAllFacesOutside.at(3));
        if (indexAllFacesOutside.at(3).empty())
        {
            //cout << "no outer neighbours here ! " << endl;
            correctRelation.at(3) = emptyVector;
        }
        else
        {
        correctRelation.at(3) = calculateNeighbourIndexRelation(indexAllFacesInside.at(3), indexAllFacesOutside.at(3));
        }

        k = Nz;
        for (i = 0 ; i < Nx+1; i = i + Nx) for (j = 0 ; j < Ny+1; j = j + Ny)
        {
            std::vector<int> temp(0);
            temp.push_back(id_hex);temp.push_back(i);temp.push_back(j);temp.push_back(k);
            int indexInner = i    +(Nx+1)*(j    +(Ny+1)* k) ;
            for (int iterIndexToAdd = 0 ; iterIndexToAdd <(int)blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).size() ; iterIndexToAdd+=4 )
            {
                std::vector<int>::const_iterator first = blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).begin() + iterIndexToAdd;
                std::vector<int>::const_iterator last  = blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).begin() + 4 + iterIndexToAdd;
                indexAllFacesOutside.at(4).push_back(std::vector<int>(first, last));
            }
            indexAllFacesInside.at(4).push_back(temp);
        }
        indexAllFacesOutside.at(4) = filterCorrectNeighbours(indexAllFacesOutside.at(4));
        if (indexAllFacesOutside.at(4).empty())
        {
            correctRelation.at(4) = emptyVector;
        }
        else
        {
            correctRelation.at(4) = calculateNeighbourIndexRelation(indexAllFacesInside.at(4), indexAllFacesOutside.at(4));
        }

        k = 0;
        for (i = 0 ; i < Nx+1; i = i + Nx) for (j = 0 ; j < Ny+1; j = j + Ny)
        {
            std::vector<int> temp(0);
            temp.push_back(id_hex);temp.push_back(i);temp.push_back(j);temp.push_back(k);
            int indexInner = i    +(Nx+1)*(j    +(Ny+1)* k) ;
            for (int iterIndexToAdd = 0 ; iterIndexToAdd <(int)blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).size() ; iterIndexToAdd+=4 )
            {
                std::vector<int>::const_iterator first = blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).begin() + iterIndexToAdd;
                std::vector<int>::const_iterator last  = blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(indexInner).begin() + 4 + iterIndexToAdd;
                indexAllFacesOutside.at(5).push_back(std::vector<int>(first, last));
            }
            indexAllFacesInside.at(5).push_back(temp);
        }
        indexAllFacesOutside.at(5) = filterCorrectNeighbours(indexAllFacesOutside.at(5));
        if (indexAllFacesOutside.at(5).empty())
        {
            correctRelation.at(5) = emptyVector;
        }
        else
        {
            correctRelation.at(5) = calculateNeighbourIndexRelation(indexAllFacesInside.at(5), indexAllFacesOutside.at(5));
        }

            //init surrounding boxes!
            for(int i=0;i<Nx;++i) for(int j=0;j<Ny;++j) for(int k=0;k<Nz;++k)
            {
                arrayBoxWSDENT.at(id_hex).at(i    +Nx*(j    +Ny* k)).resize(2);
                D3vector cWSD, cESD;
                D3vector cWND, cEND;

                D3vector cWST, cEST;
                D3vector cWNT, cENT;

                D3vector boxWSD, boxENT;
                cWSD = blockgrid->Give_coord_hexahedron(id_hex,i,  j,  k  );
                cESD = blockgrid->Give_coord_hexahedron(id_hex,i+1,j  ,k  );
                cWND = blockgrid->Give_coord_hexahedron(id_hex,i,  j+1,k  );
                cEND = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k  );

                cWST = blockgrid->Give_coord_hexahedron(id_hex,i,  j,  k+1);
                cEST = blockgrid->Give_coord_hexahedron(id_hex,i+1,j  ,k+1);
                cWNT = blockgrid->Give_coord_hexahedron(id_hex,i,  j+1,k+1);
                cENT = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k+1);
                //isPlane()

                    // bounding box calculation
                boxWSD.x = MIN(MIN(MIN(cWSD.x,cESD.x),MIN(cWND.x,cEND.x)),
                       MIN(MIN(cWST.x,cEST.x),MIN(cWNT.x,cENT.x)));// - factor *hx;
                boxWSD.y = MIN(MIN(MIN(cWSD.y,cESD.y),MIN(cWND.y,cEND.y)),
                       MIN(MIN(cWST.y,cEST.y),MIN(cWNT.y,cENT.y)));// - factor *hy;
                boxWSD.z = MIN(MIN(MIN(cWSD.z,cESD.z),MIN(cWND.z,cEND.z)),
                       MIN(MIN(cWST.z,cEST.z),MIN(cWNT.z,cENT.z)));// - factor *hz;

                boxENT.x = MAX(MAX(MAX(cWSD.x,cESD.x),MAX(cWND.x,cEND.x)),
                       MAX(MAX(cWST.x,cEST.x),MAX(cWNT.x,cENT.x)));// + factor *hx;
                boxENT.y = MAX(MAX(MAX(cWSD.y,cESD.y),MAX(cWND.y,cEND.y)),
                       MAX(MAX(cWST.y,cEST.y),MAX(cWNT.y,cENT.y)));// + factor *hy;
                boxENT.z = MAX(MAX(MAX(cWSD.z,cESD.z),MAX(cWND.z,cEND.z)),
                       MAX(MAX(cWST.z,cEST.z),MAX(cWNT.z,cENT.z)));// + factor *hz;
                arrayBoxWSDENT.at(id_hex).at(i    +Nx*(j    +Ny* k)).at(0) = boxWSD;
                arrayBoxWSDENT.at(id_hex).at(i    +Nx*(j    +Ny* k)).at(1) = boxENT;



              }


            i = Nx;
            for (j = 0 ; j < Ny+1; j++) for (k = 0 ; k < Nz+1; k++)
            {
                if (!indexAllFacesOutside.at(0).empty())
                {calculateNeighbourIndex(correctRelation.at(0), indexAllFacesOutside.at(0).at(0).at(0), id_hex, i,j,k,Nx,Ny,Nz);}
            }

            i = 0;
            for (j = 0 ; j < Ny+1; j++) for (k = 0 ; k < Nz+1; k++)
            {
                if (!indexAllFacesOutside.at(1).empty())
                {calculateNeighbourIndex(correctRelation.at(1), indexAllFacesOutside.at(1).at(0).at(0), id_hex, i,j,k,Nx,Ny,Nz);}
                correctRelation.at(1);
            }

            j = Ny;
            for (i = 0 ; i < Nx+1; i++) for (k = 0 ; k < Nz+1; k++)
            {
                if (!indexAllFacesOutside.at(2).empty())
                {calculateNeighbourIndex(correctRelation.at(2), indexAllFacesOutside.at(2).at(0).at(0), id_hex, i,j,k,Nx,Ny,Nz);}
                correctRelation.at(2);
            }


            j = 0;
            for (i = 0 ; i < Nx+1; i++) for (k = 0 ; k < Nz+1; k++)
            {
                if (!indexAllFacesOutside.at(3).empty())
                {calculateNeighbourIndex(correctRelation.at(3), indexAllFacesOutside.at(3).at(0).at(0), id_hex, i,j,k,Nx,Ny,Nz);}
                correctRelation.at(3);
            }


            k = Nz;
            for (i = 0 ; i < Nx+1; i++) for (j = 0 ; j < Ny+1; j++)
            {
                if (!indexAllFacesOutside.at(4).empty())
                {calculateNeighbourIndex(correctRelation.at(4), indexAllFacesOutside.at(4).at(0).at(0), id_hex, i,j,k,Nx,Ny,Nz);}
                correctRelation.at(4);
            }


            k = 0;
            for (i = 0 ; i < Nx+1; i++) for (j = 0 ; j < Ny+1; j++)
            {
                if (!indexAllFacesOutside.at(5).empty())
                {calculateNeighbourIndex(correctRelation.at(5), indexAllFacesOutside.at(5).at(0).at(0), id_hex, i,j,k,Nx,Ny,Nz);}
                correctRelation.at(5);
            }



    }
    boxCounter = 0;
    //test for boxes: check, if neighbour is actually the neighbour!
    if (true)
    {

    for (int idHex = 0 ; idHex < blockgrid->Give_unstructured_grid()->Give_number_hexahedra() ; idHex++)
    {
        int Nx = blockgrid->Give_Nx_hexahedron(idHex);
        int Ny = blockgrid->Give_Ny_hexahedron(idHex);
        int Nz = blockgrid->Give_Nz_hexahedron(idHex);
        for(int i=0;i<Nx;++i) for(int j=0;j<Ny;++j) for(int k=0;k<Nz;++k)
        {
            for (int iterHexaTemp = 0 ; iterHexaTemp < array_box_boundary.at(idHex).at(i    +(Nx)*(j    +(Ny)* k)).size() ; iterHexaTemp +=4)
            {
                int index = i    +(Nx)*(j    +(Ny)* k) ;
                int idHexTemp = array_box_boundary.at(idHex).at(index).at(iterHexaTemp+0);
                int iTemp = array_box_boundary.at(idHex).at(index).at(iterHexaTemp+1);
                int jTemp = array_box_boundary.at(idHex).at(index).at(iterHexaTemp+2);
                int kTemp = array_box_boundary.at(idHex).at(index).at(iterHexaTemp+3);

                int NxOuter = blockgrid->Give_Nx_hexahedron(idHexTemp);
                int NyOuter = blockgrid->Give_Ny_hexahedron(idHexTemp);
                int NzOuter = blockgrid->Give_Nz_hexahedron(idHexTemp);

                int indexOuter = iTemp    +NxOuter*(jTemp    +NyOuter* kTemp);
                int indexInner = i    +Nx*(j    +Ny* k);
                D3vector boxWSDInner =  arrayBoxWSDENT.at(idHex).at(indexInner).at(0);
                D3vector boxENTInner =  arrayBoxWSDENT.at(idHex).at(indexInner).at(1);
                D3vector boxWSDOuter=  arrayBoxWSDENT.at(idHexTemp).at(indexOuter).at(0);
                D3vector boxENTOuter=  arrayBoxWSDENT.at(idHexTemp).at(indexOuter).at(1);

                    if (!checkOverlapOfBoxes(boxWSDInner, boxENTInner, boxWSDOuter, boxENTOuter))
                    {
                        writeBox(boxWSDInner, boxENTInner, std::string("box"));
                        writeBox(boxWSDOuter, boxENTOuter, std::string("box"));
                        cout << "surrounding1 box is wrong!" << endl;
                    }
                    else
                    {
                       // cout << "surrounding box1 is right!" << endl;
                    }
             }
        }
    }
    }

//    cout << "counter " << counter << endl;
//    cout << "boundary boxes " << counterBoxesAtBoundary<< endl;
//    cout << "counterTwoNeighbours " << counterTwoNeighbours << endl;
//    cout << "counterTwoNeighbours " << counterTwoNeighbours << endl;

}

void Interpolate_direct::updateBoundaryBoxes()
{
    for (int id_hex = 0 ; id_hex < blockgrid->Give_unstructured_grid()->Give_number_hexahedra() ; id_hex++)
    {
        int Nx = blockgrid->Give_Nx_hexahedron(id_hex);
        int Ny = blockgrid->Give_Ny_hexahedron(id_hex);
        int Nz = blockgrid->Give_Nz_hexahedron(id_hex);
        for(int i=0;i<Nx;++i) for(int j=0;j<Ny;++j) for(int k=0;k<Nz;++k)
        {
            arrayBoxWSDENT.at(id_hex).at(i    +Nx*(j    +Ny* k)).resize(2);
            D3vector cWSD, cESD;
            D3vector cWND, cEND;

            D3vector cWST, cEST;
            D3vector cWNT, cENT;

            D3vector boxWSD, boxENT;
            cWSD = blockgrid->Give_coord_hexahedron(id_hex,i,  j,  k  );
            cESD = blockgrid->Give_coord_hexahedron(id_hex,i+1,j  ,k  );
            cWND = blockgrid->Give_coord_hexahedron(id_hex,i,  j+1,k  );
            cEND = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k  );

            cWST = blockgrid->Give_coord_hexahedron(id_hex,i,  j,  k+1);
            cEST = blockgrid->Give_coord_hexahedron(id_hex,i+1,j  ,k+1);
            cWNT = blockgrid->Give_coord_hexahedron(id_hex,i,  j+1,k+1);
            cENT = blockgrid->Give_coord_hexahedron(id_hex,i+1,j+1,k+1);

            // bounding box calculation
            boxWSD.x = MIN(MIN(MIN(cWSD.x,cESD.x),MIN(cWND.x,cEND.x)),
            MIN(MIN(cWST.x,cEST.x),MIN(cWNT.x,cENT.x)));// - factor *hx;
            boxWSD.y = MIN(MIN(MIN(cWSD.y,cESD.y),MIN(cWND.y,cEND.y)),
            MIN(MIN(cWST.y,cEST.y),MIN(cWNT.y,cENT.y)));// - factor *hy;
            boxWSD.z = MIN(MIN(MIN(cWSD.z,cESD.z),MIN(cWND.z,cEND.z)),
            MIN(MIN(cWST.z,cEST.z),MIN(cWNT.z,cENT.z)));// - factor *hz;

            boxENT.x = MAX(MAX(MAX(cWSD.x,cESD.x),MAX(cWND.x,cEND.x)),
            MAX(MAX(cWST.x,cEST.x),MAX(cWNT.x,cENT.x)));// + factor *hx;
            boxENT.y = MAX(MAX(MAX(cWSD.y,cESD.y),MAX(cWND.y,cEND.y)),
            MAX(MAX(cWST.y,cEST.y),MAX(cWNT.y,cENT.y)));// + factor *hy;
            boxENT.z = MAX(MAX(MAX(cWSD.z,cESD.z),MAX(cWND.z,cEND.z)),
            MAX(MAX(cWST.z,cEST.z),MAX(cWNT.z,cENT.z)));// + factor *hz;
            arrayBoxWSDENT.at(id_hex).at(i    +Nx*(j    +Ny* k)).at(0) = boxWSD;
            arrayBoxWSDENT.at(id_hex).at(i    +Nx*(j    +Ny* k)).at(1) = boxENT;
         }
    }
}


std::vector<std::vector<int> > Interpolate_direct::calculateNeighbourIndexRelation(std::vector<std::vector<int> > inner, std::vector<std::vector<int> > outer)
{
    // 1 : find index which does not change --> not part of the boundary

    if (outer.size() != inner.size())
    {
        cout << "ops " << endl;
    }
    std::vector<int> diffToAdd(4);
    std::vector<int> IndexToInvert(4);
    std::vector<int> IndexToSwitch(4);
    std::vector<std::vector<int> > retVal(0);
    int indexNotAtBoundaryInner;
    for ( int iter = 0 ; iter < 4 ; iter++)
    {
        bool found = true;
        for ( int iterInner = 0 ; iterInner < 3 ; iterInner++)
        {
            if (inner.at(iterInner).at(iter) == inner.at(iterInner+1).at(iter) )
            {

            }
            else
            {
                found = false;
            }
        }
        if (found)
        {
            //first one is always the hex id, but its getting overwritten by the correct one .
            indexNotAtBoundaryInner = iter;
        }
    }
    int indexNotAtBoundaryOuter;
    for ( int iter = 0 ; iter < 4 ; iter++)
    {
        bool found = true;
        for ( int iterInner = 0 ; iterInner < 3 ; iterInner++)
        {
            if (outer.at(iterInner).at(iter) == outer.at(iterInner+1).at(iter) )
            {

            }
            else
            {
                found = false;
            }
        }
        if (found)
        {
            //first one is always the hex id, but its getting overwritten by the correct one .
            indexNotAtBoundaryOuter = iter;
        }
    }
    diffToAdd.at(indexNotAtBoundaryInner) = outer.front().at(indexNotAtBoundaryOuter) - inner.front().at(indexNotAtBoundaryInner);
    retVal.push_back(diffToAdd);
    //check index, which does NOT change and calculate difference

    //case 1 :: 0 invert and 0 rotation
     if (compareIndicies(inner,outer,indexNotAtBoundaryOuter))
     {
        //cout << "correct found !  No changes necessary"<< endl;
     }

    std::vector<std::vector< int > > innerModified(0);

    // case 0 : 0 invert  and 0-5 rotation
    if (compareIndicies(inner,outer,indexNotAtBoundaryOuter))
    {
        retVal.push_back(IndexToInvert);
        retVal.push_back(IndexToSwitch);
        return retVal;
        cout << "correct found ! no invert , order IJK "<< endl;   }
    innerModified = switchIJ(inner); // dont check :
    if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
    {
        retVal.push_back(IndexToInvert);
        IndexToSwitch.at(0) = 1;
        retVal.push_back(IndexToSwitch);
        return retVal;
          cout << "correct found ! no invert , order JIK "<< endl;   }
    innerModified = switchJK(innerModified);
    if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
    {
        retVal.push_back(IndexToInvert);
        IndexToSwitch.at(0) = 2;
        retVal.push_back(IndexToSwitch);
        return retVal;
          cout << "correct found ! no invert , switch JKI "<< endl;   }
    innerModified = switchIJ(innerModified);
    if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
    {
        retVal.push_back(IndexToInvert);
        IndexToSwitch.at(0) = 3;
        retVal.push_back(IndexToSwitch);
        return retVal;
          cout << "correct found ! no invert , switch KJI "<< endl;   }
    innerModified = switchJK(innerModified);
    if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
    {
        retVal.push_back(IndexToInvert);
        IndexToSwitch.at(0) = 4;
        retVal.push_back(IndexToSwitch);
        return retVal;
          cout << "correct found ! no invert , switch KIJ "<< endl;   }
    innerModified = switchIJ(innerModified);
    if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
    {
        retVal.push_back(IndexToInvert);
        IndexToSwitch.at(0) = 5;
        retVal.push_back(IndexToSwitch);
        return retVal;
          cout << "correct found ! no invert , switch IKJ "<< endl;   }


    // case 1 :: 1 invert and 0 rotation



    if (indexNotAtBoundaryInner == 1)
    {
        innerModified = invertJ(inner); // dont check : 2
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 0;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ , order IJK "<< endl;   }
        innerModified = switchIJ(innerModified); // dont check :
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 1;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ , order JIK "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 2;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ , switch JKI "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 3;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ , switch KJI "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 4;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ , switch KIJ "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 5;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ , switch IKJ "<< endl;   }

        innerModified = invertK(inner);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 0;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertK , order IJK "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 1;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertK , order JIK "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 2;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertK , switch JKI "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 3;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertK , switch KJI "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 4;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertK , switch KIJ "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 5;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertK , switch IKJ "<< endl;   }

        innerModified = invertJ(inner);
        innerModified = invertK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 0;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ and invertK , order IJK "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 1;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ and invertK , order JIK "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 2;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ and invertK , order JKI "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 3;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ and invertK , order KJI "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 4;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ and invertK , order KIJ "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 5;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ and invertK , order IKJ "<< endl;   }
    }

    if (indexNotAtBoundaryInner == 2)
    {
        innerModified = invertI(inner);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 0;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI , order IJK "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 1;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI , order JIK "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 2;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI , switch JKI "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 3;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI , switch KJI "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 4;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI , switch KIJ "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 5;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI , switch IKJ "<< endl;   }

        innerModified = invertK(inner);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 0;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertK , order IJK "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 1;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertK , order JIK "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 2;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertK , switch JKI "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 3;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertK , switch KJI "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 4;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertK , switch KIJ "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 5;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertK , switch IKJ "<< endl;   }

        innerModified = invertI(inner);
        innerModified = invertK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 0;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI and invertK , order IJK "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 1;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI and invertK , order JIK "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 2;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI and invertK , order JKI "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 3;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI and invertK , order KJI "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 4;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI and invertK , order KIJ "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;IndexToInvert.at(3) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 5;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI and invertK , order IKJ "<< endl;   }

    }

    if (indexNotAtBoundaryInner == 3)
    {
        innerModified = invertI(inner);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 0;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI , order IJK "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 1;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI , order JIK "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 2;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI , switch JKI "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 3;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI , switch KJI "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 4;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI , switch KIJ "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 5;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI , switch IKJ "<< endl;   }

        innerModified = invertJ(inner);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 0;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ , order IJK "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 1;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ , order JIK "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 2;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ , switch JKI "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 3;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ , switch KJI "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 4;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ , switch KIJ "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 5;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertJ , switch IKJ "<< endl;   }

        innerModified = invertI(inner);
        innerModified = invertJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 0;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI and invertJ , order IJK "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 1;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI and invertJ , order JIK "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 2;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI and invertJ , order JKI "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 3;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI and invertJ , order KJI "<< endl;   }
        innerModified = switchJK(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 4;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI and invertJ , order KIJ "<< endl;   }
        innerModified = switchIJ(innerModified);
        if (compareIndicies(innerModified,outer,indexNotAtBoundaryOuter))
        {
            IndexToInvert.at(1) = 1;IndexToInvert.at(2) = 1;
            retVal.push_back(IndexToInvert);
            IndexToSwitch.at(0) = 5;
            retVal.push_back(IndexToSwitch);
            return retVal;
              cout << "correct found ! invertI and invertJ , order IKJ "<< endl;   }

    }

    cout << "checking ... Finished! " << endl;

    cout << endl;
    return std::vector< std::vector< int > >(0);

}

std::vector<int> Interpolate_direct::calculateNeighbourIndex(std::vector<std::vector<int> > relation, int id_hex_outside, int id_hex_inside, int i, int j, int k, int Nx, int Ny, int Nz)
{
    //cout << "calculating neughbour index ... " << endl;
    int convertedI = i;
    int convertedJ = j;
    int convertedK = k;
    int iTemp = i;//blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(convertedLocalIndex).at(iterHexaTemp+1);
    int jTemp = j;//blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(convertedLocalIndex).at(iterHexaTemp+2);
    int kTemp = k;//blockgrid->Give_blockgrid_coordinates()->blockgrid_hexa_boundary.at(id_hex).at(convertedLocalIndex).at(iterHexaTemp+3);


    //add shift , in direction which is not at boundary
    iTemp = iTemp + relation.at(0).at(1);
    jTemp = jTemp + relation.at(0).at(2);
    kTemp = kTemp + relation.at(0).at(3);
    //mark, which one was changed --> invert sign
    if (relation.at(0).at(1) != 0)
    {    iTemp = -iTemp;    }
    if (relation.at(0).at(2) != 0)
    {    jTemp = -jTemp;    }
    if (relation.at(0).at(3) != 0)
    {    kTemp = -kTemp;    }
    int indexWhichWasChanged = 0;



    //invert index
    if (relation.at(1).at(1) == 1)
    {iTemp = Nx - iTemp-1;}//cout<<"invert I" << endl;}
    if (relation.at(1).at(2) == 1)
    {jTemp = Ny - jTemp-1;}//cout<<"invert J" << endl;}
    if (relation.at(1).at(3) == 1)
    {kTemp = Nz - kTemp-1;}//cout<<"invert K" << endl;}

    //before switching : using -- or ++. Grid on blockgrid is n*n*n, in boxes (n-1)*(n-1)*(n-1)
    // -> if index == n -> either use -- if index is not inverted (n -> n-1) or ++ if index is inverted (-1 -> 0 )
    if (i == Nx && relation.at(0).at(1) == 0)
    {
        //cout << "converted index I " <<endl;
        //convertedI = Nx-1;
        convertedI--;
        if (relation.at(1).at(1) == 0)
            iTemp--;
        else
            iTemp++;
    }
    else if (i == Nx)
    {
        //cout << "reduced index I " <<endl;
        convertedI--;
    }
    if (j == Ny  && relation.at(0).at(2) == 0)
    {
        //cout << "converted index J " <<endl;
        //convertedJ = Ny-1;
        convertedJ--;
        if (relation.at(1).at(2) == 0)
            jTemp--;
        else
            jTemp++;
    }
    else if (j == Ny)
    {
        //cout << "reduced index J " <<endl;
        convertedJ--;
    }
    if (k == Nz && relation.at(0).at(3) == 0)
    {
        //cout << "converted index K " <<endl;
        //convertedK = Nz-1;
        convertedK--;
        if (relation.at(1).at(3) == 0)
            kTemp--;
        else
            kTemp++;
    }
    else if (k == Nz)
    {
        //cout << "reduced index K " <<endl;
        convertedK--;
    }



    //cout << "Rotation : " << relation.at(2).at(0) << endl;
    //switch index
    if (relation.at(2).at(0) == 0)
    {
    }
    else if (relation.at(2).at(0) == 1)
    {
        int T = iTemp;
        iTemp = jTemp;
        jTemp = T;
    }
    else if (relation.at(2).at(0) == 2)
    {
        int T = iTemp;
        iTemp = jTemp;
        jTemp = kTemp;
        kTemp = T;
    }
    else if (relation.at(2).at(0) == 3)
    {
        int T = iTemp;
        iTemp = kTemp;
        kTemp = T;
    }
    else if (relation.at(2).at(0) == 4)
    {
        int T = iTemp;
        iTemp = kTemp;
        kTemp = jTemp;
        jTemp = T;
    }
    else if (relation.at(2).at(0) == 5)
    {
        int T = jTemp;
        jTemp = kTemp;
        kTemp = T;
    }
    else
    {std::cout <<"error, switching index not possible!"<<std::endl;}

    //check tempIndex again, now against outer index
    int NxOuter = blockgrid->Give_Nx_hexahedron(id_hex_outside); // before : idHex
    int NyOuter = blockgrid->Give_Ny_hexahedron(id_hex_outside);
    int NzOuter = blockgrid->Give_Nz_hexahedron(id_hex_outside);


    if (iTemp < 0)
    {
        iTemp = -iTemp;
        if (iTemp == NxOuter)
        {    iTemp--; }
    }
    if (jTemp < 0)
    {
        jTemp = -jTemp;
        if (jTemp == NyOuter)
        {    jTemp--; }
    }
    if (kTemp < 0)
    {
        kTemp = -kTemp;
        if (kTemp == NzOuter)
        {    kTemp--; }
    }
    //test for boxes :
    int indexConverted = convertedI    +Nx*(convertedJ    +Ny* convertedK);
    //indexConverted = 0;


    //check, if boundary already written here
    bool write = true;
    for (int iter = 0 ; iter < array_box_boundary.at(id_hex_inside).at(indexConverted).size(); iter+=4)
    {
        if (array_box_boundary.at(id_hex_inside).at(indexConverted).at(iter) == id_hex_outside)
        {
            write = false;
        }
    }
    if (write)
    {
        if (array_box_boundary.at(id_hex_inside).at(indexConverted).size() > 9)
        {
            std::cout << "4 edges?? " << std::endl;
        }
        array_box_boundary.at(id_hex_inside).at(indexConverted).push_back(id_hex_outside);
        array_box_boundary.at(id_hex_inside).at(indexConverted).push_back(iTemp);
        array_box_boundary.at(id_hex_inside).at(indexConverted).push_back(jTemp);
        array_box_boundary.at(id_hex_inside).at(indexConverted).push_back(kTemp);
    }



   // cout << "overlap : " << calculateOverlapOfBoxes(boxWSD,boxENT,boxWSDInner,boxENTInner) << endl;


//    if (!checkOverlapOfBoxes(boxWSD,boxENT,boxWSDInner,boxENTInner))
//    {
//        boxCounter=0;
//        writeBox(boxWSDInner, boxENTInner, std::string("box1"));
//        writeBox(boxWSD, boxENT, std::string("box2"));
//        cout << "na super " << endl;
//    }
//    cout << "idhex " << id_hex_outside << " i " << iTemp << " j " << jTemp << " k " << kTemp << endl;
    return std::vector<int>{id_hex_outside, iTemp, jTemp, kTemp};


}

std::vector<std::vector <int> > Interpolate_direct::filterCorrectNeighbours(std::vector<std::vector<int> > outer)
{
    //filter wrong Neighbours (from other side)
//    cout << "outer size " << outer.size() << endl;
//    for (int iterInner = 0; iterInner < outer.size(); iterInner++)
//    {
//        for (int iterI = 0; iterI < outer.at(iterInner).size(); iterI++)
//        {
//            cout << outer.at(iterInner).at(iterI) << " ";
//        }
//        cout << "\n";
//    }
//    cout << endl;
    std::vector<int> sumHasToBeFour(blockgrid->Give_unstructured_grid()->Give_number_hexahedra());
    std::vector<std::vector<int > > outerCorrect(0);
    for (int iterOutside = 0 ; iterOutside < outer.size() ; iterOutside++ )
    {
        sumHasToBeFour.at(outer.at(iterOutside).at(0))++;
    }
    int correctNeighbourHex{-1};
    for (int iterOutside = 0 ; iterOutside < sumHasToBeFour.size() ; iterOutside++ )
    {
        if (sumHasToBeFour.at(iterOutside) == 4)
        {
            correctNeighbourHex = iterOutside ;
        }
    }
    //copy correct ones :
    for ( int iterOutside = 0 ; iterOutside < outer.size() ; iterOutside++ )
    {
        if (outer.at(iterOutside).at(0) ==correctNeighbourHex)
        {
           outerCorrect.push_back(outer.at(iterOutside));
        }
    }
    if (outerCorrect.size() != 4 && outerCorrect.size() != 0)
    {
        cout << "size has to be 4 or 0, but isnt"<< endl;
    }
    return outerCorrect;
}

bool Interpolate_direct::compareIndicies(std::vector<std::vector<int> > inner, std::vector<std::vector<int> > outer, int notCheck)
{
    bool isEqual = true;
    for (int iter = 0 ; iter < 4 ; iter++)
    {
         for (int iterInner = 1 ; iterInner < 4 ; iterInner++)
         {
             if (iterInner!=notCheck)
             {
                isEqual = isEqual && (outer.at(iter).at(iterInner) == inner.at(iter).at(iterInner));
             }
         }
    }
//    if (isEqual)
//    {
//        cout << "correct found !"<< endl;
//    }
    return isEqual;
}

std::vector<std::vector<int> > Interpolate_direct::switchIJ(std::vector<std::vector<int> > v)
{
    for (int iter = 0 ; iter < 4 ; iter++)
    {
       int T            = v.at(iter).at(1);
       v.at(iter).at(1) = v.at(iter).at(2);
       v.at(iter).at(2) = T;
    }
    return v;
}

std::vector<std::vector<int> > Interpolate_direct::switchIK(std::vector<std::vector<int> > v)
{
    for (int iter = 0 ; iter < 4 ; iter++)
    {
       int T            = v.at(iter).at(1);
       v.at(iter).at(1) = v.at(iter).at(3);
       v.at(iter).at(3) = T;
    }
    return v;
}

std::vector<std::vector<int> > Interpolate_direct::switchJK(std::vector<std::vector<int> > v)
{
    for (int iter = 0 ; iter < 4 ; iter++)
    {
       int T            = v.at(iter).at(2);
       v.at(iter).at(2) = v.at(iter).at(3);
       v.at(iter).at(3) = T;
    }
    return v;
}

std::vector<std::vector<int> > Interpolate_direct::invertI(std::vector<std::vector<int> > v)
{
    int Nx = blockgrid->Give_Nx_hexahedron(v.front().front());
    for (int iter = 0 ; iter < 4 ; iter++)
    {
        v.at(iter).at(1) = Nx - v.at(iter).at(1);
    }
    return v;
}

std::vector<std::vector<int> > Interpolate_direct::invertJ(std::vector<std::vector<int> > v)
{
    int Ny = blockgrid->Give_Ny_hexahedron(v.front().front());
    for (int iter = 0 ; iter < 4 ; iter++)
    {
        v.at(iter).at(2) = Ny - v.at(iter).at(2);
    }
    return v;
}

std::vector<std::vector<int> > Interpolate_direct::invertK(std::vector<std::vector<int> > v)
{
    int Nz = blockgrid->Give_Nz_hexahedron(v.front().front());
    for (int iter = 0 ; iter < 4 ; iter++)
    {
        v.at(iter).at(3) = Nz - v.at(iter).at(3);
    }
    return v;
}





void Interpolate_direct::find_cell(D3vector v)
{
//    vPrevPrev = vPrev;
//    vPrev = vNow;
    if (v == vNow)
    {
        counterSamePoint++;
        return;
    }
            badLambdaFound = false;
            bool badIsBestLambda = false;
            bool previousBadLambda = false;
            idHexNow = -1;
            vNow = v;
            lambda = D3vector(-1,-1,-1);



            D3vector boxWSD, boxENT;

            //cout << "looking for new cell!" << endl;

            int Nx, Ny, Nz;
            // prev
            if (checkBox(idHexPrev,iPrev, jPrev, kPrev, v) != -1){
                if (!badLambdaFound)
                {
                    counterFastest++;
                    setPrevIndex();
                    return;
                }
                else
                {
                    previousBadLambda = true;
                    //std::cout << "should be in next or is at boundary!" << std::endl;
                }
            }
            //std::cout << idHexPrev<< " " <<iPrev<< " " << jPrev<< " " << kPrev << std::endl;
            if (checkBoxSurroundingOptimized(idHexPrev,iPrev, jPrev, kPrev, v) != -1)
            {
                if ((previousBadLambda && badLambdaFound ) || !badLambdaFound)
                {
                    //will stay bad : probably at the boundary!
                    counterFast++;
                    setPrevIndex();
                    return;
                }


            }
            // prev prev
            if (checkBox(idHexPrevPrev,iPrevPrev, jPrevPrev, kPrevPrev, v) != -1)
            {
                if (!badLambdaFound)
                {
                    setPrevIndex();
                    setPrevPrevIndex();
                    //cout << " n.: " << idHexNow << " " << kNow << " " << jNow << " " << kNow << endl;
                    counterSecondTry++;
                    return;
                }
                else
                {
                    previousBadLambda = true;
                    //std::cout << "should be in next or is at boundary!" << std::endl;
                }

            }

            if (checkBoxSurroundingOptimized(idHexPrevPrev,iPrevPrev, jPrevPrev, kPrevPrev, v) != -1)
            {
                if ((previousBadLambda && badLambdaFound ) || !badLambdaFound)
                {
                    setPrevIndex();
                    setPrevPrevIndex();
                    //cout << " n.: " << idHexNow << " " << kNow << " " << jNow << " " << kNow << endl;
                    counterSecondTry++;
                    return;
                }

            }
            // prev prev prev
            if (checkBox(idHexPrevPrevPrev,iPrevPrevPrev, jPrevPrevPrev, kPrevPrevPrev, v) != -1)
            {
                if (!badLambdaFound)
                {
                    setPrevIndex();
                    setPrevPrevIndex();
                    setPrevPrevPrevIndex();
                    //cout << " n.: " << idHexNow << " " << kNow << " " << jNow << " " << kNow << endl;
                    counterThirdTry++;
                    return;
                }
                else
                {
                    previousBadLambda = true;
                    //std::cout << "should be in next or is at boundary!" << std::endl;
                }


            }

            if (checkBoxSurroundingOptimized(idHexPrevPrevPrev,iPrevPrevPrev, jPrevPrevPrev, kPrevPrevPrev, v) != -1)
            {
                if ((previousBadLambda && badLambdaFound ) || !badLambdaFound)
                {
                    setPrevIndex();
                    setPrevPrevIndex();
                    setPrevPrevPrevIndex();
                    //cout << " n.: " << idHexNow << " " << kNow << " " << jNow << " " << kNow << endl;
                    counterThirdTry++;
                    return;
                }

            }



    for(int id_hex=0;id_hex<blockgrid->Give_unstructured_grid()->Give_number_hexahedra();++id_hex) {
        Nx = blockgrid->Give_Nx_hexahedron(id_hex);
        Ny = blockgrid->Give_Ny_hexahedron(id_hex);
        Nz = blockgrid->Give_Nz_hexahedron(id_hex);

        for(int k=0;k<Nz;++k)
      for(int j=0;j<Ny;++j)
        for(int i=0;i<Nx;++i) {
            if (0 < checkBox(id_hex,i, j, k, v))
            {
                if (abs(iNow - iPrev) < 2 && abs(jNow - jPrev) < 2 && abs(kNow - kPrev) < 2)
                {
                    std::cout<<std::endl;
                    std::cout << "why not found?!\n";
                }
                int babsdbas=0;
                babsdbas = 1;
//                k =Nz;
//                j =Ny;
//                i =Nx;
//                id_hex = blockgrid->Give_unstructured_grid()->Give_number_hexahedra();
            }
        }
    }
    if (badLambdaFound)
    {
        if (MIN(lambda) < 0 ||  MAX(lambda)>1)
        {
        }
        else
        {
            assert(false && "must not happen!");
        }
        //lambda.Print();std::cout<<std::flush;
    }
//    cout << " pp: " << idHexPrevPrevPrev << " " << iPrevPrevPrev << " " << jPrevPrevPrev << " " << kPrevPrevPrev << endl;
//    cout << " pp: " << idHexPrevPrev << " " << iPrevPrev << " " << jPrevPrev << " " << kPrevPrev << endl;
//    cout << " p.: " << idHexPrev << " " << iPrev << " " << jPrev << " " << kPrev << endl;
//    cout << " n.: " << idHexNow << " " << kNow << " " << jNow << " " << kNow << endl;

//    if (idHexPrevPrev != -1)
//    {
//        vPrevPrevPrev.Print() ; cout << endl;
//        vPrevPrev.Print() ; cout << endl;
//        vPrev.Print() ; cout << endl;
//        vNow.Print() ; cout << endl;

//        writePoint(vPrevPrevPrev, std::string("testPoint"), 0);
//        D3vector t1 = arrayBoxWSDENT.at(idHexPrevPrevPrev).at(iPrevPrevPrev    +(Nx-0)*(jPrevPrevPrev    +(Nx-0)* kPrevPrevPrev)).at(0);
//        D3vector t2 = arrayBoxWSDENT.at(idHexPrevPrevPrev).at(iPrevPrevPrev    +(Nx-0)*(jPrevPrevPrev    +(Nx-0)* kPrevPrevPrev)).at(1);
//        writeBox(t1,t2,std::string("testBox"));

//        writePoint(vPrevPrev, std::string("testPoint"), 1);
//        t1 = arrayBoxWSDENT.at(idHexPrevPrev).at(iPrevPrev    +(Nx-0)*(jPrevPrev    +(Nx-0)* kPrevPrev)).at(0);
//        t2 = arrayBoxWSDENT.at(idHexPrevPrev).at(iPrevPrev    +(Nx-0)*(jPrevPrev    +(Nx-0)* kPrevPrev)).at(1);
//        writeBox(t1,t2,std::string("testBox"));


//        writePoint(vPrev, std::string("testPoint"),2 );
//        t1 = arrayBoxWSDENT.at(idHexPrev).at(iPrev    +(Nx-0)*(jPrev    +(Nx-0)* kPrev)).at(0);
//        t2 = arrayBoxWSDENT.at(idHexPrev).at(iPrev    +(Nx-0)*(jPrev    +(Nx-0)* kPrev)).at(1);
//        writeBox(t1,t2,std::string("testBox"));


//        writePoint(vNow, std::string("testPoint"),3);
//        if (idHexNow != -1)
//        {
//            t1 = arrayBoxWSDENT.at(idHexNow).at(kNow    +(Nx-0)*(jNow    +(Nx-0)* kNow)).at(0);
//            t2 = arrayBoxWSDENT.at(idHexNow).at(kNow    +(Nx-0)*(jNow    +(Nx-0)* kNow)).at(1);
//        }
//        writeBox(t1,t2,std::string("testBox"));
//         boxCounter = 0;
//    }


    setPrevIndex();
    setPrevPrevIndex();
    setPrevPrevPrevIndex();
    counterSlow++;
   // cout << "slow version"<<endl;
}

void Interpolate_direct::setPrevIndex()
{
    vPrev = vNow;
    idHexPrev = idHexNow;
    iPrev = iNow ;
    jPrev = jNow ;
    kPrev = kNow ;
}

void Interpolate_direct::setPrevPrevIndex()
{
    vPrevPrev = vNow;
    idHexPrevPrev = idHexNow;
    iPrevPrev = iNow ;
    jPrevPrev = jNow ;
    kPrevPrev = kNow ;
}

void Interpolate_direct::setPrevPrevPrevIndex()
{
    vPrevPrevPrev = vNow;
    idHexPrevPrevPrev = idHexNow;
    iPrevPrevPrev = iNow ;
    jPrevPrevPrev = jNow ;
    kPrevPrevPrev = kNow ;
}

int Interpolate_direct::checkBox(int id_Hex, int i, int j, int k, D3vector v)
{
    if (id_Hex < 0 )
    {
       return -1;
    }
    int Nx = blockgrid->Give_Nx_hexahedron(id_Hex);
    int Ny = blockgrid->Give_Ny_hexahedron(id_Hex);
    int Nz = blockgrid->Give_Nz_hexahedron(id_Hex);
    if ((i < -1 && j < -1 && k < -1 ) ||  (i > Nx && j > Ny && k > Nz ) ||  ( (i == -1 || i == Nx ) && (j == -1 || j == Ny) && (k == -1 || k == Nz) ) )
    {
       return -1;
    }
    int typ = -1;
    if ( (i == -1 || j == -1 || k == -1 ) ||  (i == Nx || j == Ny || k == Nz ) )
    {
        typ = checkForHexaNeighbours(id_Hex, i, j, k, v);
        if ( -1 !=typ)
        {
             return typ;
        }
    }

    // figure out, whether intersection with other hex is true;



    if ( id_Hex< 0 || i < 0 || j < 0 || k < 0)
    {
        return -1;
    }
    if (i >= Nx || j >= Ny || k >= Nz )
    {
        return -1;
    }
    checkCounter++;
//    D3vector cWSD, cESD;
//    D3vector cWND, cEND;

//    D3vector cWST, cEST;
//    D3vector cWNT, cENT;

    D3vector boxWSD, boxENT;

    D3vector ploc;

    if ( i    +Nx*(j    +Ny* k)  >= Nx*Ny*Nz)
    {
        cout << "Nx " << Nx << endl;
        cout << "Ny " << Ny << endl;
        cout << "i " << i << endl;
        cout << "j " << j << endl;
        cout << "k " << k << endl;
        cout << "id of array: " << i    +Nx*(j    +Ny* k) << endl;
    }
    boxWSD =  arrayBoxWSDENT.at(id_Hex).at(i    +Nx*(j    +Ny* k)).at(0);
    boxENT =  arrayBoxWSDENT.at(id_Hex).at(i    +Nx*(j    +Ny* k)).at(1);

   // writeBox(boxWSD,boxENT,std::string("box"));


    if (boxENT>=v && boxWSD<=v)
    {

        if (debugTest)
        {
            //cout << "debug " << endl;
        }
      // cout << "inside box!" << endl;
        cWSD = blockgrid->Give_coord_hexahedron(id_Hex,i,  j,  k  );
        cESD = blockgrid->Give_coord_hexahedron(id_Hex,i+1,j  ,k  );
        cWND = blockgrid->Give_coord_hexahedron(id_Hex,i,  j+1,k  );
        cEND = blockgrid->Give_coord_hexahedron(id_Hex,i+1,j+1,k  );

        cWST = blockgrid->Give_coord_hexahedron(id_Hex,i,  j,  k+1);
        cEST = blockgrid->Give_coord_hexahedron(id_Hex,i+1,j  ,k+1);
        cWNT = blockgrid->Give_coord_hexahedron(id_Hex,i,  j+1,k+1);
        cENT = blockgrid->Give_coord_hexahedron(id_Hex,i+1,j+1,k+1);



        D3vector lam, lamTemp ;


        if (trilinearInterpolationFlag)
        {
            D3vector lamTrilinar = trilinarInterpolation(v,id_Hex,i,  j,  k  );

            if (lamTrilinar != -1)
            {
                typ = 6;
                lambda = lamTrilinar;
                typ_tet = typ;
                idHexNow = id_Hex;
                iNow = i;
                jNow = j;
                kNow = k;
                return typ;
            }
        }
        else
        {
            //std::cout << "\n\nstart of tet search\n\n";

            lam = lambda_of_p_in_tet(v,cWND,cWNT,cWST,cEST);
            //lam.Print();
            //std::cout << "lambda bef :";lambda.Print();
            lamTemp = lambda;
            //std::cout << "new better : " << new_lam_better(lamTemp,lam) << std::endl;

            //std::cout << "\nnew lam : "; lam.Print();
            //std::cout << "old lam : "; lamTemp.Print();
            if (contained_in_tet_strong(lam) && new_lam_better(lamTemp,lam)){typ=0;typCounter0++;lamTemp = lam;}

            //std::cout << "new best : "; lamTemp.Print();
            lam = lambda_of_p_in_tet(v,cEST,cWND,cWST,cESD);
            //std::cout << "\nnew lam : "; lam.Print();
            //std::cout << "old lam : "; lamTemp.Print();
            //std::cout << "new better : " << new_lam_better(lamTemp,lam) << std::endl;
            if (contained_in_tet_strong(lam) && new_lam_better(lamTemp,lam)){typ=1;typCounter1++;lamTemp = lam;}

            //std::cout << "new best : "; lamTemp.Print();
            lam = lambda_of_p_in_tet(v,cWND,cWSD,cWST,cESD);
            //std::cout << "\nnew lam : "; lam.Print();
            //std::cout << "old lam : "; lamTemp.Print();
            //std::cout << "new better : " << new_lam_better(lamTemp,lam) << std::endl;
            if (contained_in_tet_strong(lam) && new_lam_better(lamTemp,lam)){typ=2;typCounter2++;lamTemp = lam;}

            //std::cout << "new best : "; lamTemp.Print();
            lam = lambda_of_p_in_tet(v,cEST,cWND,cESD,cEND);
            //std::cout << "\nnew lam : "; lam.Print();
            //std::cout << "old lam : "; lamTemp.Print();
            //std::cout << "new better : " << new_lam_better(lamTemp,lam) << std::endl;
            if (contained_in_tet_strong(lam) && new_lam_better(lamTemp,lam)){typ=3;typCounter3++;lamTemp = lam;}

            //std::cout << "new best : "; lamTemp.Print();
            lam = lambda_of_p_in_tet(v,cENT,cWNT,cEST,cEND);
            //std::cout << "\nnew lam : "; lam.Print();
            //std::cout << "old lam : "; lamTemp.Print();
            //std::cout << "new better : " << new_lam_better(lamTemp,lam) << std::endl;
            if (contained_in_tet_strong(lam) && new_lam_better(lamTemp,lam)){typ=4;typCounter4++;lamTemp = lam;}

            //std::cout << "new best : "; lamTemp.Print();
            lam = lambda_of_p_in_tet(v,cWNT,cWND,cEST,cEND);
            //std::cout << "\nnew lam : "; lam.Print();
            //std::cout << "old lam : "; lamTemp.Print();
            //std::cout << "new better : " << new_lam_better(lamTemp,lam) << std::endl;
            if (contained_in_tet_strong(lam) && new_lam_better(lamTemp,lam)){typ=5;typCounter5++;lamTemp = lam;}

            //std::cout << "new best : "; lamTemp.Print();
            if( typ != -1 && new_lam_better(lambda,lamTemp))
            {
               if (MIN(lamTemp) < 0.0 || MAX(lamTemp) >1.0 )
               {
                   badLambdaFound = true;
                   //lamTemp.Print();
               }
               else
               {
                   badLambdaFound = false;
               }
                //lambda = lam;
//               if (D3VectorNorm(lamTemp - D3vector(-0.184893695657936, 0.215428315449784, 0.533250940577171)) < 1e-5)
//               {

//                   std::cout << "stop" << std::endl;
//               }
//               if (D3VectorNorm(lamTemp - D3vector(0.357354990724203, 0.152269993258782, 0.41683241494242)) < 1e-5)
//               {

//                   std::cout << "stop" << std::endl;
//               }
                lambda = lamTemp;
                typ_tet = typ;
                idHexNow = id_Hex;
                iNow = i;
                jNow = j;
                kNow = k;



            }
        }


    }
    return typ;
}

int Interpolate_direct::checkBoxSurrounding(int idHex, int i, int j, int k, D3vector v)
{
    int iterationPerSurrounding = 0;
    for(int kIter=k-1;kIter<k+2;++kIter)
      for(int jIter=j-1;jIter<j+2;++jIter)
        for(int iIter=i-1;iIter<i+2;++iIter) {
          if (checkBox(idHex,iIter, jIter, kIter, v) != -1){
              //counterFast++;
              //setPrevIndex();
              //cout << "iterationPerSurrounding " << iterationPerSurrounding+1<< endl;
              return 0;}
          else { iterationPerSurrounding++;}
      }
    return -1;
}

int Interpolate_direct::checkBoxSurroundingOptimized(int idHex, int i, int j, int k, D3vector v, bool neglectBadLambda)
{
    bool temp = badLambdaFound;
    badLambdaFound = false;
    int iterationPerSurrounding = 0;
    //std::vector<
    for(int kIter=k+1;kIter>k-2;kIter-=2){
          if (checkBox(idHex,i, j, kIter, v) != -1 && !badLambdaFound){//cout << "iterationPerSurrounding " << iterationPerSurrounding+1<< endl;
              return 0;}
          else { iterationPerSurrounding++;}
      }
    if (badLambdaFound && !neglectBadLambda)
    {
        if (checkBoxSurroundingOptimized(idHexNow,iNow, jNow, kNow, v,true) != -1) return 0; else return -1;
    }

    for(int iIter=i-1;iIter<i+2;iIter+=2){
          if (checkBox(idHex,iIter, j, k, v) != -1 && !badLambdaFound){//cout << "iterationPerSurrounding " << iterationPerSurrounding+1<< endl;
              return 0;}
          else { iterationPerSurrounding++;}
      }
    if (badLambdaFound && !neglectBadLambda)
    {
        if (checkBoxSurroundingOptimized(idHexNow,iNow, jNow, kNow, v,true) != -1) return 0; else return -1;
    }
    for(int jIter=j-1;jIter<j+2;jIter+=2){
          if (checkBox(idHex,i, jIter, k, v) != -1 && !badLambdaFound){//cout << "iterationPerSurrounding " << iterationPerSurrounding+1<< endl;
              return 0;}
          else { iterationPerSurrounding++;}
      }
    if (badLambdaFound && !neglectBadLambda)
    {
        if (checkBoxSurroundingOptimized(idHexNow,iNow, jNow, kNow, v,true) != -1) return 0; else return -1;
    }

    for(int kIter=k-1;kIter<k+2;kIter+=2)
      for(int jIter=j-1;jIter<j+2;jIter+=2) {
          if (checkBox(idHex,i, jIter, kIter, v) != -1 && !badLambdaFound){
              //counterFast++;

              //setPrevIndex();
              //cout << "iterationPerSurrounding " << iterationPerSurrounding+1<< endl;
              return 0;}
          else { iterationPerSurrounding++;}
      }
    if (badLambdaFound && !neglectBadLambda)
    {
        if (checkBoxSurroundingOptimized(idHexNow,iNow, jNow, kNow, v,true) != -1) return 0; else return -1;
    }
    for(int kIter=k-1;kIter<k+2;kIter+=2)
        for(int iIter=i-1;iIter<i+2;iIter+=2) {
          if (checkBox(idHex,iIter, j, kIter, v) != -1 && !badLambdaFound){ //here
              //counterFast++;

              //setPrevIndex();
              //cout << "iterationPerSurrounding " << iterationPerSurrounding+1<< endl;
              return 0;}
          else { iterationPerSurrounding++;}
      }
    if (badLambdaFound && !neglectBadLambda)
    {
        if (checkBoxSurroundingOptimized(idHexNow,iNow, jNow, kNow, v,true) != -1) return 0; else return -1;
    }
      for(int jIter=j-1;jIter<j+2;jIter+=2)
        for(int iIter=i-1;iIter<i+2;iIter+=2) {
          if (checkBox(idHex,iIter, jIter, k, v) != -1 && !badLambdaFound){
              //counterFast++;

              //setPrevIndex();
              //cout << "iterationPerSurrounding " << iterationPerSurrounding+1<< endl;
              return 0;}
          else { iterationPerSurrounding++;}
      }
      if (badLambdaFound && !neglectBadLambda)
      {
          if (checkBoxSurroundingOptimized(idHexNow,iNow, jNow, kNow, v,true) != -1) return 0; else return -1;
      }


    for(int kIter=k-1;kIter<k+2;kIter+=2)
      for(int jIter=j-1;jIter<j+2;jIter+=2)
        for(int iIter=i-1;iIter<i+2;iIter+=2) {
          if (checkBox(idHex,iIter, jIter, kIter, v) != -1 && !badLambdaFound){
              //counterFast++;
              //setPrevIndex();
              //cout << "iterationPerSurrounding " << iterationPerSurrounding+1<< endl;
              return 0;}
          else { iterationPerSurrounding++;}
      }
    if (badLambdaFound && !neglectBadLambda)
    {
        if (checkBoxSurroundingOptimized(idHexNow,iNow, jNow, kNow, v,true) != -1) return 0; else return -1;
    }



    badLambdaFound = temp;
    return -1;
}

int Interpolate_direct::checkCorner(int idHex, int i, int j, int k, D3vector v)
{
    int typ;

    int Nx = blockgrid->Give_Nx_hexahedron(idHex);
    int Ny = blockgrid->Give_Ny_hexahedron(idHex);
    int Nz = blockgrid->Give_Nz_hexahedron(idHex);

    int index = i    +(Ny)*(j    +(Nx)* k);
    index = i    +Nx*(j    +Ny* k);

    if (array_box_boundary.at(idHex).at(index).empty())
        return -1;
    int idHexCorner1 = array_box_boundary.at(idHex).at(index).at(0);
    int iCorner1     = array_box_boundary.at(idHex).at(index).at(1);
    int jCorner1     = array_box_boundary.at(idHex).at(index).at(2);
    int kCorner1     = array_box_boundary.at(idHex).at(index).at(3);


    D3vector boxWSDInner =  arrayBoxWSDENT.at(idHex).at(i    +Nx*(j    +Ny* k)).at(0);
    D3vector boxENTInner =  arrayBoxWSDENT.at(idHex).at(i    +Nx*(j    +Ny* k)).at(1);

    int NxCorner1 = blockgrid->Give_Nx_hexahedron(idHexCorner1);
    int NyCorner1 = blockgrid->Give_Ny_hexahedron(idHexCorner1);
    int NzCorner1 = blockgrid->Give_Nz_hexahedron(idHexCorner1);

    D3vector boxWSDOuter =  arrayBoxWSDENT.at(idHexCorner1).at(iCorner1    +NxCorner1*(jCorner1    +NyCorner1* kCorner1)).at(0);
    D3vector boxENTOuter =  arrayBoxWSDENT.at(idHexCorner1).at(iCorner1    +NxCorner1*(jCorner1    +NyCorner1* kCorner1)).at(1);
    //writeBox(boxWSDInner, boxENTInner, std::string("boxCorner"));
    //writeBox(boxWSDOuter, boxENTOuter, std::string("boxCorner"));

    if (!checkOverlapOfBoxes(boxWSDInner, boxENTInner, boxWSDOuter, boxENTOuter))
    {
        writeBox(boxWSDInner, boxENTInner, std::string("box1"));
        writeBox(boxWSDOuter, boxENTOuter, std::string("box1"));
        cout << "surrounding box is wrong!" << endl;
    }

    //cout << "corner box 1 : ";

    //cout << "id: " << idHexCorner1 << " " << iCorner1 << " " << jCorner1 << " " << kCorner1 << endl;
    //boxWSDOuter.Print();cout << endl;
    //boxENTOuter.Print();cout << endl;

    typ = checkBox(idHexCorner1, iCorner1, jCorner1, kCorner1, v);
    if (typ != -1)
    {
        counterCorner++;
        return typ;
    }

    if (array_box_boundary.at(idHex).at(index).size() < 5)
        return -1;

    int idHexCorner2 = array_box_boundary.at(idHex).at(i    +(Nx)*(j    +(Ny)* k)).at(4);
    int iCorner2 = array_box_boundary.at(idHex).at(i    +(Nx)*(j    +(Ny)* k)).at(5);
    int jCorner2 = array_box_boundary.at(idHex).at(i    +(Nx)*(j    +(Ny)* k)).at(6);
    int kCorner2 = array_box_boundary.at(idHex).at(i    +(Nx)*(j    +(Ny)* k)).at(7);

    int NxCorner2 = blockgrid->Give_Nx_hexahedron(idHexCorner2);
    int NyCorner2 = blockgrid->Give_Ny_hexahedron(idHexCorner2);
    int NzCorner2 = blockgrid->Give_Nz_hexahedron(idHexCorner2);

    boxWSDOuter =  arrayBoxWSDENT.at(idHexCorner2).at(iCorner2    +NxCorner2*(jCorner2    +NyCorner2* kCorner2)).at(0);
    boxENTOuter =  arrayBoxWSDENT.at(idHexCorner2).at(iCorner2    +NxCorner2*(jCorner2    +NyCorner2* kCorner2)).at(1);



    if (!checkOverlapOfBoxes(boxWSDInner, boxENTInner, boxWSDOuter, boxENTOuter))
    {
        writeBox(boxWSDInner, boxENTInner, std::string("box1"));
        writeBox(boxWSDOuter, boxENTOuter, std::string("box1"));
        cout << "surrounding box is wrong!" << endl;
    }


    typ = checkBox(idHexCorner2, iCorner2, jCorner2, kCorner2, v);
    if (typ != -1)
    {
        counterCorner++;
        return typ;
    }

    return typ;

}

int Interpolate_direct::checkEdge(int idHex, int i, int j, int k, D3vector v)
{

    int typ;

    int Nx = blockgrid->Give_Nx_hexahedron(idHex);
    int Ny = blockgrid->Give_Ny_hexahedron(idHex);
    int Nz = blockgrid->Give_Nz_hexahedron(idHex);

    int index = i    +(Nx)*(j    +(Nx)* k);
    index = (i)+Nx*(j    +Ny*(k));
    if (array_box_boundary.at(idHex).at(index).empty())
        return -1;
    int idHexEdge = array_box_boundary.at(idHex).at(index).at(0);
    int iEdge = array_box_boundary.at(idHex).at(index).at(1);
    int jEdge = array_box_boundary.at(idHex).at(index).at(2);
    int kEdge = array_box_boundary.at(idHex).at(index).at(3);

    D3vector boxWSDInner =  arrayBoxWSDENT.at(idHex).at(i    +Nx*(j    +Ny* k)).at(0);
    D3vector boxENTInner =  arrayBoxWSDENT.at(idHex).at(i    +Nx*(j    +Ny* k)).at(1);

    int NxInner = blockgrid->Give_Nx_hexahedron(idHexEdge);
    int NyInner = blockgrid->Give_Ny_hexahedron(idHexEdge);
    int NzInner = blockgrid->Give_Nz_hexahedron(idHexEdge);

    int indexInner = iEdge    +NxInner*(jEdge    +NyInner* kEdge);
    //int indexInnerTest = iEdge    +NyInner*(jEdge    +NxInner* kEdge);

    D3vector boxWSDOuter =  arrayBoxWSDENT.at(idHexEdge).at(indexInner).at(0);
    D3vector boxENTOuter =  arrayBoxWSDENT.at(idHexEdge).at(indexInner).at(1);

    //writeBox(boxWSDInner, boxENTInner, std::string("boxEdge"));
    //writeBox(boxWSDOuter, boxENTOuter, std::string("boxEdge"));

    if (!checkOverlapOfBoxes(boxWSDInner, boxENTInner, boxWSDOuter, boxENTOuter))
    {
        writeBox(boxWSDInner, boxENTInner, std::string("box1"));
        writeBox(boxWSDOuter, boxENTOuter, std::string("box2"));
        cout << "surrounding box is wrong!" << endl;
    }

    //cout << "edge box     : ";
    //cout << "id: " << idHexEdge << " " << iEdge << " " << jEdge << " " << kEdge << endl;
    //boxWSDOuter.Print();cout << endl;
    //boxENTOuter.Print();cout << endl;

    typ = checkBox(idHexEdge, iEdge, jEdge, kEdge, v);
    if (typ != -1)
    {
        counterEdge++;
        return typ;
    }

    return typ;

}

bool Interpolate_direct::checkOverlapOfBoxes(D3vector vWSD, D3vector vENT, D3vector wWSD, D3vector wENT)
{
    double epsilon = 1e-10;
    if ( (( (vWSD.x <= wENT.x + epsilon) && (vWSD.x + epsilon >= wWSD.x) ) || ( (vENT.x <= wENT.x + epsilon) && (vENT.x + epsilon >= wWSD.x) ) )
      && (( (vWSD.y <= wENT.y + epsilon) && (vWSD.y + epsilon >= wWSD.y) ) || ( (vENT.y <= wENT.y + epsilon) && (vENT.y + epsilon >= wWSD.y) ) )
      && (( (vWSD.z <= wENT.z + epsilon) && (vWSD.z + epsilon >= wWSD.z) ) || ( (vENT.z <= wENT.z + epsilon) && (vENT.z + epsilon >= wWSD.z) ) ) )
    {
        return true;
    }

    if ( (( (wWSD.x <= vENT.x + epsilon) && (wWSD.x + epsilon >= vWSD.x) ) || ( (wENT.x <= vENT.x + epsilon) && (wENT.x + epsilon >= vWSD.x) ) )
      && (( (wWSD.y <= vENT.y + epsilon) && (wWSD.y + epsilon >= vWSD.y) ) || ( (wENT.y <= vENT.y + epsilon) && (wENT.y + epsilon >= vWSD.y) ) )
      && (( (wWSD.z <= vENT.z + epsilon) && (wWSD.z + epsilon >= vWSD.z) ) || ( (wENT.z <= vENT.z + epsilon) && (wENT.z + epsilon >= vWSD.z) ) ) )
    {
        return true;
    }
    return false;
}

double Interpolate_direct::calculateOverlapOfBoxes(D3vector vWSD, D3vector vENT, D3vector wWSD, D3vector wENT)
{
    double vol1 = (vENT.x-vWSD.x)*(vENT.y-vWSD.y)*(vENT.z-vWSD.z);
    double vol2 = (wENT.x-wWSD.x)*(wENT.y-wWSD.y)*(wENT.z-wWSD.z);
    double x,y,z;

    if (wWSD.x <= vWSD.x && vENT.x <= wENT.x)
         x   = vENT.x - vWSD.x ;
    else if (wWSD.x <= vWSD.x && wENT.x <= vENT.x)
         x   = wENT.x - vWSD.x ;
    else if (vWSD.x <= wWSD.x && wENT.x <= vENT.x)
         x   = wENT.x - wWSD.x ;
    else if (vWSD.x <= wWSD.x && vENT.x <= wENT.x)
         x   = vENT.x - wWSD.x ;
    else
        cout << "no overlap!?" << endl;



    if (wWSD.y <= vWSD.y && vENT.y <= wENT.y)
         y   = vENT.y - vWSD.y ;
    else if (wWSD.y <= vWSD.y && wENT.y <= vENT.y)
         y   = wENT.y - vWSD.y ;
    else if (vWSD.y <= wWSD.y && wENT.y <= vENT.y)
         y   = wENT.y - wWSD.y ;
    else if (vWSD.y <= wWSD.y && vENT.y <= wENT.y)
         y   = vENT.y - wWSD.y ;
    else {
        cout << "no overlap!?" << endl;}


    if (wWSD.z <= vWSD.z && vENT.z <= wENT.z)
         z   = vENT.z - vWSD.z ;
    else if (wWSD.z <= vWSD.z && wENT.z <= vENT.z)
         z   = wENT.z - vWSD.z ;
    else if (vWSD.z <= wWSD.z && wENT.z <= vENT.z)
         z   = wENT.z - wWSD.z ;
    else if (vWSD.z <= wWSD.z && vENT.z <= wENT.z)
         z   = vENT.z - wWSD.z ;
    else {
        cout << "no overlap!?" << endl;}
    double vol3 = x * y * z;

    return  vol3 / (vol1 + vol2);
}

void Interpolate_direct::writeBox(D3vector vWSD, D3vector vENT, std::string str)
{

    std::stringstream ss;
    ss << std::setw(5) << std::setfill('0') << boxCounter;
    std::string s = ss.str();
    ofstream myfile;
      myfile.open ("C:/Users/rall/Documents/UG_Blocks_thermal/testergebnisse/" + s + str + ".vtk");
myfile << "# vtk DataFile Version 4.2\n";
myfile << "vtk output\n";
myfile << "ASCII\n";
myfile << "DATASET POLYDATA\n";
myfile << "POINTS 24 float\n";
myfile << vWSD.x << " " << vWSD.y << " " << vWSD.z << "\n" ;
myfile << vWSD.x << " " << vWSD.y << " " << vENT.z << "\n" ;
myfile << vWSD.x << " " << vENT.y << " " << vWSD.z << "\n" ;
myfile << vWSD.x << " " << vENT.y << " " << vENT.z << "\n" ;
myfile << vENT.x << " " << vWSD.y << " " << vWSD.z << "\n" ;
myfile << vENT.x << " " << vWSD.y << " " << vENT.z << "\n" ;
myfile << vENT.x << " " << vENT.y << " " << vWSD.z << "\n" ;
myfile << vENT.x << " " << vENT.y << " " << vENT.z << "\n" ;
myfile << vWSD.x << " " << vWSD.y << " " << vWSD.z << "\n" ;
myfile << vWSD.x << " " << vWSD.y << " " << vENT.z << "\n" ;
myfile << vENT.x << " " << vWSD.y << " " << vWSD.z << "\n" ;
myfile << vENT.x << " " << vWSD.y << " " << vENT.z << "\n" ;
myfile << vWSD.x << " " << vENT.y << " " << vWSD.z << "\n" ;
myfile << vWSD.x << " " << vENT.y << " " << vENT.z << "\n" ;
myfile << vENT.x << " " << vENT.y << " " << vWSD.z << "\n" ;
myfile << vENT.x << " " << vENT.y << " " << vENT.z << "\n" ;
myfile << vWSD.x << " " << vWSD.y << " " << vWSD.z << "\n" ;
myfile << vENT.x << " " << vWSD.y << " " << vWSD.z << "\n" ;
myfile << vWSD.x << " " << vENT.y << " " << vWSD.z << "\n" ;
myfile << vENT.x << " " << vENT.y << " " << vWSD.z << "\n" ;
myfile << vWSD.x << " " << vWSD.y << " " << vENT.z << "\n" ;
myfile << vENT.x << " " << vWSD.y << " " << vENT.z << "\n" ;
myfile << vWSD.x << " " << vENT.y << " " << vENT.z << "\n" ;
myfile << vENT.x << " " << vENT.y << " " << vENT.z << "\n" ;
myfile << "\n" ;
myfile << "POLYGONS 6 30\n";
myfile << "4 0 1 3 2 \n";
myfile << "4 4 6 7 5 \n";
myfile << "4 8 10 11 9 \n";
myfile << "4 12 13 15 14 \n";
myfile << "4 16 18 19 17 \n";
myfile << "4 20 21 23 22 \n";

      myfile.close();

      boxCounter++;
}

void Interpolate_direct::writePoint(D3vector v, std::string str, int Counter)
{
    std::stringstream ss;
    ss << std::setw(5) << std::setfill('0') << Counter;
    std::string s = ss.str();
    ofstream myfile;
      myfile.open ("testergebnisse/" + s + str + ".vtk");
      myfile << "# vtk DataFile Version 4.2\n";
      myfile << "vtk output\n";
      myfile << "ASCII\n";
      myfile << "DATASET POLYDATA\n";
      myfile << "POINTS 1 float\n";
      myfile << v.x << " " << v.y << " " << v.z << "\n" ;
      myfile << "VERTICES 1 2\n";
      myfile << "1 0 \n";
      myfile.close();
}

void Interpolate_direct::writeInterpolationEfficiency()
{
    double fastestVersion = (double)counterFastest/(counterSecondTry+counterFast+counterFastest+counterSlow+counterThirdTry+counterSamePoint) * 100;
    double fastVersion = (double)counterFast/(counterSecondTry+counterFast+counterFastest+counterSlow+counterThirdTry+counterSamePoint) * 100 ;
    double secondTryVersion = (double)counterSecondTry/(counterSecondTry+counterFast+counterFastest+counterSlow+counterThirdTry+counterSamePoint) ;
    double thirdTryVersion = (double)counterThirdTry/(counterSecondTry+counterFast+counterFastest+counterSlow+counterThirdTry+counterSamePoint) * 100;
    double slowVersion = (double)counterSlow/(counterSecondTry+counterFast+counterFastest+counterSlow+counterThirdTry+counterSamePoint) * 100 ;
    double samePoint = (double)counterSamePoint/(counterSecondTry+counterFast+counterFastest+counterSlow+counterThirdTry+counterSamePoint) * 100 ;

    cout << "same point      : " << samePoint << " %" << endl;
    cout << "fastest version : " << fastestVersion << " %"<<endl;
    cout << "fast version    : " << fastVersion << " %"<<endl;
    cout << "2. try version  : " << secondTryVersion << " %"<<endl;
    cout << "3. try version  : " << thirdTryVersion << " %"<<endl;
    cout << "slowest version : " << slowVersion << " %"<<endl;
    cout << "Hexa hits   :    " << counterHexa<< endl;
    cout << "Edge hits   :    " << counterEdge<< endl;
    cout << "Corner hits :    " << counterCorner<< endl;
    // check sum:
    double test = fastestVersion+fastVersion+secondTryVersion+thirdTryVersion+slowVersion+samePoint;
    if (fabs(test - 100)>1e-5)
    {
        cout << "error : in total " << test << " % !" << endl;
    }

}
bool isInRange(double d0, double d1)
{
    if (d0 * d1 >=0 )
    {
        return true;
    }
    return false;
}

D3vector Interpolate_direct::trilinarInterpolation(D3vector X, int id_Hex, int i, int j, int k)
{
//    cWSD = {1,1,0};      // 1,1,0 : x2
//    cESD = {1,0,0};     // 1,0,0 : x1
//    cWND = {0,1,0};      // 0,1,0 : x3
//    cEND = {0,0,0};  // 0,0,0 : x0
//    cWST = {1,1,1};      // 1,1,1 : x5
//    cEST = {1,0,1};      // 1,0,1 : x6
//    cWNT = {0,1,1};      // 0,1,1 : x4
//    cENT = {0,0,1};     // 0,0,1 : x7
//    D3vector shift{16,-26,12};
//        cWSD +=shift;      // 1,1,0 : x2
//        cESD +=shift;     // 1,0,0 : x1
//        cWND +=shift;      // 0,1,0 : x3
//        cEND +=shift;  // 0,0,0 : x0
//        cWST +=shift;      // 1,1,1 : x5
//        cEST +=shift;      // 1,0,1 : x6
//        cWNT +=shift;      // 0,1,1 : x4
//        cENT +=shift;     // 0,0,1 : x7


    D3vector B = cESD - cEND; // 100 - 000
    D3vector C = cWND - cEND; // 010 - 000
    D3vector D = cENT - cEND; // 001 - 000
    //std::cout << "to be implemented" << std::endl;


//    X = (cWSD+ cESD+ cWND + cEND + cWST + cEST + cWNT + cENT   ) / 8.0 ;

//    X.x = X.x +15;
//    X.y = X.y +15;
    D3vector normalTD = cross_product(-1*B,C);
    D3vector normalTDOPP = cross_product(cWST-cEST , cWNT - cWST);
    //D3vector normalTDOPP2 = cross_product(cENT-cEST ,cWNT - cENT);
    D3vector normalNS = cross_product(-1*C,D);
    D3vector normalNSOPP = cross_product(cWST-cEST,cWST - cWSD);
    //D3vector normalNSOPP2 = cross_product(cESD-cEST,cESD - cWSD);
    D3vector normalEW = cross_product(-1*D,B);
    D3vector normalEWOPP = cross_product(cWST-cWSD,cWST - cWNT);
    //D3vector normalEWOPP2 = cross_product(cWND-cWSD,cWND - cWNT);
    normalTD = normalTD / D3VectorNorm(normalTD);
    normalTDOPP = normalTDOPP / D3VectorNorm(normalTDOPP);
    normalNS = normalNS / D3VectorNorm(normalNS);
    normalNSOPP = normalNSOPP / D3VectorNorm(normalNSOPP);
    normalEW = normalEW / D3VectorNorm(normalEW);
    normalEWOPP = normalEWOPP / D3VectorNorm(normalEWOPP);
    double eta = 0.5;
    double xi  = 0.5;
    double phi = 0.5;
    bool fixEta = false;
    bool fixXi = false;
    bool fixPhi = false;
    D3vector coord(eta,xi,phi);

    double distTD0 = product(normalTD,cEND);
    double distTD1 = product(normalTDOPP,cWST);
    double distTDMid  = distTD0-product(normalTD,X);
    double distTDMid2 = distTD1-product(normalTDOPP,X);

    double distNS0 = product(normalNS,cEND);
    double distNS1 = product(normalNSOPP,cWST);
    double distNSMid = distNS0-product(normalNS,X);
    double distNSMid2 = distNS1 -product(normalNSOPP,X);

    double distEW0 = product(normalEW,cEND);
    double distEW1 = product(normalEWOPP,cWST);
    double distEWMid = distEW0-product(normalEW,X);
    double distEWMid2 = distEW1 -product(normalEWOPP,X);


//    std::cout << "in range " << isInRange(distTDMid,distTDMid2) << std::endl;
//    std::cout << "in range " << isInRange(distNSMid,distNSMid2) << std::endl;
//    std::cout << "in range " << isInRange(distEWMid,distEWMid2) << std::endl;

    if (!isInRange(distTDMid,distTDMid2) || !isInRange(distNSMid,distNSMid2) || !isInRange(distEWMid,distEWMid2) )
    {
        return D3vector{-1,-1,-1};
    }
//    std::cout << "in range " << isInRange(distTDMid,distTDMid2) << std::endl;
//    std::cout << "in range " << isInRange(distNSMid,distNSMid2) << std::endl;
//    std::cout << "in range " << isInRange(distEWMid,distEWMid2) << std::endl;

    D3vector A = cEND;        // 000
    D3vector E = cWSD - cESD - cWND + cEND;        // 110 - 100 - 010 + 000
    D3vector F = cWNT - cWND - cENT + cEND;        // 011 - 010 - 001 + 000
    D3vector G = cEST - cESD - cENT + cEND;        // 101 - 100 - 001 + 000
    D3vector H = cWST + cESD + cWND + cENT - cEND - cWSD - cWNT - cEST;        // 111 + 100 + 010 + 001 - 000 - 110 - 011 - 101
    if (fabs(angle_between_vectors(normalTD,normalTDOPP)-180) < 0.5)
    {
        double delta = (distTD1 - distTD0);
        phi = (distTDMid + delta - distTDMid2) /delta/ 2.0;
        coord.z = phi;
        fixPhi = true;
    }
    if (fabs(angle_between_vectors(normalNS,normalNSOPP)-180) < 0.5)
    {
        double delta = (distNS1 - distNS0);
        eta = (distNSMid + delta - distNSMid2) /delta / 2.0;
        coord.x = eta;
        fixEta = true;
    }
    if (fabs(angle_between_vectors(normalEW,normalEWOPP)-180) < 0.5)
    {
        double delta = (distEW1 - distEW0);
        xi = (distEWMid + delta - distEWMid2) /delta/ 2.0;
        //xi = (distEWMid + distEWMid2)/ 2.0 / (distEW1 - distEW0);
        coord.y = xi;
        fixXi = true;
    }
//        std::cout << angle_between_vectors(normalEW,normalEWOPP)-180 << std::endl;
//        std::cout << angle_between_vectors(normalNS,normalNSOPP)-180 << std::endl;
//        std::cout << angle_between_vectors(normalTD,normalTDOPP)-180 << std::endl;
//            std::cout << angle_between_vectors(normalEWOPP2,normalEWOPP)-180 << std::endl;
//            std::cout << angle_between_vectors(normalNSOPP2,normalNSOPP)-180 << std::endl;
//            std::cout << angle_between_vectors(normalTDOPP2,normalTDOPP)-180 << std::endl;




    D3vector J = cross_product(E / D3VectorNorm(E),H / D3VectorNorm(H));
    D3vector K = cross_product(F / D3VectorNorm(F),H / D3VectorNorm(H));
    D3vector L = cross_product(G / D3VectorNorm(G),H / D3VectorNorm(H));

    if (J != 0)
    {
        J = J / D3VectorNorm(J);
    }
    if (K != 0)
    {
        K = K / D3VectorNorm(K);
    }
    if (L != 0)
    {
        L = L / D3VectorNorm(L);
    }
    bool orthogonal = false;

    if (J == 0 || K == 0 || L == 0 )
    {
        orthogonal = true;
        std::cout << "seems to be orthogonal!" << std::endl;
    }

    //coord.Print();std::cout<<std::endl;
    D3vector R = A    + B * coord.x + C * coord.y + D * coord.z + E * coord.x*coord.y + F * coord.y*coord.z + G * coord.x * coord.z + H * coord.x*coord.y*coord.z;
//    R.Print();std::cout<<std::endl;
//  X.Print();


//    double f = product(B,J) * coord.x + product(C,J) * coord.y + product(D,J) * coord.z                                    + product(F,J) * coord.y * coord.z + product(G,J) * coord.x * coord.z + product(A,J) - product(X,J);
//    double g = product(B,K) * coord.x + product(C,K) * coord.y + product(D,K) * coord.z + product(E,K) * coord.x * coord.y                                    + product(G,K) * coord.x * coord.z + product(A,K) - product(X,K);
//    double h = product(B,L) * coord.x + product(C,L) * coord.y + product(D,L) * coord.z + product(E,L) * coord.x * coord.y + product(F,L) * coord.y * coord.z                                    + product(A,L) - product(X,L);

    if (!fixEta && !fixPhi && !fixPhi && !orthogonal && false)
    {
        for (int iter = 0 ; iter < 20 ; iter++)
        {
            double dxdx = product(B,J) + product(G,J) *  coord.z ;
            double dydx = product(B,K) +  product(E,K)  * coord.y + product(G,K) * coord.z ;
            double dzdx = product(B,L) + product(E,L) * coord.y ;
            double dxdy = product(C,J) +  product(F,J) * coord.z ;
            double dydy = product(C,K) + product(E,K) * coord.x;
            double dzdy = product(C,L) +  product(E,L) * coord.x  + product(F,L) * coord.z;
            double dxdz = product(D,J) + product(F,J) * coord.y + product(G,J) * coord.x ;
            double dydz = product(D,K) +  product(G,K) * coord.x ;
            double dzdz = product(D,L)  + product(F,L) * coord.y ;

            D3vector cx(dxdx,dydx,dzdx); // first column
            D3vector cy(dxdy,dydy,dzdy); // second column
            D3vector cz(dxdz,dydz,dzdz); // third column



            R = A    + B * coord.x + C * coord.y + D * coord.z + E * coord.x*coord.y + F * coord.y*coord.z + G * coord.x * coord.z + H * coord.x*coord.y*coord.z -X;



            D3matrix jac(cx,cy,cz);
            //jac.Print();
            jac.invert_gauss_elimination();

            double f = product(B,J) * coord.x + product(C,J) * coord.y + product(D,J) * coord.z + product(F,J) * coord.y * coord.z + product(G,J) * coord.x * coord.z + product(A,J) - product(X,J);
            double g = product(B,K) * coord.x + product(C,K) * coord.y + product(D,K) * coord.z + product(E,K) * coord.x * coord.y + product(G,K) * coord.x * coord.z + product(A,K) - product(X,K);
            double h = product(B,L) * coord.x + product(C,L) * coord.y + product(D,L) * coord.z + product(E,L) * coord.x * coord.y + product(F,L) * coord.y * coord.z + product(A,L) - product(X,L);
            D3vector f_xn = {f,g,h};

            //std::cout << "residuum f_xn" <<D3VectorNorm(f_xn)<<std::endl;
            if (D3VectorNormSquared(f_xn) < 1e-10)
            {
                iter = 1000;
            }

            coord = coord - jac.vectorMultiply(f_xn) * 0.8;
            //coord.Print();std::cout<<std::endl;

        }
    }
    else
    {
        for (int iter = 0 ; iter < 20 ; iter++)
        {
            D3vector partialEta = B + E * coord.y + G * coord.z + H * coord.y*coord.z;
            D3vector partialXi =  C + E * coord.x + F * coord.z + H * coord.x*coord.z;
            D3vector partialPhi = D + F * coord.y + G * coord.x + H * coord.x*coord.y;

            D3matrix jac2(partialEta,partialXi,partialPhi);
            jac2.transpose();
            jac2.invert_gauss_elimination();

            R = A    + B * coord.x + C * coord.y + D * coord.z + E * coord.x*coord.y + F * coord.y*coord.z + G * coord.x * coord.z + H * coord.x*coord.y*coord.z -X;


            //std::cout << "residuum f_xn" <<D3VectorNorm(R)<<std::endl;
            if (D3VectorNormSquared(R) < 1e-10)
            {
                iter = 1000;
            }
            coord = coord - jac2.vectorMultiply(R) * 0.8;
            //coord.Print();std::cout<<std::endl;

        }
    }


    D3vector x = A    + B * coord.x + C * coord.y + D * coord.z + E * coord.x*coord.y + F * coord.y*coord.z + G * coord.x * coord.z + H * coord.x*coord.y*coord.z;
//    X.Print();
//    std::cout << "tirlinear interp \n";
//    x.Print();
//    std::cout << std::flush;

    if (MAX(coord) > 1.01 || MIN(coord) < -0.01)
    {
        return D3vector{-1,-1,-1};
    }
    return coord;
}


int Interpolate_direct::checkForHexaNeighbours(int idHex, int i, int j, int k, D3vector v)
{
    counterHexa++;
    int Nx = blockgrid->Give_Nx_hexahedron(idHex);
    int Ny = blockgrid->Give_Ny_hexahedron(idHex);
    int Nz = blockgrid->Give_Nz_hexahedron(idHex);

    int typ = -1;
    if ( i == -1 || i == Nx )
    {
        if ( j == -1 || j == Ny )
        {
            int Tj = (j<0) ? (Tj = j + 1 ) : (Tj = j - 1 );
            int Ti = (i<0) ? (Ti = i + 1) : (Ti = i - 1);
            debugTest = true;
            typ = checkCorner(idHex, Ti, Tj, k, v);
            debugTest = false;
            if (typ != -1 ) return typ;

        }
        else if (k == -1 || k == Nz )
        {
            int Tk = (k<0) ? (Tk = k + 1) : (Tk = k - 1);
            int Ti = (i<0) ? (Ti = i + 1) : (Ti = i - 1);
            debugTest = true;
            typ = checkCorner(idHex, Ti, j, Tk, v);
            debugTest = false;
            if (typ != -1 ) return typ;
        }
        else
        {
            int Ti = (i<0) ? (Ti = i + 1) : (Ti = i - 1);
            typ = checkEdge(idHex, Ti, j, k, v);
            if (typ != -1 ) return typ;
        }
    }
    else if ( j == -1 || j == Ny )
    {
        /*if ( i == -1)
        {
            checkCorner(id_Hex, i, j, k, v);
        }
        else */if (k == -1 || k == Nz )
        {
            int Tk = (k<0) ? (Tk = k + 1) : (Tk = k - 1);
            int Tj = (j<0) ? (Tj = j + 1) : (Tj = j - 1);
            debugTest = true;
            typ = checkCorner(idHex, i, Tj, Tk, v);
            debugTest = false;
            if (typ != -1 ) return typ;
        }
        else
        {
            int Tj = (j<0) ? (Tj = j + 1) : (Tj = j - 1);
            typ = checkEdge(idHex, i, Tj, k, v);
            if (typ != -1 ) return typ;
        }
    }
    else if ( k == -1 || k == Nz )
    {
            int Tk = (k<0) ? (Tk = k + 1) : (Tk = k - 1);
            typ = checkEdge(idHex, i, j, Tk, v);
            if (typ != -1 ) return typ;
    }
    counterHexa--;
    return typ;
}

Interpolate_on_block_grid_from_pointinterpolator::Interpolate_on_block_grid_from_pointinterpolator(PointInterpolator *interp, Blockgrid *blockgrid_to_)
{

    blockgrid_to = blockgrid_to_;
    pointInterpolator = interp;

}

Interpolate_on_block_grid_from_pointinterpolator::~Interpolate_on_block_grid_from_pointinterpolator()
{
    delete pointInterpolator;
    delete[] data;
}

void Interpolate_on_block_grid_from_pointinterpolator::interpolate(Variable<double> *U_to, Boundary_Marker *marker)
{
    Functor3<double,double,PointInterpolator> myFunctor(pointInterpolator);

    X_coordinate Xc(*blockgrid_to);
    Y_coordinate Yc(*blockgrid_to);
    Z_coordinate Zc(*blockgrid_to);
    if (marker == NULL)
    {
        (*U_to) = myFunctor(Xc,Yc,Zc);

    }
    else
    {
        (*U_to) = myFunctor(Xc,Yc,Zc) | *marker;
    }

}