rk4_ls;

NODETOL = 1e-9;

N_v = size(P,2);
K = size(E,2);

x_opt_eq = load(sprintf('x_opt_eq_Hesthaven_N=%d.txt',N));
y_opt_eq = load(sprintf('y_opt_eq_Hesthaven_N=%d.txt',N));
z_opt_eq = load(sprintf('z_opt_eq_Hesthaven_N=%d.txt',N));
[r,s,t] = xyztorst(x_opt_eq,y_opt_eq,z_opt_eq);

V = VandermondeMatrix3D(N,r,s,t); invV = inv(V);
M = invV' * invV; invM = V * V';
[D_r,D_s,D_t] = DiffMatrices3D(N,r,s,t,V,invV);

v_1 = E(1,:); v_2 = E(2,:); v_3 = E(3,:); v_4 = E(4,:);
x = 0.5 * (-(r + s + t + 1)' * P(1,v_1) + (r + 1)' * P(1,v_2) + (s + 1)' * P(1,v_3) + (t + 1)' * P(1,v_4));
y = 0.5 * (-(r + s + t + 1)' * P(2,v_1) + (r + 1)' * P(2,v_2) + (s + 1)' * P(2,v_3) + (t + 1)' * P(2,v_4));
z = 0.5 * (-(r + s + t + 1)' * P(3,v_1) + (r + 1)' * P(3,v_2) + (s + 1)' * P(3,v_3) + (t + 1)' * P(3,v_4));

[r_x,r_y,r_z,s_x,s_y,s_z,t_x,t_y,t_z,J] = GeometricFactors3D_my(x,y,z,D_r,D_s,D_t);

fmask1 = find(abs(t + 1) < NODETOL);
fmask2 = find(abs(s + 1) < NODETOL);
fmask3 = find(abs(r + s + t + 1) < NODETOL);
fmask4 = find(abs(r + 1) < NODETOL);
Fmask = [fmask1; fmask2; fmask3; fmask4]';
Fx = x(Fmask(:),:); Fy = y(Fmask(:),:); Fz = z(Fmask(:),:);

[n_x,n_y,n_z,surfJ] = Normals3D_my();
Fscale = surfJ ./ J(Fmask(:),:);

[EtoE,EtoF] = Connectivity3D();

[mapM, vmapM, mapP, vmapP, mapB, vmapB] = BuildMaps3D();