clc,clear all, close all
%% Import and visualize LA surface
selpath = uigetdir;
region_files = dir(fullfile(selpath,'*.mat'));
selpath_surf = uigetdir;
LA_files = dir(fullfile(selpath_surf,'*.stl'));
for f = 1:length(region_files)                                          % To load all stl LAA_files
    filename = fullfile(selpath,region_files(f).name);
    load(filename)
    clearvars -except selpath region_files selpath_surf LA_files f filename wb mv mp cv_len
    LA_surface = stlread(fullfile(selpath_surf,LA_files(f).name));
    LA = []; LA.faces = LA_surface.ConnectivityList; LA.vertices = LA_surface.Points;
    figure, trisurf(LA_surface, 'FaceColor','flat', 'LineStyle', 'none','FaceAlpha',1); axis equal
    hold on, plot3([mv(1) mp(1) wb(1)], [mv(2) mp(2) wb(2)], [mv(3) mp(3) wb(3)], '*k','LineWidth',2); 
    plot3([mv(1) wb(1)], [mv(2) wb(2)], [mv(3) wb(3)], '--r','LineWidth',2);hold off
    xlabel('X-axis'),ylabel('Y-axis'),zlabel('Z-axis')
%     axis([0 25 -15 15 -150 -120]);
    x_tick = xticks; y_tick = yticks; z_tick = zticks;
    %% Draw plane perpendicular to long axis and passing through midpoint
    vv = mv - wb;                             % Vector from MV to weighted barycenter(on the surface)
    [xn yn zn] = norm_plane(mp,vv);
    % [xp yp zp] = plane_fit(X',Y',Z')
    hold on,ss = surf(xn,yn,zn,'LineStyle', 'none','FaceAlpha',0); hold off
    clear ss
    %% Scale the plane
    [Sx, Sy, Sz] = scaled_plane(xn,yn,zn,2);
    hold on, ts = surf(Sx, Sy, Sz,'FaceColor','b','LineStyle', 'none', 'FaceAlpha',0); hold off
    clear xn yn zn Sx Sy Sz
    %% Translate the plane towards MV
    nAB = vv ./ norm(vv);   % Normalize
    ab = 5;
    n = 7;
    t = linspace(0,1,n+1)';
    xy = (1-t)*mp + t*mv;
    xy([1 end],:)=[];                                       % Remove mp and mv coordinates
    hold on, plot3(xy(:,1),xy(:,2),xy(:,3),'b-o'), hold off
    for dd = 1:size(xy,1)                                        % No of planes
        % mC = mp + ab * nAB;                      % Check incremental distance  'ab'
        % inc = (mC - mp);                % Initial displacement of scaled ostium plane
        inc = (xy(dd,:) - mp);
        XD = get(ts, 'XData'); YD = get(ts, 'YData'); ZD = get(ts, 'ZData');
        hold on, sf = surf([XD + inc(1)], [YD + inc(2)], [ZD + inc(3)], ...
            'FaceColor','r','LineStyle', 'none','FaceAlpha',0); hold off
        % ab = ab + 3;
        clear mC inc XD YD ZD
        %% Surface triangulation
        tri = delaunay(sf.XData,sf.YData); 
        tt = triangulation(tri,sf.XData(:),sf.YData(:),sf.ZData(:));
        clear sf
        %% Compute intersecting profile curve
        surf1 = []; surf1.faces = tt.ConnectivityList; surf1.vertices = tt.Points;
        [~, intSurface] = SurfaceIntersection(LA, surf1);
        px=intSurface.vertices(:,1); py=intSurface.vertices(:,2); pz=intSurface.vertices(:,3);
        clear tt tri
        %% Arrange vertices
        cx = mean(px); cy = mean(py);
        a = atan2(py - cy, px - cx);
        [~, order] = sort(a, 'descend');
        sortedX = px(order); sortedY = py(order); sortedZ = pz(order);
        hold on, plot3([sortedX; sortedX(1)],[sortedY; sortedY(1)],[sortedZ; sortedZ(1)],'b', LineWidth=2), hold off
        clear cx cy a order px py pz
        %% Compute length of the profile curve
        cv_len(f,dd) = curve_length(sortedX,sortedY,sortedZ);
    end
    clear ab nAB
end