clear;clc;
if 1

    obj= readObj('/home/cflin/MatlabCode/TetWild/build/plate_r1_2w__sf.obj');%%

    S.Vertices=min(max(obj.v,[0 0 0]),[4 4 1]);%%
    S.Faces=obj.f.v;%%

    cor=load('/home/cflin/MatlabCode/TetWild/build/plate_r1_2w_cor.txt');
    elem=load('/home/cflin/MatlabCode/TetWild/build/plate_r1_2w_ele.txt');

    eeps=1e-4;
    DBCleft=@(X) abs(X(:,1)-0)<eeps;
    DBCdown=@(X) abs(X(:,2)-0)<eeps;
    DBCbottom=@(X) abs(X(:,3)-0)<eeps;

    NBC=@(X) abs(X(:,1)-4)<eeps;


    fea=FEA(cor,elem);
    fea.set_DBC(DBCleft,[1 0 0]);
    fea.set_DBC(DBCdown,[0 1 0]);
    fea.set_DBC(DBCbottom,[0 0 1]);

    order=5;
    [GlobalGP,GlobalGW]=fea.set_NBC(NBC,order,[1e-3 0 0]);
    fprintf("##Total gs_w: %.2f \n",sum(GlobalGW(:)));
    save NBC_GlobalGPW_for_plate GlobalGP GlobalGW

    fea.solve();


    sur_query=min([4 4 1],max([0 0 0],S.Vertices));
    [Q_U,Q_S]=fea.query_US(sur_query);
    J_FEA=fea.J;
    save US.mat Q_U Q_S J_FEA

    Vert_tot=min(max(fea.cor,[0 0 0]),[4 4 1]);
    [Q_tU,Q_tS]=fea.query_US(Vert_tot);
    save USt.mat Q_tU Q_tS Vert_tot

    %------------cut line:A to B of FEA
    
    CutLine=get_cutline();
    [Q_U_CL,Q_S_CL]=fea.query_US(CutLine);
    save US_CL.mat Q_U_CL Q_S_CL 




end

%

%-----------------------------PdeFEA
pfea=PdeFEA('/home/cflin/MatlabCode/TetWild/build/plate_r1.STL');
pfea.set_view_point([-50 30]);
pfea.set_mesh(0.12);

pfea.set_DBC(3,[1 0 0]);
pfea.set_DBC(5,[0 1 0]);
pfea.set_DBC(7,[0 0 1]);
pfea.set_NBC(1,[1e-3 0 0]);
%--------------------------solve
tic
pfea.solve();
toc
%--------------------------plot
pfea.plot_displacement_norm();
pfea.plot_von_Mises_stress();
%--------------------------post_process
%------------cut line:A to B of pFEA

    CutLine=get_cutline();

norm_U_CL_pFEA=pfea.query_norm_U(CutLine);
von_S_CL_pFEA=pfea.query_von_S(CutLine);
save US_CL_pFEA.mat norm_U_CL_pFEA von_S_CL_pFEA



%
% extract FEA data & Surface data S

if ~exist("Q_U",'var')
    load US.mat Q_U Q_S J_FEA
    load USt.mat Q_tU Q_tS Vert_tot
    load US_CL.mat Q_U_CL Q_S_CL 
    load US_CL_pFEA.mat norm_U_CL_pFEA von_S_CL_pFEA


    if ~exist("obj",'var')
        obj= readObj('/home/cflin/MatlabCode/TetWild/build/plate_r1_2w__sf.obj');%%

        S.Vertices=min(max(obj.v,[0 0 0]),[4 4 1]);%%
        S.Faces=obj.f.v;%%
    end
end
if ~exist('CutLine','var')
    CutLine=get_cutline();
end

norm_U_FEA=vecnorm(Q_U,2,2);
von_S_FEA=cal_von_Mises_Stress(Q_S);

%---------tot U S
norm_U_total_FEA=vecnorm(Q_tU,2,2);
von_S_total_FEA=cal_von_Mises_Stress(Q_tS);   
%----------cut line U S
norm_U_CL_FEA=vecnorm(Q_U_CL,2,2);
von_S_CL_FEA=cal_von_Mises_Stress(Q_S_CL);


% extract fcm data

if ~exist('plt')
    plt=Plotter('Cube_tmp','NNew_Data');
end
plt.mp.viewxy=[90 70];
cur_order=plt.mp.order;
cur_deep=plt.mp.maxdeep;
cur_c=plt.mp.recorder.intermediate_c{1};
Q_U_XV=compute_u(S.Vertices,plt.mp.recorder.U{1},plt.mp);%3x1xLen
Q_U_XV=reshape(Q_U_XV,3,[])';% lenx3
Q_S_XV=squeeze(compute_stress(S.Vertices,plt.mp.recorder.U{1},plt.mp))';% len x  6
J_XV=plt.mp.recorder.J(1);

