%% Read STL in MATLAB
clc,clear all,close all
global import_stl
%% Import and visualize LA surface
file_path = uigetdir(['C:\Users\sachal.hussain3\OneDrive - Alma Mater Studiorum Università di Bologna\' ...
    'FATA project\Regional segmentation\Contraction parameters\Left atrium_contraction\Cardiac cycle implementation']);
stl_files = dir(fullfile(file_path,'*.stl'));
for lo = 1:length(stl_files)                                          % To load all stl stl_files
    import_stl = stlread(fullfile(file_path,stl_files(lo).name));
    figure('WindowState','maximized'),trisurf(import_stl, ...
        'FaceColor','flat', 'LineStyle', 'none','FaceAlpha',1); axis equal
    xlabel('X-axis'),ylabel('Y-axis'),zlabel('Z-axis')
    x_tick = xticks; y_tick = yticks; z_tick = zticks;
    %% Findout openings
    [F,P]= freeBoundary(import_stl);
    %% Separate out each openings
    i=1; j=1; k=1;
    while (i<length(F))
        [row col] = find(F==i,1,'last');
        ID(j) = row;                    % closing edges for each opening
        total(k) = (row-i)+1;           % Total no. of edges in each opening
        i = row+1;    
        j= j+1; k=k+1;
    end
    clear i j k row col
    
    OP = zeros(max(total), length(ID)); % To store edges of each opening
    dx = 1;
    for ip = 1 : length(ID)
        OP([1:total(ip)],ip) = [dx : ID(ip)]';      % Each column corresponds one opening
        dx = ID(ip) + 1;
    end
    clear dx ip k
    %% Delete smallest opening if number of openings are more than 6
    if length(total) > 6
        nd = length(total) - 6;
        [~, is] = mink(total,nd);
        total(is) = [];
        ID(is) = [];
        OP(:,is) = [];
    end
    clear nd nnd is
    %% Identify mitral valve
    if size(OP,2) == 6
        for ip = 1 : size(OP,2)
            k(ip) = max(find(OP(:,ip)));        % Excluding trunk, MV should have maximum edges
        end
        
        [~,kk] = max(k);
        [~,~,MV_edges] = find(OP(:,kk));              % Used 'find' to exclude zeros
        OP(:,kk) = [];
        MV_points = P(MV_edges,:);              % Coordinates of mitral valve opening
        clear ip kk k
    else
        % Compute each barycenter
        for ip = 1 : size(OP,2)
            [~,~,k] = find(OP(:,ip));
            ct(ip,:) = mean(P(k,:));
            clear k 
        end
        clear ip 
        
        % Find two biggest openings
        for ip = 1 : size(OP,2)
            k(ip) = max(find(OP(:,ip)));       
        end
        [~,kk] = maxk(k,2);
        clear k
    
        % Find distance of two barycenter with all other barycenters
        [q] = pdist2(ct, ct(kk,:));
        
        % Calculate the mean of the distances
        for ip = 1 : size(q,2)
            md(ip) = mean(q(:,ip));
        end
        
        % MV should have the highest distance
        [~,id] = max(md);
        aa = kk(id);
        [~,~,MV_edges] = find(OP(:,aa));              % Used 'find' to exclude zeros
        OP(:,aa) = [];
        MV_points = P(MV_edges,:);              % Coordinates of mitral valve opening
        clear ct kk q md aa id
    end
    %% Highlight mitral valve
    hold on, trisurf(F(MV_edges,:),P(:,1),P(:,2),P(:,3),'EdgeColor','black','LineWidth',2.5), hold off  %Highlight MV
    xlim([x_tick(1)-15 x_tick(end)+15]), ylim([y_tick(1)-25 y_tick(end)+25]), zlim([z_tick(1)-15 z_tick(end)+15])
    %% Calculate barycenter of mitral valve
    mv = mean(MV_points);
    mX = mv(1); mY = mv(2); mZ = mv(3);
    hold on, plot3(mX,mY,mZ,'*k','LineWidth',2), hold off
    %% Calculate barycenter of other openings
    for ip = 1 : size(OP,2)
        [~,~,k] = find(OP(:,ip));
        ct(ip,:) = mean(P(k,:));
        clear k 
    end
    X = ct(:,1)'; Y = ct(:,2)'; Z = ct(:,3)';
    clear ct ip
    %% Delete coordinates of barycenter of mitral valve from X,Y,Z
    [~,inx] = find(X==mX);
    X(inx) = []; Y(inx) = []; Z(inx) = [];
    clear inx
    %% Identify LAA barycenter
    for ii = 1:length(X)
        dif = mv - [X(ii) Y(ii) Z(ii)];
        sq = dif.*dif;
        ss = sum(sq,2);
        dist(ii) = sqrt(ss);
    end
    [~,I] = mink(dist,2);
    t_LAA = [X(I(1)) Y(I(1)) Z(I(1));
        X(I(2)) Y(I(2)) Z(I(2))];
    clear ii dif sq ss dist I
    %% Best fitting plane for MV
    [Ox,Oy,Oz]=plane_fit(MV_points(:,1),MV_points(:,2),MV_points(:,3));              % Draw best fitting plane for MV
    hold on, surf(Ox,Oy,Oz,'FaceColor','r','LineStyle', 'none', 'FaceAlpha',0), hold off
    %% Normal to the plane
    V1 = ([Ox(1), Oy(1), Oz(1)] - mv)/norm([Ox(1), Oy(1), Oz(1)] - mv);
    V2 = ([Ox(end), Oy(end), Oz(end)] - mv)/norm([Ox(end), Oy(end), Oz(end)] - mv);
    N_mv = cross(V1,V2)/norm(cross(V1,V2));
    % hold on, quiver3(mv(1), mv(2), mv(3), N_mv(1), N_mv(2), N_mv(3), 4), hold off
    clear V1 V2 Ox Oy Oz
    %% Shortest distance from MV plane to the openings
    for ii = 1:size(t_LAA,1)
        P1 = t_LAA(ii,:) - mv;
        D = dot(P1,N_mv);
        dist(ii) = abs(D);
    end
    [~,I] = min(dist);
    LAA = t_LAA(I,:);
    [~,II] = intersect(X,LAA(1));
    X(II) = []; Y(II) = []; Z(II) = [];
    hold on, plot3(LAA(1),LAA(2),LAA(3),'*g','LineWidth',2), hold off
    clear ii P1 D dist I t_LAA II 
    %% Identify LAA ostium coordinates
    for i = 1 : size(OP,2)
        [~,~,v] = find(OP(:,i));
        bb = mean(P(v,:));
        if bb == LAA
            LAA_vert =P(v,:);
            LAA_edges =F(v,:);
            OP(:,i) = [];
            break
        end
    end
    clear i v bb
    hold on, trisurf(LAA_edges,P(:,1),P(:,2),P(:,3),'EdgeColor','green','LineWidth',2.5), hold off  %Highlight MV
    %% Plot barycenters
    hold on, plot3(X,Y,Z,'*b','LineWidth',2), hold off
    clear n nc m
    %% Calculate barycenter of atria using four PVs
    % bx = mean(X);by = mean(Y); bz = mean(Z);
    % hold on, plot3(bx,by,bz,'*r','Markersize',12), hold off
    %% Check trunk
    if size(OP,2)<4
        trunk = 1;
    
        for ip = 1 : size(OP,2)
            [~,~,q] = find(OP(:,ip));
            ed = F(q,:);
            ed = unique(ed(:));
            mid(ip,:) = mean(P(ed,:));
        end
        clear ip q ed
    
        % Find distance between LAA and 03 barycenters
        for ip = 1 : size(OP,2)
            d(ip) = norm(mid(ip,:) - LAA);
        end
        [~,id] = min(d);             % Closest to LAA is trunk
        TC = mid(id,:);             % Barycenter of trunk
        clear d
    
        % Separate trunk
        [~,~,ck] = find(OP(:,id));
        trunk_points = P(ck,:);
        hold on, trisurf(F(ck(1):ck(end),:),P(:,1),P(:,2),P(:,3), ...
            'EdgeColor','r','LineWidth',2.5), hold off  % Highlight trunk
        clear ip kk k
    
        LSPV_points = trunk_points; LSPV_ID = F(ck,:);
        LIPV_points = trunk_points; LIPV_ID = F(ck,:);
    
        % Delete trunk barycenter
        it = find(X==TC(1));
        X(it) = []; Y(it) = []; Z(it) = [];
    
        % Compute end-points of major axis
        [q] = pdist2(trunk_points, trunk_points);
        ind = max(q(:));
        [r,c] = find(q==ind, 1);
        hold on, plot3(trunk_points([r,c],1), trunk_points([r,c],2), trunk_points([r,c],3), ...
            'b*', 'LineWidth',2.5), hold off
        
        % Add end-points of major axis
        X = [X, trunk_points([r,c],1)'];
        Y = [Y, trunk_points([r,c],2)'];
        Z = [Z, trunk_points([r,c],3)'];
    else
        trunk = 0;
    end
    clear c ck ind it q r id mid
    %% Automatic identification of LSPV
    % Best fitting plane for LAA
    [LAAx,LAAy,LAAz]=plane_fit(LAA_vert(:,1),LAA_vert(:,2),LAA_vert(:,3));              % Draw best fitting plane for LAA
    hold on, surf(LAAx,LAAy,LAAz,'FaceColor','r','LineStyle', 'none', 'FaceAlpha',0), hold off
    
    % Normal to the plane
    V1 = ([LAAx(1), LAAy(1), LAAz(1)] - LAA)/norm([LAAx(1), LAAy(1), LAAz(1)] - LAA);
    V2 = ([LAAx(end), LAAy(end), LAAz(end)] - LAA)/norm([LAAx(end), LAAy(end), LAAz(end)] - LAA);
    N = cross(V1,V2)/norm(cross(V1,V2));
    % hold on, quiver3(mv(1), mv(2), mv(3), N(1), N(2), N(3), 4), hold off
    clear V1 V2 LAAx LAAy LAAz
    
    % Find two PVs which are closest to LAA plane
    all_PVs = [X', Y', Z'];
    for ii = 1:size(all_PVs,1)
        P1 = all_PVs(ii,:) - LAA;
        D = dot(P1,N);
        dist(ii) = abs(D);
    end
    [~,I] = mink(dist,2);
    two_PVs = all_PVs(I,:);
    clear ii P1 D dist I N all_PVs
    %%
    z_2PVs = two_PVs(:,end);
    [~,II] = max(z_2PVs);               % Among these two points, the point having highest z-value is LSPV
    LSPV_Z = two_PVs(II,end);
    
    [~,ind] = intersect(Z,LSPV_Z);
    % X(II) = []; Y(II) = []; Z(II) = [];
    LSPV(1) = X(ind); LSPV(2) = Y(ind); LSPV(3) = Z(ind);
    % hold on, plot3(LSPV(1),LSPV(2),LSPV(3),'*r','LineWidth',2), hold off
    clear t_LAA II  z_2PVs LSPV_Z ind
    %% Identifing the barycenters of PVs
    X1 = X; Y1 = Y; Z1 = Z;
    % d = datacursormode;
    % disp("Select LSPV and press enter:")
    % pause
    % xyz = getfield(getCursorInfo(d),'Position');
    % clc
    % 
    % LSPV = xyz;                % Visually select LSPV
    % clear d xyz
    
    [~,jj] = find(X==LSPV(1));
    X1(jj) = 0; Y1(jj) = 0; Z1(jj) = 0;
%     k = [X1' Y1' Z1'];
%     nn = dsearchn(k,LSPV);                  % Location of closest barycenter to LSPV
%     LIPV = [X(nn) Y(nn) Z(nn)];                % LIPV
    %
    [~, ng] = intersect(two_PVs, LSPV, "rows");
    two_PVs(ng,:) = [];
    LIPV = two_PVs; 
    [~,nn] = find(X==LIPV(1));
    clear two_PVs
    %% Check: Connect each point with LAA and MV, then evaluate area of trianagle
    two_PVs = [LSPV;LIPV];
    for i = 1 : size(two_PVs,1)
        A1 = (mv - two_PVs(i,:))/norm(mv - two_PVs(i,:));
        A2 = (LAA - two_PVs(i,:))/norm(LAA - two_PVs(i,:));
        NN = cross(A1,A2);
        tri_area(i) = (norm(NN))/2;
        clear A1 A2 NN
    end
    [~,II] = min(tri_area);
    LSPV = two_PVs(II,:);
    two_PVs(II,:) = [];
    LIPV = two_PVs;
    hold on, plot3(LSPV(1),LSPV(2),LSPV(3),'*r','LineWidth',2), hold off
    clear i A1 A2 NN tri_area II two_PVs
    %%
    if LSPV(3)>LIPV(3)
        Z1(jj) = nan; Z1(nn) = nan;
        [~,iz] = mink(Z1,2);
        RIPV = [X(iz(1)) Y(iz(1)) Z(iz(1))];                % RIPV
        RSPV = [X(iz(2)) Y(iz(2)) Z(iz(2))];                % RSPV
    else
        Z1(jj) = nan; Z1(nn) = nan;
        [~,iz] = mink(Z1,2);
        RSPV = [X(iz(1)) Y(iz(1)) Z(iz(1))];                % RSPV
        RIPV = [X(iz(2)) Y(iz(2)) Z(iz(2))];                % RIPV
    end
    hold on, plot3(LIPV(1),LIPV(2),LIPV(3),'*g','LineWidth',2)
    plot3(RSPV(1),RSPV(2),RSPV(3),'*b','LineWidth',2)
    plot3(RIPV(1),RIPV(2),RIPV(3),'*y','LineWidth',2), hold off
    clear X1 Y1 Z1 jj nn iz k
    %% Identifing the vertices of each pulmonary vein
    % LSPV
    for i = 1 : size(OP,2)
        [~,~,v] = find(OP(:,i));
        bb = mean(P(v,:));
        if bb == LSPV
            LSPV_points = P(v,:);
            LSPV_ID = F(v,:);
        end
    end
    clear i v bb
    hold on, trisurf(LSPV_ID,P(:,1),P(:,2),P(:,3),'EdgeColor','blue','LineWidth',2.5), hold off  %Highlight LSPV
    
    % LIPV
    for i = 1 : size(OP,2)
        [~,~,v] = find(OP(:,i));
        bb = mean(P(v,:));
        if bb == LIPV
            LIPV_points = P(v,:);
            LIPV_ID = F(v,:);
        end
    end
    clear i v bb
    hold on, trisurf(LIPV_ID,P(:,1),P(:,2),P(:,3),'EdgeColor','blue','LineWidth',2.5), hold off  %Highlight LIPV
    
    % RSPV
    for i = 1 : size(OP,2)
        [~,~,v] = find(OP(:,i));
        bb = mean(P(v,:));
        if bb == RSPV
            RSPV_points = P(v,:);
            RSPV_ID = F(v,:);
        end
    end
    clear i v bb
    hold on, trisurf(RSPV_ID,P(:,1),P(:,2),P(:,3),'EdgeColor','blue','LineWidth',2.5), hold off  %Highlight RSPV
    
    % RIPV
    for i = 1 : size(OP,2)
        [~,~,v] = find(OP(:,i));
        bb = mean(P(v,:));
        if bb == RIPV
            RIPV_points = P(v,:);
            RIPV_ID = F(v,:);
        end
    end
    clear i v bb
    hold on, trisurf(RIPV_ID,P(:,1),P(:,2),P(:,3),'EdgeColor','blue','LineWidth',2.5), hold off  %Highlight RIPV
    
    %% Area of PVs
    PV1 = delaunay(P(LSPV_ID,1),P(LSPV_ID,3));
    PV2 = delaunay(P(LIPV_ID,1),P(LIPV_ID,3));
    PV3 = delaunay(P(RSPV_ID,1),P(RSPV_ID,3));
    PV4 = delaunay(P(RIPV_ID,1),P(RIPV_ID,3));
    
    P_1 = [P(LSPV_ID,1),P(LSPV_ID,2),P(LSPV_ID,3)];
    P_2 = [P(LIPV_ID,1),P(LIPV_ID,2),P(LIPV_ID,3)];
    P_3 = [P(RSPV_ID,1),P(RSPV_ID,2),P(RSPV_ID,3)];
    P_4 = [P(RIPV_ID,1),P(RIPV_ID,2),P(RIPV_ID,3)];
    
    v1_1 = P_1(PV1(:,2),:) - P_1(PV1(:,1),:);
    v2_1 = P_1(PV1(:,3),:) - P_1(PV1(:,2),:);
    v1_2 = P_2(PV2(:,2),:) - P_2(PV2(:,1),:);
    v2_2 = P_2(PV2(:,3),:) - P_2(PV2(:,2),:);
    v1_3 = P_3(PV3(:,2),:) - P_3(PV3(:,1),:);
    v2_3 = P_3(PV3(:,3),:) - P_3(PV3(:,2),:);
    v1_4 = P_4(PV4(:,2),:) - P_4(PV4(:,1),:);
    v2_4 = P_4(PV4(:,3),:) - P_4(PV4(:,2),:);
    
    cp_1 = 0.5*cross(v1_1,v2_1);
    cp_2 = 0.5*cross(v1_2,v2_2);
    cp_3 = 0.5*cross(v1_3,v2_3);
    cp_4 = 0.5*cross(v1_4,v2_4);
    
    area1 = sum(sqrt(dot(cp_1, cp_1, 2)));          % area of LSPV
    area2 = sum(sqrt(dot(cp_2, cp_2, 2)));          % area of LIPV
    area3 = sum(sqrt(dot(cp_3, cp_3, 2)));          % area of RSPV
    area4 = sum(sqrt(dot(cp_4, cp_4, 2)));          % area of RIPV
    
    clear PV1 PV2 PV3 PV4 P_1 P_2 P_3 P_4 
    clear cp_1 cp_2 cp_3 cp_4 v1_1 v2_1 v1_2 v2_2 v1_3 v2_3 v1_4 v2_4
    
    %% Weighted barycenter of Atria
    if trunk == 1
        % In case of trunk
        area1 = 0; area2 = 0;
        PV1 = delaunay(P(LSPV_ID,1),P(LSPV_ID,3));
        P_1 = [P(LSPV_ID,1),P(LSPV_ID,2),P(LSPV_ID,3)];
        v1_1 = P_1(PV1(:,2),:) - P_1(PV1(:,1),:);
        v2_1 = P_1(PV1(:,3),:) - P_1(PV1(:,2),:);
        cp_1 = 0.5*cross(v1_1,v2_1);
        area1 = sum(sqrt(dot(cp_1, cp_1, 2)));          % area of LSPV
        clear PV1 cp_1  v1_1 v2_1 
    
        mean_bar = (TC*area1 + LIPV*area2 + RSPV*area3 + RIPV*area4)/(area1 + area2 +area3 + area4);
    else
        % Without trunk
        mean_bar = (LSPV*area1 + LIPV*area2 + RSPV*area3 + RIPV*area4)/(area1 + area2 +area3 + area4);
    end
    clear area1 area2 area3 area4 PV1 P_1 V1_1 V2_1 cp_1
    %% Project weighted barycenter on atrium
    NN = nearestNeighbor(import_stl,mean_bar);
    XX = [import_stl.Points(NN,1);mX]; YY = [import_stl.Points(NN,2);mY]; ZZ = [import_stl.Points(NN,3);mZ];
    hold on, plot3(import_stl.Points(NN,1),import_stl.Points(NN,2),import_stl.Points(NN,3),'*g','LineWidth',2)
    plot3(XX,YY,ZZ,'--r','LineWidth',2), hold off 
    wb = [import_stl.Points(NN,1) import_stl.Points(NN,2) import_stl.Points(NN,3)];
    vv = wb - mv  ;                             % Vector from MV to weighted barycenter(on the surface)
    mp = (mv+wb)/2;                             % midpoint of long axis
    clear NN XX YY ZZ
    %% LA surface
    surface = []; surface.faces = import_stl.ConnectivityList; surface.vertices = import_stl.Points;
    %% Li to Ls
    if trunk == 0
        tt1 = tri_plane(LIPV, LSPV, mp);
        surface1 = []; surface1.faces = tt1.ConnectivityList; surface1.vertices = tt1.Points;
        [intm1, intSurface1] = SurfaceIntersection(surface, surface1);
        clear tri1 tt1 surface1 intSurface1
    end
    %% Ls to Rs
    tt2 = tri_plane(LSPV, RSPV, mp);
    surface2 = []; surface2.faces = tt2.ConnectivityList; surface2.vertices = tt2.Points;
    [intm2, intSurface2] = SurfaceIntersection(surface, surface2);
    % surf(x_LsRs,y_LsRs,z_LsRs)
    clear tri2 tt2 surface2 intSurface2
    %% Rs to Ri
    tt3 = tri_plane(RSPV, RIPV, mp);
    surface3 = []; surface3.faces = tt3.ConnectivityList; surface3.vertices = tt3.Points;
    [intm3, intSurface3] = SurfaceIntersection(surface, surface3);
    % surf(x_RsRi,y_RsRi,z_RsRi)
    clear tri3 tt3 surface3 intSurface3
    clear x_LiLs y_LiLs z_LiLs x_LsRs y_LsRs z_LsRs x_RsRi y_RsRi z_RsRi
    %% Defining triangles of each opening
    LSPV_tri = vert2triangle(LSPV_points,surface);              % LSPV
    LIPV_tri = vert2triangle(LIPV_points,surface);              % LIPV
    RSPV_tri = vert2triangle(RSPV_points,surface);              % RSPV
    RIPV_tri = vert2triangle(RIPV_points,surface);              % RIPV
    MV_tri = vert2triangle(MV_points,surface);                  % MV
    LAA_tri = vert2triangle(LAA_vert,surface);                  % LAA
    %% Deleting unwanted connections
    % LI to LS
    if trunk == 0
        [rr,~] = find(intm1==1);                               % original IDs of faces
    %     sets = separate_regs(surface.faces(rr,:),surface.vertices);
    %     new_IDs = correct_connections(LSPV,LIPV,sets);
    %     ID_LiLs = rr(new_IDs,:);
        ID_LiLs = rr;
        clear rr sets new_IDs intm1
        hold on, trisurf(surface.faces(ID_LiLs,:), surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'EdgeColor', 'r', 'LineW', 2), hold off
    else 
        ID_LiLs = [];
    end
    
    % LS to RS
    [rr,~] = find(intm2==1);                               % original IDs of faces
    % sets = separate_regs(surface.faces(rr,:),surface.vertices);
    % new_IDs = correct_connections(LSPV,RSPV,sets);
    % ID_LsRs = rr(new_IDs,:);
    ID_LsRs = rr;
    clear rr sets new_IDs intm2
    % hold on, trisurf(surface.faces(ID_LsRs,:), surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
    %     'EdgeColor', 'r', 'LineW', 2), hold off
    
    % RS to RI 
    [rr,~] = find(intm3==1);                               % original IDs of faces
    % sets = separate_regs(surface.faces(rr,:),surface.vertices);
    % new_IDs = correct_connections(RSPV,RIPV,sets);
    % ID_RsRi = rr(new_IDs,:);
    ID_RsRi = rr;
    clear rr sets new_IDs intm3
    hold on, trisurf(surface.faces(ID_RsRi,:), surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'EdgeColor', 'r', 'LineW', 2), hold off
    %% Arrange vertices of RSPV
    P_RS = P(RSPV_ID,:);
    dv = bsxfun(@minus,P_RS,RSPV);
    a = atan2(dv(:,2),dv(:,1)) + pi;
    [~,s] = sort(a,'ascend');
    P_RS = P_RS(s,:);
    clear dv a s
    
    % Finding first end point of LsRs
    LsRs_1 = triangles2vertex(ID_LsRs, RSPV, RSPV_points, RSPV_tri, surface);
    hold on, plot3(LsRs_1(1),LsRs_1(2),LsRs_1(3),'*k'), hold off
    
    RsRi_1 = triangles2vertex(ID_RsRi, RSPV, RSPV_points, RSPV_tri, surface);
    hold on, plot3(RsRi_1(1),RsRi_1(2),RsRi_1(3),'*k'), hold off
    
    RsRi_2 = triangles2vertex(ID_RsRi, RIPV, RIPV_points, RIPV_tri, surface);
    hold on, plot3(RsRi_2(1),RsRi_2(2),RsRi_2(3),'*k'), hold off
    
    % Draw an extended line to connect RSPV % point on RSPV
    uv = LsRs_1 - RSPV;                                          % vector
    pext1 = RSPV + uv*4.7;                                    % extension
    hold on, pp1 = plot3([RSPV(1),pext1(1)],[RSPV(2),pext1(2)],[RSPV(3),pext1(3)],'color','none'); hold off
    clear uv
    
    % 90 degrees rotation
    aa = (P_RS(1,:) - RSPV)/norm(P_RS(1,:) - RSPV);
    bb = (P_RS(end,:) - RSPV)/norm(P_RS(end,:) - RSPV);
    normal = cross(aa,bb);
    normal = normal/norm(normal);
    
    theta = 90;
    rotate(pp1,normal,theta,RSPV)
    pt1_x = pp1.XData(2);     pt1_y = pp1.YData(2);     pt1_z = pp1.ZData(2);
    % plot3([RSPV(1),pt1_x],[RSPV(2),pt1_y],[RSPV(3),pt1_z],'LineW',2,'color','y');
    pt1 = [pt1_x,pt1_y,pt1_z];
    
    hold on, pp1 = plot3([RSPV(1),pext1(1)],[RSPV(2),pext1(2)],[RSPV(3),pext1(3)],'color','none'); hold off
    theta = -90;
    rotate(pp1,normal,theta,RSPV)
    pt1_x = pp1.XData(2);     pt1_y = pp1.YData(2);     pt1_z = pp1.ZData(2);
    % plot3([RSPV(1),pt1_x],[RSPV(2),pt1_y],[RSPV(3),pt1_z],'LineW',2,'color','b');
    pt1_1 = [pt1_x,pt1_y,pt1_z];
    clear aa bb normal theta pext1 pp1
    
    df_1 = norm(pt1 - RIPV);
    df_2 = norm(pt1_1 - RIPV);
    
    va = max([df_1 df_2]);
    
    if va == df_1
        pt1 = pt1;
    else
        pt1 = pt1_1;
    end
    
    clear df_1 df_2 va pt1_1 pt1_x pt1_y pt1_z
    
    [ID_RsPts intRs] = line2triangles(RSPV,pt1,mp,surface);
    
    % Finding point on RSPV
    MvRs_1 = triangles2vertex(ID_RsPts, RSPV, RSPV_points, RSPV_tri, surface);
    hold on, plot3(MvRs_1(1),MvRs_1(2),MvRs_1(3),'*k','LineWidth',2), hold off
    clear intRs pt1
    %% Draw line from MvRs_1 to mv
    % tt5 = tri_plane(MvRs_1, mv, mp);
    mid3 = (MvRs_1+mv)/2; e1 = MvRs_1 - mid3; e2 = mv - mid3;
    po1 = MvRs_1 + e1*2; po2 = mv + e2*2;
    tt5 = tri_plane(po1,po2,mp);
    % hold on, trisurf(tt5, 'FaceColor', 'b'), hold off
    clear mid3 po1 po2 e1 e2
    
    % Making faces and triangles of planes
    surface5 = []; surface5.faces = tt5.ConnectivityList; surface5.vertices = tt5.Points;
    [intm5, intSurface5] = SurfaceIntersection(surface, surface5);
    clear tri5 tt5 surface5 intSurface5 x_MvRs y_MvRs z_MvRs
    
    % Deleting unwanted connections
    [rr,~] = find(intm5==1);                               % original IDs of faces
    sets = separate_regs(surface.faces(rr,:),surface.vertices);
%     new_IDs = correct_connections(MvRs_1,mv,sets);
%     ID_MvRs = rr(new_IDs,:);
    ID_MvRs = correct_triangles(mv,MvRs_1,rr,sets);
    clear rr sets new_IDs intm5
    
    % Plotting connecting triangles from RSPV to MV
    hold on, trisurf(surface.faces(ID_MvRs,:), surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'EdgeColor', 'r', 'LineW', 2),hold off
    %% Arrange vertices of LSPV
    if trunk == 0
        P_LS = P(LSPV_ID,:);
        dv = bsxfun(@minus,P_LS,LSPV);
        a = atan2(dv(:,2),dv(:,1)) + pi;
        [~,s] = sort(a, 'ascend');
        P_LS = P_LS(s,:);
        clear dv a s 
        
        % Finding second end point of LsRs
        LsRs_2 = triangles2vertex(ID_LsRs, LSPV, LSPV_points, LSPV_tri, surface);
        hold on, plot3(LsRs_2(1),LsRs_2(2),LsRs_2(3),'*k'), hold off
        
        LsLi_1 = triangles2vertex(ID_LiLs, LSPV, LSPV_points, LSPV_tri, surface);
        hold on, plot3(LsLi_1(1),LsLi_1(2),LsLi_1(3),'*b'), hold off
        
        LsLi_2 = triangles2vertex(ID_LiLs, LIPV, LIPV_points, LIPV_tri, surface);
        hold on, plot3(LsLi_2(1),LsLi_2(2),LsLi_2(3),'*b'), hold off
        
        % Draw an extended line to connect LSPV & point on LSPV
        uv = LsRs_2 - LSPV;                                          % vector
        pext2 = LSPV + uv*2.7;                                    % extension
        hold on, pp2 = plot3([LSPV(1),pext2(1)],[LSPV(2),pext2(2)],[LSPV(3),pext2(3)],'color','none'); hold off
        clear uv
        
        % 90 degrees rotation
        aa = (P_LS(1,:) - LSPV)/norm(P_LS(1,:) - LSPV);
        bb = (P_LS(end,:) - LSPV)/norm(P_LS(end,:) - LSPV);
        normal = cross(aa,bb);
        normal = normal/norm(normal);
        
        theta = -90;
        rotate(pp2,normal,theta,LSPV)
        pt2_x = pp2.XData(2);     pt2_y = pp2.YData(2);     pt2_z = pp2.ZData(2);
        % plot3([LSPV(1),pt2_x],[LSPV(2),pt2_y],[LSPV(3),pt2_z],'LineW',2,'color','r');
        pt2 = [pt2_x,pt2_y,pt2_z];
        
        hold on, pp2 = plot3([LSPV(1),pext2(1)],[LSPV(2),pext2(2)],[LSPV(3),pext2(3)],'color','none'); hold off
        theta = 90;
        rotate(pp2,normal,theta,LSPV)
        pt2_x = pp2.XData(2);     pt2_y = pp2.YData(2);     pt2_z = pp2.ZData(2);
        % plot3([LSPV(1),pt2_x],[LSPV(2),pt2_y],[LSPV(3),pt2_z],'LineW',2,'color','g');
        pt2_1 = [pt2_x,pt2_y,pt2_z];
        clear aa bb normal theta pext2 pp2
    
        
        df_1 = norm(pt2 - LIPV);
        df_2 = norm(pt2_1 - LIPV);
        va = max([df_1 df_2]);
        if va == df_1
            pt2 = pt2;
        else
            pt2 = pt2_1;
        end
        clear df_1 df_2 va pt2_1 pt2_x pt2_y pt2_z
        
        [ID_LsPts intLsMv] = line2triangles(LSPV,pt2,mp,surface);
        
        % Finding exact point on LSPV
        MvLs_1 = triangles2vertex(ID_LsPts, LSPV, LSPV_points, LSPV_tri, surface);
        clear ID_LsPts intLsMv MvLs_ID
    else
        MvLs_1 = LSPV;
        MvLs_1 = LSPV;
        LsLi_1 = []; LsLi_2 = [];
    end
    hold on, plot3(MvLs_1(1),MvLs_1(2),MvLs_1(3),'*k','LineWidth',2), hold off
    %% Drawing intersecting triangles for LsRs line (roof line)
    global G
    E = edges(import_stl);                              % Edges of atria
    G = graph(E(:,1),E(:,2));                           % Defining graph object using edges
    [~,ia,~] = intersect(surface.vertices,MvLs_1,'rows'); % Finding ID
    [~,ib,~] = intersect(surface.vertices,MvRs_1,'rows'); % Finding ID
    q1(1)= ia; q2(1)=ib;
    mesh = import_stl;
    
    % Creating graph with edges
    PP = shortestpath(G,q1(1),q2(1));
    rows = cell(1,length(PP));
    for i = 1:length(PP)
        [rows{1,i},~] = find(surface.faces == PP(i));
    end
    rows = unique(cell2mat(rows(:)));
    roof_faces = surface.faces(rows,:);
    roof_vertices = surface.vertices(unique(roof_faces),:);
    % Define a single line of faces
    q = unique(roof_faces);
    x = find(ismember(q,PP) == 0);
    V = vertexAttachments(mesh,q(x));
    V = unique(cell2mat(V'));
    face = surface.faces(V,:);
    Q = find(ismember(face,roof_faces,'rows') == 0);
    Out = share_nodes(face(Q,:),surface.vertices);
    [~,r] = max(arrayfun(@(x) size(Out(x).faces,1),1:numel(Out)));
    W = find(ismember(surface.vertices(q(x),:),Out(r).vertices,'rows') == 1);
    X = vertexAttachments(mesh,q(x(W)));
    X = unique(cell2mat(X'));
    face = surface.faces(X,:);
    M = find(ismember(face,roof_faces,'rows') == 1);
    roof_faces = face(M,:);
    ID_roof = find(ismember(surface.faces,roof_faces,'rows') == 1);
    clear k1 k2 q1 q2 PP rows q x V face Q Out r W X face M
    % PPP = shortestpath(G,ia,ib);                        % Finding shortest path using graph
    % 
    % LsRs_tri = vertexAttachments(import_stl,PPP');      % IDs of triangels of shortest path
    % j = cellfun('size',LsRs_tri,2);
    % j = max(j);
    % LsRs_mat = zeros(j,size(LsRs_tri,1));
    % for i = 1:size(LsRs_tri,1)
    %     jj = size(LsRs_tri{i,1},2);
    %     LsRs_mat(1:jj,i) = LsRs_tri{i,1}';
    %     i = i+1;
    % end
    % ID_roof = unique(LsRs_mat(:));
    % ID_roof(ID_roof <= 0) = [];
    hold on, trisurf(surface.faces(ID_roof,:),surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'EdgeColor','green','LineWidth',1.5), hold off
    clear ia ib i j jj PPP LsRs_mat
    %% Find first point on the boundary of Mitral valve coming from MvRS
    % set MV in a clockwise direction
    P_MV = P(MV_edges,:);
    dv = bsxfun(@minus,P_MV,mv);
    a = atan2(dv(:,2),dv(:,1)) + pi;
    [~,s] = sort(a);
    P_MV = P_MV(s,:);
    MV_vertices = P_MV;
    clear dv a s
    
    MvRs_2 = triangles2vertex(ID_MvRs, mv, MV_points, MV_tri, surface);
    hold on, plot3(MvRs_2(1),MvRs_2(2),MvRs_2(3),'*k','LineWidth',2), hold off
    
    %% Generate points at [90,90,90] angles
    uv = MvRs_2 - mv;                                       % vector
    pext1 = mv + uv*5;                                    % extension
    hold on, pp = plot3([mv(1),pext1(1)],[mv(2),pext1(2)],[mv(3),pext1(3)],'color','k','LineWidth',2); hold off
    clear uv pext1
    %%
    % Finding exact point on MV
    % aa = (P_MV(1,:) - mv)/norm(P_MV(1,:) - mv);% bb = (P_MV(2,:) - mv)/norm(P_MV(2,:) - mv);
    [ID_rand ~] = line2triangles(mv,LSPV,mp,surface);
    rand_pt = triangles2vertex(ID_rand, mv, MV_points, MV_tri, surface);
    % hold on, plot3(rand_pt(1),rand_pt(2),rand_pt(3),'*k','LineWidth',2), hold off
    
    aa = (MvRs_2 - mv)/norm(MvRs_2 - mv);
    bb = (rand_pt - mv)/norm(rand_pt - mv);
    normal = cross(aa,bb);
    % normal = (mv - wb)/norm(mv - wb);
    normal = normal/norm(normal);
    theta = [90 90 90];
    % hold on, quiver3(mv(1),mv(2),mv(3),normal(1),normal(2),normal(3),30), hold off
    for j = 1:length(theta);
        rotate(pp,normal,theta(j),mv)
        pt_x(j) = pp.XData(2);     pt_y(j) = pp.YData(2);     pt_z(j) = pp.ZData(2);
    end
    
    pts = [pt_x' pt_y' pt_z'];
    clear aa bb cc normal theta j pt_x pt_y pt_z pp
      
    [ID_MvP1 intSurface6] = line2triangles(mv,pts(1,:),mp,surface);
    [ID_MvP2 intSurface7] = line2triangles(mv,pts(2,:),mp,surface);
    [ID_MvP3 intSurface8] = line2triangles(mv,pts(3,:),mp,surface);
    
    MvP1 = triangles2vertex(ID_MvP1, mv, MV_points, MV_tri, surface);
    MvP2 = triangles2vertex(ID_MvP2, mv, MV_points, MV_tri, surface);
    MvP3 = triangles2vertex(ID_MvP3, mv, MV_points, MV_tri, surface);
    
    clear pts ID_MvP1 intSurface6 ID_MvP2 intSurface7 ID_MvP3 intSurface8 MvP1_ID MvP2_ID MvP3_ID
    
    hold on  
    plot3(MvP1(1),MvP1(2),MvP1(3),'*k','LineWidth',2)
    plot3(MvP2(1),MvP2(2),MvP2(3),'*k','LineWidth',2)
    plot3(MvP3(1),MvP3(2),MvP3(3),'*k','LineWidth',2)
    hold off
    %% Connect MvP1 to LSPV 
    % Draw plane from MvP1 to LSPV
    mid3 = (MvP1+MvLs_1)/2; e1 = MvP1 - mid3; e2 = MvLs_1 - mid3;
    po1 = MvP1 + e1*.7; po2 = MvLs_1 + e2*.7;
    tt6 = tri_plane(po1,po2,mp);
    % tt6 = tri_plane(MvP1,MvLs_1,mp);
    % hold on,trisurf(tt6), hold off
    
    % Making faces and triangles of planes
    surface6 = []; surface6.faces = tt6.ConnectivityList; surface6.vertices = tt6.Points;
    [intm6, ~] = SurfaceIntersection(surface, surface6);
    clear  tri6 tt6 surface6 intSurface6_1
    
    % Deleting unwanted connections
    [rr,~] = find(intm6==1);                               % original IDs of faces
    sets = separate_regs(surface.faces(rr,:),surface.vertices);
    % ID_MvLs = correct_connectionsLAA(MvP1,MvLs_1,rr,sets);
    ID_MvLs = LAA_connections_improved(MvP1,MvLs_1,rr,sets, LAA_vert);
    
    hold on, trisurf(surface.faces(ID_MvLs,:), surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'EdgeColor', 'r', 'LineW', 2), hold off
    clear rr intm6 sets
    
    %% LAA points
    fc = surface.faces(ID_MvLs,:);
    fc = unique(fc(:));
    vert_MvLs = surface.vertices(fc,:);
    [vert_MvLs, ia, ib] = intersect(LAA_vert, vert_MvLs,"rows");
    clear fc 
    
    if isempty(vert_MvLs) 
        LaaPt1 = []; LaaPt2 = [];
    else
        dist2 = pdist2(vert_MvLs, vert_MvLs);
        if dist2 == 0
            LaaPt1 = []; LaaPt2 = [];
        else
            v = max(dist2);
            ind = max(v);
            [r,c] = find(dist2==ind);
            hold on, plot3(vert_MvLs(r,1), vert_MvLs(r,2), vert_MvLs(r,3), ...
                'b*', 'LineWidth',2.5), hold off
            LaaPt1 = [vert_MvLs(r(1),1), vert_MvLs(r(1),2), vert_MvLs(r(1),3)]; 
            LaaPt2 = [vert_MvLs(r(2),1), vert_MvLs(r(2),2), vert_MvLs(r(2),3)];
            clear r c ind dist2
        end
    end
    clear vert_MvLs vert_MvLs ia ib v
    %% Connect MvP2 to LIPV to draw lateral region
    % Draw plane from MvP2 to LIPV
    if (trunk == 1)
        mid3 = (MvP2+LIPV)/2; e1 = MvP2 - mid3; e2 = LIPV - mid3;
        po1 = MvP2 + e1*2; po2 = LIPV + e2*2;
        tt7 = tri_plane(po1,po2,mp);
    %     hold on, trisurf(tt7, 'FaceColor', 'b')
    else
        tt7 = tri_plane(MvP2,LIPV,mp);
    %         hold on, trisurf(tt7, 'FaceColor', 'b')
    end
    
    % Making faces and triangles of planes
    surface7 = []; surface7.faces = tt7.ConnectivityList; surface7.vertices = tt7.Points;
    [intm7, intSurface7] = SurfaceIntersection(surface, surface7);
    clear tt7 surface7 intSurface7
    
    % Deleting unwanted connections
    [rr,~] = find(intm7==1);                               % original IDs of faces
    sets = separate_regs(surface.faces(rr,:),surface.vertices);
    if (trunk == 1)
        ID_MvLi = LAA_connections_improved(MvP2,LIPV,rr,sets,LAA_vert);
    else
        ID_MvLi = rr;
    %         new_IDs = correct_connections(MvP2,LIPV,sets);
    %         ID_MvLi = rr(new_IDs,:);
    end
    
    clear rr sets new_IDs intm7
    
    % Plotting connecting triangles from MV to LIPV
    hold on, trisurf(surface.faces(ID_MvLi,:), surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'EdgeColor', 'r', 'LineW', 2), hold off
    
    %% Connect MvP3 to RIPV to draw septal region
    % Draw plane from MvP3 to RIPV
    tt8 = tri_plane(MvP3,RIPV,mp);
    
    % Making faces and triangles of planes
    surface8 = []; surface8.faces = tt8.ConnectivityList; surface8.vertices = tt8.Points;
    [intm8, intSurface8] = SurfaceIntersection(surface, surface8);
    clear tt8 surface8 intSurface8
    
    % Deleting unwanted connections
    [rr,~] = find(intm8==1);                               % original IDs of faces
%     sets = separate_regs(surface.faces(rr,:),surface.vertices);
%     new_IDs = correct_connections(MvP3,RIPV,sets);
%     ID_MvRi = rr(new_IDs,:);
    ID_MvRi = rr;
    clear rr sets new_IDs intm8
    
    % Plotting connecting triangles from MV to RIPV
    hold on, trisurf(surface.faces(ID_MvRi,:), surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'EdgeColor', 'r', 'LineW', 2), hold off
    
    %% Draw a line for posterior region
    % Find a point on LIPV
    LiP1 = triangles2vertex(ID_MvLi, LIPV, LIPV_points, LIPV_tri, surface);
    hold on, plot3(LiP1(1),LiP1(2),LiP1(3),'*k'), hold off
    
    % Find a point on RIPV
    RiP1 = triangles2vertex(ID_MvRi, RIPV, RIPV_points, RIPV_tri, surface);
    hold on, plot3(RiP1(1),RiP1(2),RiP1(3),'*k'), hold off
    
    % Drawing intersecting triangles for LiRi line
    [~,ia,~] = intersect(surface.vertices,LiP1,'rows'); % Finding ID of LiP1
    [~,ib,~] = intersect(surface.vertices,RiP1,'rows'); % Finding ID of RiP1
    
    q1(1)= ia; q2(1)=ib;
    mesh = import_stl;
    % Creating graph with edges
    PP = shortestpath(G,q1(1),q2(1));
    rows = cell(1,length(PP));
    for i = 1:length(PP)
        [rows{1,i},~] = find(surface.faces == PP(i));
    end
    rows = unique(cell2mat(rows(:)));
    RiLi_faces = surface.faces(rows,:);
    RiLi_vertices = surface.vertices(unique(RiLi_faces),:);
    % Define a single line of faces
    q = unique(RiLi_faces);
    x = find(ismember(q,PP) == 0);
    V = vertexAttachments(mesh,q(x));
    V = unique(cell2mat(V'));
    face = surface.faces(V,:);
    Q = find(ismember(face,RiLi_faces,'rows') == 0);
    Out = share_nodes(face(Q,:),surface.vertices);
    [~,r] = max(arrayfun(@(x) size(Out(x).faces,1),1:numel(Out)));
    W = find(ismember(surface.vertices(q(x),:),Out(r).vertices,'rows') == 1);
    X = vertexAttachments(mesh,q(x(W)));
    X = unique(cell2mat(X'));
    face = surface.faces(X,:);
    M = find(ismember(face,RiLi_faces,'rows') == 1);
    RiLi_faces = face(M,:);
    ID_LiRi = find(ismember(surface.faces,RiLi_faces,'rows') == 1);
    clear k1 k2 q1 q2 PP rows q x V face Q Out r W X face M
    % PPP = shortestpath(G,ia,ib);                        % Finding shortest path using graph
    % 
    % % vert = surface.vertices(PPP,:);
    % % [ID_LiRi] = vert2triangle(vert,surface);
    % LiRi_tri = vertexAttachments(import_stl,PPP');      % IDs of triangels of shortest path
    % j = cellfun('size',LiRi_tri,2);
    % j = max(j);
    % LiRi_mat = zeros(j,size(LiRi_tri,1));
    % for i = 1:size(LiRi_tri,1)
    %     jj = size(LiRi_tri{i,1},2);
    %     LiRi_mat(1:jj,i) = LiRi_tri{i,1}';
    %     i = i+1;
    % end
    % ID_LiRi = unique(LiRi_mat(:));
    % ID_LiRi(ID_LiRi <= 0) = [];
    hold on, trisurf(surface.faces(ID_LiRi,:),surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'EdgeColor','green','LineWidth',1.5), hold off
    clear ia ib i j jj PPP LiRi_tri LiRi_mat
    ID_LsRs = ID_roof;
    %% Correcting endpoints
    [C ia] = intersect(surface.vertices,RsRi_1, "rows");
    aa = vertexAttachments(import_stl, ia);
    aa = cell2mat(aa)';
    ID_RsRi = unique([ID_RsRi; aa]);
    clear C ia aa 

    [C ia] = intersect(surface.vertices,RsRi_2, "rows");
    aa = vertexAttachments(import_stl, ia);
    aa = cell2mat(aa)';
    ID_RsRi = unique([ID_RsRi; aa]);
    clear C ia aa 

    [C ia] = intersect(surface.vertices,MvP2, "rows");
    aa = vertexAttachments(import_stl, ia);
    aa = cell2mat(aa)';
    ID_MvLi = unique([ID_MvLi; aa]);
    clear C ia aa 

    [C ia] = intersect(surface.vertices,LiP1, "rows");
    aa = vertexAttachments(import_stl, ia);
    aa = cell2mat(aa)';
    ID_MvLi = unique([ID_MvLi; aa]);
    clear C ia aa 

    [C ia] = intersect(surface.vertices,MvP3, "rows");
    aa = vertexAttachments(import_stl, ia);
    aa = cell2mat(aa)';
    ID_MvRi = unique([ID_MvRi; aa]);
    clear C ia aa

    [C ia] = intersect(surface.vertices,RiP1, "rows");
    aa = vertexAttachments(import_stl, ia);
    aa = cell2mat(aa)';
    ID_MvRi = unique([ID_MvRi; aa]);
    clear C ia aa

    [C ia] = intersect(surface.vertices,MvRs_1, "rows");
    aa = vertexAttachments(import_stl, ia);
    aa = cell2mat(aa)';
    ID_MvRs = unique([ID_MvRs; aa]);
    clear C ia aa

    [C ia] = intersect(surface.vertices,MvRs_2, "rows");
    aa = vertexAttachments(import_stl, ia);
    aa = cell2mat(aa)';
    ID_MvRs = unique([ID_MvRs; aa]);
    clear C ia aa

    [C ia] = intersect(surface.vertices,MvP1, "rows");
    aa = vertexAttachments(import_stl, ia);
    aa = cell2mat(aa)';
    ID_MvLs = unique([ID_MvLs; aa]);
    clear C ia aa

    [C ia] = intersect(surface.vertices,MvLs_1, "rows");
    aa = vertexAttachments(import_stl, ia);
    aa = cell2mat(aa)';
    ID_MvLs = unique([ID_MvLs; aa]);
    clear C ia aa
    
    % [C ia] = intersect(surface.vertices,LaaPt2, "rows");
    % aa = vertexAttachments(import_stl, ia);
    % aa = cell2mat(aa)';
    % ID_MvLs = unique([ID_MvLs; aa]);
    % clear C ia aa
    %% Collecting all borders
    borders = unique([ID_LiLs;ID_LsRs;ID_LiRi;ID_RsRi;ID_MvRs;ID_MvLs;ID_MvLi;ID_MvRi]);
    deformed_surface = surface.faces;
    figure()
    trisurf(deformed_surface(borders,:),surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'FaceColor','red', 'LineStyle', '-','FaceAlpha',1);
    xlabel('X-axis'),ylabel('Y-axis'),zlabel('Z-axis'), axis equal
    clear deformed_surface
    %% LA_regions
    post_borders = unique([ID_LiLs;ID_LsRs;ID_LiRi;ID_RsRi]);
    ant_borders = unique([ID_roof;ID_MvLs;ID_MvRs]);
    inf_borders = unique([ID_LiRi;ID_MvRi;ID_MvLi]);
    % roof_borders = unique([ID_roof;ID_LsRs]);
    lat_borders = unique([ID_LiLs;ID_MvLs;ID_MvLi]);
    sept_borders = unique([ID_RsRi;ID_MvRs;ID_MvRi]);
    %% Anterior region
    anterior_surface = surface.faces;
    anterior_surface(ant_borders,:) = [];
    regions = separate_regs(anterior_surface,surface.vertices);
    a = size(regions(1).original_face_IDs,1);
    b = size(regions(2).original_face_IDs,1);
    mm = [a b];
    [~,in] = min(mm);
    anterior_faces = regions(in).original_face_IDs;
    hold on
    trisurf(anterior_surface(anterior_faces,:),surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'FaceColor',[1 0.5 0], 'LineStyle', '-','FaceAlpha',1);
    xlabel('X-axis'),ylabel('Y-axis'),zlabel('Z-axis'), axis equal
    ant_faces = anterior_surface(anterior_faces,:);
    clear regions a b mm in anterior_surface anterior_faces
    %% Posterior region
    posterior_surface = surface.faces;
    posterior_surface(post_borders,:) = [];
    regions = separate_regs(posterior_surface,surface.vertices);
    a = size(regions(1).original_face_IDs,1);
    b = size(regions(2).original_face_IDs,1);
    mm = [a b];
    [~,in] = min(mm);
    posterior_faces = regions(in).original_face_IDs;
    hold on
    trisurf(posterior_surface(posterior_faces,:),surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'FaceColor',[60/255 179/255 113/255], 'LineStyle', '-','FaceAlpha',1);
    xlabel('X-axis'),ylabel('Y-axis'),zlabel('Z-axis'), axis equal
    post_faces = posterior_surface(posterior_faces,:);
    clear regions a b mm in posterior_surface posterior_faces
    %% Lateral region
    lateral_surface = surface.faces;
    lateral_surface(lat_borders,:) = [];
    regions = separate_regs(lateral_surface,surface.vertices);
    a = size(regions(1).original_face_IDs,1);
    b = size(regions(2).original_face_IDs,1);
    mm = [a b];
    [~,in] = min(mm);
    lateral_faces = regions(in).original_face_IDs;
    hold on
    trisurf(lateral_surface(lateral_faces,:),surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'FaceColor',[0.4 0 0.8], 'LineStyle', '-','FaceAlpha',1);
    xlabel('X-axis'),ylabel('Y-axis'),zlabel('Z-axis'), axis equal
    lat_faces = lateral_surface(lateral_faces,:);
    clear regions a b mm in lateral_surface lateral_faces
    %% Septal region
    septal_surface = surface.faces;
    septal_surface(sept_borders,:) = [];
    regions = separate_regs(septal_surface,surface.vertices);
    a = size(regions(1).original_face_IDs,1);
    b = size(regions(2).original_face_IDs,1);
    mm = [a b];
    [~,in] = min(mm);
    septal_faces = regions(in).original_face_IDs;
    % septal_vertices = regions(in).vertices;
    hold on
    trisurf(septal_surface(septal_faces,:),surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'FaceColor',[0.5 0 0], 'LineStyle', '-','FaceAlpha',1);
    xlabel('X-axis'),ylabel('Y-axis'),zlabel('Z-axis'), axis equal
    septal_faces = septal_surface(septal_faces,:);
    vertices = surface.vertices;
    clear regions a b mm in septal_surface
    %% Inferior region
    inferior_surface = surface.faces;
    inferior_surface(inf_borders,:) = [];
    regions = separate_regs(inferior_surface,surface.vertices);
    a = size(regions(1).original_face_IDs,1);
    b = size(regions(2).original_face_IDs,1);
    mm = [a b];
    [~,in] = min(mm);
    inferior_faces = regions(in).original_face_IDs;
    hold on
    trisurf(inferior_surface(inferior_faces,:),surface.vertices(:,1),surface.vertices(:,2),surface.vertices(:,3),...
        'FaceColor',[80/255 80/255 0], 'LineStyle', '-','FaceAlpha',1);
    xlabel('X-axis'),ylabel('Y-axis'),zlabel('Z-axis'), axis equal
    inf_faces = inferior_surface(inferior_faces,:);
    
    clear regions a b mm in inferior_surface inferior_faces
    %% Import LAA
%     LAA_surface = stlread(['\Contraction parameters\LAA_contraction\Cardiac cycle implementation\2412ctrl\' ...
%         '2412ctrl_LAA_surfaces\LAA_05.stl']);
%     hold on, trisurf(LAA_surface,'FaceColor',[205/255 92/255 92/255], 'LineStyle', '-','FaceAlpha',1 ), hold off
%     figure(2)
%     legend('', 'Anterior', 'Posterior', 'Lateral', 'Septal', 'Inferior', 'LAA')

    [f_p] = fileparts(file_path);
    file_number = sprintf('%02d',lo);
    matfile = fullfile(f_p, ['regions_',file_number]);
    % save(matfile)
    clear f_p matfile file_number
end
clear file_path stl_files
%% Local functions
function [X,Y,Z]=plane_fit(x,y,z)

% function [A,B,C]=plane_fit(x,y,z)
% ------------------------------------------------------------------------
%   Fit a plane to x,y,z data.
%   [A,B,C]=plane_fit(x,y,z) calculates the coefficients A,B,C that fit the data
%   defined by the vectors x,y,z. 
%
%   Uses command svd

P=[mean(x),mean(y),mean(z)];
[U,S,V]=svd([x-P(1),y-P(2),z-P(3)],0);
N=-1/V(end,end)*V(:,end);
A=N(1); B=N(2); C=-P*N;

[X,Y]=meshgrid(linspace(min(x),max(x),2),linspace(min(y),max(y),2));
Z=(A*X)+(B*Y)+C;

end
%%
function new_TR = tri_plane(aa, bb, md)
%tri_plane: This function generates a triangle by connecting three points.
%Later it also scales the triangle.
%   Inputs:
%     aa = 1st point.
%     bb = 2nd point.
%     md = 3rd point (usually midpoint of long axis)

%   Output:
%     new_TR = Triangulation of triangle.


mid1 = (aa+md)/2; s1 = aa - mid1;
mid2 = (bb+md)/2; s2 = bb - mid2;

new_v = [aa+s1*15; bb+s2*15; md];
c = [1 2 3];

new_TR = triangulation(c,new_v);
% hold on, trisurf(new_TR, 'FaceColor', 'b'), hold off
end
%%
function [intMatrix, intSurface] = SurfaceIntersection(surface1, surface2, varargin)
%SURFACEINTERSECTION intersection of 2 surfaces
% [intMatrix, intSurface] = SurfaceIntersection(surface1, surface2)
% calculates the intersection of surfaces 1 and 2. Code can either return
% just the matrix indicating which face of surface1 intersected with face
% of surface2, which is calculated using Tomas Moller algorithm, or can
% also return the actual line of intersection. In case when parts of the
% surface 1 and 2 lay on the same plane the intersection is a 2D area
% instead of 1D edge. In such a case the intersection area will be
% triangulated and intSurface.edges will hold the edges of the
% triangulation surface and intSurface.faces will hold the faces.
%
% INPUT:
%  * surface1 & surface2 - two surfaces defined as structs or classes.
%    Several inputs are possible:
%    - struct with "faces" and "vertices" fields
%    - 'triangulation' class (only the boundary surface will be used)
%    - 'delaunayTriangulation' class
%
% OUTPUT:
% * intMatrix - sparse Matrix with n1 x n2 dimension where n1 and n2 are
%               number of faces in surfaces
% * intSurface - a structure with following fields:
%     intSurface.vertices - N x 3 array of unique points
%     intSurface.edges    - N x 2 array of edge vertex ID's
%     intSurface.faces    - N x 3 array of face vertex ID's
%
% ALGORITHM:
% Based on Triangle/triangle intersection test routine by Tomas Möller, 1997.
%  See article "A Fast Triangle-Triangle Intersection Test",
%  Journal of Graphics Tools, 2(2), 1997
%  http://web.stanford.edu/class/cs277/resources/papers/Moller1997b.pdf
%  http://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/code/opttritri.txt

%% Get FACES and VERTICES inputs
if isa(surface1, 'triangulation')
  [surface1.faces, surface1.vertices] = freeBoundary(surface1);
elseif isa(surface1, 'delaunayTriangulation')
  S = surface1;
  surface1 = [];
  surface1.faces    = S.ConnectivityList;
  surface1.vertices = S.Points;
  clear S
end
if isa(surface2, 'triangulation')
  [surface2.faces, surface1.vertices] = freeBoundary(surface2);
elseif isa(surface2, 'delaunayTriangulation')
  S = surface2;
  surface2 = [];
  surface2.faces    = S.ConnectivityList;
  surface2.vertices = S.Points;
  clear S
end
ok1 = isstruct(surface1) && isfield(surface1, 'vertices') && isfield(surface1, 'faces');
ok2 = isstruct(surface2) && isfield(surface2, 'vertices') && isfield(surface2, 'faces');
assert(ok1, 'Surface #1 must be a struct with "faces" and "vertices" fields' );
assert(ok2, 'Surface #2 must be a struct with "faces" and "vertices" fields' );

%% Flip dimentions if necessery
if size(surface1.faces,1)==3 && size(surface1.faces,2)~=3
  surface1.faces = surface1.faces';
end
if size(surface1.vertices,1)==3 && size(surface1.vertices,2)~=3
  surface1.vertices = surface1.vertices';
end
if size(surface2.faces,1)==3 && size(surface2.faces,2)~=3
  surface2.faces = surface2.faces';
end
if size(surface2.vertices,1)==3 && size(surface2.vertices,2)~=3
  surface2.vertices = surface2.vertices';
end

%% Parse extra parameters
getIntersection = (nargout>1);
debug = false;
PointRoundingTol = 1e6;
algorithm = 'moller';
k=1;
nVarargs = length(varargin);
while (k<=nVarargs)
  assert(ischar(varargin{k}), 'Incorrect input parameters')
  switch lower(varargin{k})
    case 'debug'
      debug = varargin{k+1}~=0;
      k = k+1;
    case 'algorithm'
      algorithm = lower(strtrim(varargin{k+1}));
      k = k+1;
    case 'pointroundingtol'
      PointRoundingTol = varargin{k+1};
      k = k+1;
  end
  k = k+1;
end

%% Initialize variables
epsilon = eps;
nFace1 = size(surface1.faces,1);
nFace2 = size(surface2.faces,1);
nVert1 = size(surface1.vertices,1);
nVert2 = size(surface2.vertices,1);

%% create strip down versions of MATLAB cross and dot function
cross_prod = @(a,b) [...
  a(:,2).*b(:,3)-a(:,3).*b(:,2), ...
  a(:,3).*b(:,1)-a(:,1).*b(:,3), ...
  a(:,1).*b(:,2)-a(:,2).*b(:,1)];
dot_prod = @(a,b) a(:,1).*b(:,1)+a(:,2).*b(:,2)+a(:,3).*b(:,3);
normalize = @(V) bsxfun(@rdivide,V, sqrt(sum(V.^2,2)));

%% Initialize output variables
% intersect is a nFace1 x nFace2 matrix. Possible values: -2 (do not know),
% -1 (coplanar with unknown overlap), 0 (no intersections), 1 (intersects).
% Negative values are internal only.
intMatrix  = zeros([nFace1,nFace2], 'int8')-2; % -2 indicates that there was no succesful test yet
intSurface.vertices = [];
intSurface.faces    = [];
intSurface.edges    = [];

% =======================================================================
%% === Stage 1 ==========================================================
% =======================================================================
% Each triangle is a subset of the plane it lies in, so for two triangles
% to intersect they must overlap along the line of intersection of their
% planes. Hence, a necessary condition for intersection is that each
% triangle must intersect the plane of the other.
% Möller’s method begins by checking the mutual intersection of each
% triangle with the plane of the other. To do so, it determines for each
% triangle on which side of the other triangle’s supporting plane its
% vertices lie. Now, if all vertices of one triangle lie on the same side
% and no vertex is on the plane, the intersection is rejected.

%% compute plane equations for each triangle of the surface #1
% plane equation #1: N1.X-d1=0
V1 = surface1.vertices(surface1.faces(:,1),:);
V2 = surface1.vertices(surface1.faces(:,2),:);
V3 = surface1.vertices(surface1.faces(:,3),:);
N1 = cross_prod(V2-V1,V3-V1); % array size nFace1 x 3
N1 = normalize(N1);
d1 = dot_prod(N1,V1);         % array size nFace1 x 1

%% Distance from surface #2 vertices to planes of surface #1
% Calculate signed distance from all vertices of surface #2 to each plane
% of of surface #1
du = zeros(nFace1,nVert2);
for iVert2 = 1:nVert2
  p = surface2.vertices(iVert2,:);
  du(:,iVert2) = N1(:,1)*p(1) + N1(:,2)*p(2) + N1(:,3)*p(3) - d1;
end
if debug
  assert(all(size(du)==[nFace1,nVert2]), 'Incorrect array dimensions: dv')
end
du(abs(du)<epsilon)=0; % robustness check
% Distances from vertex 1, 2 & 3 of faces of surface #2 to planes of surface #1
du1 = du(:,surface2.faces(:,1));
du2 = du(:,surface2.faces(:,2));
du3 = du(:,surface2.faces(:,3));
if debug
  assert(all(size(du1)==size(intMatrix)), 'Incorrect array dimensions: du1')
end
clear du
intMatrix(du1.*du2>0 & du1.*du3>0) = 0;   % same sign on all of them & not equal 0
if(all(intMatrix==0)), return; end        % no intersections
intMatrix(du1==0 & du2==0 & du3==0) = -1; % coplanar with unknown overlap

%% compute plane of triangle (U0,U1,U2)
% plane equation 2: N2.X-d2=0
U1 = surface2.vertices(surface2.faces(:,1),:);
U2 = surface2.vertices(surface2.faces(:,2),:);
U3 = surface2.vertices(surface2.faces(:,3),:);
N2 = cross_prod(U2-U1,U3-U1); % array size nFace1 x 3
N2 = normalize(N2);
d2 = dot_prod(N2,U1);        % array size nFace1 x 1

%% Distance from surface #1 vertices to planes of surface #2
% Calculate signed distance from all vertices of surface #1 to each plane
% of of surface #2
dv = zeros(nFace2,nVert1);
for iVert1 = 1:nVert1
  p = surface1.vertices(iVert1,:);
  dv(:,iVert1) = N2(:,1)*p(1) + N2(:,2)*p(2) + N2(:,3)*p(3) - d2;
end
if debug
  assert(all(size(dv)==[nFace2,nVert1]), 'Incorrect array dimensions: dv')
end
dv(abs(dv)<epsilon)=0; % robustness check
% Distances from vertex 1, 2 & 3 of faces of surface #1 to planes of surface #2
dv1 = dv(:,surface1.faces(:,1))';
dv2 = dv(:,surface1.faces(:,2))';
dv3 = dv(:,surface1.faces(:,3))';
if debug
  assert(all(size(dv1)==size(intMatrix)), 'Incorrect array dimensions: dv1')
end
clear dv
intMatrix(dv1.*dv2>0 & dv1.*dv3>0) = 0;   % same sign on all of them & not equal 0
if(all(intMatrix==0)), return; end        % no intersections
intMatrix(dv1==0 & dv2==0 & dv3==0) = -1; % coplanar with unknown overlap

% =======================================================================
%% === Stage 2 ==========================================================
% =======================================================================

%% Process remaining (non-coplanar) triangle pairs
tMsk = (intMatrix==-2);
n = nnz(tMsk);
if n>0
  [face1, face2] = find(tMsk);
  switch lower(algorithm)
    case 'moller'
      if size(dv1(tMsk),1)==1
        dv = [dv1(tMsk)', dv2(tMsk)', dv3(tMsk)'];
        du = [du1(tMsk)', du2(tMsk)', du3(tMsk)'];
      else
        dv = [dv1(tMsk), dv2(tMsk), dv3(tMsk)];
        du = [du1(tMsk), du2(tMsk), du3(tMsk)];
      end
      
      [intMatrix(tMsk), intSurface] = TriangleIntersection3D_Moller(...
        V1(face1,:), V2(face1,:), V3(face1,:), N1(face1,:), d1(face1,:), dv, ...
        U1(face2,:), U2(face2,:), U3(face2,:), N2(face2,:), d2(face2,:), du, ...
        getIntersection, debug);
    case 'rapid'
      % Undocumented experimental feature. In some experiments I got
      % identical results as with Moller algorithm, but others gave
      % different results. Often faster tham Moller.
      intMatrix(tMsk) = TriangleIntersection3D_Rapid( ...
        V1(face1,:), V2(face1,:), V3(face1,:), ...
        U1(face2,:), U2(face2,:), U3(face2,:), N1(face1,:), N2(face2,:) );
    otherwise
      error('Unknown algorithm name');
  end
end % if

%% Process coplanar triangle pairs. Pass #1:
% compare the overlap of the bounding boxes
tMsk = (intMatrix==-1);
if nnz(tMsk)>0
  [face1, face2] = find(tMsk);
  overlap = true;
  for idim = 1:3
    v = [V1(face1,idim), V2(face1,idim), V3(face1,idim)];
    u = [U1(face2,idim), U2(face2,idim), U3(face2,idim)];
    t1 = min(v,[],2);
    t2 = max(v,[],2);
    s1 = min(u,[],2);
    s2 = max(u,[],2);
    overlap = overlap & (s1<=t2 & t1<=s2);
  end
  % if overlap intMatrix will remain "-1" otherwise it will change to "0"
  intMatrix(tMsk) = -1*overlap;
  clear v u t1 t2 s1 s2 overlap
end

%% Process coplanar triangle pairs. Pass #2:
% use edge-edge intersections
tMsk = (intMatrix==-1);
if nnz(tMsk)>0
  [face1, face2] = find(tMsk);
  
  % repack data prior to function call
  V(:,:,1)=V1(face1,:); V(:,:,2)=V2(face1,:); V(:,:,3)=V3(face1,:);
  U(:,:,1)=U1(face2,:); U(:,:,2)=U2(face2,:); U(:,:,3)=U3(face2,:);
  [intMatrix(tMsk), intSurface2] = TriangleIntersection2D(V, U, ...
    N1(face1,:), getIntersection, debug);
  
  %% Merge surfaces
  if getIntersection
    np = size(intSurface.vertices,1);
    intSurface.vertices = [intSurface.vertices; intSurface2.vertices];
    intSurface.faces    = [intSurface.faces;    intSurface2.faces+np];
    intSurface.edges    = [intSurface.edges;    intSurface2.edges+np];
    if debug
      np = size(intSurface.vertices,1);
      assert(max(intSurface.faces(:))<=np, 'Bad surface definition')
      assert(max(intSurface.edges(:))<=np, 'Bad surface definition')
    end
  end
end

%% Clean up the outputs
intMatrix = sparse(double(intMatrix));
if(getIntersection)
  % make point array unique
  P = round(intSurface.vertices*PointRoundingTol)/PointRoundingTol;
  [~,ia,ic] = unique(P,'rows'); % V = P(ia,:) and P = V(ic,:).
  intSurface.vertices = intSurface.vertices(ia,:);
  intSurface.faces = ic(intSurface.faces);
  intSurface.edges = ic(intSurface.edges);
end
end % function

%% ========================================================================
function [iMsk, intSurface] = TriangleIntersection3D_Moller(...
  V1, V2, V3, N1, d1, dv, ...
  U1, U2, U3, N2, d2, du, ...
  getIntersection, debug)
%TriangleIntersection3D tests if 2 triangles defined in 3D intersect.
% This is a secondary test following Tomas Moller algorithm
%
% INPUTS:
%   V1, V2, V3, - Nx3 array of surface 1 triangle vertex coordinates
%   U1, U2, U3, - Nx3 array of surface 2 triangle vertex coordinates
%   N1, d1      - Nx3 array of surface 1 triangle plane equations N1.X-d1=0
%   N2, d2      - Nx3 array of surface 2 triangle plane equations N2.X-d2=0
%   dv          - Nx3 array of distances of surface 1 triangle vertices to surface 2 planes
%   du          - Nx3 array of distances of surface 2 triangle vertices to surface 1 planes
%   getIntersection - do we need to output the intersecting surface?
%      Algorithm is much simpler if we do not.
%   debug       - In the debugging mode much more extra "sanity check" test
%      are performed.
%
% OUTPUT:
%   iMsk - N x 1 intersection boolean mask marking which triangles overlap
%   intSurface - intersection surface
%
% ALGORITHM:
% The input triangles are guaranteed to intersect the line of intersection
% of the two planes. Furthermore, these intersections form intervals on
% this line, and the triangles overlap iff these intervals overlap as well.
% Hence, the last part of  the algorithm computes a parametric equation
% L(t) of the line of intersection of the two planes, finds the intervals
% (i.e. scalar intervals on L(t)) for which the line lies inside each
% triangle and performs a one-dimensional interval overlap test.
if debug
  ok = size(N1,2)==3 && size(N2,2)==3 && size(dv,2)==3 && size(du,2)==3 && ...
    size(V1,2)==3 && size(V2,2)==3 && size(V3,2)==3 && ...
    size(U1,2)==3 && size(U2,2)==3 && size(U3,2)==3;
  assert(ok, 'Incorrect array dimensions');
end

%% create strip down versions of MATLAB cross and dot function
cross_prod = @(a,b) [...
  a(:,2).*b(:,3)-a(:,3).*b(:,2), ...
  a(:,3).*b(:,1)-a(:,1).*b(:,3), ...
  a(:,1).*b(:,2)-a(:,2).*b(:,1)];
dot_prod = @(a,b) a(:,1).*b(:,1)+a(:,2).*b(:,2)+a(:,3).*b(:,3);
normalize = @(V) bsxfun(@rdivide,V, sqrt(sum(V.^2,2)));

%% Find intervals of surface 1 and 2 triangles
% compute the scalar intervals on L(t) for which the line lies inside each
% triangle

% Plane creates two open half-spaces. Find the odd vertex, which:
% 1) if no or two vertices are on the plane than pick the vertex which is
%    by itself in its half-space
% 2) if one vertex is on the plane and the other two occupy the same
%    half-space than pick the vertex on the plane
% 3) if one vertex is on the plane and the other two occupy different
%    half-spaces than pick one of the vertices off the plane
% Find vertex using a look-up table "lut" with key calculated based on
% sign of dv and du arrays
lut = [0;3;3;2;1;3;2;2;1;1;2;3;3;0;3;3;2;1;1;2;2;3;1;2;3;3;0];
n = numel(d1);
rows = (1:n)';

%% order surface 1 triangle vertices
a1 = lut(sign(dv)*[9; 3; 1] + 14); % calculate the key and call the look-up table
[b1, c1] = otherDim(a1);
if debug
  assert(all(a1>0), 'Something Wrong: triangles are coplanar')
end
a1 = sub2ind([n,3],rows,a1); % convert row and column IDs to array indecies
b1 = sub2ind([n,3],rows,b1);
c1 = sub2ind([n,3],rows,c1);

%% order surface 2 triangle vertices
a2 = lut(sign(du)*[9; 3; 1] + 14); % calculate the key and call the look-up table
[b2, c2] = otherDim(a2);
if debug
  assert(all(a2>0), 'Something Wrong: triangles are coplanar')
end
a2 = sub2ind([n,3],rows,a2);
b2 = sub2ind([n,3],rows,b2);
c2 = sub2ind([n,3],rows,c2);

%% compute direction of L the line of intersection of 2 planes
% containing 2 triangles. Line L parametric equation: t*D+O=0
D = cross_prod(N1,N2);    % D must be perpendicular to both N1 and N2
[~, maxDim] = max(abs(D),[],2); % compute and index to the largest component of D
if(getIntersection)
  D = normalize(D);
  O = zeros(n,3);
  d = [d1, d2, zeros(n,1)];
  for r =1:n
    N = [N1(r,:); N2(r,:); 0, 0, 0];
    N(3,maxDim(r)) = 1;
    dd = d(r,:)';
    O(r,:) = (N\dd)'; %Solve systems of linear equations N*D3 = d for D3
  end
  clear N d dd
end

%% projection of triangle(V1,V2,V3) and triangle(U1,U2,U3) onto intersection line
% Vp and Up are Nx3 arrays with columns indicating corners of triangles 1 and 2
if(getIntersection)
  Vp=[dot_prod(V1-O,D), dot_prod(V2-O,D), dot_prod(V3-O,D)];
  Up=[dot_prod(U1-O,D), dot_prod(U2-O,D), dot_prod(U3-O,D)];
else
  % Project on one of the axis (closest to the intersection line) instead.
  % Simplified projection is faster and sufficient if we do not need
  % intersection line
  idx = sub2ind([n,3],rows,maxDim);
  Vp = [V1(idx), V2(idx), V3(idx)];
  Up = [U1(idx), U2(idx), U3(idx)];
end
clear V1 V2 V3 U1 U2 U3

%% Calculate surface 1 and 2 triangle intervals
% t1 and t2 are intersection points of surface 1 with the intersection line
% t*D+O=0, and s1 & s2 are intersection points of surface 2 with the same
% line. Tomas Moller algorithm made this section much more complicated
% trying to avoid divisions. However, I could not detect any speed-up.
% Operations (ADD: 12; MUL:4 ; DIV:4 )
t1 = Vp(a1) - (Vp(b1)-Vp(a1)).*dv(a1)./(dv(b1)-dv(a1));
t2 = Vp(a1) - (Vp(c1)-Vp(a1)).*dv(a1)./(dv(c1)-dv(a1));
s1 = Up(a2) - (Up(b2)-Up(a2)).*du(a2)./(du(b2)-du(a2));
s2 = Up(a2) - (Up(c2)-Up(a2)).*du(a2)./(du(c2)-du(a2));

%% Order the intervals as to t1<t2 and s1<s2
msk = t2<t1; % order t1 and t2 so t1<t2
t = t1(msk); t1(msk)=t2(msk); t2(msk)=t; % swap
msk = s2<s1; % order s1 and s2 so s1<s2
t = s1(msk); s1(msk)=s2(msk); s2(msk)=t; % swap

%% Perform THE final test we were preparying for.
% It test for the overlap of 2 1D intervals s1->s2 and t1->t2
iMsk = (s1<t2 & t1<s2);

%% calculate intersection segments
n = nnz(iMsk);
if(getIntersection && n>0)
  % p1 = D*max(t1,s1) + O;    p2 = D*min(t2,s2) + O
  p1 = bsxfun(@times,D(iMsk,:),max(t1(iMsk),s1(iMsk))) + O(iMsk,:);
  p2 = bsxfun(@times,D(iMsk,:),min(t2(iMsk),s2(iMsk))) + O(iMsk,:);
  intSurface.vertices = [p1; p2];
  intSurface.faces    = [1:n; n+1:2*n; n+1:2*n]';
  intSurface.edges    = intSurface.faces(:,1:2);
else
  intSurface.vertices = [];
  intSurface.faces    = [];
  intSurface.edges    = [];
end % if
end % function

%% ========================================================================
function [overlap, intSurface] = TriangleIntersection2D(V, U, N, ...
  getIntersection, debug)
% Triangles V(V0,V1,V2) and U(U0,U1,U2) are are coplanar. Do they overlap?
% INPUTS:
% N - array(n,3) of surface normals where V(i,:,:) and U(i,:,:) are on the same plane
% V - array(n,3,3) (nFace x 3 dimensions x 3 vertices) of surface #1 vertices
% U - array(n,3,3) (nFace x 3 dimensions x 3 vertices) of surface #2 vertices
%
% OUTPUT:
%   iMsk - N x 1 intersection boolean mask marking which triangles overlap
%   intSurface - intersection surface


%  * parameters: vertices of triangle 1: V0,V1,V2
%  *             vertices of triangle 2: U0,U1,U2
%  * result    : returns 1 if the triangles intersect, otherwise 0

%% Constants needed for creating a mesh based on 3 to 6 points in a circle
tri_mesh{6}  = [1 2 6; 2 4 6; 2 3 4; 4 5 6];
tri_mesh{5}  = [1 2 3; 1 3 4; 4 5 1];
tri_mesh{4}  = [1 2 3; 1 3 4];
tri_mesh{3}  = 1:3;
vertices = [];
faces    = [];
pairs    = [];  % each row corresponds to pair of faces. match row number with face number
nVert    = 0;

%% use edge-edge intersections
overlap = false(size(N,1),1);
i1Idx = [1 1 1 2 2 2 3 3 3];
i2Idx = [3 3 3 1 1 1 2 2 2];
j1Idx = [1 2 3 1 2 3 1 2 3];
j2Idx = [3 1 2 3 1 2 3 1 2];
for row = 1:size(N,1)
  % When it is necesary to project 3D plane on 2D, dIdx will be the optimal
  % dimensions to use.
  [~, a] = max(abs(N(row,:))); 
  [b, c] = otherDim(a); 
  dIdx = [b, c]; 
  order = [];

  %% test all edges of triangle 1 against the edges of triangle 2
  % triangles overlap if edges cross
  [edgeMat, P] = EdgesIntersect3D(...
    squeeze(V(row,:,i1Idx))',squeeze(V(row,:,i2Idx))', ...
    squeeze(U(row,:,j1Idx))',squeeze(U(row,:,j2Idx))');
  overlap(row) = any(edgeMat);
  if ~getIntersection && overlap(row), continue; end
  
  if ~overlap(row)
    %% project onto an axis-aligned plane, that maximizes the area
    % of the triangles, compute indices: dIdx which correspond to 2 smallest N1
    % components.
    V2d = [V(row,dIdx,1); V(row,dIdx,2); V(row,dIdx,3)]; % each row is a 2D vertex
    U2d = [U(row,dIdx,1); U(row,dIdx,2); U(row,dIdx,3)];
    
    %% test if tri1 is totally contained in tri2 or vice varsa
    if PointInTriangle2D(V2d(1,:), U2d) % tri1 is totally contained in tri2
      overlap(row) = true;
      order = 1:3;
    elseif PointInTriangle2D(U2d(1,:), V2d) % tri2 is totally contained in tri1
      overlap(row) = true;
      order = 4:6;
    end
    if overlap(row) && ~getIntersection, continue; end
    clear V2d U2d
  end
  
  %% Build the intersection surface
  if getIntersection && overlap(row)
    %Assemble all the points which might be needed for desining
    %intersection polygon: Intersection points and points from triangle 1
    %and 2
    points   = [P(edgeMat,:); squeeze(V(row,:,:))'; squeeze(U(row,:,:))'];
    if isempty(order) % when one tri is totally contained in the other tri then order is set
      order = IntersectionPolygon(edgeMat>0, points, dIdx, debug);
      if isempty(order), continue; end
    end
    nPoint   = length(order);    % how many points will be added?
    nFace    = nPoint-2;         % how many faces will be added?
    vertices = [vertices; points(order,:)]; %#ok<*AGROW>
    faces    = [faces; nVert+tri_mesh{nPoint} ];
    pairs    = [pairs; row+zeros(nFace,1)];  % each row corresponds to pair of faces. match row number with face number
    nVert    = nVert + nPoint;
    if debug
      assert(max(faces(:))<=size(vertices,1), 'Bad surface definition')
    end
  end
end % for

%% Prepare outputs
intSurface.vertices = vertices;
intSurface.faces    = faces;
if isempty(faces)
  intSurface.edges = [];
else
  intSurface.edges = [faces(:,1:2); faces(:,2:3); faces(:,[1,3])];
end
end % function

%% ========================================================================
function polygon = IntersectionPolygon(edgeMat, points, dIdx, debug)
% edgeMat is an edge intersection matrix with 3 rows for edges between
% the points 1-3, 1-2, & 2-3 of the triangle 1 and 3 columns for the same
% edges of the triangle 2. If 2 edges intersect a point of intersection
% is calculated and stored in array "points" followed by points of the
% triangles 1 & 2.  This function calculates the polygon of the intersection
% between 2 triangles.

persistent orderLUT verified
if isempty(orderLUT) || isempty(orderLUT{3})
  % This pre-calculated look-up table is used to quickly look up the order of
  % the vertices in array "points" which make up polygon of the intersection
  % between 2 triangles. A unique key is calculated for each edgeMat using
  % dot product between edgeMat(:) and [256 128 64 32 16 8 4 2 1], which is
  % used to look up point order around the polygon. Negative numbers in the
  % LUT indicate values which were not observed yet so they were not
  % independently verified.
  % reshape(sprintf('%09s',dec2base(key, 2)),3,3) will convert from the key
  % to matrix.
  OrderLUT = zeros(432,1);  
  OrderLUT(003) = 127;
  OrderLUT(005) = 128;
  OrderLUT(006) = 126;
  OrderLUT(009) = 124;
  OrderLUT(010) = 1427;
  OrderLUT(012) = 1428;
  OrderLUT(017) = 1427;
  OrderLUT(018) = 124;
  OrderLUT(020) = 1426;
  OrderLUT(024) = 127;
  OrderLUT(027) = 1243;
  OrderLUT(029) = 12438;
  OrderLUT(030) = 12034;
  OrderLUT(033) = 1428;
  OrderLUT(034) = 1426;
  OrderLUT(036) = 124;
  OrderLUT(040) = 128;
  OrderLUT(043) = 21834;
  OrderLUT(045) = 1243;
  OrderLUT(046) = 21349;
  OrderLUT(048) = 126;
  OrderLUT(051) = 12340;
  OrderLUT(053) = 12943;
  OrderLUT(054) = 1243;
  OrderLUT(065) = 125;
  OrderLUT(066) = 1527;
  OrderLUT(068) = 1825;
  OrderLUT(072) = 123;
  OrderLUT(080) = 1327;
  OrderLUT(083) = 15234;
  OrderLUT(085) = -15234;
  OrderLUT(086) = -15243;
  OrderLUT(090) = 13247;
  OrderLUT(092) = -13247;
  OrderLUT(096) = 1328;
  OrderLUT(099) = 152834;
  OrderLUT(101) = 15234;
  OrderLUT(102) = 152349;
  OrderLUT(106) = 132847;
  OrderLUT(108) = 13247;
  OrderLUT(114) = 102347;
  OrderLUT(116) = -13247;
  OrderLUT(129) = 1527;
  OrderLUT(130) = 125;
  OrderLUT(132) = 1526;
  OrderLUT(136) = 1327;
  OrderLUT(139) = 15243;
  OrderLUT(141) = 152438;
  OrderLUT(142) = 152034;
  OrderLUT(144) = 123;
  OrderLUT(153) = 12347;
  OrderLUT(156) = 123047;
  OrderLUT(160) = 1326;
  OrderLUT(163) = -152043;
  OrderLUT(165) = 13247;
  OrderLUT(166) = 15234;
  OrderLUT(169) = -182347;
  OrderLUT(172) = 193247;
  OrderLUT(177) = -132047;
  OrderLUT(180) = 13247;
  OrderLUT(192) = 127;
  OrderLUT(195) = 1243;
  OrderLUT(197) = 12438;
  OrderLUT(198) = 12034;
  OrderLUT(202) = 12364;
  OrderLUT(204) = 123648;
  OrderLUT(209) = 21364;
  OrderLUT(212) = -21364;
  OrderLUT(216) = 1243;
  OrderLUT(225) = -124638;
  OrderLUT(226) = 120364;
  OrderLUT(232) = 12438;
  OrderLUT(238) = 124356;
  OrderLUT(240) = 12034;
  OrderLUT(245) = -214356;
  OrderLUT(257) = 1528;
  OrderLUT(258) = 1526;
  OrderLUT(260) = 125;
  OrderLUT(264) = 1328;
  OrderLUT(267) = -152438;
  OrderLUT(269) = 15243;
  OrderLUT(270) = -152943;
  OrderLUT(272) = 1326;
  OrderLUT(275) = 152340;
  OrderLUT(277) = 152943;
  OrderLUT(278) = 15243;
  OrderLUT(281) = 182347;
  OrderLUT(282) = -103247;
  OrderLUT(288) = 123;
  OrderLUT(297) = 12347;
  OrderLUT(298) = -123947;
  OrderLUT(305) = 123947;
  OrderLUT(306) = 12347;
  OrderLUT(320) = 128;
  OrderLUT(323) = 21834;
  OrderLUT(325) = 1243;
  OrderLUT(326) = 21349;
  OrderLUT(330) = -123648;
  OrderLUT(332) = 12364;
  OrderLUT(337) = 183642;
  OrderLUT(340) = -129364;
  OrderLUT(344) = 21834;
  OrderLUT(350) = -124365;
  OrderLUT(353) = 12463;
  OrderLUT(354) = 136492;
  OrderLUT(360) = 1243;
  OrderLUT(368) = 12943;
  OrderLUT(371) = 126543;
  OrderLUT(384) = 126;
  OrderLUT(387) = 12340;
  OrderLUT(389) = 12943;
  OrderLUT(390) = 1243;
  OrderLUT(394) = -103642;
  OrderLUT(396) = 129364;
  OrderLUT(401) = 123640;
  OrderLUT(404) = 12364;
  OrderLUT(408) = 12340;
  OrderLUT(413) = 215643;
  OrderLUT(417) = -136492;
  OrderLUT(418) = 12463;
  OrderLUT(424) = 13492;
  OrderLUT(427) = -213456;
  OrderLUT(432) = 1342;
  
  % Convert to more convinient format
  orderLUT = cell(size(OrderLUT));
  for i = 1:size(OrderLUT,1)
    polygon = abs(OrderLUT(i));
    if polygon>0
      polygon = num2str(polygon)-48; % Convert from a single number to array of digits
      polygon(polygon==0) = 10;      % 0 stands for 10
      orderLUT{i} = polygon;
    end
  end
  % Negative numbers in the LUT indicate values which were not observed yet
  % so they were not independently verified.
  verified = OrderLUT>0;
  clear OrderLUT
end

%% Calculate unique key for each edgeMat configuration
key = dot(1*edgeMat(:)', [256 128 64 32 16 8 4 2 1]);
assert(key<=432, 'Error: in IntersectionPolygon: key is out of bound');

%% Look up the point order around the polygon
polygon = orderLUT{key};
if (isempty(polygon))
  return
end

%% in a rare case of 2 intersections there is ambiguity if one or two
% vertices of the triangle lay inside the other triangle. OrderLUT stores
% only the single vertex cases.
nx = nnz(edgeMat(:));
if nx==2
  pList = polygon;       % list of vertices to check
  pList(pList<=nx) = []; % keep only the triangle points of the polygon
  flip = false;    % was there a flip from single vertex to vertices case?
  for ip = 1:length(pList)
    p = pList(ip);                 % point to check
    t = floor((p-nx-1)/3);         % does it belong to triangle 0 or 1 (actually 1 or 2)
    tri = (1:3) + nx + 3*abs(1-t); % Points belonging to the other triangle
    if ~PointInTriangle2D(points(p,dIdx), points(tri,dIdx))
      d = nx+t*3;    % offset
      % "p-d" is vertex number of point just tested: 1, 2, or 3. "b, c" are
      % the other 2 vertices
      [b, c] = otherDim(p-d);
      polygon = [polygon(polygon~=p), b+d, c+d]; % remove i2 and add i0 and i1
      flip = true;
    end
  end
  if flip
    % if ther were any flips than use existing codes to figure out the
    % order of the points around the polygon
    DT = delaunayTriangulation(points(polygon,dIdx));
    idx = freeBoundary(DT)';
    idx(2,:) = [];
    polygon = polygon(idx);
  end
end

%% Check to duplicate points
tol = 1e6;
P = round(points(polygon,:)*tol)/tol;
[~,ia] = unique(P,'rows'); % V = P(ia,:) and P = V(ic,:).
polygon = polygon(sort(ia));

%% Test the results using more expensive function
doPlot = (~verified(key));
if debug && length(polygon)>3
  DT = delaunayTriangulation(points(polygon,dIdx));
  idx = freeBoundary(DT)';
  idx(2,:) = [];
  k = max(abs(diff(idx)));
  %doPlot = (k>1 && k<(length(idx)-1)) || (~verified(key));
  assert(k==1 || k==(length(idx)-1), 'Two triangle intersection polygon is not convex')
end
if debug && doPlot % plot the interesting cases
  PlotTwoTriangles(points, polygon, 'm')
  title(sprintf('key = %i', key));
end 

end % function

%% ========================================================================
function PlotTwoTriangles(points, polygon, color)
% Plotting function used for debugging
nx = size(points,1)-6;
d = (max(points,[],1)-min(points,[],1))/200;
figure(2)
clf
hold on
line( points(nx+(1:2),1), points(nx+(1:2),2), points(nx+(1:2),3), 'Color', 'g');
line( points(nx+(2:3),1), points(nx+(2:3),2), points(nx+(2:3),3), 'Color', 'g');
line( points(nx+[1,3],1), points(nx+[1,3],2), points(nx+[1,3],3), 'Color', 'g');
line( points(nx+(4:5),1), points(nx+(4:5),2), points(nx+(4:5),3), 'Color', 'b');
line( points(nx+(5:6),1), points(nx+(5:6),2), points(nx+(5:6),3), 'Color', 'b');
line( points(nx+[4,6],1), points(nx+[4,6],2), points(nx+[4,6],3), 'Color', 'b');
plot3( points(:,1), points(:,2), points(:,3), 'm.');
if (length(polygon)>2)
  idx = polygon([1:end, 1]);
  plot3( points(idx,1), points(idx,2),points(idx,3), 'Color', color, 'LineWidth', 1);
end
for i = 1:nx+6
  text(points(i,1)+d(1), points(i,2)+d(2), points(i,3), num2str(i))
end

end % function

%% ========================================================================
function [intersect, X] = EdgesIntersect3D(V1,V2, U1,U2)
%EdgesIntersectPoint3D calculates point of intersection of 2 coplanar
% segments in 3D
%
% INPUTS:
%   V1,V2 - 1 x 3 coordinates of endpoints of edge 1
%   U1,U2 - 1 x 3 coordinates of endpoints of edge 2
% OUTPUT:
%   X - 1 x 3 coordinates of the intersection point
A = V2-V1;
B = U1-U2;
C = U1-V1;
%% Solve system of equations [A,B,1] * [d;e;0] = C for d and e
det3 = @(a,b) ... % determinant of a matrix with columns: [a, b, 1]
  a(:,1).*b(:,2)-a(:,3).*b(:,2) + ...
  a(:,2).*b(:,3)-a(:,2).*b(:,1) + ...
  a(:,3).*b(:,1)-a(:,1).*b(:,3);
f=det3(A,B); % https://en.wikipedia.org/wiki/Cramer%27s_rule#Explicit_formulas_for_small_systems
t=det3(C,B)./f; % use Cramer's rule
s=det3(A,C)./f;
intersect = (t>=0 & t<=1 & s>=0 & s<=1);
X = V1 + bsxfun(@times,A,t);
end % function

%% ========================================================================
function inside = PointInTriangle2D(V1, U)
% check if V1 is inside triangle U (U1,U2,U3)
% Algorithm is checking on which side of the half-plane created by the
% edges the point is. It uses sign of determinant to calculate orientation
% of point triplets.
% INPUTS:
%   V1 - 1 x 2 coordinates of a point
%   U  - 3 x 2 coordinates of endpoints of 3 edges of a triangle
% OUTPUT:
%   inside - a boolean or boolean array
det2 = @(A,B,C) (A(:,1)-C(:,1))*(B(:,2)-C(:,2)) - (B(:,1)-C(:,1))*(A(:,2)-C(:,2));
b1 = (det2(U(1,:), U(2,:), V1) > 0);
b2 = (det2(U(2,:), U(3,:), V1) > 0);
b3 = (det2(U(3,:), U(1,:), V1) > 0);
inside = ((b1 == b2) & (b2 == b3)); % inside if same orientation for all 3 edges
end % function

%% ========================================================================
function [b, c] = otherDim(a)
% return set [1 2 3] without k
b = mod(a+1,3)+1;  % b and c are vertices which are on the same side of the plane
c = 6-a-b;         % a+b+c = 6
end


%% ========================================================================
function overlap = TriangleIntersection3D_Rapid( v1, v2, v3, u1, u2, u3, n1, n2 )
%TriangleIntersection3D tests if 2 triangles defined in 3D intersect.
%
% INPUTS:
%   v1, v2, v3, - Nx3 array of surface 1 triangle vertex coordinates
%   u1, u2, u3, - Nx3 array of surface 2 triangle vertex coordinates
%   n1, n2      - Nx3 array of surface 1 & 2 triangle plane normals. Those
%      are optional and if provided than the first 2 steps of the algorithm
%      (which are equivalent to first 2 steps of Moller algorithm) will be 
%      skipped.
%
% OUTPUT:
%   iMsk - N x 1 intersection boolean mask marking which triangles overlap
%
% ALGORITHM:
%   translated from the UNC-CH V-Collide RAPID code
%    https://wwwx.cs.unc.edu/~geom/papers/COLLISION/vcol.pdf

global V1 V2 V3 U1 U2 U3

cross_prod = @(a,b) [...
  a(:,2).*b(:,3)-a(:,3).*b(:,2), ...
  a(:,3).*b(:,1)-a(:,1).*b(:,3), ...
  a(:,1).*b(:,2)-a(:,2).*b(:,1)];

%% shift t1 and t2 by p1#
V1 = zeros(size(v1));
V2 = v2-v1;
V3 = v3-v1;
U1 = u1-v1;
U2 = u2-v1;
U3 = u3-v1;
clear v1 v2 v3 u1 u2 u3

if(nargin<7)
  %% now begin the series of tests
  n1 = cross_prod( V2-V1, V3-V1 ); % face normals
  n2 = cross_prod( U2-U1, U3-U1 ); % face normals
end
  
%% test the face normals
overlap = project6(n1) & project6(n2);
V1 = V1(overlap,:);
V2 = V2(overlap,:);
V3 = V3(overlap,:);
U1 = U1(overlap,:);
U2 = U2(overlap,:);
U3 = U3(overlap,:);
n1 = n1(overlap,:);
n2 = n2(overlap,:);

%% compute triangle edges
e1 = V2-V1;
e2 = V3-V2;
e3 = V1-V3;
f1 = U2-U1;
f2 = U3-U2;
f3 = U1-U3;

%% run more tests
overlap2 = project6(cross_prod(e1, f1));
overlap2 = project6(cross_prod(e1, f2)) & overlap2;
overlap2 = project6(cross_prod(e1, f3)) & overlap2;
overlap2 = project6(cross_prod(e2, f1)) & overlap2;
overlap2 = project6(cross_prod(e2, f2)) & overlap2;
overlap2 = project6(cross_prod(e2, f3)) & overlap2;
overlap2 = project6(cross_prod(e3, f1)) & overlap2;
overlap2 = project6(cross_prod(e3, f2)) & overlap2;
overlap2 = project6(cross_prod(e3, f3)) & overlap2;
overlap2 = project6(cross_prod(e1, n1)) & overlap2;
overlap2 = project6(cross_prod(e2, n1)) & overlap2;
overlap2 = project6(cross_prod(e3, n1)) & overlap2;
overlap2 = project6(cross_prod(f1, n2)) & overlap2;
overlap2 = project6(cross_prod(f2, n2)) & overlap2;
overlap2 = project6(cross_prod(f3, n2)) & overlap2;
overlap(overlap) = overlap2;
end

%% ========================================================================
function pass = project6( p )
% project all 6 vertices of both triangles onto vector p and check if two
% projections overlap
global V1 V2 V3 U1 U2 U3
dot_prod = @(a,b) a(:,1).*b(:,1)+a(:,2).*b(:,2)+a(:,3).*b(:,3);
%% Project vertices of triangle 1 and find the bounds min1 and max1
P = [dot_prod(p, V1), dot_prod(p, V2), dot_prod(p, V3)];
max1 = max(P,[],2);
min1 = min(P,[],2);
%% Project vertices of triangle 2 and find the bounds min1 and max1
P = [dot_prod(p, U1), dot_prod(p, U2), dot_prod(p, U3)];
max2 = max(P,[],2);
min2 = min(P,[],2);
%% Compare the bounds to see if they overlap
pass = (( min1 < max2 ) & ( min2 < max1 )) | ~dot_prod(p, p);
end
%%
function [triangle] = vert2triangle(vert,S)

% vert2triangle: This function generates/finds the triangles using the vertices having those vertices.

% INPUTS:
% vert : one/ list of vertices
% S: Surface, a structure including all the faces and vertices of surface 

% OUTPUTS:
% triangle: List of IDs of triangles

[~,ia,~] = intersect(S.vertices,vert,'rows');
% [~,ib,~] = intersect(S.faces(:,1),ia);
% [~,ic,~] = intersect(S.faces(:,2),ia);
% [~,id,~] = intersect(S.faces(:,3),ia);
% triangle = unique([ib; ic; id]);

[ib, ~] = find(S.faces(:,1) == ia');
[ic, ~] = find(S.faces(:,2) == ia');
[id, ~] = find(S.faces(:,3) == ia');
triangle = unique([ib; ic; id]);

% trisurf(S.faces(triangle,:),S.vertices(:,1),S.vertices(:,2),S.vertices(:,3),...
%     'EdgeColor','green','LineWidth',1.5);
end
%%
function [final_pt] = triangles2vertex(line_ID,BC,op_vertices,tri_ID,s)
%triangles2vertex: This function finds the coordinates of the vertex in
%common with two segment of intersecting triangles

% Inputs: 
% line_ID: IDs of intersecting triangles belong to one segment.
% BC: Barycenter of the opening.
% op_vertices: Coordinates of the vertices of the opening.
% tri_ID: IDs of triangles belong to the opening.
% s: a structure having faces and vertices of the complete mesh.
% 
% Output:
% final_pt: coordinates of one point which is common in both the segments.

inter_tri = intersect(line_ID, tri_ID);
inter_coord = s.faces(inter_tri,:);
inter_coord = unique(inter_coord(:)); near_vert = s.vertices(inter_coord,:);
int1 = intersect(op_vertices,near_vert,'rows'); rff1 = dsearchn(int1,BC);
final_pt = int1(rff1,:); 
end
%%
function [tri_Pt,intSurface] = line2triangles(BC,pt,md,S)
% line2triangles: This function finds the Ids of intersecting triangles by
% projecting line on the surface.

% Inputs: 
% BC: start point of the line/ barycenter
% pt: End point of line.
% md: a point for the direction of plane (midpoint of long axis)
% surface: a structure having faces and vertices of the complete mesh.

% Output:
% tri_pt: IDs of the intersecting triangles.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% mid3 = (BC+pt)/2; s3 = pt - mid3;
new_TR = tri_plane(BC, pt, md);
% hold on, trisurf(new_TR, 'FaceColor', 'b'), hold off

s = []; s.faces = new_TR.ConnectivityList; s.vertices = new_TR.Points;
[intm, intSurface] = SurfaceIntersection(S, s);

% Deleting unwanted connections
[rr,~] = find(intm==1);                               % original IDs of faces
% sets = separate_regs(S.faces(rr,:),S.vertices);
% new_IDs = correct_connections(BC,pt,sets);
% tri_Pt = rr(new_IDs,:);
tri_Pt = rr;
clear rr sets new_IDs
end
function fvOut = separate_regs( f, v )
%SPLITFV Splits faces and vertices into connected pieces
%   FVOUT = SPLITFV(F,V) separates disconnected pieces inside a patch defined by faces (F) and
%   vertices (V). FVOUT is a structure array with fields "faces" and "vertices". Each element of
%   this array indicates a separately connected patch.
%
%   FVOUT = SPLITFV(FV) takes in FV as a structure with fields "faces" and "vertices"
%
%   For example:
%     fullpatch.vertices = [2 4; 2 8; 8 4; 8 0; 0 4; 2 6; 2 2; 4 2; 4 0; 5 2; 5 0];
%     fullpatch.faces = [1 2 3; 1 3 4; 5 6 1; 7 8 9; 11 10 4];
%     figure, subplot(2,1,1), patch(fullpatch,'facecolor','r'), title('Unsplit mesh');
%     splitpatch = splitFV(fullpatch);
%     colours = lines(length(splitpatch));
%     subplot(2,1,2), hold on, title('Split mesh');
%     for i=1:length(splitpatch)
%         patch(splitpatch(i),'facecolor',colours(i,:));
%     end
%
%   Note: faces and vertices should be defined such that faces sharing a coincident vertex reference
%   the same vertex number, rather than having a separate vertice defined for each face (yet at the
%   same vertex location). In other words, running the following command: size(unique(v,'rows') ==
%   size(v) should return TRUE. An explicit test for this has not been included in this function so
%   as to allow for the deliberate splitting of a mesh at a given location by simply duplicating
%   those vertices.
%% Organise faces into connected fSets that share nodes
fSets = zeros(size(f,1),1,'uint32');
currentSet = 0;

while any(fSets==0)
    currentSet = currentSet + 1;
%     fprintf('Connecting set #%d vertices...',currentSet);
    nextAvailFace = find(fSets==0,1,'first');
    openVertices = f(nextAvailFace,:);
    while ~isempty(openVertices)
        availFaceInds = find(fSets==0);
        [availFaceSub, ~] = find(ismember(f(availFaceInds,:), openVertices)); 
        fSets(availFaceInds(availFaceSub)) = currentSet;
        openVertices = f(availFaceInds(availFaceSub),:);
    end
%     fprintf(' done! Set #%d has %d faces.\n',currentSet,nnz(fSets==currentSet));
end
numSets = currentSet;

%% Create separate faces/vertices structures for each fSet
fvOut = repmat(struct('faces',[],'vertices',[],'original_face_IDs',[]),numSets,1);

for currentSet = 1:numSets
    setF = f(fSets==currentSet,:);
    [unqVertIds, ~, newVertIndices] = unique(setF);
    fvOut(currentSet).faces = reshape(newVertIndices,size(setF));
    fvOut(currentSet).vertices = v(unqVertIds,:);
    fvOut(currentSet).original_face_IDs = find(fSets == currentSet);
end
end
%%
function [IDs] = correct_triangles(p1,p2,fc_ID,sets)
%CORRECT_TRIANGLES: finds the correct intersecting triangles
%   [IDs] = correct_triangles(p1,p2)

%   Inputs:
%   p1,p2: Two points for which we need correct connection
%   fc_ID = all face IDs
%   sets: a structure containing set of connections, among which we need to find correct connection

%   Output:
%   correct_triangles: IDs of faces which makes a desire connection
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global import_stl

if size(sets,1) == 1            % there is only one connection
    new_IDs = sets.original_face_IDs;
    fc_ID = fc_ID(new_IDs);
else
    md_point = (p1+p2)/2;                                   % midpoint of two barycenter
    [r,~] = size(sets);                                     % r = set of different connections 
    cp = zeros(r,3);
    
    for i = 1 : r
        [IID(i) dd(i)] = dsearchn(sets(i).vertices,md_point);
        cp(i,:) = sets(i).vertices(IID(i),:);
    end
    [~, ind] = min(dd);
    new_IDs = sets(ind).original_face_IDs;
    fc_ID = fc_ID(new_IDs);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Check if 2nd point is present in the connection or not
fc = import_stl.ConnectivityList(fc_ID,:);
fc = unique(fc(:));
vert = import_stl.Points(fc,:);
[~,ia,ib] = intersect(vert,p2 ,"rows");

if size(ib,1) == 1                         % Endpoint is present
    [~,iq,~] = intersect(import_stl.Points,p2 ,"rows");
    [v cc ~] = find(ismember(import_stl.ConnectivityList(fc_ID,:),iq));
%     hold on, trisurf(import_stl.ConnectivityList(fc_ID(v),:), import_stl.Points(:,1), import_stl.Points(:,2), import_stl.Points(:,3), ...
%         'EdgeColor', 'b', 'LineW', 3), hold off
    ID_temp = fc_ID;
    ID_temp(v) = [];                                     % IDs of faces without endpoint ID
    sets_temp = separate_regs(import_stl.ConnectivityList(ID_temp,:),import_stl.Points);
    if size(sets_temp,1) == 1
        IDs = fc_ID;
    else
        for i = 1 : size(sets_temp,1)
            a(i) = size(sets_temp(i).original_face_IDs,1);
        end
        [~,ind] = max(a);
        big_IDs = sets_temp(ind).original_face_IDs;                 % take biggest connection
        IDs = [ID_temp(big_IDs); ID_temp(v)];                                     % New complete ID set
    end

else                                        % Endpoint is not present, means connection is not complete
    p3 = dsearchn(vert, p2);       
    path_IDs = direct_path(p2,vert(p3,:),import_stl);       % Make connection with endpoint

    IDs = [path_IDs; fc_ID];
end
end
%%
function full_IDs = LAA_connections_improved(p1,p2,fc_ID,sets,LAA_pts)

% correct_connectionsLAA: This function works to find correct connection b/w LSPV and MV passing through LAA.

% Inputs:
    % p1 = 1st endpoint
    % p2 = 2nd endpoint
    % fc_ID = all face IDs
    % LAA_pts = LAA vertices
    % sets
% Output:
    % IDs = correct IDs of intersecting faces
%     fc_ID = rr;
%     p1 = MvP1;
%     p2 = MvLs_1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global import_stl

if size(sets,1) == 1            % there is only one connection
    new_IDs = sets.original_face_IDs;
    fc_ID = fc_ID(new_IDs);
    ID_mv = fc_ID; ID_ls = [];
else                            % more than one connections
    setsLAA = sets; s1 = []; s2 = [];
    for ni = 1 : size(sets,1)
        dd = find(ismember(sets(ni).vertices, p1));      % Identify those connections in which mv point is present
        if isempty(dd)
            % do nothing
        else
           s1 = sets(ni).original_face_IDs;
           setsLAA(ni).vertices = [];
        end
    end
    ID_mv = fc_ID(unique(s1),:);
 
    
    for ni = 1 : size(sets,1)
        tt = find(ismember(setsLAA(ni).vertices, LAA_pts));      % Identify those connections in which LAA points are present
        if isempty(tt)
            % do nothing
        else
           s2 = setsLAA(ni).original_face_IDs; 
        end
    end
    ID_ls = fc_ID(unique(s2),:);
%     new_IDs = unique([s1;s2]);
    fc_ID = [ID_mv; ID_ls];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Check if LS point is present in the connection or not
if isempty(ID_ls)
    ID_ls = ID_mv;
end

fc = import_stl.ConnectivityList(ID_ls,:);
fc = unique(fc(:));
vert = import_stl.Points(fc,:);
[~,ia,ib] = intersect(vert,p2 ,"rows");

if size(ib,1) == 1                         % Endpoint is present
    [~,iq,~] = intersect(import_stl.Points,p2 ,"rows");
    [v cc ~] = find(ismember(import_stl.ConnectivityList(ID_ls,:),iq));
%     hold on, trisurf(import_stl.ConnectivityList(ID_ls(v),:), import_stl.Points(:,1), import_stl.Points(:,2), import_stl.Points(:,3), ...
%         'EdgeColor', 'b', 'LineW', 3), hold off
    ID_temp = ID_ls;
    ID_temp(v) = [];                                     % IDs of faces without endpoint ID
    sets_temp = separate_regs(import_stl.ConnectivityList(ID_temp,:),import_stl.Points);
    if size(sets_temp,1) == 1
        IDs = ID_ls;
    else
        for i = 1 : size(sets_temp,1)
            a(i) = size(sets_temp(i).original_face_IDs,1);
        end
        [~,ind] = max(a);
        big_IDs = sets_temp(ind).original_face_IDs;                 % take biggest connection
        IDs = [ID_temp(big_IDs); ID_ls(v)];                                     % New complete ID set
    end

else                                        % Endpoint is not present, means connection is not complete
    p3 = dsearchn(vert, p2);       
    path_IDs = direct_path(p2,vert(p3,:),import_stl);       % Make connection with endpoint

    IDs = [path_IDs; ID_ls];
end

if size(sets,1) == 1
    full_IDs = unique([IDs]);
else
    full_IDs = unique([ID_mv; IDs]);
end

end
%%
function Out = share_nodes(f,v)

% Label connected faces that share nodes
fSets = zeros(size(f,1),1);
currentSet = 0;

while any(fSets == 0)
    currentSet = currentSet + 1;
    nextAvailFace = find(fSets == 0,1,'first');
    openVertices = f(nextAvailFace,:);
    while ~isempty(openVertices)
        availFaceInds = find(fSets == 0);
        [availFaceSub, ~] = find(ismember(f(availFaceInds,:), openVertices)); 
        fSets(availFaceInds(availFaceSub)) = currentSet;
        openVertices = f(availFaceInds(availFaceSub),:);
    end
end

numSets = currentSet;
Out = repmat(struct('faces',[],'vertices',[]),numSets,1);

for currentSet = 1:numSets
    [rows,~] = find(fSets == currentSet);
    Out(currentSet).faces = f(rows,:);
    Out(currentSet).vertices = v(unique(f(rows,:)),:);
end
end
%%
function IDs = direct_path(pt1,pt2,surface)
%direct_path: This function generaates direct connection b/w two points onto the surface

% Inputs:
    % pt1 = 1st point
    % pt1 = 2nd point
    % surface = surface on which connections will be generated
    % sets
% Output:
    % IDs = IDs of intersecting faces
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global G
[~,ia,~] = intersect(surface.Points,pt1,'rows'); % Finding ID
[~,ib,~] = intersect(surface.Points,pt2,'rows'); % Finding ID
PPP = shortestpath(G,ia,ib);                        % Finding shortest path using graph

xyz_tri = vertexAttachments(surface,PPP');          % IDs of triangels of shortest path
j = cellfun('size',xyz_tri,2);
j = max(j);
xyz_mat = zeros(j,size(xyz_tri,1));

for i = 1:size(xyz_tri,1)
    jj = size(xyz_tri{i,1},2);
    xyz_mat(1:jj,i) = xyz_tri{i,1}';
    i = i+1;
end

IDs = unique(xyz_mat(:));
IDs(IDs <= 0) = [];
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%







 
