function varargout = alignWithTargetPoints(varargin)
% function varargout = alignWithTargetPoints(varargin)
%
% Description:
%
% Utility function to align a surface and its associated landmarks with
% a target point set. This function may be used prior to warpSurface to
% minimise the extend of warping to be performed using MCT
%
% Author: Johann Strasser
% Date: 080114

% Parameters
targetPoints = [];
sourcePoints = [];
sourceSurface = [];

% 'svd' calculates rotation using singular value decomposition, uses
% AIH function pcalib_rotateshape in PCALib
% 'pca' calculates rotation based on PCA on both point sets.

method = 'svd';
verbose = 0;
axesHandle = -1;
targetColours = ['y'; 'r'];
sourceColours = ['b'; 'g'];
alignedColours = ['g'; 'b'];
targetLabel = 'Target Point Set';
sourceLabel = 'Source Point Set';
alignedLabel = 'Aligned Point Set';
edges = [];

for i=1:2:length(varargin)
    switch lower(varargin{i})
        case 'targetpoints'
            targetPoints = varargin{i+1};
        case 'sourcepoints'
            sourcePoints = varargin{i+1};
        case 'sourcesurface'
            sourceSurface = varargin{i+1};
        case 'method'
            method = varargin{i+1};
        case 'verbose'
            verbose = lower(varargin{i+1});
        case 'axeshandle'
            axesHandle = lower(varargin{i+1});
        case 'targetcolours'
            targetColours = lower(varargin{i+1});
        case 'sourcecolours'
            sourceColours = lower(varargin{i+1});
        case 'alignedcolours'
            alignedColours = lower(varargin{i+1});
        case 'targetlabel'
            targetLabel = lower(varargin{i+1});
        case 'sourcelabel'
            sourceLabel = lower(varargin{i+1});
        case 'alignedlabel'
            alignedLabel = lower(varargin{i+1});
        case 'edges'
            edges = varargin{i+1};
        otherwise
            error(['Unknown parameter name passed to ', mfilename, '.  Name was ' varargin{i}])
    end
end

if isempty(targetPoints)
    error([mfilename, ': Parameter ''targetPoints'' not specified or empty.']);
end

if isempty(sourcePoints)
    error([mfilename, ': Parameter ''sourcePoints'' not specified or empty.']);
end

if isempty(sourceSurface)
    error([mfilename, ': Parameter ''sourceSurface'' not specified or empty.']);
end

sourcePointsBackup = sourcePoints;
targetPointsBackup = targetPoints;

alignedPoints = sourcePoints;
alignedSurface = sourceSurface;
dim = size(targetPoints, 2);

if isequal(method, 'pca')

    % Method #2
    % BEGIN PCA alignment
    % Do the alignment manually with centroid translations and pca-derived
    % rotations. See the journal for details.
    
    % Template points
    sourceMean = mean(sourcePoints);
    sourceCov = cov(sourcePoints);
    [sourceRot, sourceEigVals] = eig(sourceCov);
    
    % Reference points
    targetMean = mean(targetPoints); % Is centred at the origin already
    targetCov = cov(targetPoints);
    [targetRot, targetEigVals] = eig(targetCov);
    
    % We transform the surface vertices using
    % targetMean * targetRot * sourceRot^1 * sourceMean^1 * sourceSurface
    % Note that the source surface vertices are in global space and not local space,
    % hence we pre-multiply the reference transformations in the above
    % equations
    % From back to front:
    
    % The following works. Note the non-general 180 degree rotation.
    r = makehgtform('xrotate', pi);
    alignedPoints = sourcePoints;
    alignedPoints = alignedPoints - repmat(sourceMean, size(alignedPoints, 1), 1);
    alignedPoints = r(1:3, 1:3) * sourceRot' * alignedPoints';
    alignedPoints = targetRot * alignedPoints;
    alignedPoints = alignedPoints';
    alignedPoints = alignedPoints + repmat(targetMean, size(alignedPoints, 1), 1);
    
    aSurfVerticesTemp = sourceSurface.vertices - repmat(sourceMean, size(sourceSurface.vertices, 1), 1);
    aSurfVerticesTemp = r(1:3, 1:3) * sourceRot' * aSurfVerticesTemp';
    aSurfVerticesTemp = targetRot * aSurfVerticesTemp;
    aSurfVerticesTemp = aSurfVerticesTemp';
    aSurfVerticesTemp = aSurfVerticesTemp + repmat(targetMean, size(aSurfVerticesTemp, 1), 1);
    alignedSurface.vertices = aSurfVerticesTemp;
    
    % % For testing centre target points at mean and align with its main axis
    % targetPoints = targetPoints - repmat(targetMean, size(targetPoints, 1), 1);
    % targetPoints = targetRot' * targetPoints';
    % targetPoints = targetPoints';
    % END PCA alignment

