function [ScapulaLandmarks, message] = findScapulaLandmarks(fv, options)
%FINDSCAPULALANDMARKS Find scapula landmarks from scapula surface file ply
%   Detailed explanation goes here

% Can only be run from lbovenus to avoid error
% Undefined function or variable 'smoothpatch_curvature_double'
% in function smoothpatch in function findScapulaLandmarks

% 


% it works only for the mesh that was segmented using our segmentation
% method that provides correspondences to our statistical shape model.
% fv.faces: contains the connectivity information of the surface mesh (25000 x 3)
% fv.vertices: information on the vertices of the mesh (12502 x 3)
% options stores some important information regarding the bone:
%      .side                    Can be 'left' or 'right'
%      .curvature_smoothing     Curvature smoothing parameter. Default value is 10
%      .verb                    step-by-step reporting. by default it is set to false
% out is a struct contains different measurements


% The function merge the part of doMeasuremnts (written by Elham
% Taghizadeh) that get the scapula landmarks. The anatomical measurements
% are no more performed here, but in database/measureSCase. The function
% should save the scapular surface, the glenoid surface, and the scapula
% landmarks.

% This is done by another function findScapulaLandmarks

% Comment by AT: This should be adapted to the
% ShoulderCase class We should add below the
% identification of the scapula landmarks and the
% glenoid surface. The scapula landmarks are currently
% embeded in function doMeasurements (by ET). Glenoid
% surface is currently obtained as a second step with
% function extractAllGlenoid and extractGlenoidSurface
% (by BN).

% Variable finalSurface is the argument of function
% doMeasurements to get landmarks and perform
% anatomical measurements. It might be replaced by a
% function getLanmarks.

% ***********************************************************
% Notes: landmarks.extremities (From Elham T.)
% (1) Bottom of the scapula  --> angulusInferior
% (2) Back of the scapula    --> trigonumSpinae
% (3) Tip of acromion        --> processusCoracoideus ? 
% (4) Top coracoid           --> acromioClavicular ?
% (5) Back coracoid          --> angulusAcromialis ?
% (6) Front scapula          --> spinoGlenoidNotch
% ************************************************************


%  Perform anatomical data save in this function instead of bharath2remove/extractAllGlenoid.m


%   loadOpenFlipperFV: within this file (adapted from doMeasurement version of Elham Taghizadeh)
%     Innput:
%     Output:
%   simplePlyRead: (original version of Elham Taghizadeh, in functions dir)
%     Innput:
%     Output:
%   simplePlyWrite: (original version of Elham Taghizadeh, in functions dir)
%     Innput:
%     Output:
%   loadmodel: load SSM (original version of Elham Taghizadeh, in functions dir)
%     Innput:
%     Output:
%   compute_curvature: functions/toolbox_graph
%     Innput:
%     Output:

% TODO
%   Check why goove landmarks can be less than 5

message = ''; % Added by AT

% Add path to segmentation functions
%path2Add = 'functions';
%addpath(path2Add);
addpath(genpath('./functions'));


% check the input's validity
if(~isfield(fv, 'faces'))
    error('The provided mesh is not in correct format: the field .faces is not provided');
end
if(~isfield(fv, 'vertices'))
    error('The provided mesh is not in correct format: the field .vertices is not provided');
end

sz_F = size(fv.faces);
sz_V = size(fv.vertices);

if (numel(sz_F)~=2 || sum(sz_F == [25000, 3])~=2 || numel(sz_V)~=2 || sum(sz_V == [12502, 3])~=2)
    error('The provided mesh is not in correct format.');
end


% AT: values of default options if not provided
if (nargin == 1)
    options.side = 'right';
    options.curvature_smoothing = 10;
    options.verb = 0;% 0: do not print progress bar; 1: print progress bar in the consol
else
    if(~isfield(options, 'side'))
        options.side = 'right';
    end
    if(~isfield(options, 'curvature_smoothing'))
        options.curvature_smoothing = 10;
    end
    if(~isfield(options, 'verb'))
        options.verb = 0;
    end
end

% AT: if side is right, do a relexion, for left? Ask Elham!
if(strcmp(options.side, 'right'))
    reflect = true;
else
    reflect = false;
end

