#include <iostream>
#include <armadillo>
#include <algorithm> // kvuli funkci replace pro nahrazovani znaku ve stringu
#include "dG_library.h"
#include "matrix_utils.h"

using namespace arma;

namespace dG_global
{
  std::string directoryname, filename, N_string, mesh_size_parameter_string;

  // lokalni aproximace
  int N, N_p, N_fp, N_faces;

  // lokalni operatory
  vec r, s, t;
  mat V, invV;
  mat M, invM;
  mat V_r, V_s, V_t;
  mat D_r, D_s, D_t;
  mat D_r_weak, D_s_weak, D_t_weak;
  umat Fmask;
  mat LIFT;

  // prostorova diskretizace
  double mesh_size_parameter;

  // informace o siti
  double NODETOL;
  mat P;
  rowvec Px, Py, Pz;
  int N_v;
  umat E;
  int K;
  mat x, y, z;
  mat r_x, r_y, r_z;
  mat s_x, s_y, s_z;
  mat t_x, t_y, t_z;
  mat J;
  mat Fx, Fy, Fz;
  mat n_x, n_y, n_z;
  mat surfJ, Fscale;
  umat EtoE, EtoF;
  uvec mapM, vmapM;
  uvec mapP, vmapP;
  uvec mapB, vmapB;
  uvec mapI, vmapI;
  uvec mapO, vmapO;

  // casova diskretizace
  double T;
  double dt;
  int N_steps;
  vec dtscale;

  // koeficienty RK metody
  vec rk4a, rk4b, rk4c;

  void SetRKMethodCoefficients()
  {
 	//rk4a << 0 << -567301805773 / 1357537059087 << -2404267990393 / 2016746695238 << -3550918686646 / 2091501179385 << -1275806237668 / 842570457699;
 	//rk4b << 1432997174477 / 9575080441755 << 5161836677717 / 13612068292357 << 1720146321549 / 2090206949498 << 3134564353537 / 4481467310338 << 2277821191437 / 14882151754819;
	//rk4c << 0 << 1432997174477 / 9575080441755 << 2526269341429 / 6820363962896 << 2006345519317 / 3224310063776 << 2802321613138 / 2924317926251;
	rk4a << 0.000000000000000 << -0.417890474499852 << -1.192151694642677 << -1.697784692471528 << -1.514183444257156;
 	rk4b << 0.149659021999229 << 0.379210312999627 << 0.822955029386982 << 0.699450455949122 << 0.153057247968152;
	rk4c << 0.000000000000000 << 0.149659021999229 << 0.370400957364205 << 0.622255763134443 << 0.958282130674690;
  }
  
  void DiscretizationParamatersToStrings()
  {
    char str_from_sprintf_buffer[64];
    
    sprintf(str_from_sprintf_buffer, "%d", N);
    N_string = std::string(str_from_sprintf_buffer);
    sprintf(str_from_sprintf_buffer, "%0.3f", mesh_size_parameter);
    mesh_size_parameter_string = std::string(str_from_sprintf_buffer);
    replace(mesh_size_parameter_string.begin(), mesh_size_parameter_string.end(), '.', ',');
  }
  
  void GeometricFactors1D()
  {
  	J = D_r * x;
  	mat mat_ones = ones<mat>(N_p,K);
	r_x = mat_ones / J;
  }
  
  void Normals1D()
  {
  	n_x = zeros<mat>(N_faces * N_fp,K);
  	mat temp = ones<mat>(1,K);
	n_x.row(0) = -temp; n_x.row(1) = temp;
  }
  
  void Connectivity1D() // nefunguje, protoze nelze hledat ve sparse matici pomoci == jako v dense matici
  {
    /*umat face_def = zeros<umat>(2,1);
	face_def(0,0) = 0;
	face_def(1,0) = 1;
	
	sp_umat spFtoV = SpMat<uword>(N_faces * K,N_v);
	int face = 0;
	for(int i = 0; i < K; i++)
	{
	  for(int j = 0; j < N_faces; j++)
	  {
	    spFtoV(face,E(face_def(j,0),i)) = 1;
		face = face + 1;
	  }
	}
	
	sp_umat spFtoF = spFtoV * trans(spFtoV) - speye<sp_umat>(N_faces * K,N_faces * K);
	
	uvec indices = find(spFtoF == 1);
	uvec faces1 = zeros<uvec>(indices.n_rows);
	uvec faces2 = faces1;
	ind2sub(indices, N_faces * K, N_faces * K, faces1, faces2);
	
	uvec element1 = floor(((faces1 + 1) - 1) / N_faces);
	uvec element2 = floor(((faces2 + 1) - 1) / N_faces);
	
	uvec face1 = faces1;
	uvec face2 = faces2;
	for(int i = 0; i < faces1.n_rows; i++)
	{
	  face1(i) = (faces1(i) + 1) - 1 % N_faces;
	  face2(i) = (faces2(i) + 1) - 1 % N_faces;
	}
	
	EtoE = linspace<uvec>(0,K - 1,K) * ones<urowvec>(N_faces);
	EtoF = ones<uvec>(K) * linspace<urowvec>(0,N_faces - 1,N_faces);
	
	sub2ind(element1,face1,K,N_faces,indices);
	EtoE(indices) = element2;
	EtoF(indices) = face2;*/
  }
  
