clc
clear all

% 算例配置（改材料/模态数请编辑 case_config.m）
[phy, cfg] = case_config();

% 四面体体积分：高斯积分节点与权重
IntegralOrder=cfg.IntegralOrder;
xt = [0.25,...
    0.0919710780526,0.0919710780526,0.0919710780526,0.724086765841,...
    0.319793627829,0.319793627829,0.319793627829,0.0406191165118,...
    0.0563508326895,0.0563508326895,0.44364916731,0.0563508326895,...
    0.44364916731,0.44364916731];
yt = [0.25,...
    0.0919710780526,0.0919710780526,0.724086765841,0.0919710780526,...
    0.319793627829,0.319793627829,0.0406191165118,0.319793627829,...
    0.0563508326895,0.44364916731,0.0563508326895,0.44364916731,...
    0.0563508326895,0.44364916731];
zt = [0.25,...
    0.0919710780526,0.724086765841,0.0919710780526,0.0919710780526,...
    0.319793627829,0.0406191165118,0.319793627829,0.319793627829,...
    0.44364916731,0.0563508326895,0.0563508326895,0.44364916731,...
    0.44364916731,0.0563508326895];
pt = [0.0197530864198,...
    0.0119895139632,0.0119895139632,0.0119895139632,0.0119895139632,...
    0.011511367871,0.011511367871,0.011511367871,0.011511367871,...
    0.00881834215168,0.00881834215168,0.00881834215168,0.00881834215168,...
    0.00881834215168,0.00881834215168];

model=mphload(cfg.mphFile);
[m1, m2]=mphmeshstats(model);
NbrNodes=max(size(m2.vertex));       % 顶点数
NbrElements=max(size(m2.elem{2}));   % 四面体数
Nodes=m2.vertex;                     % 顶点坐标 3*n
Domains=m2.elementity{2};            % 四面体区域索引 n*1
Elements=m2.elem{2}+1;               % 四面体顶点索引 4*n（COMSOL 0-based -> MATLAB 1-based）

% 四面体顶点重新排序，节点按从小到大顺序排列
Elements = sort(Elements);

el2no=Elements;
% 面排序
f1=el2no([1 1 1 2],:);
f2=el2no([2 2 3 3],:);
f3=el2no([3 4 4 4],:);
f_array=[f1(:) f2(:) f3(:)];
% Faces 全局面 n*3；FacesOfElements 四面体面索引 4N*1
[Faces,~,FacesOfElements]=unique(f_array,'rows');
NbrFaces=max(size(Faces));           % 面数
% 边排序
n1=el2no([1 1 1 2 2 3],:);
n2=el2no([2 3 4 3 4 4],:);
el_ed2no_array=[n1(:) n2(:)];
% Edges 全局边 n*2；EdgesOfElements 四面体边索引 6N*1
[Edges,~,EdgesOfElements]=unique(el_ed2no_array,'rows');
NbrEdges=max(size(Edges));            % 边数

epsilon=phy.eps;                     % 相对介电常数

Dof=NbrEdges*2+NbrFaces*2;           % 自由度：二阶矢量基，边数*2+面数*2
A=zeros(Dof,Dof);
B=zeros(Dof,Dof);

