function [DTItracts] = FindPennation_Probabilistic(DTItracts, M, pointA, pointB)

% ----------------- INPUT -----------------
% DTItracts: structure created in "LSQ_Polynomial" function
% M: the generated number of tracts
% pointA: distal point of action line vector
% pointB: proximal point of action line vector

% ----------------- OUTPUT -----------------
% DTItracts: structure with the fields
%     - penangle (pennation angle): Mx1 with pennation angle for each tract
%     - action_line (action line): 2x3 array with points for action line (A=row 1, B= row 2)

%% Create Action Line Vector (point A to point B)

pt_A = pointA;  % 1x3 vector
pt_B = pointB;  % 1x3 vector

action_line = pt_B - pt_A;  % Create vector

% Save points of action line
DTItracts.action_line = [pt_A; pt_B];

%% Find Pennation Anges Between First 5 Points and Action Line

DTItracts.penangle = zeros(M,1);    % Create container for pennation angle of each of M tracts

% Loop through each tract
for m = 1:M     % For each m-th tract
    
    % Load polynomial coefficients and t values for the m-th fiber --> x(t), y(t), z(t)
    coeff.x = DTItracts.PolyCoeff(m).x;
    coeff.y = DTItracts.PolyCoeff(m).y;
    coeff.z = DTItracts.PolyCoeff(m).z;
    coeff.t0 = DTItracts.PolyCoeff(m).t0;
    coeff.t1 = DTItracts.PolyCoeff(m).t1;
    
    % Sample from t with N values
    N = 1000;                               % Can be changed
    t_int = linspace(coeff.t0,coeff.t1,N);  % Create vector with N points from t0 to t1 (1xN vector)
    
    % Create container to save the angles of the first 5 points
    angles = zeros(5,1);                    % 5x1 vector with pennation angle (in degrees) for each of the first 5 points of curve
    
    % Define the first point of the tract
    t0 = t_int(1);                          % because first point = t0
    P0 = [polyval(coeff.x,t0), polyval(coeff.y,t0), polyval(coeff.z,t0)];   % Define start point (x,y,z) (1x3 vector)

    % Loop through each of the first 5 points AFTER the first point
    for i = 1:5     % For each of the first 5 points

        t = t_int(i+1);                                                     % Find t value for that point (each of the first 5 points = indices 2-6)
        P = [polyval(coeff.x,t), polyval(coeff.y,t), polyval(coeff.z,t)];   % Define point (x,y,z) (1x3 vector)
        r = P - P0;                                                         % Make a vector from first point to that point (P-P0)
        angle_rad = atan2(norm(cross(action_line, r)), dot(action_line, r));% Find angle (in radians) between vectors in 3D using 2 arguemnt arctan: angle=arctan2(|AxB|,A.B)
        angles(i) = rad2deg(angle_rad);                                     % Convert the angle to degrees

        % If the angle is greater than 90, the vector points in the opposite direction, so subtract the angle from 180°
        if angles(i) > 90
            angles(i) = 180 - angles(i);
        end

    end

    % Take the average of the five angles to find the pennation angle, save to the structure
    DTItracts.penangle(m) = mean(angles);

end

end