  void BuildMaps1D()
  {
    umat nodeIDs = reshape(linspace<uvec>(0,K * N_p - 1,K * N_p), N_p, K);
	ucube vmapM_temp = zeros<ucube>(N_fp, N_faces, K);
	ucube vmapP_temp = zeros<ucube>(N_fp, N_faces, K);
	
	uvec temp;
	for(int element1 = 0; element1 < K; element1++)
	{
	  for(int face1 = 0; face1 < N_faces; face1++)
	  {
	    temp = Fmask.col(face1);
	    for(int i = 0; i < N_fp; i++)
		{
		  vmapM_temp(i,face1,element1) = nodeIDs(temp(i),element1);
		}
	  }
	}
	
	int element2, face2;
	uvec vertexIDM, vertexIDP;
	vec x1, x2;
	vec D;
	for(int element1 = 0; element1 < K; element1++)
	{
	  for(int face1 = 0; face1 < N_faces; face1++)
	  {
	    element2 = EtoE(element1,face1);
        face2 = EtoF(element1,face1);
        
        vertexIDM = vmapM_temp.subcube(span(),span(face1),span(element1));
        vertexIDP = vmapM_temp.subcube(span(),span(face2),span(element2));
        x1 = x(vertexIDM); x2 = x(vertexIDP);
        
        D = square(x1 - x2);
        if(D(0) < NODETOL)
        {
          vmapP_temp.subcube(span(),span(face1),span(element1)) = vertexIDP;
        }
	  }
	}
	
	// vectorise nefunguje pro cube jako v MATLABu
	vmapM = vmapM_temp(linspace<uvec>(0,K * N_faces * N_fp - 1, K * N_faces * N_fp));
	vmapP = vmapP_temp(linspace<uvec>(0,K * N_faces * N_fp - 1, K * N_faces * N_fp));
    
    mapB = find(vmapM == vmapP);
    vmapB = vmapM(mapB);
  }
  
  void StartUp1D() // nektere veci docasne nacitany z MATLABu
  {
    SetRKMethodCoefficients();
    
    N_p = N + 1;
    N_fp = 1;
    N_faces = 2;
    NODETOL = 1e-12;
    
    N_v = P.n_cols;
    K = E.n_cols;
    
    filename = "dG_1D_general_data/r_1D_N=" + N_string + ".txt";
    r.load(filename);
  
    filename = "dG_1D_general_data/V_1D_N=" + N_string + ".txt";
    V.load(filename);
    filename = "dG_1D_general_data/invV_1D_N=" + N_string + ".txt";
    invV.load(filename);
  
    filename = "dG_1D_general_data/M_1D_N=" + N_string + ".txt";
    M.load(filename);
    filename = "dG_1D_general_data/invM_1D_N=" + N_string + ".txt";
    invM.load(filename);
  
    filename = "dG_1D_general_data/V_r_1D_N=" + N_string + ".txt";
    V_r.load(filename);
  
    filename = "dG_1D_general_data/D_r_1D_N=" + N_string + ".txt";
    D_r.load(filename);
    
    urowvec v_1 = E.row(0);
    urowvec v_2 = E.row(1);
    vec temp = ones<vec>(N_p,1);
    x = temp * trans(Px(v_1)) + 0.5 * (r + 1) * trans((Px(v_2) - Px(v_1)));
  
    GeometricFactors1D();
    
    filename = "dG_1D_general_data/Fmask_1D_N=" + N_string + ".txt";
    Fmask.load(filename);
    Fmask = Fmask - 1; // kvuli cislovani od 0 v C++
    Fx = x.rows(vectorise(Fmask));
    
    filename = "dG_1D_general_data/LIFT_1D_N=" + N_string + ".txt";
    LIFT.load(filename);
  
    Normals1D();
    Fscale = r_x.rows(vectorise(Fmask));
  
    filename = directoryname + "EtoE_for_h=" + mesh_size_parameter_string + ".txt";
    EtoE.load(filename);
    EtoE = EtoE - 1; // kvuli cislovani od 0 v C++
    filename = directoryname + "EtoF_for_h=" + mesh_size_parameter_string + ".txt";
    EtoF.load(filename);
    EtoF = EtoF - 1; // kvuli cislovani od 0 v C++
    //Connectivity1D();
  
    BuildMaps1D();
  }
  