norm_U_FCM=vecnorm(Q_U_XV,2,2);
von_S_FCM=sqrt(0.5*((Q_S_XV(:,1)-Q_S_XV(:,2)).^2+(Q_S_XV(:,1)-Q_S_XV(:,3)).^2+(Q_S_XV(:,3)-Q_S_XV(:,2)).^2)+...
    3*(vecnorm(Q_S_XV(:,4:6),2,2).^2));

%------------tot U S of FCM
Q_tU_XV=compute_u(Vert_tot,plt.mp.recorder.U{1},plt.mp);
Q_tU_XV=reshape(Q_tU_XV,3,[])';% lenx3
norm_U_total_FCM=vecnorm(Q_tU_XV,2,2);
Q_tS_XV=squeeze(compute_stress(Vert_tot,plt.mp.recorder.U{1},plt.mp))';
von_S_total_FCM=sqrt(0.5*((Q_tS_XV(:,1)-Q_tS_XV(:,2)).^2+(Q_tS_XV(:,2)-Q_tS_XV(:,3)).^2+(Q_tS_XV(:,3)-Q_tS_XV(:,1)).^2)+...
    3*(vecnorm(Q_tS_XV(:,4:6),2,2).^2));

%------------cut line:A to B of FCM
Q_U_CL_XV=compute_u(CutLine,plt.mp.recorder.U{1},plt.mp);
Q_U_CL_XV=reshape(Q_U_CL_XV,3,[])';% lenx3
Q_S_CL_XV=squeeze(compute_stress(CutLine,plt.mp.recorder.U{1},plt.mp))';

norm_U_CL_FCM=vecnorm(Q_U_CL_XV,2,2);
von_S_CL_FCM=cal_von_Mises_Stress(Q_S_CL_XV);

%%-------------extract data from fcmlab
load /home/cflin/MatlabCode/fcmlab/drivers/FCM_3D_AnalysisDriver/ThickPlateWithAHole_Benchmark/FCMlab_plate_p4d5_X_u_stress.mat u stress
norm_U_CL_lab=vecnorm(u,2,2);
von_S_CL_lab=cal_von_Mises_Stress(stress);
%%-------------end extract


%-------------plot movement contrast
% figure
% plot(CutLine(:,1).*sqrt(2),norm_U_CL_lab,'r');
% hold on
plot(CutLine(:,1).*sqrt(2),norm_U_CL_pFEA);
hold on
plot(CutLine(:,1).*sqrt(2),norm_U_CL_FCM,'b');
legend({'fcmlab','pFEA','FCM, p=4'},'Location',"best");
xlabel('cut line A-B: r')
ylabel('movement')
%-------------plot von Mises stress contrast
figure
% plot(CutLine(:,1).*sqrt(2),von_S_CL_FEA);
% hold on
% plot(CutLine(:,1).*sqrt(2),von_S_CL_lab);
% hold on
plot(CutLine(:,1).*sqrt(2),von_S_CL_pFEA);
hold on
plot(CutLine(:,1).*sqrt(2),von_S_CL_FCM);
legend({'pFEA','FCM, p=4'},'Location',"best");
xlabel('cut line A-B: r')
ylabel('von Mises stress')
%-------------plot relative error contrast
figure
rela_U=sqrt(abs(norm_U_CL_pFEA-norm_U_CL_FCM)./norm_U_CL_pFEA)*100;
plot(CutLine(:,1).*sqrt(2),rela_U);
xlabel('cut line A-B: r')
ylabel('relative error in movement norm (%)')
%-------------plot relative error contrast
figure
rela_S=sqrt(abs(von_S_CL_pFEA-von_S_CL_FCM)./von_S_CL_pFEA)*100;
% semilogy(CutLine(:,1).*sqrt(2),rela_S);ylim([1e-3 1e2]);

% plot(CutLine(:,1).*sqrt(2),rela_S);
semilogy(CutLine(:,1).*sqrt(2),rela_S);

xlabel('cut line A-B: r')
ylabel('relative error in von Mises stress (%)')

%%-------------save data
eval(['norm_U' num2str(cur_order) '=norm_U_CL_FCM;']);
eval(['von_S' num2str(cur_order) '=von_S_CL_FCM;']);
eval(['rela_U' num2str(cur_order) '=rela_U;']);
eval(['rela_S' num2str(cur_order) '=rela_S;']);
eval(['save p' num2str(cur_order) 'd' num2str(cur_deep) '.mat ' ...
    'norm_U' num2str(cur_order) ' von_S' num2str(cur_order) ' rela_U' num2str(cur_order) ' rela_S' num2str(cur_order)]);

