clc; clear; close all;

print_figures = 0;
arrows_width = 2;

N = 6;
float_format = '%0.4f';

fileID = fopen('OptimalNodes2D_Demonstration.txt','w');

fprintf(fileID,'N = %d',N);
fprintf(fileID,'\n\n');

disp('Equidistant grid:');
r_e = linspace(-1,1,N + 1)
fprintf(fileID,'r^{e}:\n');
LatexCodeMatrix(r_e,float_format,fileID);
fprintf(fileID,'\n\n');
disp('and the LGL nodes:');
r_LGL = GaussLobattoNodes(N + 1)
fprintf(fileID,'r^{LGL}:\n');
LatexCodeMatrix(r_LGL,float_format,fileID);
fprintf(fileID,'\n\n');

LagPols = LagrangePolynomialBasis(r_e);
warp_function = zeros(1,N + 1);
for i = 1:(N + 1)
    warp_function = warp_function + (r_LGL(i) - r_e(i)) * LagPols{i};
end

disp('Warp function evaluated at the equidistant grid points:');
nodes_shift = polyval(warp_function,r_e)
fprintf(fileID,'w(r^{e}):\n');
LatexCodeMatrix(nodes_shift,float_format,fileID);
fprintf(fileID,'\n\n');


% warp function in 1D
warp1D = figure('Name', 'Warp function 1D');
dr = -1:0.01:1; plot(dr, polyval(warp_function,dr), 'LineWidth', 2);
xlabel('r'); ylabel('w(r)');
if print_figures == 1
    filename_string = ['Warp_function_1D_for_N=',num2str(N)];
    saveas(warp1D,filename_string,'pdf');
    saveas(warp1D,filename_string,'png');
end

fclose(fileID);


% warp function in 2D
alpha_opt = [0.0000, 0.0000, 1.4152, 0.1001, 0.2751, 0.9800, 1.0999, 1.2832, 1.3648, 1.4773, 1.4959, 1.5743, 1.5770, 1.6223, 1.6258];

if (N < 16)
    alpha_nodes = alpha_opt(N);
else
    alpha_nodes = 5/3;
end;

N_p = ((N + 1) * (N + 2)) / 2;
lambda1 = zeros(1,N_p); lambda2 = lambda1; lambda3 = lambda1;
temp = 1;
for i = 0:N
    for j = 0:(N - i)
        lambda1(temp) = i / N;
        lambda3(temp) = j / N;
        temp = temp + 1;
    end
end
lambda2 = 1 - lambda1 - lambda3;

x = -lambda2 + lambda3;
y = (-lambda2  - lambda3 + 2 * lambda1) / sqrt(3);

% jak se tyto vztahy dostaly???
blend1 = 4 * lambda2 .* lambda3;
blend2 = 4 * lambda1 .* lambda3;
blend3 = 4 * lambda1 .* lambda2;

w_edge1 = Warpfactor(N,lambda3 - lambda2);
w_edge2 = Warpfactor(N,lambda1 - lambda3);
w_edge3 = Warpfactor(N,lambda2 - lambda1);

w_final1 = (1 + (alpha_nodes * lambda1) .^ 2) .* blend1 .* w_edge1;
w_final2 = (1 + (alpha_nodes * lambda2) .^ 2) .* blend2 .* w_edge2;
w_final3 = (1 + (alpha_nodes * lambda3) .^ 2) .* blend3 .* w_edge3;

warp2Dface1 = figure('Name', 'Warp function along edge 1');
u = 1 * w_final1;
v = 0 * w_final1;
plot([-1, 1, 0, -1],[-sqrt(3) / 3, -sqrt(3) / 3, 2 * sqrt(3) / 3, -sqrt(3) / 3], 'LineWidth', 2, 'Color', 'black'); hold on;
quiver(x, y, u, v, 0, 'LineWidth', arrows_width, 'Color', 'red');
%set(gca,'DataAspectRatio',[1 1 1]);
axis equal;
axis off;
if print_figures == 1
    filename_string = ['Warp_function_along_edge_1_for_N=',num2str(N)];
    saveas(warp2Dface1,filename_string,'pdf');
    saveas(warp2Dface1,filename_string,'png');
end

warp2Dface2 = figure('Name', 'Warp function along edge 2');
u = cos(2 * pi / 3) * w_final2;
v = sin(2 * pi / 3) * w_final2;
plot([-1, 1, 0, -1],[-sqrt(3) / 3, -sqrt(3) / 3, 2 * sqrt(3) / 3, -sqrt(3) / 3], 'LineWidth', 2, 'Color', 'black'); hold on;
quiver(x, y, u, v, 0, 'LineWidth', arrows_width, 'Color', 'red');
%set(gca,'DataAspectRatio',[1 1 1]);
axis equal;
axis off;
if print_figures == 1
    filename_string = ['Warp_function_along_edge_2_for_N=',num2str(N)];
    saveas(warp2Dface2,filename_string,'pdf');
    saveas(warp2Dface2,filename_string,'png');
end

warp2Dface3 = figure('Name', 'Warp function along edge 3');
u = cos(4 * pi / 3) * w_final3;
v = sin(4 * pi / 3) * w_final3;
plot([-1, 1, 0, -1],[-sqrt(3) / 3, -sqrt(3) / 3, 2 * sqrt(3) / 3, -sqrt(3) / 3], 'LineWidth', 2, 'Color', 'black'); hold on;
quiver(x, y, u, v, 0, 'LineWidth', arrows_width, 'Color', 'red');
%set(gca,'DataAspectRatio',[1 1 1]);
axis equal;
axis off;
if print_figures == 1
    filename_string = ['Warp_function_along_edge_3_for_N=',num2str(N)];
    saveas(warp2Dface3,filename_string,'pdf');
    saveas(warp2Dface3,filename_string,'png');