  void GeometricFactors2D() // moje modifikovana verze oproti Hesthavenovi
  {
    mat x_r = D_r * x; mat x_s = D_s * x;
    mat y_r = D_r * y; mat y_s = D_s * y;
    
	J = x_r % y_s - x_s % y_r;
	
	r_x = y_s / J; r_y = -x_s / J;
	s_x = -y_r / J; s_y = x_r / J;
	
	J = abs(J); // aby jakobiany byly kladne
  }
  
  void Normals2D() // moje modifikovana verze oproti Hesthavenovi
  {
    n_x = zeros<mat>(N_faces * N_fp,K);
    n_y = zeros<mat>(N_faces * N_fp,K);
    
	umat faceIDs = zeros<umat>(N_fp,3);
	uvec temp = linspace<uvec>(0,N_fp - 1,N_fp);
	faceIDs.col(0) = temp;
	temp = linspace<uvec>(N_fp,2 * N_fp - 1,N_fp);
	faceIDs.col(1) = temp;
	temp = linspace<uvec>(2 * N_fp,3 * N_fp - 1,N_fp);
	faceIDs.col(2) = temp;
	
	umat face_def = zeros<umat>(3,2);
	face_def(0,0) = 0; face_def(0,1) = 1;
	face_def(1,0) = 1; face_def(1,1) = 2;
	face_def(2,0) = 2; face_def(2,1) = 0;
	
	uvec pointID = zeros<uvec>(3,1);
	pointID(0) = 2; pointID(1) = 0; pointID(2) = 1;
	
	urowvec face;
	mat t_matrix = zeros<mat>(2,K);
	mat n_matrix = zeros<mat>(2,K);
	mat points = zeros<mat>(2,K);
	rowvec const_vect, signs;
	
	for(int i = 0; i < N_faces; i++)
	{
      face = face_def.row(i);
    
      // smerove a normalove vektory
      t_matrix.row(0) = trans(Px(E.row(face(0))) - Px(E.row(face(1))));
      t_matrix.row(1) = trans(Py(E.row(face(0))) - Py(E.row(face(1))));
      n_matrix.row(0) = -t_matrix.row(1);
      n_matrix.row(1) = t_matrix.row(0);
    
      // otoceni vnitrnich normal
      const_vect = -(n_matrix.row(0) % trans(Px(E.row(face(0)))) + n_matrix.row(1) % trans(Py(E.row(face(0)))));
      points.row(0) = trans(Px(E.row(pointID(i))));
      points.row(1) = trans(Py(E.row(pointID(i))));
      signs = n_matrix.row(0) % points.row(0) + n_matrix.row(1) % points.row(1) + const_vect;
      n_matrix.cols(find(signs > 0)) = -n_matrix.cols(find(signs > 0));
    
      // zapsani do n_x a n_y
      n_x.each_row(faceIDs.col(i)) = n_matrix.row(0);
      n_y.each_row(faceIDs.col(i)) = n_matrix.row(1);
	}

	surfJ = sqrt(n_x % n_x + n_y % n_y);
	n_x = n_x / surfJ; n_y = n_y / surfJ;
	surfJ = 0.5 * surfJ; // protoze puvodne byly smerove vektory jen poloviny stran
  }
  
  void Connectivity2D()
  {
  }
  