%


%%-------------plot sum
load p1d5.mat
load p2d5.mat
load p3d5.mat
load p4d5.mat
load p5d5.mat
load p6d5.mat


%-------------plot relative error contrast
figure
rela_S=sqrt(abs(von_S_CL_pFEA-von_S_CL_FCM)./von_S_CL_pFEA)*100;
% semilogy(CutLine(:,1).*sqrt(2),rela_S);ylim([1e-3 1e2]);

semilogy(CutLine(:,1).*sqrt(2),rela_S2);
hold on
semilogy(CutLine(:,1).*sqrt(2),rela_S4);
hold on
semilogy(CutLine(:,1).*sqrt(2),rela_S6);

yticks([1e-3 1e-2 1e-1 1e0 1e1 1e2])
yticklabels({'0.0001','0.001','0.1','1','10','100'})

xlabel('cut line A-B: r')
ylabel('relative error in von Mises stress (%)')
legend({'p=2','p=4','p=6'},'Location',"best");

%%-------------end plot

%

close all
load p3d5.mat
load p4d5.mat
load p5d5.mat
load p6d5.mat
load p7d5.mat
CutLine=get_cutline();
% plot(CutLine(:,1).*sqrt(2),p3d5);
% hold on
plot(CutLine(:,1).*sqrt(2),rela_U4,'LineWidth',1);
hold on
plot(CutLine(:,1).*sqrt(2),rela_U5,'LineWidth',1);
hold on
plot(CutLine(:,1).*sqrt(2),rela_U6,'LineWidth',1);
hold on
plot(CutLine(:,1).*sqrt(2),rela_U7,'LineWidth',1);
xlabel('cut line A-B: r')
% ylabel('relative error in von Mises stress (%)')
ylabel('relative error in von Mises stress (%)')

legend({'p=4','p=5','p=6','p=7'},'Location',"best");

%



% plot surface

%--------------------------------------------------------------------plot deformation
% close all;
plot_surf(S,norm_U_FEA,'norm deformation of FEA');% plot norm_U_FEA
plot_surf(S,norm_U_FCM,'norm deformation of FCM');% plot norm_U_FCM
%--------------------------------------------------------------------plot stress
plot_surf(S,von_S_FEA,'Von Mises stress of FEA')% plot von_S_FEA
plot_surf(S,von_S_FCM,'Von Mises stress of FCM')% plot von_S_FEA

%--------------------------------------------------------hist of U
figure;
histogram(norm_U_FEA);
hold on;
histogram(norm_U_FCM);
xlabel('deformation distribution of FEA and FCM');
legend({'FEA','FCM'},'Location',"best");
%--------------------------------------------------------hist of S
figure;
histogram(von_S_FEA);
hold on;
histogram(von_S_FCM);
xlabel('von Mises stress distribution of FEA and FCM');
legend({'FEA','FCM'},'Location',"best");

%
%% Surface Statistic

%-----------------------------------------compliance
compliance_FEA=J_FEA
compliance_FCM=J_XV
abs(compliance_FEA-compliance_FCM)./compliance_FCM
%----------------------------
max_U_FEA=max(norm_U_FEA(:))
max_U_FCM=max(norm_U_FCM(:))
abs(max_U_FEA-max_U_FCM)/max_U_FCM
max_S_FEA=max(von_S_FEA(:))
max_S_FCM=max(von_S_FCM(:))
abs(max_S_FEA-max_S_FCM)/max_S_FCM
%------------------------------------------relative error
threshold=0.2;
%---------------------------------for U
sorted_norm_U_FEA=sort(norm_U_FEA);
thresh_val=sorted_norm_U_FEA(round(size(sorted_norm_U_FEA,1)*threshold));
mini_U_idx=(norm_U_FEA<thresh_val);
% mini_U_idx=(norm_U_FEA<0);%% wait to del
rela_error_U=abs(norm_U_FEA-norm_U_FCM)./norm_U_FEA;
rela_error_U(mini_U_idx)=0;
rela_error_U(rela_error_U==Inf)=0;
mean(rela_error_U)
max(rela_error_U)
%----------------------------------for S
sorted_von_S_FEA=sort(von_S_FEA);
thresh_val=sorted_von_S_FEA(round(size(sorted_von_S_FEA,1)*threshold));
mini_S_idx=von_S_FEA<thresh_val;
mini_S_idx=(mini_S_idx<0);%% wait to del
rela_error_S=abs(von_S_FEA-von_S_FCM)./von_S_FEA;
rela_error_S(mini_S_idx)=0;
rela_error_S(rela_error_S==Inf)=0;
mean(rela_error_S)
max(rela_error_S)


