%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
%                   Calculation of a simple Angle                        %
%                      Author Michael Holljes                            %
%                                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% This method calculates an angle between two marker datasets using a
% fulcrum to do so. Valid input datasets are marker matrices in nx3 form.
% The method returns a plot of the desired angle over time and a nx1
% vector.
% Contract: nx3 matrix (1st marker) & nx3 matrix (2nd marker) & nx3
% matrix (fulcrum) -> plot & 1 vector (angle)
% Example of command window input:
% Which variable contains your first marker dataset? L_Hee
% Which variable contains your second marker dataset? L_Hip
% Which variable contains your fulcrum dataset? L_Kne_O
%
% This input produces a plot and a vector both containing the desired
% angle.


%% (1) Clearing Workspace
clc

%% (2) Loading of Measurement Data
prompt = 'Which variable contains your first marker dataset? ';
firstSet = input(prompt);
prompt = 'Which variable contains your second marker dataset? ';
secondSet = input(prompt);
prompt = 'Which variable contains your fulcrum dataset? ';
fulcrum = input(prompt);

%% (3) Setting Relative Coordinates

firstSetX = firstSet(:,1);
firstSetY = firstSet(:,2);
firstSetZ = firstSet(:,3);

secondSetX = secondSet(:,1);
secondSetY = secondSet(:,2);
secondSetZ = secondSet(:,3);

fulcrumX = fulcrum(:,1);
fulcrumY = fulcrum(:,2);
fulcrumZ = fulcrum(:,3);


targetAngle = 1:length(firstSet);

%% (4) Calculating Angle

for j = 1:length(firstSetX)
    firstLimb = [firstSetX(j) - fulcrumX(j), firstSetY(j) - fulcrumY(j), ...
        firstSetZ(j) - fulcrumZ(j)];
    secondLimb = [secondSetX(j) - fulcrumX(j), secondSetY(j) ...
        - fulcrumY(j), secondSetZ(j) - fulcrumZ(j)];
    cosGamma = dot(firstLimb, secondLimb)/dot(norm(firstLimb), ...
        norm(secondLimb));
    targetAngle(j) = acosd(cosGamma);
end

clear prompt
clear firstSet
clear secondSet
clear fulcrum
clear firstSetX
clear firstSetY
clear firstSetZ
clear secondSetX
clear secondSetY
clear secondSetZ
clear fulcrumX
clear fulcrumY
clear fulcrumZ
clear firstLimb
clear secondLimb
clear cosGamma

%% (5) Plotting of acceleration, velocity and position
frames = 1:length(targetAngle);
frames = frames/.1000;
plot(frames, targetAngle);
legend('Angle', 'Location', 'northoutside', 'Orientation', 'horizontal');

%% (6) Saving Data
button = questdlg('Do you want to save your results?','Save?');
if strcmpi(button,'yes') == 1
    uisave();
    disp('saved successfully!')
end