  void BuildMaps2D()
  {
    umat nodeIDs = reshape(linspace<uvec>(0,K * N_p - 1,K * N_p), N_p, K);
	ucube vmapM_temp = zeros<ucube>(N_fp, N_faces, K);
	ucube vmapP_temp = zeros<ucube>(N_fp, N_faces, K);
	ucube mapM_temp(K * N_fp * N_faces,1,1); // schvalne cube, aby slo provest reshape na mapP_temp
	mapM_temp.subcube(span(),span(0),span(0)) = linspace<uvec>(0,K * N_fp * N_faces - 1,K * N_fp * N_faces); 
	ucube mapP_temp = reshape(mapM_temp, N_fp, N_faces, K);
	
	uvec temp;
	for(int element1 = 0; element1 < K; element1++)
	{
	  for(int face1 = 0; face1 < N_faces; face1++)
	  {
	    temp = Fmask.col(face1);
	    for(int i = 0; i < N_fp; i++)
		{
		  vmapM_temp(i,face1,element1) = nodeIDs(temp(i),element1);
		}
	  }
	}
	
	int element2, face2;
	uword vertex1, vertex2;
	double ref_length;
	uvec vertexIDM, vertexIDP;
	vec x1, x2, y1, y2;
	mat X1, X2, Y1, Y2;
	mat D;
	uvec indeces, IDM, IDP;
	int n_temp;
	for(int element1 = 0; element1 < K; element1++)
	{
	  for(int face1 = 0; face1 < N_faces; face1++)
	  {
	    element2 = EtoE(element1,face1);
        face2 = EtoF(element1,face1);
        
        vertex1 = E(face1,element1);
        vertex2 = E((face1 + 1) % N_faces,element1);
        ref_length = sqrt(pow(Px(vertex1) - Px(vertex2),2) + pow(Py(vertex1) - Py(vertex2),2));
        
        vertexIDM = vmapM_temp.subcube(span(),span(face1),span(element1));
        vertexIDP = vmapM_temp.subcube(span(),span(face2),span(element2));
        x1 = x(vertexIDM); x2 = x(vertexIDP);
        y1 = y(vertexIDM); y2 = y(vertexIDP);
        
        X1 = zeros<mat>(N_fp,N_fp); X2 = zeros<mat>(N_fp,N_fp);
        Y1 = zeros<mat>(N_fp,N_fp); Y2 = zeros<mat>(N_fp,N_fp);
        X1.each_col() = x1; X2.each_col() = x2;
        Y1.each_col() = y1; Y2.each_col() = y2;
        
        D = square(X1 - trans(X2)) + square(Y1 - trans(Y2));
        indeces = find(sqrt(D) < NODETOL * ref_length);
        n_temp = indeces.n_rows;
        IDM = zeros<uvec>(n_temp); IDP = zeros<uvec>(n_temp);
        ind2sub(indeces, N_fp, N_fp, IDM, IDP);
        for(int i = 0; i < n_temp; i++)
        {
          vmapP_temp(IDM(i),face1,element1) = vertexIDP(IDP(i));
          mapP_temp(IDM(i),face1,element1) = IDP(i) + face2 * N_fp + element2 * N_faces * N_fp;
        }
	  }
	}
	
	// vectorise nefunguje pro cube jako v MATLABu
	uvec vectorise_tool = linspace<uvec>(0,K * N_faces * N_fp - 1, K * N_faces * N_fp);
	mapM = mapM_temp(vectorise_tool);
	mapP = mapP_temp(vectorise_tool);
	vmapM = vmapM_temp(vectorise_tool);
	vmapP = vmapP_temp(vectorise_tool);
    
    mapB = find(vmapM == vmapP);
    vmapB = vmapM(mapB);
  }
 
  
  void StartUp2D() // nektere veci docasne nacitany z MATLABu
  {
    SetRKMethodCoefficients();
    
    N_p = (int)((N + 1) * (N + 2)) / 2;
    N_fp = N + 1;
    N_faces = 3;
    NODETOL = 1e-12;
    
    N_v = P.n_cols;
    K = E.n_cols;
    
    filename = "dG_2D_general_data/r_2D_N=" + N_string + ".txt";
    r.load(filename);
    filename = "dG_2D_general_data/s_2D_N=" + N_string + ".txt";
    s.load(filename);
  
    filename = "dG_2D_general_data/V_2D_N=" + N_string + ".txt";
    V.load(filename);
    filename = "dG_2D_general_data/invV_2D_N=" + N_string + ".txt";
    invV.load(filename);
  
    filename = "dG_2D_general_data/M_2D_N=" + N_string + ".txt";
    M.load(filename);
    filename = "dG_2D_general_data/invM_2D_N=" + N_string + ".txt";
    invM.load(filename);
  
    filename = "dG_2D_general_data/V_r_2D_N=" + N_string + ".txt";
    V_r.load(filename);
    filename = "dG_2D_general_data/V_s_2D_N=" + N_string + ".txt";
    V_s.load(filename);
  
    filename = "dG_2D_general_data/D_r_2D_N=" + N_string + ".txt";
    D_r.load(filename);
    filename = "dG_2D_general_data/D_s_2D_N=" + N_string + ".txt";
    D_s.load(filename);
    
    filename = "dG_2D_general_data/D_r_weak_2D_N=" + N_string + ".txt";
    D_r_weak.load(filename);
    filename = "dG_2D_general_data/D_s_weak_2D_N=" + N_string + ".txt";
    D_s_weak.load(filename);
  
    urowvec v_1 = E.row(0);
    urowvec v_2 = E.row(1);
    urowvec v_3 = E.row(2);
    x = 0.5 * (-(r + s) * trans(Px(v_1)) + (r + 1) * trans(Px(v_2)) + (s + 1) * trans(Px(v_3)));
	y = 0.5 * (-(r + s) * trans(Py(v_1)) + (r + 1) * trans(Py(v_2)) + (s + 1) * trans(Py(v_3)));
  
    GeometricFactors2D();
    
    filename = "dG_2D_general_data/Fmask_2D_N=" + N_string + ".txt";
    Fmask.load(filename);
    Fmask = Fmask - 1; // kvuli cislovani od 0 v C++
    Fx = x.rows(vectorise(Fmask));
    Fy = y.rows(vectorise(Fmask));
    
    filename = "dG_2D_general_data/LIFT_2D_N=" + N_string + ".txt";
    LIFT.load(filename);
  
    Normals2D();
    Fscale = surfJ / J.rows(vectorise(Fmask));
  
    filename = directoryname + "EtoE_for_h=" + mesh_size_parameter_string + ".txt";
    EtoE.load(filename);
    EtoE = EtoE - 1; // kvuli cislovani od 0 v C++
    filename = directoryname + "EtoF_for_h=" + mesh_size_parameter_string + ".txt";
    EtoF.load(filename);
    EtoF = EtoF - 1; // kvuli cislovani od 0 v C++
    
    BuildMaps2D();
  }
  