%-------------------------------------------------------plot rela error U
plot_surf(S,rela_error_U,'relative error of deformation between FEA and FCM');
plot_surf(S,rela_error_S,'relative error of Von Mises stress between FEA and FCM');
%-------------------------------------------------------hist relative error
figure;
histogram(rela_error_U);
xlabel('hist of relative error of deformation');

figure;
histogram(rela_error_S);
xlabel('hist of relative error of Von Mises stress');
% legend({'FEA','FCM'},'Location',"best");

%% total vertex analysis

%-----------------------------------------compliance
compliance_total_FEA=J_FEA
compliance_total_FCM=J_XV
abs(compliance_total_FEA-compliance_total_FCM)./compliance_total_FCM
%--------------------------------------max
max_U_total_FEA=max(norm_U_total_FEA(:))
max_U_total_FCM=max(norm_U_total_FCM(:))
abs(max_U_total_FEA-max_U_total_FCM)/max_U_total_FCM
max_S_total_FEA=max(von_S_total_FEA(:))
max_S_total_FCM=max(von_S_total_FCM(:))
abs(max_S_total_FEA-max_S_total_FCM)/max_S_total_FCM

%------------------------------------------relative error
threshold=0.2;
%---------------------------------for U
sorted_norm_U_FEA=sort(norm_U_total_FEA);
thresh_val=sorted_norm_U_FEA(round(size(sorted_norm_U_FEA,1)*threshold));
mini_U_idx=(norm_U_total_FEA<thresh_val);
% mini_U_idx=(norm_U_FEA<0);%% wait to del
rela_error_U=abs(norm_U_total_FEA-norm_U_total_FCM)./norm_U_total_FEA;
rela_error_U(mini_U_idx)=0;
rela_error_U(rela_error_U==Inf)=0;
mean(rela_error_U)
max(rela_error_U)
%----------------------------------for S
sorted_von_S_FEA=sort(von_S_total_FEA);
thresh_val=sorted_von_S_FEA(round(size(sorted_von_S_FEA,1)*threshold));
mini_S_idx=von_S_total_FEA<thresh_val;
% mini_S_idx=(mini_S_idx<0);%% wait to del
rela_error_S=abs(von_S_total_FEA-von_S_total_FCM)./von_S_total_FEA;
rela_error_S(mini_S_idx)=0;
rela_error_S(rela_error_S==Inf)=0;
mean(rela_error_S)
max(rela_error_S)

%
%% fcmlab contrast

load /home/cflin/MatlabCode/fcmlab/drivers/FCM_3D_AnalysisDriver/ThickPlateWithAHole_Benchmark/FCMlab_plate_p4d5_X_u_stress X u stress U 
von_S_fcmlab=cal_von_Mises_Stress(stress);
U_fcmlab=U;
norm_U_fcmlab=vecnorm(u,2,2);
if ~exist('plt')
    plt=Plotter('Cube_tmp','NNew_Data');
end
plt.mp.viewxy=[90 70];
cur_c=plt.mp.recorder.intermediate_c{1};
Q_U_FCM=compute_u(X,plt.mp.recorder.U{1},plt.mp);%3x1xLen
Q_U_FCM=reshape(Q_U_FCM,3,[])';% lenx3
Q_S_FCM=squeeze(compute_stress(X,plt.mp.recorder.U{1},plt.mp))';% len x  6
J_XV=plt.mp.recorder.J(1);

norm_U_FCM=vecnorm(Q_U_FCM,2,2);
von_S_FCM=cal_von_Mises_Stress(Q_S_FCM);

% analysis
% compliance_fcmlab=C
% compliance_FCM=J_XV

% abs(compliance_fcmlab-compliance_FCM)./compliance_fcmlab

relative_error=zeros(size(norm_U_FCM));
zero_U_idx=norm_U_fcmlab==0|norm_U_FCM==0;
relative_error(~zero_U_idx)=abs(norm_U_FCM(~zero_U_idx)-norm_U_fcmlab(~zero_U_idx))./norm_U_fcmlab(~zero_U_idx);
% relative_error=abs(norm_U_FCM-norm_U_fcmlab)./norm_U_fcmlab;
% relative_error(relative_error==Inf)=0;
mean(relative_error)

max(relative_error)

relative_error=abs(von_S_FCM-von_S_fcmlab)./von_S_fcmlab;
relative_error(relative_error==Inf)=0;
mean(relative_error)

max(relative_error)
% U_FCM=plt.mp.recorder.U{1};
% figure
% histogram(U_FCM);
% hold on;
% histogram(U_fcmlab);
% legend({'FCM','fcmlab'},'Location',"best");
%
% sU_FCM=sort(U_FCM);
% sU_fcmlab=sort(U_fcmlab);




%

