%% The code is written by Behnaz Pirzamanbein, bepi@dtu.dk last version 2020.08.04
%% Transformation
addpath(genpath('.\Functions'))

samples = {'out_out_CD','out_out_MD','in_in_CD','in_in_MD'};
layer_name = {'outer','inner'};

%% move the mesh between the degree
% go from 
% 0dg 2D (points) to 0dg 2D in 180 then 0dg 3D in 180 -> T0 transform then G180 transform
%  -   -             -   -         then T45 inverse 2D 0dg to 45 space then G45
%  -   -             -   -         then T90 inverse 2D 0dg to 90 space then G90
% this gives us the representation of 0 degree in 45 90 and 180
load_folder_Reg = 'Results\registration\layer\';
load_folder_layers = 'Results\layer\';

save_folder =  'Results\layer\transfer\0dg_rest\';
if ~exist(save_folder,'dir')
    mkdir(save_folder)
end

% if you want to run the measure separately you can use the for loop
%for i = 1:4
    sample = samples{i};
    disp(['Load Data ',sample])
            
    % registrtaion
    ref = load(['Results\registration\tform\reg_tfrom_inner_',sample]);
    sz180 = size(ref.dg180_inner);
    sz90 = size(ref.dg90_inner);
    sz45 = size(ref.dg45_inner);
    sz0 = size(ref.dg0_inner);
    
    % T
    tform0 = ref.Reg_dg0_inner.Transformation;
    tform45 = ref.Reg_dg45_inner.Transformation;
    tform90 = ref.Reg_dg90_inner.Transformation;
    
    % location of the mesh (points) in 180 degree;
    dg180 = load([load_folder_Reg,'Reg_inner_points_',sample,'_180']);
    dg0 = load([load_folder_Reg,'Reg_inner_points_',sample,'_0']);
    dg45 = load([load_folder_Reg,'Reg_inner_points_',sample,'_45']);
    dg90 = load([load_folder_Reg,'Reg_inner_points_',sample,'_90']);
    
    % meshes in 3D as reference for G transform
    inner180 = load([load_folder_layers,'inner_converted_layer_',sample,'_180']); % 3D surface
    outer180 = load([load_folder_layers,'outer_converted_layer_',sample,'_180']); % 3D surface
    
    inner90 = load([load_folder_layers,'inner_converted_layer_',sample,'_90']); % 3D surface
    outer90 = load([load_folder_layers,'outer_converted_layer_',sample,'_90']); % 3D surface
    
    inner45 = load([load_folder_layers,'inner_converted_layer_',sample,'_45']); % 3D surface
    outer45 = load([load_folder_layers,'outer_converted_layer_',sample,'_45']); % 3D surface
    
    inner0 = load([load_folder_layers,'inner_converted_layer_',sample,'_0']); % 3D surface
    outer0 = load([load_folder_layers,'outer_converted_layer_',sample,'_0']); % 3D surface
    
    % 2D of 0dg T0 to 2D 180 dg -> 2D of 0 dg in 180 space to 3D of 0 dg in 180 space
    [inner_surface_0_180, outer_surface_0_180] = extrapolate_mesh(dg0.points,sz0,inner180,outer180,sz180);% this is equavalnt of reading load('['Results\layer\transfer\',sample,'_transfomred_90to180']')
    % to 45
    for s = 1:2
        [p_0to45(:,1,s), p_0to45(:,2,s)] = transformPointsInverse(tform45, dg0.points(:,1,s), dg0.points(:,2,s));
    end
    [inner_surface_0_45, outer_surface_0_45] = extrapolate_mesh(p_0to45,sz0,inner45,outer45,sz45);% this is equavalnt of reading load('['Results\layer\transfer\',sample,'_transfomred_90to180']')
    % 90
    for s = 1:2
        [p_0to90(:,1,s), p_0to90(:,2,s)] = transformPointsInverse(tform90, dg0.points(:,1,s), dg0.points(:,2,s));
    end
    [inner_surface_0_90, outer_surface_0_90] = extrapolate_mesh(p_0to90,sz0,inner90,outer90,sz90);% this is equavalnt of reading load('['Results\layer\transfer\',sample,'_transfomred_90to180']')
    
    if flag_fig
        p = 10;
        % outer
        for s = 1:2
            figure
            title([sample,' outer 0 to 45,90,180 dg surface: ',s])
            surf(outer_surface_0_180(1:p:end,1:p:end,1,s),outer_surface_0_180(1:p:end,1:p:end,2,s),outer_surface_0_180(1:p:end,1:p:end,3,s),...
                'EdgeColor','k','FaceColor','none','FaceAlpha',0.6, 'FaceLighting','phong')
            hold on 
            surf(outer_surface_0_45(1:p:end,1:p:end,1,s),outer_surface_0_45(1:p:end,1:p:end,2,s),outer_surface_0_45(1:p:end,1:p:end,3,s),...
                'EdgeColor','r','FaceColor','none','FaceAlpha',0.6, 'FaceLighting','phong')
            surf(outer_surface_0_90(1:p:end,1:p:end,1,s),outer_surface_0_90(1:p:end,1:p:end,2,s),outer_surface_0_90(1:p:end,1:p:end,3,s),...
                'EdgeColor','g','FaceColor','none','FaceAlpha',0.6, 'FaceLighting','phong')

            surf(outer0.surface_XYZ(1:p:end,1:p:end,1,s),outer0.surface_XYZ(1:p:end,1:p:end,2,s),outer0.surface_XYZ(1:p:end,1:p:end,3,s),...
                'EdgeColor','b','FaceColor','none','FaceAlpha',0.6, 'FaceLighting','phong')
            saveas(gcf,[save_folder,sample,'_outer_s',int2str(s),'.png'])
            
            % inner
            figure
            title([sample,' inner 0 to 45,90,180 dg surface: ',s])
            surf(inner_surface_0_180(1:p:end,1:p:end,1,s),inner_surface_0_180(1:p:end,1:p:end,2,s),inner_surface_0_180(1:p:end,1:p:end,3,s),...
                'EdgeColor','k','FaceColor','none','FaceAlpha',0.6, 'FaceLighting','phong')
            hold on 
            surf(inner_surface_0_45(1:p:end,1:p:end,1,s),inner_surface_0_45(1:p:end,1:p:end,2,s),inner_surface_0_45(1:p:end,1:p:end,3,s),...
                'EdgeColor','r','FaceColor','none','FaceAlpha',0.6, 'FaceLighting','phong')
            surf(inner_surface_0_90(1:p:end,1:p:end,1,s),inner_surface_0_90(1:p:end,1:p:end,2,s),inner_surface_0_90(1:p:end,1:p:end,3,s),...
                'EdgeColor','g','FaceColor','none','FaceAlpha',0.6, 'FaceLighting','phong')

            surf(inner0.surface_XYZ(1:p:end,1:p:end,1,s),inner0.surface_XYZ(1:p:end,1:p:end,2,s),inner0.surface_XYZ(1:p:end,1:p:end,3,s),...
                'EdgeColor','b','FaceColor','none','FaceAlpha',0.6, 'FaceLighting','phong')
            saveas(gcf,[save_folder,sample,'_inner_s',int2str(s),'.png'])
            
        end
    end
    if flag_save
        save([save_folder,sample],'inner0','outer0','inner_surface_0_45', 'outer_surface_0_45','inner_surface_0_90', 'outer_surface_0_90','inner_surface_0_180', 'outer_surface_0_180')
    end
    clear p_0to45 p_0to90 inner_surface_0_180 outer_surface_0_180 inner_surface_0_45 outer_surface_0_45 inner_surface_0_90 outer_surface_0_90
