                                                        %%%%%Anterior Region%%%%%

clc,clear all,close all
%% Import parameters
uiopen('*.mat')
selpath_LA = uigetdir(['C:\Users\sachal.hussain3\OneDrive - Alma Mater Studiorum Università di Bologna\' ...
    'FATA project\Regional segmentation\Contraction parameters\Left atrium_contraction\Cardiac cycle implementation']);
LA_files = dir(fullfile(selpath_LA,'*.mat')); 
%% Plot region, long axis and points
for lo = 1:length(LA_files)                                          % To load all stl LA_files
    filename = fullfile(selpath_LA,LA_files(lo).name);
    load(filename)
    int_p = new_wb(lo,:);
    figure(lo)
    trisurf(ant_faces, vertices(:,1), vertices(:,2), vertices(:,3))
    hold on, plot3([mv(1) wb(1)],[mv(2) wb(2)],[mv(3) wb(3)],'--r','LineWidth',2)
    plot3([mv(1) wb(1)],[mv(2) wb(2)],[mv(3) wb(3)],'*', 'LineWidth',2)
    plot3([RSPV(1) LSPV(1) LAA(1)],[RSPV(2) LSPV(2) LAA(2)],[RSPV(3) LSPV(3) LAA(3)],'*b', 'LineWidth',2)
    plot3(int_p(1), int_p(2), int_p(3), 'k*', 'LineWidth',2), hold off
    %% Extract boundry curve
    TR = triangulation(ant_faces, vertices);
    [F,P] = freeBoundary(TR);
    hold on, trisurf(F,P(:,1),P(:,2),P(:,3),'EdgeColor','red','LineWidth',1.5); hold off
    %% Checking
    while true
        [nn nf] = mode(F, "all");       % find most repeated vertex, if any
    %     hold on, plot3(P(nn,1), P(nn,2), P(nn,3),'*r', 'LineWidth',2 ), hold off
        if nf<3
            break
        else
            [cv iv ip] = intersect(vertices, P(nn,:), "rows");
            clear cv ip
            ant_faces = removeBadFaces(ant_faces, vertices, iv);
            TR = triangulation(ant_faces, vertices);
            [F,P] = freeBoundary(TR);
            close(lo), 
            figure(lo), trisurf(ant_faces, vertices(:,1), vertices(:,2), vertices(:,3))
            hold on, plot3([mv(1) wb(1)],[mv(2) wb(2)],[mv(3) wb(3)],'--r','LineWidth',2)
            plot3([mv(1) wb(1)],[mv(2) wb(2)],[mv(3) wb(3)],'*', 'LineWidth',2)
            plot3([RSPV(1) LSPV(1) LAA(1)],[RSPV(2) LSPV(2) LAA(2)],[RSPV(3) LSPV(3) LAA(3)],'*b', 'LineWidth',2)
            plot3(int_p(1), int_p(2), int_p(3), 'k*', 'LineWidth',2), hold off
        end
        clear nn nf
    end
    %% Check if there is hole(LAA) or not
    if F(:,2) == [(2:length(P)) 1]'
        hole = 0;
    else
        hole = 1;
        % Separate out opening vertices
        [r,~] = find(F==1, 1, 'last');
        op_1 = P([1:r 1],:);
        op_2 = P([(r+1):end (r+1)],:);
        % Identify LAA vertices
        if length(op_1) < length(op_2)
            LAA_vert = op_1;
            P = op_2;
        else
            LAA_vert = op_2;
            P = op_1;
        end
        hold on, plot3(LAA_vert(:,1),LAA_vert(:,2),LAA_vert(:,3),'black','LineWidth',2); hold off
    end
    %% Plot four corner points
    ID = dsearchn(P,[MvRs_1; MvRs_2; MvP1; MvLs_1]);
    pts = P(ID,:);
    hold on, plot3(pts(:,1), pts(:,2), pts(:,3),'*g', 'LineWidth',2 ), hold off
    %% Divide boundry curve in four sections
    IDD = unique(ID);
    Eg_1 = P(IDD(1):IDD(2),:);
    Eg_2 = P(IDD(2):IDD(3),:);
    Eg_3 = P(IDD(3):IDD(4),:);
    Eg_4 = [P(IDD(4):end,:); P([1,end],:); P(1:IDD(1),:)];
    Eg_4 = unique(Eg_4,"rows","stable");
    hold on, plot3(Eg_1(:,1), Eg_1(:,2), Eg_1(:,3), 'r', 'LineWidth',2 )
    plot3(Eg_2(:,1), Eg_2(:,2), Eg_2(:,3), 'g', 'LineWidth',2 )
    plot3(Eg_3(:,1), Eg_3(:,2), Eg_3(:,3), 'b', 'LineWidth',2 )
    plot3(Eg_4(:,1), Eg_4(:,2), Eg_4(:,3), 'y', 'LineWidth',2 ), hold off
    %% Create a structure array and store all edges in it
    box = cell(length(IDD),1);
    box{1,1} = Eg_1; box{2,1} = Eg_2; box{3,1} = Eg_3; box{4,1} = Eg_4;
    %% Edge detection and surface formation algorithm
    j = 1;
    TR_2 = cell(2,1);
    for i = 1 : length(IDD)
        mat = box{i,1};                                         % Take edge
        mid = mean(mat);                                        % Find mean
        pv_mid = (LSPV+RSPV)/2;    
        pv_RS = (MvRs_1+MvRs_2)/2;
        mv_mid = (MvP1+MvRs_2)/2;
        M_pts = [pv_mid; mv_mid; LAA; pv_RS];                                   % Real points
        k = dsearchn(M_pts, mid);
        cl_pt =  M_pts(k,:),                                    % Point closest to edge     
        if find(ismember(mat,pts(2,:),'rows')) & find(ismember(mat,pts(3,:),'rows'))
            [TR_1] = ClosedEdge_OC(mat, mv);                 % Make surface with mitral valve
            hold on, trisurf(TR_1, 'FaceColor', 'r'), hold off
        elseif find(ismember(mat,pts(1,:),'rows')) & find(ismember(mat,pts(2,:),'rows'))
            la = [RSPV;mv];
            kk = dsearchn(la, mat(1,:));
            if kk == 1
                xx = linspace(int_p(1), mv(1), length(mat))'; yy = linspace(int_p(2), mv(2), length(mat))'; 
                zz = linspace(int_p(3), mv(3), length(mat))';
