function [DTItracts,M,nan_rows] = FindCoordinates_Probabilistic(DTItracts,path)

% ----------------- INPUT -----------------
% DTItracts: structure created in "MuscleParameters_Probabilistic" function
% path: string of pathway directory of where the tracked files are located (EX: 'FCU/thresholded/sample*.nii.gz')

% ----------------- OUTPUT -----------------
% DTItracts: structure with the fields 
%     - M: the generated number of tracts
%     - nan_rows: logical array for rows (samples) that were eliminated due to non-tracking or low intensity
%     - overall_anat_length: Mx2 vector of actual lengths of each m-th fiber (2nd col) (1st col = fiber index)
%     - max_intensity: Mx2 vector of actual lengths of each m-th fiber (2nd col) (1st col = fiber index)
%     - anat_coord: Mx1 vector of anatomical space coordinates for each m-th fiber
%     - vox_coord_dti: Mx1 vector of DTI space coordinates for each m-th fiber
%     - vox_coord_t1: Mx1 vector of T1 space coordinates for each m-th fiber
%     - tracts_xyz: 3x(n*M) array of all points of all fibers (anatomical space)
%     - tracts_xyz_DTIspace: 3x(n*M) array of all points of all fibers (DTI space)
%     - tracts_xyz_T1space: 3x(n*M) array of all points of all fibers (T1 space)
%     - fibindex: Mx2 array of start index and end index (in tracts_xyz) of each m-th fiber

%% Load Files and Initialize Containers

% List Filenames of Tracts in Order
list_files = dir(path);                         % List all filenames in the directory
M = length(list_files);                         % M = # of fibers
filenames = natsortfiles({list_files.name});    % Sort file names in order of tract number
filenames = filenames';                         % Create Mx1 cell array of filenames for each m-th tract
path_prefix = list_files(1).folder;             % Find directory name in which all the tracts are stored