  void dtscale2D()
  {
    uvec vmask1 = find(abs(r + s + 2) < NODETOL);
	uvec vmask2 = find(abs(r - 1) < NODETOL);
	uvec vmask3 = find(abs(s - 1) < NODETOL);
	uvec vmask = zeros<uvec>(3);
	vmask(0) = vmask1(0); vmask(1) =  vmask2(0); vmask(2) =  vmask3(0);
	mat vx = x.rows(vmask); mat vy = y.rows(vmask);
	
	rowvec lengths1 = sqrt(square(vx.row(0) - vx.row(1)) + square(vy.row(0) - vy.row(1)));
	rowvec lengths2 = sqrt(square(vx.row(1) - vx.row(2)) + square(vy.row(1) - vy.row(2)));
	rowvec lengths3 = sqrt(square(vx.row(2) - vx.row(0)) + square(vy.row(2) - vy.row(0)));
	rowvec semiperimeters = 0.5 * (lengths1 + lengths2 + lengths3);
	// Heron's formula for triangle area
	rowvec areas = sqrt(semiperimeters % (semiperimeters - lengths1) % (semiperimeters - lengths2) % (semiperimeters - lengths3));
	
	dtscale = trans(areas / semiperimeters);
  }
  
  void GeometricFactors3D() // moje modifikovana verze oproti Hesthavenovi
  {
  	mat x_r = D_r * x; mat x_s = D_s * x; mat x_t = D_t * x;
	mat y_r = D_r * y; mat y_s = D_s * y; mat y_t = D_t * y;
	mat z_r = D_r * z; mat z_s = D_s * z; mat z_t = D_t * z;

	J = x_r % (y_s % z_t - z_s % y_t) - y_r % (x_s % z_t - z_s % x_t) + z_r % (x_s % y_t - y_s % x_t);

	r_x = (y_s % z_t - z_s % y_t) / J; r_y = -(x_s % z_t - z_s % x_t) / J; r_z = (x_s % y_t - y_s % x_t) / J;
	s_x = -(y_r % z_t - z_r % y_t) / J; s_y = (x_r % z_t - z_r % x_t) / J; s_z = -(x_r % y_t - y_r % x_t) / J;
	t_x = (y_r % z_s - z_r % y_s) / J; t_y = -(x_r % z_s - z_r % x_s) / J; t_z = (x_r % y_s - y_r % x_s) / J;

	J = abs(J); // aby byly jakobiany kladne

  }
  