%end

%% move 90,45,0 dg to 180 dg 
load_folder_Reg = 'Results\registration\layer\';
load_folder_layers = 'Results\layer\';

save_folder =  'Results\layer\transfer\all_to_180dg\';
if ~exist(save_folder,'dir')
    mkdir(save_folder)
end

% if you want to run the measure separately you can use the for loop
%for i = 1:4
    sample = samples{i};
    disp(['Load Data ',sample])
            
    % registrtaion
    ref = load(['Results\registration\tform\reg_tfrom_inner_',sample]);
    
    % location of the mesh (points); this is same for inner and outer but
    % the interpolation value of 180 is different
    %dg180 = load(['Results\registration\layer\Reg_inner_points_',sample,'_180']);
    dg0 = load([load_folder_Reg,'Reg_inner_points_',sample,'_0']);
    dg45 = load([load_folder_Reg,'Reg_inner_points_',sample,'_45']);
    dg90 = load([load_folder_Reg,'Reg_inner_points_',sample,'_90']);
    
    %% extrapolate from 2D into 3D --- 90,45,0 dg to 180 dg:
    %load(['Results\layer\stitched\smoothed_layers_',sample,'_180'])% smooth 3D surface
    inner = load([load_folder_layers,'inner_converted_layer_',sample,'_180']); % 3D surface
    outer = load([load_folder_layers,'outer_converted_layer_',sample,'_180']); % 3D surface
    
    %% 90 dg 
    [inner_surface_90, outer_surface_90] = extrapolate_mesh(dg90.points,size(ref.dg90_inner),inner,outer,size(ref.dg180_inner),0);
    
    %% 45 dg 
    [inner_surface_45, outer_surface_45] = extrapolate_mesh(dg45.points,size(ref.dg45_inner),inner,outer,size(ref.dg180_inner),0);
    
    %% 0 dg   
    [inner_surface_0, outer_surface_0] = extrapolate_mesh(dg0.points,size(ref.dg0_inner),inner,outer,size(ref.dg180_inner),0);

    %% figure
    if flag_fig
        figure
        s = 1;
        p = 10;
        title([sample,' inner 0,45,90 to 180 dg'],'Interpreter','none')
        surf(inner.surface_XYZ(1:p:end,1:p:end,1,s),inner.surface_XYZ(1:p:end,1:p:end,2,s),inner.surface_XYZ(1:p:end,1:p:end,3,s),...
            'EdgeColor','none','FaceColor','r','FaceAlpha',0.2, 'FaceLighting','phong')
        hold on 
        surf(inner_surface_45(1:p:end,1:p:end,1,s),inner_surface_45(1:p:end,1:p:end,2,s),inner_surface_45(1:p:end,1:p:end,3,s),...
            'EdgeColor','none','FaceColor','r','FaceAlpha',0.4, 'FaceLighting','phong')
        surf(inner_surface_90(1:p:end,1:p:end,1,s),inner_surface_90(1:p:end,1:p:end,2,s),inner_surface_90(1:p:end,1:p:end,3,s),...
            'EdgeColor','none','FaceColor','g','FaceAlpha',0.4, 'FaceLighting','phong')

        surf(inner_surface_0(1:p:end,1:p:end,1,s),inner_surface_0(1:p:end,1:p:end,2,s),inner_surface_0(1:p:end,1:p:end,3,s),...
            'EdgeColor','none','FaceColor','b','FaceAlpha',0.4, 'FaceLighting','phong')
        saveas(gcf,[save_folder,sample,'_inner_allto180.png'])

        figure
        title([sample,' outer 0,45,90 to 180 dg'],'Interpreter','none')
        surf(outer.surface_XYZ(1:p:end,1:p:end,1,s),outer.surface_XYZ(1:p:end,1:p:end,2,s),outer.surface_XYZ(1:p:end,1:p:end,3,s),...
            'EdgeColor','none','FaceColor','k','FaceAlpha',0.2, 'FaceLighting','phong')
        hold on 
        surf(outer_surface_45(1:p:end,1:p:end,1,s),outer_surface_45(1:p:end,1:p:end,2,s),outer_surface_45(1:p:end,1:p:end,3,s),...
            'EdgeColor','none','FaceColor','r','FaceAlpha',0.4, 'FaceLighting','phong')
        surf(outer_surface_90(1:p:end,1:p:end,1,s),outer_surface_90(1:p:end,1:p:end,2,s),outer_surface_90(1:p:end,1:p:end,3,s),...
            'EdgeColor','none','FaceColor','g','FaceAlpha',0.4, 'FaceLighting','phong')

        surf(outer_surface_0(1:p:end,1:p:end,1,s),outer_surface_0(1:p:end,1:p:end,2,s),outer_surface_0(1:p:end,1:p:end,3,s),...
            'EdgeColor','none','FaceColor','b','FaceAlpha',0.4, 'FaceLighting','phong')
        saveas(gcf,[save_folder,sample,'_outer_allto180.png'])
    end
    %% save the mesh
    if flag_save
        save([save_folder,sample,'_transfomred_90to180'],'inner_surface_90', 'outer_surface_90')
        save([save_folder,sample,'_transfomred_45to180'],'inner_surface_45', 'outer_surface_45')
        save([save_folder,sample,'_transfomred_0to180'],'inner_surface_0', 'outer_surface_0')
    end    