% Initialize Containers
voxIMG = cell(M,1);                             % Empty cell array for each tract's coordinates array -- DTI MRI space voxel coordinates
voxIMG_real = cell(M,1);                        % Empty cell array for each tract's coordinates array, after elimination of invalid coordinates -- DTI MRI space voxel coordinates
voxT1 = cell(M,1);                              % Empty cell array for each tract's coordinates array -- T1 MRI space voxel coordinates
voxANAT = cell(M,1);                            % Empty cell array for each tract's coordinates array -- Anatomical space voxel coordinates
DTItracts.overall_anat_length = zeros(M,2);     % Empty container for anatomical space lengths of each tract (1st col = fiber #, 2nd col = lengths)
DTItracts.max_intensity = zeros(M,2);           % Empty container for maximum intensity in each tract (1st col = fiber #, 2nd col = intensities)
DTItracts.anat_coord = cell(M,1);               % Empty container for anatomical coordinates in each tract, each (cols 1-3 = x,y,z) 
DTItracts.vox_coord_dti = cell(M,1);            % container for DTI space voxel coordinates in each tract, each (cols 1-3 = x,y,z)
DTItracts.vox_coord_t1 = cell(M,1);             % container for T1 space voxel coordinates in each tract, each (cols 1-3 = x,y,z)

%% Find Coordinates

f = waitbar(0,'Finding coordinates and calculating distances');         % Create wait bar to show progress
pause(0.5)

for m = 1:M     % for each tract saved
    
    waitbar(m/M ,f);
    
    %% Setup

    DTItracts.overall_anat_length(m,1) = DTItracts.seed_array(m,1);     % Specify seedpoint number, save to array
    
    pathname = strcat(path_prefix,'/',filenames{m});                    % Name the path to the tract
    A = load_untouch_nii(pathname);                                     % Load NIFTI structure
    tract = A.img;                                                      % Get image from the structure
    
    % Get transformation (image space to anatomical space) matrix
    tf_matrix = [A.hdr.hist.srow_x; A.hdr.hist.srow_y; A.hdr.hist.srow_z; 0 0 0 1];
    
    %% Extract Coordinates and Intensity Values in an Nx4 Matrix
    % N = number of rows = number of voxels the tract contains
    % x = column 1 = x-coordinates in MRI space
    % y = column 2 = y-coordinates in MRI space
    % z = column 3 = z-coordinates in MRI space

    idx = find(tract);      % Find linear indices of all NON-ZERO intensity values
    
    % Delete seedpoint numbers that did not track
    if isempty(idx)                                             % If there is no tract (did not track, did not meet termination/waypoints conditions, etc)
        fprintf('Deleting sample %i because it was not tracked \n', m)
        DTItracts.overall_anat_length(m,2) = NaN;               % Replace length with NaN

    % Find and save coordinates of tracts (seedpoints that did track)
    else
        [row, col, slice] = ind2sub(size(tract),idx);           % Separate indices to subscripts to find x,y,z
        mri_matrix = [row(:), col(:), slice(:), tract(idx)];    % Matrix of all non-zero voxels tracked, with xyz coordinates (col 1-3) and intensity (col 4)
    
        % Save Maximum Intensity
        DTItracts.max_intensity(m,1) = m;                       % Save fiber number
        DTItracts.max_intensity(m,2) = max(mri_matrix(:,4));    % Save maximum intensity of that tract
    
        % Find range of z-coordinates for the tract
        inf_z = min(mri_matrix(:,3));                           % Find most inferior (minimum) z-slice of tract
        sup_z = max(mri_matrix(:,3));                           % Find most superior (maximum) z-slice of tract
        voxIMG{m} = zeros(sup_z - inf_z,3);                     % Create empty container: rows are # points in tract, columns are xyz coordinates for each point

        for i = inf_z:sup_z                                     % For each slice that was tracked from inferior --> superior
         
            if ismember(i,mri_matrix(:,3))                      % If that slice was part of tracking
                v = find(mri_matrix(:,3) == i);                 % Find rows for which voxels in the tract are in this slice
                
                if size(v,1) <= 2                               % If the number of coordinates (rows) in that slice are <= 3, tract is just noise
                    voxIMG{m}((i-inf_z)+1,:) = NaN(1,3);        % Do not count that slice (replace with NaN)
                else
                    w = find(mri_matrix(v,4) == max(mri_matrix(v,4)));  % Find which voxel in this slice has the maximum intensity
                    if length(w) > 1                                    % If there are multiple voxels with tied maximum intensity, just take the first one
                        w = w(1);
                    end
                    u = v(w);                                           % Find the corresponding index row for that voxel
                    voxIMG{m}((i-inf_z)+1,:) = mri_matrix(u,1:3);       % Extract the xyz coordinates of the voxel with maximum intensity              
                end
       
            else                                                % If that slice was not part of tracking
                voxIMG{m}((i-inf_z)+1,:) = NaN(1,3);            % Replace coordinates with NaN
             
            end
        end
    
        % Delete the seedpoints that did not have proper tracts
        nan_rows1 = any(isnan(voxIMG{m}),2);        % Find which rows have NaN for coordinates
        voxIMG{m}(nan_rows1,:)=[];                  % Delete these rows
        
        % Make sure tracts span consecutive slices
        slices_vector = voxIMG{m}(:,3);         % Create vector of just z-coordinates (slices)
        D = diff([0;diff(slices_vector)==1;0]); % Take the difference
        start_slice = slices_vector(D > 0);     % Vector of beginning slice for each consecutive block
        end_slice = slices_vector(D < 0);       % Vector of end slice for each consecutive block
        k = find((slices_vector(D<0) - slices_vector(D>0)) == max(slices_vector(D<0) - slices_vector(D>0))); % Find which tracts spans most slices       
        if numel(k) == 1                                    % If one continuous tract
            idx1 = find(slices_vector == start_slice(k));
            idx2 = find(slices_vector == end_slice(k));
            voxIMG_real{m} = voxIMG{m}(idx1:idx2,:);        % Save the coordinates to the new array
        else                                                % if there are multiple portions that are the same maximum length
            voxIMG_real{m} = voxIMG{m};                     % Save all the coordinates to the new array
        end
    
        % Transform to anatomical coordinates
        for j = 1:size(voxIMG_real{m},1)                            % For each final voxel coordinates in the tract
            anat_coord = tf_matrix * [voxIMG_real{m}(j,1:3)';1];    % Multiply coordiantes by transformation matrix (4x1 = 4x4 * 4x1)
            voxANAT{m}(j,1:3) = anat_coord(1:3,:)';                 % Save an Nx3 array of voxel Anatomical space coordinates
        end
    
        %% Calculate Distances and Lengths
    
        % Calculate distance between each coordinate in the tract
        distance = zeros(size(voxANAT{m},1) - 1 , 1);                   % Create empty container: distance between each coordinate in array
        for d = 1 : (size(voxANAT{m},1) - 1)                            % For each of the coordinates
            distance(d,:) = pdist2(voxANAT{m}(d,:),voxANAT{m}(d+1,:));  % Calculate the distance between that coordinate and the next one
        end
    
        % Sum all the distances to get the overall tract length in anatomical space
        DTItracts.overall_anat_length(m,2) = sum(distance);
        DTItracts.anat_coord{m} = voxANAT{m};

        %% Save Voxel Coordinates in DTI space and T1 space
    
        % Transform coordinates from DTI space to T1 space
        dti_to_t1_matrix = load('dti_reg2t1.mat','-ascii');           % Load inverse transformation matrix
        for j = 1:size(voxIMG_real{m},1)                              % For each voxel that is extracted
            t1_coord = dti_to_t1_matrix * [voxIMG_real{m}(j,1:3)';1]; % Multiply coordiantes by transformation matrix (4x1 = 4x4 * 4x1)
            voxT1{m}(j,1:3) = t1_coord(1:3,:)';                       % Save an Nx3 array of voxel T1 space coordinates
        end
      
        % Save Voxel Coordinates in MRI Space (DTI and T1)
        DTItracts.vox_coord_dti{m} = voxIMG_real{m};
        DTItracts.vox_coord_t1{m} = voxT1{m};
        
    end
    
end

close(f)    % Close wait bar

%% Eliminate samples that have NaN (either because empty, low intensity, or outside length bounds)

% Find which rows have NaN
nan_rows = any(isnan(DTItracts.overall_anat_length),2);
DTItracts.nan_rows = nan_rows;

% Eliminate these rows from saved arrays
DTItracts.overall_anat_length(nan_rows,:) = [];
DTItracts.max_intensity(nan_rows,:) = [];
DTItracts.anat_coord(nan_rows) = [];
DTItracts.vox_coord_dti(nan_rows) = [];
DTItracts.vox_coord_t1(nan_rows) = [];
voxANAT(nan_rows) = [];
voxIMG_real(nan_rows) = [];
voxT1(nan_rows) = [];

%% Input all coordinates and indices into structures for further calculations

disp('Creating structures');
% Field tracts_xyz: 3 x N array containing the xyz-coordinates of all points of all tracts 
    % x-coord = row 1; y-coord = row 2; z-coord = row 3
% Field fibindex: containing the start index (first column) and end index (second column) of the tracts points of a tract
    % M = number of tracts in the set; N = (point in each tract) * M = number of points total in all tracts

M = length(DTItracts.overall_anat_length);  % Find new size -- how many M tracts after eliminations
n = zeros(M,1);                             % Intialize container to have size of all the M tracts

% Initialize arrays for first tract
for m = 1
    n(m) = size(voxANAT{m},1);                                      % Find how many points are in the tract
    DTItracts.tracts_xyz(:, 1 : n(m) ) = voxANAT{m}';               % Save as 3 x N array with xyz coordinates (anatomical space)
    DTItracts.tracts_xyz_DTIspace(:, 1 : n(m) ) = voxIMG_real{m}';  % Save as 3 x N array with xyz coordinates (DTI space)
    DTItracts.tracts_xyz_T1space(:, 1 : n(m) ) = voxT1{m}';         % Save as 3 x N array with xyz coordinates (T1 space)
    DTItracts.fibindex(m,:) = [1, n(m)];                            % Save as M x 2 array
end

% Fill in arrays for the rest of the fibers
for m = 2:M
    
    n(m) = size(voxANAT{m},1);                                      % Find how many points are in the tract
    
    % each tract starts at fibindex(previous,2)+1, fills in n(current) columns
    % initial value will be 1 (column 1 for tracts_xyz, row 1 for fibindex)
    % final value will be N (column end for tracts_xyz, row end for fibindex)
    
    DTItracts.tracts_xyz(:, DTItracts.fibindex(m-1,2) + 1 : DTItracts.fibindex(m-1,2) + n(m) ) = voxANAT{m}';               % Save as 3 x N array with xyz coordinates (anatomical space)
    DTItracts.tracts_xyz_DTIspace(:, DTItracts.fibindex(m-1,2) + 1 : DTItracts.fibindex(m-1,2) + n(m) ) = voxIMG_real{m}';  % Save as 3 x N array with xyz coordinates (DTI space)
    DTItracts.tracts_xyz_T1space(:, DTItracts.fibindex(m-1,2) + 1 : DTItracts.fibindex(m-1,2) + n(m) ) = voxT1{m}';         % Save as 3 x N array with xyz coordinates (T1 space)  
    DTItracts.fibindex(m,:) = [DTItracts.fibindex(m-1,2) + 1, DTItracts.fibindex(m-1,2) + n(m)];                            % Save as M x 2 array
    
end

end