  void Normals3D() // moje modifikovana verze oproti Hesthavenovi
  {
  	n_x = zeros<mat>(N_faces * N_fp,K);
    n_y = zeros<mat>(N_faces * N_fp,K);
    n_z = zeros<mat>(N_faces * N_fp,K);
    
	umat faceIDs = zeros<umat>(N_fp,4);
	uvec temp = linspace<uvec>(0,N_fp - 1,N_fp);
	faceIDs.col(0) = temp;
	temp = linspace<uvec>(N_fp,2 * N_fp - 1,N_fp);
	faceIDs.col(1) = temp;
	temp = linspace<uvec>(2 * N_fp,3 * N_fp - 1,N_fp);
	faceIDs.col(2) = temp;
	temp = linspace<uvec>(3 * N_fp,4 * N_fp - 1,N_fp);
	faceIDs.col(3) = temp;
	
	umat face_def = zeros<umat>(4,3);
	face_def(0,0) = 0; face_def(0,1) = 1; face_def(0,2) = 2;
	face_def(1,0) = 0; face_def(1,1) = 1; face_def(1,2) = 3;
	face_def(2,0) = 1; face_def(2,1) = 2; face_def(2,2) = 3;
	face_def(3,0) = 0; face_def(3,1) = 2; face_def(3,2) = 3;
	
	uvec pointID = zeros<uvec>(4,1);
	pointID(0) = 3; pointID(1) = 2; pointID(2) = 0; pointID(3) = 1;
	
	urowvec face;
	mat t_matrix1 = zeros<mat>(3,K);
	mat t_matrix2 = zeros<mat>(3,K);
	mat n_matrix = zeros<mat>(3,K);
	mat points = zeros<mat>(3,K);
	rowvec const_vect, signs;
	
	for(int i = 0; i < N_faces; i++)
	{
      face = face_def.row(i);
    
      // smerove a normalove vektory
      t_matrix1.row(0) = trans(Px(E.row(face(0))) - Px(E.row(face(1))));
      t_matrix1.row(1) = trans(Py(E.row(face(0))) - Py(E.row(face(1))));
      t_matrix1.row(2) = trans(Pz(E.row(face(0))) - Pz(E.row(face(1))));
      t_matrix2.row(0) = trans(Px(E.row(face(0))) - Px(E.row(face(2))));
      t_matrix2.row(1) = trans(Py(E.row(face(0))) - Py(E.row(face(2))));
      t_matrix2.row(2) = trans(Pz(E.row(face(0))) - Pz(E.row(face(2))));
      for(int j = 0; j < K; j++)
      {
      	n_matrix.col(j) = cross(t_matrix1.col(j),t_matrix2.col(j));
      }
      
      // otoceni vnitrnich normal
      const_vect = -(n_matrix.row(0) % trans(Px(E.row(face(0)))) + n_matrix.row(1) % trans(Py(E.row(face(0)))) + n_matrix.row(2) % trans(Pz(E.row(face(0)))));
      points.row(0) = trans(Px(E.row(pointID(i))));
      points.row(1) = trans(Py(E.row(pointID(i))));
      points.row(2) = trans(Pz(E.row(pointID(i))));
      signs = n_matrix.row(0) % points.row(0) + n_matrix.row(1) % points.row(1) + n_matrix.row(2) % points.row(2) + const_vect;
      n_matrix.cols(find(signs > 0)) = -n_matrix.cols(find(signs > 0));
    
      // zapsani do n_x a n_y
      n_x.each_row(faceIDs.col(i)) = n_matrix.row(0);
      n_y.each_row(faceIDs.col(i)) = n_matrix.row(1);
      n_z.each_row(faceIDs.col(i)) = n_matrix.row(2);
	}

	surfJ = sqrt(n_x % n_x + n_y % n_y + n_z % n_z);
	n_x = n_x / surfJ; n_y = n_y / surfJ; n_z = n_z / surfJ;
	surfJ = 0.5 * surfJ; // protoze puvodne byly smerove vektory jen poloviny stran
	surfJ = surfJ % J.rows(vectorise(Fmask));
  }
  
  void Connectivity3D()
  {
  }
  
  void BuildMaps3D()
  {
    umat nodeIDs = reshape(linspace<uvec>(0,K * N_p - 1,K * N_p), N_p, K);
	ucube vmapM_temp = zeros<ucube>(N_fp, N_faces, K);
	ucube vmapP_temp = zeros<ucube>(N_fp, N_faces, K);
	ucube mapM_temp(K * N_fp * N_faces,1,1); // schvalne cube, aby slo provest reshape na mapP_temp
	mapM_temp.subcube(span(),span(0),span(0)) = linspace<uvec>(0,K * N_fp * N_faces - 1,K * N_fp * N_faces); 
	ucube mapP_temp = reshape(mapM_temp, N_fp, N_faces, K);
	
	uvec temp;
	for(int element1 = 0; element1 < K; element1++)
	{
	  for(int face1 = 0; face1 < N_faces; face1++)
	  {
	    temp = Fmask.col(face1);
	    for(int i = 0; i < N_fp; i++)
		{
		  vmapM_temp(i,face1,element1) = nodeIDs(temp(i),element1);
		}
	  }
	}
	
	int element2, face2;
	uword vertex1, vertex2;
	double ref_length;
	uvec vertexIDM, vertexIDP;
	vec x1, x2, y1, y2, z1, z2;
	mat X1, X2, Y1, Y2, Z1, Z2;
	mat D;
	uvec indeces, IDM, IDP;
	int n_temp;
	for(int element1 = 0; element1 < K; element1++)
	{
	  for(int face1 = 0; face1 < N_faces; face1++)
	  {
	    element2 = EtoE(element1,face1);
        face2 = EtoF(element1,face1);
        
        vertex1 = E(face1,element1);
        vertex2 = E((face1 + 1) % N_faces,element1);
        ref_length = sqrt(pow(Px(vertex1) - Px(vertex2),2) + pow(Py(vertex1) - Py(vertex2),2) + pow(Pz(vertex1) - Pz(vertex2),2));
        
        vertexIDM = vmapM_temp.subcube(span(),span(face1),span(element1));
        vertexIDP = vmapM_temp.subcube(span(),span(face2),span(element2));
        x1 = x(vertexIDM); x2 = x(vertexIDP);
        y1 = y(vertexIDM); y2 = y(vertexIDP);
        z1 = z(vertexIDM); z2 = z(vertexIDP);
        
        X1 = zeros<mat>(N_fp,N_fp); X2 = zeros<mat>(N_fp,N_fp);
        Y1 = zeros<mat>(N_fp,N_fp); Y2 = zeros<mat>(N_fp,N_fp);
        Z1 = zeros<mat>(N_fp,N_fp); Z2 = zeros<mat>(N_fp,N_fp);
        X1.each_col() = x1; X2.each_col() = x2;
        Y1.each_col() = y1; Y2.each_col() = y2;
        Z1.each_col() = z1; Z2.each_col() = z2;
        
        D = square(X1 - trans(X2)) + square(Y1 - trans(Y2)) + square(Z1 - trans(Z2));
        indeces = find(sqrt(D) < NODETOL * ref_length);
        n_temp = indeces.n_rows;
        IDM = zeros<uvec>(n_temp); IDP = zeros<uvec>(n_temp);
        ind2sub(indeces, N_fp, N_fp, IDM, IDP);
        for(int i = 0; i < n_temp; i++)
        {
          vmapP_temp(IDM(i),face1,element1) = vertexIDP(IDP(i));
          mapP_temp(IDM(i),face1,element1) = IDP(i) + face2 * N_fp + element2 * N_faces * N_fp;
        }
	  }
	}
	
	// vectorise nefunguje pro cube jako v MATLABu
	uvec vectorise_tool = linspace<uvec>(0,K * N_faces * N_fp - 1, K * N_faces * N_fp);
	mapM = mapM_temp(vectorise_tool);
	mapP = mapP_temp(vectorise_tool);
	vmapM = vmapM_temp(vectorise_tool);
	vmapP = vmapP_temp(vectorise_tool);
    
    mapB = mapM(find(vmapM == vmapP));
    vmapB = vmapM(mapB);
  }
  