%                 hold on, plot3(xx,yy,zz,'k*'), hold off
            else
                xx = linspace(mv(1), int_p(1), length(mat))'; yy = linspace(mv(2), int_p(2), length(mat))'; 
                zz = linspace(mv(3), int_p(3), length(mat))';
%                 hold on, plot3(xx,yy,zz,'k*'), hold off
            end
            clear kk
            vert_2 = [mat; xx, yy, zz]; 
            a1 = length(mat)+1 : length(vert_2)-1; b1 = length(mat)+2 : length(vert_2); c1 = 1 : length(mat)-1
            a2 = 1 : length(mat)-1; b2 = 2 : length(mat); c2 = length(mat)+2 : length(vert_2);
            face_2 = [a1', b1', c1';
                c2', b2',a2' ];
            TR_2 = triangulation(face_2, vert_2);
            hold on, trisurf(TR_2, 'FaceColor', 'g'), hold off
            clear xx yy zz a1 b1 c1 a2 b2 c2 
        elseif find(ismember(mat,pts(3,:),'rows')) & find(ismember(mat,pts(4,:),'rows'))
            if hole == 0 & ~isempty(LaaPt2) & (LaaPt1 ~= LaaPt2)
                % Divide edge in 3 sections
                Ind = dsearchn(P,[LaaPt1; LaaPt2]);
                C = P(Ind,:);
                cl_LAA1 = P(Ind(1),:);
                cl_LAA2 = P(Ind(2),:);
                hold on, plot3(C(:,1), C(:,2), C(:,3),'*g', 'LineWidth',2 ), hold off
                clear C Ind 
                LAA_pts = [cl_LAA1; cl_LAA2];
                % Find nearest point to 1st point of edge
                k = dsearchn(LAA_pts, mat(1,:));
                [~,ik] = intersect(mat, LAA_pts(k,:),'rows');
                sect_1 = mat([1:ik],:);
                % Compute 2nd section
                LAA_pts(k,:) = [];
                [~,ikk] = intersect(mat, LAA_pts,'rows');
                nb = [ik,ikk];
                nb = sort(nb);
                sect_2 = mat(nb(1):nb(2),:);
                % Compute 3rd section
                sect_3 = mat([ikk:end],:);
                hold on, plot3(sect_1(:,1), sect_1(:,2), sect_1(:,3), 'r', 'LineWidth', 2 )
                plot3(sect_2(:,1), sect_2(:,2), sect_2(:,3), 'g', 'LineWidth', 2 )
                plot3(sect_3(:,1), sect_3(:,2), sect_3(:,3), 'k', 'LineWidth', 2 ), hold off
                clear LAA_pts k ik ikk 
                % Generate surfaces        
                TR_3 = ClosedEdge_OC(sect_2, LAA);
                hold on, trisurf(TR_3, 'FaceColor', 'c'), hold off
                %
                LAA_pts = [cl_LAA1; cl_LAA2];
                iq = dsearchn(LAA_pts, [mat(1,:); mat(end,:)] );
                eg_LAA = [sect_1; LAA_pts(iq(1),:); LAA; LAA_pts(iq(2),:); sect_3];
                clear LAA_pts iq
                la = [LSPV;mv];
                kk = dsearchn(la, eg_LAA(1,:));
                if kk == 1
                    xx = linspace(int_p(1), mv(1), length(eg_LAA))'; yy = linspace(int_p(2), mv(2), length(eg_LAA))'; 
                    zz = linspace(int_p(3), mv(3), length(eg_LAA))';
%                     hold on, plot3(xx,yy,zz,'k*'), hold off
                else
                    xx = linspace(mv(1), int_p(1), length(eg_LAA))'; yy = linspace(mv(2), int_p(2), length(eg_LAA))'; 
                    zz = linspace(mv(3), int_p(3), length(eg_LAA))';
%                     hold on, plot3(xx,yy,zz,'k*'), hold off
                end
                clear kk
                vert_4 = [eg_LAA; xx, yy, zz]; 
                a1 = length(eg_LAA)+1 : length(vert_4)-1; b1 = length(eg_LAA)+2 : length(vert_4); c1 = 1 : length(eg_LAA)-1;
                a2 = 1 : length(eg_LAA)-1; b2 = 2 : length(eg_LAA); c2 = length(eg_LAA)+2 : length(vert_4);
                face_4 = [a1', b1', c1';
                    c2', b2',a2' ];
                TR_4 = triangulation(face_4, vert_4);
                hold on, trisurf(TR_4, 'FaceColor', 'g'), hold off
                clear xx yy zz a1 b1 c1 a2 b2 c2 

            elseif hole == 0 & (isempty(LaaPt2)|(LaaPt1 == LaaPt2))
                la = [LSPV;mv];
                kk = dsearchn(la, mat(1,:));
                if kk == 1
                    xx = linspace(int_p(1), mv(1), length(mat))'; yy = linspace(int_p(2), mv(2), length(mat))'; 
                    zz = linspace(int_p(3), mv(3), length(mat))';
%                     hold on, plot3(xx,yy,zz,'k*'), hold off
                else
                    xx = linspace(mv(1), int_p(1), length(mat))'; yy = linspace(mv(2), int_p(2), length(mat))'; 
                    zz = linspace(mv(3), int_p(3), length(mat))';
%                     hold on, plot3(xx,yy,zz,'k*'), hold off
                end
                clear kk
                vert_3 = [mat; xx, yy, zz]; 
                a1 = length(mat)+1 : length(vert_3)-1; b1 = length(mat)+2 : length(vert_3); c1 = 1 : length(mat)-1;
                a2 = 1 : length(mat)-1; b2 = 2 : length(mat); c2 = length(mat)+2 : length(vert_3);                
                face_3 = [a1', b1', c1';
                    c2', b2',a2' ];               
                TR_3 = triangulation(face_3, vert_3);
                hold on, trisurf(TR_3, 'FaceColor', 'g'), hold off
                TR_4.ConnectivityList = []; TR_4.Points = [];
                clear xx yy zz a1 b1 c1 a2 b2 c2 
            else
                % Fill LAA opening
                LAA = mean(LAA_vert);
                TR_3 = ClosedEdge_CC(LAA_vert, LAA);
                hold on, trisurf(TR_3, 'FaceColor', 'c'), hold off
                la = [LSPV;mv];
                kk = dsearchn(la, mat(1,:));
                if kk == 1
                    xx = linspace(int_p(1), mv(1), length(mat))'; yy = linspace(int_p(2), mv(2), length(mat))'; 
                    zz = linspace(int_p(3), mv(3), length(mat))';
%                     hold on, plot3(xx,yy,zz,'k*'), hold off
                else
                    xx = linspace(mv(1), int_p(1), length(mat))'; yy = linspace(mv(2), int_p(2), length(mat))'; 
                    zz = linspace(mv(3), int_p(3), length(mat))';
%                     hold on, plot3(xx,yy,zz,'k*'), hold off
                end
                clear kk
                vert_4 = [mat; xx, yy, zz]; 
                a1 = length(mat)+1 : length(vert_4)-1; b1 = length(mat)+2 : length(vert_4); c1 = 1 : length(mat)-1;
                a2 = 1 : length(mat)-1; b2 = 2 : length(mat); c2 = length(mat)+2 : length(vert_4);
                face_4 = [a1', b1', c1';
                    c2', b2',a2' ];
                TR_4 = triangulation(face_4, vert_4);
                hold on, trisurf(TR_4, 'FaceColor', 'g'), hold off
                clear xx yy zz a1 b1 c1 a2 b2 c2 
            end
        else
            [TR_5] = ClosedEdge_OC(mat, int_p);                 % Make surface with intersecting point
            hold on, trisurf(TR_5, 'FaceColor', 'b'), hold off
        end
    end
    
    %% Join all the surfaces as one
    meshes = repmat(struct('faces',[],'vertices',[]),6,1);
    
    meshes(1).faces = ant_faces; meshes(1).vertices = vertices;
    meshes(2).faces = TR_1.ConnectivityList; meshes(2).vertices = TR_1.Points;
    meshes(3).faces = TR_2.ConnectivityList; meshes(3).vertices = TR_2.Points;
    meshes(4).faces = TR_3.ConnectivityList; meshes(4).vertices = TR_3.Points;
    meshes(5).faces = TR_4.ConnectivityList; meshes(5).vertices = TR_4.Points;
    meshes(6).faces = TR_5.ConnectivityList; meshes(6).vertices = TR_5.Points;
    
    new_mesh = spm_mesh_join(meshes);
    new_F = new_mesh.faces;
    new_V = new_mesh.vertices;
    TRR = triangulation(flip(new_F,2), new_V);
%     CheckFaceNormals(TRR);
%     figure,trisurf(TRR);
%     stlwrite(TRR, 'new_anterior.stl')
    %% Evaluate volumetric change
    Tri = TRR.ConnectivityList; V = TRR.Points; 
    % Face vertices
    V1=V(Tri(:,1),:); V2=V(Tri(:,2),:); V3=V(Tri(:,3),:);
    % Face centroids
    C=(V1+V2+V3)/3;
    % Face normals
    FN=cross(V2-V1,V3-V1,2);
    % Volume
    Vol = sum(dot(C,FN,2))/6;
    ant_vol(lo) = abs(Vol/1000)
    clearvars -except selpath_LA LA_files lo ant_vol new_wb
end
figure, plot(ant_vol)