%% The important vertices needed for measurements
% AT: Ask Elham what is ScapulaToExclude and GlenoidToExclude
[selectedVertices.ScapulaToExclude,   ~] = loadOpenFlipperFV(['.' filesep '00_vertices' filesep 'OpenFlipper_ScapulaToExclude.ini']);
[selectedVertices.ScapulaGroove,      ~] = loadOpenFlipperFV(['.' filesep '00_vertices' filesep 'OpenFlipper_Groove.ini']);
[selectedVertices.ScapulaPillar,      ~] = loadOpenFlipperFV(['.' filesep '00_vertices' filesep 'OpenFlipper_Pillar.ini']);
[selectedVertices.GlenoidToExclude,   ~] = loadOpenFlipperFV(['.' filesep '00_vertices' filesep 'OpenFlipper_GlenoidToExclude.ini']);
[selectedVertices.Spinoglenoidnotch,  ~] = loadOpenFlipperFV(['.' filesep '00_vertices' filesep 'OpenFlipper_Spinoglenoidnotch.ini']);
[selectedVertices.GlenoidSurface,     ~] = loadOpenFlipperFV(['.' filesep '00_vertices' filesep 'OpenFlipper_GlenoidSurface.ini']);
selectedVertices.ScapulaExtremities     = load(['.' filesep '00_vertices' filesep 'indiceVertices_ScapulaExtremities.txt']);

Sref = simplePlyRead(['.' filesep '00_vertices' filesep 'ReferenceShape.ply']);


%% "Choose" landmarks based on curvature

% AT: Choosing "best" vertices might cause error.
% This could be replaced by finding "new" points that best match the curvature criteria

% AT: S for shape, F for faces and V for vertices
S.FV = fv;
% Line below aleady commented by Elham
% [~, S.FV.vertices] = procrustes(Sref.vertices, fv.vertices, 'scaling', false, 'reflection', reflect);
% AT: procrustes(X,Y) = linear transformation between X and Y, returns the
% tranformed data and the transformation T.
[~, S.FV.vertices, T] = procrustes(Sref.vertices, fv.vertices, 'scaling', false, 'reflection', reflect);
%try to compute the curvature, if it does not work, we slightly smooth
%the surface and retry
temp = 1;
while(temp)
    try
        temp = 0;
        [S.Curvature.DirectionMinCurvature,...
            S.Curvature.DirectionMaxCurvature,...
            S.Curvature.MinCurvature,...
            S.Curvature.MaxCurvature,...
            S.Curvature.MeanCurvature,...
            S.Curvature.GaussianCurvature,...
            S.Curvature.Normal] = ...
            compute_curvature(S.FV.vertices, S.FV.faces, options);
    catch %...if an error occurs, we smooth the face and recompute the curvature
        S.FV = smoothpatch(S.FV, 1, 1, 0.0001);
        if(options.verb)
            disp('\tThe registered shape is smoothed to compute the curvature\n');
        end
        temp = 1;
    end
end

% find Landmarks based on Curvatures
temp_FV = S.FV;
temp_Curvature = S.Curvature;
temp_Landmarks.Extremities = temp_FV.vertices(selectedVertices.ScapulaExtremities,:);
% ----------- find the spinoglenoid notch -----------------
%select the vertice which has the smallest gaussian curvature
[~,temp] = min(temp_Curvature.GaussianCurvature(selectedVertices.Spinoglenoidnotch));
temp     = selectedVertices.Spinoglenoidnotch(temp);
temp_Landmarks.Extremities(6,:) = temp_FV.vertices(temp,:);


% ------ find the scapula groove landmarks ------
temp_dist = [12 44 28 20 36]; % AT: Are these numbers distance in mm? It might cause problem for partial scapulae
skipInd = [];
for iL=1:5
    %get all the candidate vertices
    temp_indices = selectedVertices.ScapulaGroove;
    temp_vertices = temp_FV.vertices(temp_indices,:);

    %get the vector front-scapula->back-scapula
    [temp_n,~]  = fitLineToPoints(temp_vertices);
    temp_n     = temp_n';
    temp_Lfront = temp_Landmarks.Extremities(6,:);
    %from the front-scapula, go X mm in direction of the back, and remove
    %all candidate vertices behind
    temp_origine = temp_Lfront + (temp_dist(iL)-2)*temp_n;
    for i=length(temp_indices):-1:1
        temp_p = temp_vertices(i,:)-temp_origine;
        if(dot(temp_p,temp_n)<0)
            temp_indices(i)=[];
            temp_vertices(i,:)=[];
        end
    end

    %from the front-scapula, go Y mm in direction of the back, and remove
    %all candidate vertices ahead
    temp_origine = temp_Lfront + (temp_dist(iL)+2)*temp_n;
    for i=length(temp_indices):-1:1
        temp_p = temp_vertices(i,:)-temp_origine;
        if(dot(temp_p,temp_n)>0)
            temp_indices(i)=[];
            temp_vertices(i,:)=[];
        end
    end

    %get the best vertice based on the min curvature
    if ~isempty(temp_indices)
        [temp, i] = min(temp_Curvature.MinCurvature(temp_indices));
        temp_Landmarks.Groove(iL,:) = temp_vertices(i,:);
    else
        skipInd = [skipInd iL];
    end
end
if(numel(skipInd) == 5)
    message = 'The shape is distorted! Could not find enough landmarks on the Groove';
    temp_Landmarks.Groove = temp_vertices; % Added by AT
    % return; % Commented by AT