  void StartUp3D() // nektere veci docasne nacitany z MATLABu
  {
    SetRKMethodCoefficients();
    
    N_p = (int)((N + 1) * (N + 2) * (N + 3)) / 6;
    N_fp = (int)((N + 1) * (N + 2)) / 2;
    N_faces = 4;
    NODETOL = 1e-9;
    
    N_v = P.n_cols;
    K = E.n_cols;
    
    filename = "dG_3D_general_data/r_3D_N=" + N_string + ".txt";
    r.load(filename);
    filename = "dG_3D_general_data/s_3D_N=" + N_string + ".txt";
    s.load(filename);
    filename = "dG_3D_general_data/t_3D_N=" + N_string + ".txt";
    t.load(filename);
  
    filename = "dG_3D_general_data/V_3D_N=" + N_string + ".txt";
    V.load(filename);
    filename = "dG_3D_general_data/invV_3D_N=" + N_string + ".txt";
    invV.load(filename);
  
    filename = "dG_3D_general_data/M_3D_N=" + N_string + ".txt";
    M.load(filename);
    filename = "dG_3D_general_data/invM_3D_N=" + N_string + ".txt";
    invM.load(filename);
  
    filename = "dG_3D_general_data/V_r_3D_N=" + N_string + ".txt";
    V_r.load(filename);
    filename = "dG_3D_general_data/V_s_3D_N=" + N_string + ".txt";
    V_s.load(filename);
    filename = "dG_3D_general_data/V_t_3D_N=" + N_string + ".txt";
    V_t.load(filename);
  
    filename = "dG_3D_general_data/D_r_3D_N=" + N_string + ".txt";
    D_r.load(filename);
    filename = "dG_3D_general_data/D_s_3D_N=" + N_string + ".txt";
    D_s.load(filename);
    filename = "dG_3D_general_data/D_t_3D_N=" + N_string + ".txt";
    D_t.load(filename);
  
    filename = "dG_3D_general_data/D_r_weak_3D_N=" + N_string + ".txt";
    D_r_weak.load(filename);
    filename = "dG_3D_general_data/D_s_weak_3D_N=" + N_string + ".txt";
    D_s_weak.load(filename);
    filename = "dG_3D_general_data/D_t_weak_3D_N=" + N_string + ".txt";
    D_t_weak.load(filename);
    
    urowvec v_1 = E.row(0);
    urowvec v_2 = E.row(1);
    urowvec v_3 = E.row(2);
    urowvec v_4 = E.row(3);
	x = 0.5 * (-(r + s + t + 1) * trans(Px(v_1)) + (r + 1) * trans(Px(v_2)) + (s + 1) * trans(Px(v_3)) + (t + 1) * trans(Px(v_4)));
	y = 0.5 * (-(r + s + t + 1) * trans(Py(v_1)) + (r + 1) * trans(Py(v_2)) + (s + 1) * trans(Py(v_3)) + (t + 1) * trans(Py(v_4)));
	z = 0.5 * (-(r + s + t + 1) * trans(Pz(v_1)) + (r + 1) * trans(Pz(v_2)) + (s + 1) * trans(Pz(v_3)) + (t + 1) * trans(Pz(v_4)));
 
    GeometricFactors3D();
  
    filename = "dG_3D_general_data/Fmask_3D_N=" + N_string + ".txt";
    Fmask.load(filename);
    Fmask = Fmask - 1; // kvuli cislovani od 0 v C++
    Fx = x.rows(vectorise(Fmask));
    Fy = y.rows(vectorise(Fmask));
    Fz = z.rows(vectorise(Fmask));
    
    filename = "dG_3D_general_data/LIFT_3D_N=" + N_string + ".txt";
    LIFT.load(filename);
 
    Normals3D();
    Fscale = surfJ / J.rows(vectorise(Fmask));
  
    filename = directoryname + "EtoE_for_h=" + mesh_size_parameter_string + ".txt";
    EtoE.load(filename);
    EtoE = EtoE - 1; // kvuli cislovani od 0 v C++
    filename = directoryname + "EtoF_for_h=" + mesh_size_parameter_string + ".txt";
    EtoF.load(filename);
    EtoF = EtoF - 1; // kvuli cislovani od 0 v C++
  
    BuildMaps3D();
  }
  