end

warp2D = figure('Name', 'Warp function in 2D');
u = 1 * w_final1 + cos(2 * pi / 3) * w_final2 + cos(4 * pi / 3) * w_final3;
v = 0 * w_final1  + sin(2 * pi / 3) * w_final2 + sin(4 * pi / 3) * w_final3;
plot([-1, 1, 0, -1],[-sqrt(3) / 3, -sqrt(3) / 3, 2 * sqrt(3) / 3, -sqrt(3) / 3], 'LineWidth', 2, 'Color', 'black'); hold on;
quiver(x, y, u, v, 0, 'LineWidth', arrows_width, 'Color', 'red');
%set(gca,'DataAspectRatio',[1 1 1]);
axis equal;
axis off;
if print_figures == 1
    filename_string = ['Warp_function_2D_for_N=',num2str(N)];
    saveas(warp2D,filename_string,'pdf');
    saveas(warp2D,filename_string,'png');
end


% reference equilateral triangle
% equidistant grid
equidistant_grid_equilateral = figure('Name', 'Equidistant grid');
[x,y] = EquidistantNodes2D(N);
plot([-1, 1, 0, -1], [-sqrt(3) / 3, -sqrt(3) / 3, 2 * sqrt(3) / 3, -sqrt(3) / 3], 'LineWidth', 2, 'Color', 'black'); hold on;
plot(x,y,'o', 'MarkerEdgeColor', 'k', 'MarkerFaceColor', 'b', 'MarkerSize', 10);
%set(gca,'DataAspectRatio',[1 1 1]);
axis equal;
axis off;
if print_figures == 1
    filename_string = ['Equidistant_grid_for_N=',num2str(N),'_reference_equilateral'];
    saveas(equidistant_grid_equilateral,filename_string,'pdf');
    saveas(equidistant_grid_equilateral,filename_string,'png');
end


% optimized grid
optimized_grid_equilateral = figure('Name', 'Optimized grid');
[x,y] = OptimalNodes2D(N);
plot([-1, 1, 0, -1],[-sqrt(3) / 3, -sqrt(3) / 3, 2 * sqrt(3) / 3, -sqrt(3) / 3], 'LineWidth', 2, 'Color', 'black'); hold on;
plot(x,y,'o', 'MarkerEdgeColor', 'k', 'MarkerFaceColor', 'b', 'MarkerSize', 10);
%set(gca,'DataAspectRatio',[1 1 1]);
axis equal;
axis off;
if print_figures == 1
    filename_string = ['Optimized_grid_for_N=',num2str(N),'_reference_equilateral'];
    saveas(optimized_grid_equilateral,filename_string,'pdf');
    saveas(optimized_grid_equilateral,filename_string,'png');
end


% both nodes in one figure with their shifts
figure;
[x1,y1] = EquidistantNodes2D(N);
[x2,y2] = OptimalNodes2D(N);
plot([-1, 1, 0, -1],[-sqrt(3) / 3, -sqrt(3) / 3, 2 * sqrt(3) / 3, -sqrt(3) / 3], 'LineWidth', 2, 'Color', 'black'); hold on;
plot(x1,y1,'o', 'MarkerEdgeColor', 'k', 'MarkerFaceColor', 'green', 'MarkerSize', 10); hold on;
plot(x2,y2,'o', 'MarkerEdgeColor', 'k', 'MarkerFaceColor', 'b', 'MarkerSize', 10); hold on;
quiver(x1, y1, u, v, 0, 'LineWidth', arrows_width, 'Color', 'red');
%set(gca,'DataAspectRatio',[1 1 1]);
axis equal;
axis off;
if print_figures == 1
    filename_string = ['Shifts_of_nodes_2D_for_N=',num2str(N),'_reference_equilateral'];
    saveas(gcf,filename_string,'pdf');
    saveas(gcf,filename_string,'png');
end


% reference triangle
% equidistant grid
equidistant_grid = figure('Name', 'Equidistant grid');
[x,y] = EquidistantNodes2D(N);
[r,s] = xytors(x,y);
plot([-1, 1, -1, -1], [-1, -1, 1, -1], 'LineWidth', 2, 'Color', 'black'); hold on;
plot(r,s,'o', 'MarkerEdgeColor', 'k', 'MarkerFaceColor', 'b', 'MarkerSize', 10);
%set(gca,'DataAspectRatio',[1 1 1]);
axis equal;
axis off;
if print_figures == 1
    filename_string = ['Equidistant_grid_for_N=',num2str(N),'_reference'];
    saveas(equidistant_grid,filename_string,'pdf');
    saveas(equidistant_grid,filename_string,'png');
end


% optimized grid
optimized_grid = figure('Name', 'Optimized grid');
[x,y] = OptimalNodes2D(N);
[r,s] = xytors(x,y);
plot([-1, 1, -1, -1], [-1, -1, 1, -1], 'LineWidth', 2, 'Color', 'black'); hold on;
plot(r,s,'o', 'MarkerEdgeColor', 'k', 'MarkerFaceColor', 'b', 'MarkerSize', 10);
%set(gca,'DataAspectRatio',[1 1 1]);
axis equal;
axis off;
if print_figures == 1
    filename_string = ['Optimized_grid_for_N=',num2str(N),'_reference'];
    saveas(optimized_grid,filename_string,'pdf');
    saveas(optimized_grid,filename_string,'png');
end