%end
 
%% move all the bend to the concave part for inner

load_folder_layers = 'Results\layer\transfer\0dg_rest\';

save_folder = 'Results\layer\transfer\align_0dg_rest\';
if ~exist(save_folder,'dir')
    mkdir(save_folder)
end

% if you want to run the measure separately you can use the for loop
%for i = 1
    sample = samples{i};
    disp(['Load Data ',sample])

    load([load_folder_layers,sample])
    sz0 = size(outer0.surface_XYZ);
    
    p = 10;
    for s = 1:2

        % move dg 180,90,45 to the most concave part
        % 180 dg
        X180 = outer_surface_0_180(:,:,1,s);
        Y180 = outer_surface_0_180(:,:,2,s);
        Z180 = outer_surface_0_180(:,:,3,s);
        
        mY180 = min(Y180,[],'all');
        indmY180 = find(Y180 == mY180);
        mY180X = X180(indmY180);
        mY180Z = Z180(indmY180);
        
        mX180 = min(X180,[],'all');
        MX180 = max(X180,[],'all');
        rX0 =  (mY180X - mX180)/(MX180-mX180);
        
        nX180 = X180;
        nY180 = Y180 - mY180;
        nZ180 = Z180;
        
        aligned_outer_dg180(:,:,:,s) = cat(3,nX180,nY180,nZ180);
        
        % same translation on inner
        X180_inner = inner_surface_0_180(:,:,1,s);
        Y180_inner = inner_surface_0_180(:,:,2,s);
        Z180_inner = inner_surface_0_180(:,:,3,s);
        
        nX180_inner = X180_inner;
        nY180_inner = Y180_inner - mY180;
        nZ180_inner = Z180_inner;

        aligned_inner_dg180(:,:,:,s) = cat(3,nX180_inner,nY180_inner,nZ180_inner);
        
        % 45 dg
        X45 = outer_surface_0_45(:,:,1,s);
        Y45 = outer_surface_0_45(:,:,2,s);
        Z45 = outer_surface_0_45(:,:,3,s);

        mY45 = min(Y45,[],'all');
        indmY45 = find(Y45 == mY45);
        mY45X = X45(indmY45);
        mY45Z = Z45(indmY45);
        
        nX45 = X45 - (mY45X - mY180X);
        nY45 = Y45 - mY45;
        nZ45 = Z45;
        
        aligned_outer_dg45(:,:,:,s) = cat(3,nX45,nY45,nZ45);
        
        % same translation on inner
        X45_inner = inner_surface_0_45(:,:,1,s);
        Y45_inner = inner_surface_0_45(:,:,2,s);
        Z45_inner = inner_surface_0_45(:,:,3,s);
                     
        nX45_inner = X45_inner - (mY45X - mY180X);
        nY45_inner = Y45_inner - mY45;
        nZ45_inner = Z45_inner;        
        
        aligned_inner_dg45(:,:,:,s) = cat(3,nX45_inner,nY45_inner,nZ45_inner);
        
        % 90 dg
        X90 = outer_surface_0_90(:,:,1,s);
        Y90 = outer_surface_0_90(:,:,2,s);
        Z90 = outer_surface_0_90(:,:,3,s);
        
        mY90 = min(Y90,[],'all');
        indmY90 = find(Y90 == mY90);
        mY90X = X90(indmY90);
        mY90Z = Z90(indmY90);
        
        nX90 = X90 - (mY90X - mY180X);
        nY90 = Y90 - mY90;
        nZ90 = Z90;
        
        aligned_outer_dg90(:,:,:,s) = cat(3,nX90,nY90,nZ90);
        
        % same translation on inner
        X90_inner = inner_surface_0_90(:,:,1,s);
        Y90_inner = inner_surface_0_90(:,:,2,s);
        Z90_inner = inner_surface_0_90(:,:,3,s);
                
        nX90_inner = X90_inner - (mY90X - mY180X);
        nY90_inner = Y90_inner - mY90;
        nZ90_inner = Z90_inner;  

        aligned_inner_dg90(:,:,:,s) = cat(3,nX90_inner,nY90_inner,nZ90_inner);
        
        % 0 dg
        % since there is no concavity we move it according to the ratio of
        % the lenght on each side of the curve
        X0 = outer0.surface_XYZ(:,:,1,s);
        Y0 = outer0.surface_XYZ(:,:,2,s);
        Z0 = outer0.surface_XYZ(:,:,3,s);
        
        mY0 = min(Y0,[],'all');
        mX0 = min(X0,[],'all');
        
        lX = max(X0,[],'all') - mX0;
        indmX0 = find(X0 < lX*rX0);
        mX0Y180X = X0(indmX0(end)) - mY180X;
        
        nX0 = X0 - (mX0Y180X - mX0);
        nY0 = Y0 - ( mY0);
        nZ0 = Z0;
        
        aligned_outer_dg0(:,:,:,s) = cat(3,nX0,nY0,nZ0);
        
        % inner
        X0_inner = inner0.surface_XYZ(:,:,1,s);
        Y0_inner = inner0.surface_XYZ(:,:,2,s);
        Z0_inner = inner0.surface_XYZ(:,:,3,s);
              
        nX0_inner = X0_inner - (mX0Y180X - mX0);
        nY0_inner = Y0_inner - ( mY0);
        nZ0_inner = Z0_inner;
        
        aligned_inner_dg0(:,:,:,s) = cat(3,nX0_inner,nY0_inner,nZ0_inner);
        
        if flag_fig 
            % illustration
            figure(1)
            subplot(122)
            title('aligned')
            hold on
            surf(nX0(1:p:end,1:p:end),nY0(1:p:end,1:p:end),nZ0(1:p:end,1:p:end),...
                   'EdgeColor','b','FaceColor','none','FaceAlpha',0.4, 'FaceLighting','phong') 
            surf(nX45(1:p:end,1:p:end),nY45(1:p:end,1:p:end),nZ45(1:p:end,1:p:end),...
                   'EdgeColor','r','FaceColor','none','FaceAlpha',0.4, 'FaceLighting','phong')
            surf(nX90(1:p:end,1:p:end),nY90(1:p:end,1:p:end),nZ90(1:p:end,1:p:end),...
                   'EdgeColor','g','FaceColor','none','FaceAlpha',0.4, 'FaceLighting','phong')
            surf(nX180(1:p:end,1:p:end),nY180(1:p:end,1:p:end),nZ180(1:p:end,1:p:end),...
                   'EdgeColor','k','FaceColor','none','FaceAlpha',0.4, 'FaceLighting','phong')
            % inner
            surf(nX0_inner(1:p:end,1:p:end),nY0_inner(1:p:end,1:p:end),nZ0_inner(1:p:end,1:p:end),...
                   'EdgeColor','b','FaceColor','none','FaceAlpha',0.4, 'FaceLighting','phong')
            surf(nX45_inner(1:p:end,1:p:end),nY45_inner(1:p:end,1:p:end),nZ45_inner(1:p:end,1:p:end),...
                   'EdgeColor','r','FaceColor','none','FaceAlpha',0.4, 'FaceLighting','phong')
            surf(nX90_inner(1:p:end,1:p:end),nY90_inner(1:p:end,1:p:end),nZ90_inner(1:p:end,1:p:end),...
                   'EdgeColor','g','FaceColor','none','FaceAlpha',0.4, 'FaceLighting','phong')
            surf(nX180_inner(1:p:end,1:p:end),nY180_inner(1:p:end,1:p:end),nZ180_inner(1:p:end,1:p:end),...
                   'EdgeColor','k','FaceColor','none','FaceAlpha',0.4, 'FaceLighting','phong')
            axis image
            view(0,90)
            hold off
            % before transformation
            subplot(121)
            title('original')
            hold on
            surf(X0(1:p:end,1:p:end),Y0(1:p:end,1:p:end),Z0(1:p:end,1:p:end),...
                   'EdgeColor','none','FaceColor','b','FaceAlpha',0.2, 'FaceLighting','phong')
            surf(X45(1:p:end,1:p:end),Y45(1:p:end,1:p:end),Z45(1:p:end,1:p:end),...
                   'EdgeColor','none','FaceColor','r','FaceAlpha',0.2, 'FaceLighting','phong')
            surf(X90(1:p:end,1:p:end),Y90(1:p:end,1:p:end),Z90(1:p:end,1:p:end),...
                   'EdgeColor','none','FaceColor','g','FaceAlpha',0.2, 'FaceLighting','phong')
            surf(X180(1:p:end,1:p:end),Y180(1:p:end,1:p:end),Z180(1:p:end,1:p:end),...
                   'EdgeColor','none','FaceColor','k','FaceAlpha',0.2, 'FaceLighting','phong')
            % inner   
            surf(X0_inner(1:p:end,1:p:end),Y0_inner(1:p:end,1:p:end),Z0_inner(1:p:end,1:p:end),...
                   'EdgeColor','none','FaceColor','b','FaceAlpha',0.2, 'FaceLighting','phong')
            surf(X45_inner(1:p:end,1:p:end),Y45_inner(1:p:end,1:p:end),Z45_inner(1:p:end,1:p:end),...
                   'EdgeColor','none','FaceColor','r','FaceAlpha',0.2, 'FaceLighting','phong')
            surf(X90_inner(1:p:end,1:p:end),Y90_inner(1:p:end,1:p:end),Z90_inner(1:p:end,1:p:end),...
                   'EdgeColor','none','FaceColor','g','FaceAlpha',0.2, 'FaceLighting','phong')
            surf(X180_inner(1:p:end,1:p:end),Y180_inner(1:p:end,1:p:end),Z180_inner(1:p:end,1:p:end),...
                   'EdgeColor','none','FaceColor','k','FaceAlpha',0.2, 'FaceLighting','phong')
            view(0,90)
            axis image
            hold off
        end
    end
    if flag_fig
        legend('0 dg','45 dg','90 dg','180 dg')
        suptitle_modified(sample,'none')
        saveas(gcf,[save_folder,sample,'_aligned.png'])
    end
    
    if flag_save
        save([save_folder,sample,'_aligned'],'aligned_outer_dg0','aligned_inner_dg0','aligned_outer_dg45','aligned_inner_dg45','aligned_outer_dg90','aligned_inner_dg90','aligned_outer_dg180','aligned_inner_dg180')
    end
    clear aligned_outer_dg0 aligned_inner_dg0 aligned_outer_dg45 aligned_inner_dg45 aligned_outer_dg90 aligned_inner_dg90 aligned_outer_dg180 aligned_inner_dg180
%end