  void dtscale3D()
  {
    uvec vmask1 = find(abs(r + s + t + 3) < NODETOL);
	uvec vmask2 = find(abs(r - 1) < NODETOL);
	uvec vmask3 = find(abs(s - 1) < NODETOL);
	uvec vmask4 = find(abs(t - 1) < NODETOL);
	uvec vmask = zeros<uvec>(4);
	vmask(0) = vmask1(0); vmask(1) =  vmask2(0); vmask(2) =  vmask3(0); vmask(3) =  vmask4(0);
	mat vx = x.rows(vmask); mat vy = y.rows(vmask); mat vz = z.rows(vmask);
	
	mat vectors1 = zeros<mat>(3,K);
	mat vectors2 = zeros<mat>(3,K);
	mat vectors3 = zeros<mat>(3,K);
	vectors1.row(0) = vx.row(1) - vx.row(0);
	vectors1.row(1) = vy.row(1) - vy.row(0);
	vectors1.row(2) = vz.row(1) - vz.row(0);
	vectors2.row(0) = vx.row(2) - vx.row(0);
	vectors2.row(1) = vy.row(2) - vy.row(0);
	vectors2.row(2) = vz.row(2) - vz.row(0);
	vectors3.row(0) = vx.row(3) - vx.row(0);
	vectors3.row(1) = vy.row(3) - vy.row(0);
	vectors3.row(2) = vz.row(3) - vz.row(0);
	rowvec volumes = zeros<rowvec>(K);
	for(int i = 0; i < K; i++)
	{
	  volumes(i) = dot(vectors1.col(i),cross(vectors2.col(i),vectors3.col(i)));
	}
	volumes = abs(volumes) / 6.0;
	
	umat face_def = zeros<umat>(4,3);
	face_def(0,0) = 0; face_def(0,1) = 1; face_def(0,2) = 2;
	face_def(1,0) = 0; face_def(1,1) = 1; face_def(1,2) = 3;
	face_def(2,0) = 1; face_def(2,1) = 2; face_def(2,2) = 3;
	face_def(3,0) = 0; face_def(3,1) = 2; face_def(3,2) = 3;
	urowvec face;
	mat areas = zeros<mat>(N_faces,K);
	mat cross_products = zeros<mat>(3,K);
	rowvec norms;
	for(int i = 0; i < N_faces; i++)
	{
	  face = face_def.row(i);
	  vectors1.row(0) = vx.row(face(1)) - vx.row(face(0));
	  vectors1.row(1) = vy.row(face(1)) - vy.row(face(0));
	  vectors1.row(2) = vz.row(face(1)) - vz.row(face(0));
	  vectors2.row(0) = vx.row(face(2)) - vx.row(face(0));
	  vectors2.row(1) = vy.row(face(2)) - vy.row(face(0));
	  vectors2.row(2) = vz.row(face(2)) - vz.row(face(0));
	  for(int j = 0; j < K; j++)
	  {
	    cross_products.col(j) = cross(vectors1.col(j),vectors2.col(j));
	  }
	  norms = sqrt(square(cross_products.row(0)) + square(cross_products.row(1)) + square(cross_products.row(2)));
	  areas.row(i) = 0.5 * norms;
	}
	
	dtscale = 6.0 * trans(volumes / sum(areas));
  }
}