elseif isequal(method, 'svd')

    % Method #3
    % Calculate rotation matrix to rotate from mean centred source point set
    % to target point set
    sTemp = sourcePoints';
    [sTemp, sMu] = pcalib_centershape(sTemp(:), dim);
    sourceMean = sMu';

    % Centre target points
    tTemp = targetPoints';
    [tTemp, tMu] = pcalib_centershape(tTemp(:), dim);
    targetMean = tMu';
    targetMeanMat = repmat(targetMean, size(targetPoints, 1), 1);
    
    % Rotate source point set and shift it to the target point sets
    % centroid
    [sTempRotated, sRot] = pcalib_rotateshape(sTemp, tTemp);
    alignedPoints = sTempRotated' + targetMeanMat;

    % Centre source surface and apply rotation
    aSurfVerticesTemp = sourceSurface.vertices - repmat(sourceMean, size(sourceSurface.vertices, 1), 1);
    aSurfVerticesTemp = (sRot * aSurfVerticesTemp')';
    alignedSurface.vertices = aSurfVerticesTemp + repmat(targetMean, size(sourceSurface.vertices, 1), 1);

    % verbose = 1;
    if verbose
        if ~ishandle(axesHandle)
           figure('Position', [50, 50, 660, 660]);
            axesHandle = gca;
        end

        hold on;
        view(3);
        axis vis3d;
        box on;

        targetHandle = plot3(axesHandle, targetPoints(:, 1), targetPoints(:, 2), ...
            targetPoints(:, 3), 'o', 'MarkerFaceColor', targetColours(1, :), 'MarkerEdgeColor', targetColours(2, :));
%         targetTextHandle = text(targetPoints(1, 1), targetPoints(1, 2), ...
%             targetPoints(1, 3), targetLabel, 'Color', targetColours(2, :), ...
%             'BackgroundColor', targetColours(1, :), 'EdgeColor', targetColours(2, :));

        for i=1:size(edges,1)
            targetEdgeHandle = plot3(axesHandle, [targetPoints(edges(i,1),1), targetPoints(edges(i,2), 1)],...
                [targetPoints(edges(i,1),2), targetPoints(edges(i,2),2)], [targetPoints(edges(i,1),3), targetPoints(edges(i,2),3)], ...
                '-', 'Color', targetColours(2, :), 'LineWidth', 2);
        end
        

        sourceHandle = plot3(axesHandle, sourcePoints(:, 1), sourcePoints(:, 2), ...
            sourcePoints(:, 3), 'o', 'MarkerFaceColor', sourceColours(1, :), 'MarkerEdgeColor', sourceColours(2, :));
%         sourceTextHandle = text(sourcePoints(1, 1), sourcePoints(1, 2), ...
%             sourcePoints(1, 3), sourceLabel, 'Color', sourceColours(2, :), ...
%             'BackgroundColor', sourceColours(1, :), 'EdgeColor', sourceColours(2, :));

        for i=1:size(edges,1)
            sourceEdgeHandle = plot3(axesHandle, [sourcePoints(edges(i,1),1), sourcePoints(edges(i,2), 1)],...
                [sourcePoints(edges(i,1),2), sourcePoints(edges(i,2),2)], [sourcePoints(edges(i,1),3), sourcePoints(edges(i,2),3)], ...
                '-', 'Color', sourceColours(2, :), 'LineWidth', 2);
        end


        alignedHandle = plot3(axesHandle, alignedPoints(:, 1), alignedPoints(:, 2), ...
            alignedPoints(:, 3), 'o', 'MarkerFaceColor', alignedColours(1, :), 'MarkerEdgeColor', alignedColours(2, :));
%         alignedTextHandle = text(alignedPoints(1, 1), alignedPoints(1, 2), ...
%             alignedPoints(1, 3), alignedLabel, 'Color', alignedColours(2, :), ...
%             'BackgroundColor', alignedColours(1, :), 'EdgeColor', alignedColours(2, :));

        for i=1:size(edges,1)
            alignedEdgeHandle = plot3(axesHandle, [alignedPoints(edges(i,1),1), alignedPoints(edges(i,2), 1)],...
                [alignedPoints(edges(i,1),2), alignedPoints(edges(i,2),2)], [alignedPoints(edges(i,1),3), alignedPoints(edges(i,2),3)], ...
                '-', 'Color', alignedColours(2, :), 'LineWidth', 2);
        end



        % Render source surface
        sourceSurfaceHandle = patch('Faces', sourceSurface.faces, 'Vertices', sourceSurface.vertices, ...
            'EdgeColor', 'none', 'FaceColor', [1, 0, 0], 'FaceLighting', 'gouraud',...
            'Parent', axesHandle);

        % Render aligned surface
        alignedSurfaceHandle = patch('Faces', alignedSurface.faces, 'Vertices', alignedSurface.vertices, ...
            'EdgeColor', 'none', 'FaceColor', [0, 0, 1], 'FaceLighting', 'gouraud',...
            'Parent', axesHandle, 'FaceAlpha', 1);

        axis tight;
        axis equal;
        view(3);
        axis vis3d;
        box on;
        
        axis ij;
        
        cameraPos = get(axesHandle, 'CameraPosition'); %
        lightHandle = light('Position', cameraPos);
        
        
%         view(-140, 10);
        
        view(264, 10); % Lateral viewpoint
%         view(-6, 10); % Adaxial viewpoint

%         legendHandles = [sourceHandle, targetHandle];
%         legendNames = {'Template', 'Model point set'};
%         l = legend(legendHandles, legendNames, 'Location', 'Northeast');
% 
%         
        legendHandles = [sourceHandle, alignedHandle, targetHandle];
        legendNames = {'Template', 'Aligned template', 'Model point set'};
        l = legend(legendHandles, legendNames, 'Location', 'Northeast');

        
        hold off;
    end
end

varargout{1} = alignedPoints;
varargout{2} = alignedSurface;