else
    try % AT added try/catch because of error in line below
    temp_Landmarks.Groove(skipInd, :) = [];
    catch
        message = 'Less than 5 points remaing in groove after curvature optimisation';
        % Take back initial values
        temp_Landmarks.Groove = temp_vertices;
    end
    
end

% --------- find the scapula pillar landmarks ------
temp_dist = [30 58 44 37 51]; % AT: Same as for groove
for iL=1:5
    %get all the candidate vertices
    temp_indices = selectedVertices.ScapulaPillar;
    temp_vertices = temp_FV.vertices(temp_indices,:);

    %get the vector up->down
    [temp_n,~] = fitLineToPoints(temp_vertices);
    temp_n = temp_n';

    %check the vector point in the right direction
    temp_up = temp_Landmarks.Extremities(6,:);
    if(dot(temp_n, temp_FV.vertices(selectedVertices.ScapulaPillar(1,:))-temp_up)<0)
        temp_n =-temp_n;
    end
    %from the front-scapula, go X mm in direction of the back, and remove
    %all candidate vertices behind
    temp_origine = temp_up + (temp_dist(iL)-2)*temp_n;
    for i=length(temp_indices):-1:1
        temp_p = temp_vertices(i,:)-temp_origine;
        if(dot(temp_p,temp_n)<0)
            temp_indices(i)=[];
            temp_vertices(i,:)=[];
        end
    end

    %from the front-scapula, go Y mm in direction of the back, and remove
    %all candidate vertices ahead
    temp_origine = temp_up + (temp_dist(iL)+2)*temp_n;
    for i=length(temp_indices):-1:1
        temp_p = temp_vertices(i,:)-temp_origine;
        if(dot(temp_p,temp_n)>0)
            temp_indices(i)=[];
            temp_vertices(i,:)=[];
        end
    end

    %get the best vertice based on the max curvature
    [temp, i] = max(temp_Curvature.MaxCurvature(temp_indices));
        try % AT added try/catch because of error in line below
            temp_Landmarks.Pillar(iL,:) = temp_vertices(i,:);
        catch
            message = [message ' Less than 5 points remaing in pillar after curvature optimisation'];
            % Take back initial values
            temp_Landmarks.Pillar = temp_vertices;
        end
end

% ------------- save everyting (and glenoid surface) in the structure S----------------
temp_v = selectedVertices.GlenoidSurface(temp_Curvature.MaxCurvature(selectedVertices.GlenoidSurface)<1);
S.indiceVertices.GlenoidSurface = temp_v;
S.Landmarks = temp_Landmarks;


% AT: These final lines need to be better understood...

out = rmfield(S, 'FV'); % AT: Remove field FV from structure S and put in out
out = rmfield(out, 'indiceVertices');

% AT: What is T? Transformation obtained by procrustes (T.c = translation)
scapulaExtremities = bsxfun(@minus, out.Landmarks.Extremities, T.c(1, :)) * T.T';
scapulaGroove      = bsxfun(@minus, out.Landmarks.Groove, T.c(1, :)) * T.T';
scapulaPillar      = bsxfun(@minus, out.Landmarks.Pillar, T.c(1, :)) * T.T';

% ***********************************************************
% Notes: landmarks.extremities (From Elham T.)
% (1) Bottom of the scapula  --> angulusInferior
% (2) Back of the scapula    --> trigonumSpinae
% (3) Tip of acromion        --> processusCoracoideus ? 
% (4) Top coracoid           --> acromioClavicular ?
% (5) Back coracoid          --> angulusAcromialis ?
% (6) Front scapula          --> spinoGlenoidNotch
% ************************************************************

ScapulaLandmarks.angulusInferior      = scapulaExtremities(1, :); % Angulus Inferior 
ScapulaLandmarks.trigonumSpinae       = scapulaExtremities(2, :);  % Trigonum Spinae Scapulae (the midpoint of the triangular surface on the medial border of the scapula in line with the scaoular spine
ScapulaLandmarks.processusCoracoideus = scapulaExtremities(3, :);  % Processus Coracoideus (most lateral point)
ScapulaLandmarks.acromioClavicular    = scapulaExtremities(4, :);  % Acromio-clavicular joint (most lateral part on acromion)
ScapulaLandmarks.angulusAcromialis    = scapulaExtremities(5, :);  % Angulus Acromialis (most laterodorsal point)
ScapulaLandmarks.spinoGlenoidNotch    = scapulaExtremities(6, :);  % Spino-glenoid notch
ScapulaLandmarks.groove               = scapulaGroove; % 5 landmarks on the scapula (supraspinatus) groove
ScapulaLandmarks.pillar               = scapulaPillar; % 5 landmarks on the pillar

if isempty(ScapulaLandmarks.pillar)
    ScapulaLandmarks = -1;
end

end