for n=1:NbrElements
   % 网格坐标
    x=zeros(4,1);y=zeros(4,1);z=zeros(4,1);
    x(1)=Nodes(1,Elements(1,n));y(1)=Nodes(2,Elements(1,n));z(1)=Nodes(3,Elements(1,n));
    x(2)=Nodes(1,Elements(2,n));y(2)=Nodes(2,Elements(2,n));z(2)=Nodes(3,Elements(2,n));
    x(3)=Nodes(1,Elements(3,n));y(3)=Nodes(2,Elements(3,n));z(3)=Nodes(3,Elements(3,n));
    x(4)=Nodes(1,Elements(4,n));y(4)=Nodes(2,Elements(4,n));z(4)=Nodes(3,Elements(4,n));
    % 边长度
    l=zeros(6,1);
    l(1)=sqrt((x(1)-x(2))*(x(1)-x(2))+(y(1)-y(2))*(y(1)-y(2))+(z(1)-z(2))*(z(1)-z(2)));
    l(2)=sqrt((x(1)-x(3))*(x(1)-x(3))+(y(1)-y(3))*(y(1)-y(3))+(z(1)-z(3))*(z(1)-z(3)));
    l(3)=sqrt((x(4)-x(1))*(x(4)-x(1))+(y(4)-y(1))*(y(4)-y(1))+(z(4)-z(1))*(z(4)-z(1)));
    l(4)=sqrt((x(2)-x(3))*(x(2)-x(3))+(y(2)-y(3))*(y(2)-y(3))+(z(2)-z(3))*(z(2)-z(3)));
    l(5)=sqrt((x(4)-x(2))*(x(4)-x(2))+(y(4)-y(2))*(y(4)-y(2))+(z(4)-z(2))*(z(4)-z(2)));
    l(6)=sqrt((x(4)-x(3))*(x(4)-x(3))+(y(4)-y(3))*(y(4)-y(3))+(z(4)-z(3))*(z(4)-z(3)));
    ll=zeros(20,1);                  % 基函数归一化系数
    ll(1)=l(1);ll(2)=l(1);ll(3)=l(2);ll(4)=l(2);ll(5)=l(3);ll(6)=l(3);
    ll(7)=l(4);ll(8)=l(4);ll(9)=l(5);ll(10)=l(5);ll(11)=l(6);ll(12)=l(6);
    ll(13)=1.5*l(1);ll(14)=1.5*l(2);ll(15)=1.5*l(1);ll(16)=1.5*l(3);
    ll(17)=1.5*l(2);ll(18)=1.5*l(3);ll(19)=1.5*l(4);ll(20)=1.5*l(5);
    
    % 雅可比矩阵
    Jac=zeros(3,3);
    Jac(1,1)=x(1)-x(4);Jac(1,2)=y(1)-y(4);Jac(1,3)=z(1)-z(4);
    Jac(2,1)=x(2)-x(4);Jac(2,2)=y(2)-y(4);Jac(2,3)=z(2)-z(4);
    Jac(3,1)=x(3)-x(4);Jac(3,2)=y(3)-y(4);Jac(3,3)=z(3)-z(4);
    InvJac=inv(Jac);
    DetJac=abs(det(Jac));
    TJac=Jac'/DetJac;
    
    % 计算高斯节点对应的基函数
    tempE=zeros(1,3);E=zeros(20,3,IntegralOrder);curlE=zeros(20,3,IntegralOrder);
    for i=1:IntegralOrder
        for j=1:20
            tempE(:)=BF_E(xt(i),yt(i),zt(i),j)*ll(j);
            E(j,:,i)=(InvJac*tempE(:))';
            
            tempE(:)=BF_CurlE(xt(i),yt(i),zt(i),j)*ll(j);
            curlE(j,:,i)=(TJac*tempE(:))';
        end
    end
    % 计算映射数组（局部 DOF -> 全局 DOF）
    map=zeros(20,1);
    for i=1:6
        map(i*2-1)=EdgesOfElements((n-1)*6+i);
        map(i*2)=EdgesOfElements((n-1)*6+i)+NbrEdges;
    end
    for i=1:4
        map(12+i*2-1)=2*NbrEdges+FacesOfElements((n-1)*4+i);
        map(12+i*2)=FacesOfElements((n-1)*4+i)+NbrFaces+2*NbrEdges;
    end
    % 双循环计算子矩阵
    Ae=zeros(20,20);Be=zeros(20,20);
    for i=1:20
        for j=1:20
            for k=1:IntegralOrder
                Ae(i,j)=Ae(i,j)+pt(k)*DetJac*(curlE(i,1,k)*curlE(j,1,k)+curlE(i,2,k)*curlE(j,2,k)+curlE(i,3,k)*curlE(j,3,k));
                Be(i,j)=Be(i,j)+pt(k)*DetJac*(E(i,1,k)*E(j,1,k)+E(i,2,k)*E(j,2,k)+E(i,3,k)*E(j,3,k));
            end
        end
    end
    % 矩阵组装
    for i=1:20
       for j=1:20
          A(map(i),map(j))= A(map(i),map(j))+Ae(i,j);
          B(map(i),map(j))= B(map(i),map(j))+Be(i,j);
       end
    end
end

% 广义本征值求解：A*x = lambda*B*x，lambda = k0^2
[V,D] = eigs(A,B,cfg.NbrMode,cfg.eigsSigma);
k0=diag(sqrt(D))