classdef PTKAirwayGenerator < handle
    % PTKAirwayGenerator. Creates an artifical airway tree using a volume-filling
    % algorithm.
    %
    %     This class generates an artificial model of the airway tree. You
    %     specify the lung volume to fill, and provide a starting tree (such as
    %     that produced by a CT region-growing algorithm). The airway growing
    %     starts at the endpoints of the starting tree and grows into the
    %     provided volume. The resulting tree includes the starting tree.
    %
    %     You can also choose to grow specific parts of the tree into specific
    %     volumes, so for example a lobar region can be exclusively allocated to
    %     a lobar bronchus and its descendents.
    %
    %     This code has in part been adapted from C++ code by Rafel Bordas which
    %     forms part of the Chaste project at the University of Oxford.
    %     The algorithm is derived from Tawhai et al. (2004), although some
    %     changes have been made to the algorithm.
    %
    %     Syntax:
    %         airway_generator = PTKAirwayGenerator(
    %             lung_mask,              % A binary mask of the whole lung volume
    %             centreline_tree,          % A PTKModelTree produced from PTKAirwayCentreline
    %             point_limit_voxels,     % Branches will terminate if the size of the region they grow into in voxels is less than this limit
    %             approx_number_points,   %
    %             reporting               % A CoreReportingInterface object for error, warning and progress reporting
    %         )
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    properties (Constant)
        LengthLimitMm = 1.2 % A branch is terminated if its length is less than this value. Tawhai et al 2004 use value 2.
        PointLimitVoxels = 1 % A branch is terminated if the point cloud of the apex has fewer points than this limit
        NumberOfGenerationsToReallocate = 20  % If ReallocatePointsAtEachGeneration is set to true, this is the number of generations for which points in the volume will be reallocated at each generation
        PointNumberMultiple = 1 % The desired number of grid points is obtained by multiplying approx_number_points by this value
        MaximumGenerationNumber = 25     % All branches of the output tree will terminate if they extend beyond this generation number
        InitialTerminatingBranchLengthLimit = 6
        BranchingFraction = 0.4 % The fraction a branch extends towards the centre of the point cloud. Value 0.4 from Tawhai et al., 2004
        ReallocatePointsAtEachGeneration = true % If true, points will be reassigned to apices at each generation up to the generation number set in NumberOfGenerationsToReallocate
        AngleLimitRadians = 60*(pi/180.0); % Value 60 degrees from Tawhai et al, 2004
        PointDistanceLimit = 5 % Points greather than a voxel distance of this number multiplied by the number of cloud points are removed from the apex cloud
        BranchLengthToParentRatioLimit = 1 % Child branches cannot be longer than this factor multiplied by ther parent's length
        BranchLengthToParentGenerationLimit = 20 % The BranchLengthToParentRatioLimit parameter starts taking effect from this generation number
    end

    properties
        AirwayTree
        InitialApexImage
        GridSpacingMm
    end

    methods
        function obj = PTKAirwayGenerator(lung_mask, centreline_tree, approx_number_points, reporting)

            % Compute the grid spacing. We choose to have more grid points than
            % the minimum required as a finer grid will give better branching
            approx_number_grid_points = PTKAirwayGenerator.PointNumberMultiple*approx_number_points;
            obj.GridSpacingMm = lung_mask.ComputeResamplingGridSpacing(approx_number_grid_points);

            % Initialise airway tree
            initial_airway_tree = obj.CreateInitialTreeFromSegmentation(centreline_tree, PTKAirwayGenerator.MaximumGenerationNumber, reporting);
            obj.RemoveSmallTerminatingAirways(initial_airway_tree, PTKAirwayGenerator.InitialTerminatingBranchLengthLimit, reporting);
            obj.AirwayTree = initial_airway_tree;
        end

        function delete(~)
        end

        % Starting from the initial airway tree generated from AddTree(),
        function GrowTree(obj, growth_volume, starting_segment, reporting)
            obj.InitialApexImage = PTKAirwayGenerator.GrowTreeUsingThisGridSpacing(obj.AirwayTree, growth_volume, starting_segment, obj.GridSpacingMm, reporting);
        end
    end

    methods (Static, Access = private)
        function initial_apex_image = GrowTreeUsingThisGridSpacing(airway_tree, growth_volume, starting_segment, grid_spacing_mm, reporting)

            reporting.PushProgress;

            resampled_volume = PTKAirwayGenerator.CreatePointCloud(growth_volume, grid_spacing_mm);
            disp(['Number of seed points:' int2str(sum(resampled_volume.RawImage(:)))]);

            initial_apex_image = PTKAirwayGenerator.Grow(resampled_volume, airway_tree, starting_segment, reporting);

            reporting.PopProgress;
        end

        % Use CreateInitialTreeFromSegmentation to create an initial airway tree
        % from the airway centreline results
        function airway_tree = CreateInitialTreeFromSegmentation(segmented_centreline_tree, maximum_generation_number, reporting)
            airway_tree = PTKAirwayGrowingTree;
            airway_tree.CentrelineTreeSegment = segmented_centreline_tree;
            segments_to_do = airway_tree;
            while ~isempty(segments_to_do)
                segment = segments_to_do(end);
                segments_to_do(end) = [];

                % Get the first and last voxel coordinates from the segment of
                % the centreline airway tree
                centreline_segment = segment.CentrelineTreeSegment;
                if isempty(segment.Parent)
                    first_point = centreline_segment.Centreline(1);
                else
                    first_point = centreline_segment.Parent.Centreline(end);
                end
                end_point = centreline_segment.Centreline(end);

                segment.StartPoint = first_point;
                segment.EndPoint = end_point;

                if isempty(centreline_segment.GenerationNumber)
                    error('program error');
                end

                if centreline_segment.GenerationNumber >= maximum_generation_number
                    reporting.ShowWarning('PTKAirwayGenerator:SegmentedBranchesExcluded', 'Initial branches have been excluded due to the maximum generation parameter', []);
                else

                    if ~isempty(centreline_segment.Children)
                        % Add a new branch to the tree for each child
                        for child = centreline_segment.Children
                            % Create a new segment
                            new_segment = PTKAirwayGrowingTree(segment);
                            new_segment.CentrelineTreeSegment = child;
                            new_segment.IsGenerated = false;
                            segments_to_do = [segments_to_do, new_segment];
                        end
                    end
                end

            end
        end

        function RemoveSmallTerminatingAirways(initial_airway_tree, length_limit, reporting)
            branches_to_do = initial_airway_tree;
            has_changed = true;
            iteration_number = 0;
            while (has_changed)
                has_changed = false;
                iteration_number = iteration_number + 1;
                while ~isempty(branches_to_do)
                    branch = branches_to_do(end);
                    branches_to_do(end) = [];
                    if isempty(branch.Children)
                        branch_length = MimImageCoordinateUtilities.DistanceBetweenPoints(branch.StartPoint, branch.EndPoint);
                        if branch_length < length_limit
                            if ~isempty(branch.Parent)
                                reporting.ShowWarning('PTKAirwayGenerator:SegmentedBranchesBelowLimit', 'Initial branches have been excluded due to their length being below the limit', []);
                                branch.Parent.RemoveChildren;
                                has_changed = true;
                            end
                        end

                    else
                        branches_to_do = [branches_to_do, branch.Children];
                    end
                end
            end
        end

        function in_volume = IsInsideVolume(point_mm, lung_volume)
            global_coordinates = MimImageCoordinateUtilities.GetGlobalCoordinatesForPoints(point_mm, lung_volume);
            if ~lung_volume.IsPointInImage(global_coordinates)
                in_volume = false;
            else
                in_volume = lung_volume.GetVoxel(global_coordinates);
            end
        end

        function apices = GetApicesBelowThisBranchForTerminalSegments(centreline_branches, airway_tree, generation_number, reporting)
            apices = [];

            % Find the starting segments
            segments_to_do = [];
            for centreline_branch = centreline_branches
                branch = airway_tree.FindCentrelineBranch(centreline_branch, reporting);
                segments_to_do = [segments_to_do, branch];
            end

            % Now search the tree below the starting segments and create an apex
            % for each one at the correct generation number
            while ~isempty(segments_to_do)
                segment = segments_to_do(end);
                segments_to_do(end) = [];
                children = segment.Children;
                segments_to_do = [segments_to_do, children];

                % Now search the tree below the starting segments and create an apex
                % for each one that is a terminal segment
                if isempty(children) && isempty(segment.IsTerminal)
                    % Add apices for airway growing
                    new_apex = PTKAirwayGeneratorApex(segment, PTKCoords, true);
                    apices = [apices new_apex];
                end
            end
        end

        function apices = GetApicesBelowThisBranchForThisGeneration(start_centreline_branches, airway_tree, generation_number, reporting)
            apices = [];

            % Find the starting segments
            segments_to_do = [];
            for centreline_branch = start_centreline_branches
                branch = airway_tree.FindCentrelineBranch(centreline_branch, reporting);
                segments_to_do = [segments_to_do, branch];
            end

            % Now search the tree below the starting segments and create an apex
            % for each one at the correct generation number
            while ~isempty(segments_to_do)
                segment = segments_to_do(end);
                segments_to_do(end) = [];
                children = segment.Children;
                segments_to_do = [segments_to_do, children];

                % Find all branches of this generation which have not been
                % terminaetd by the algorithm. This includes branches which
                % already have child branches - these will not be grown, but
                % points will be allocated to their apices so that they may
                % later be available for their child branches to grow
                if segment.GenerationNumber == generation_number
                    if isempty(segment.IsTerminal)
                        is_growing_apex = isempty(children);
                        new_apex = PTKAirwayGeneratorApex(segment, PTKCoords, is_growing_apex);
                        apices = [apices new_apex];
                    end
                end
            end
        end

        function generation_number = GetMinimumTerminalGeneration(apices)
            generation_number = [];
            for apex = apices
                apex_generation = apex.AirwayGrowingTreeSegment.GenerationNumber;
                if isempty(generation_number)
                    generation_number = apex_generation;
                else
                    generation_number = min(generation_number, apex_generation);
                end
            end
        end

        % Find the smallest generation number of a branch in the airway tree
        % which is still growing. This will return an empty variable if no more
        % airways are growing
        function generation_number = GetMinimumActiveTerminalGeneration(airway_tree, start_centreline_branches, reporting)
            % Find the starting segments
            segments_to_do = [];
            for centreline_branch = start_centreline_branches
                branch = airway_tree.FindCentrelineBranch(centreline_branch, reporting);
                segments_to_do = [segments_to_do, branch];
            end

            generation_number = [];
            while ~isempty(segments_to_do)
                next_branch = segments_to_do(end);
                child_branches = next_branch.Children;

                % A branch is still growing if it has no child branches and it
                % has not been terminated by the growing algorithm
                if isempty(child_branches) && isempty(next_branch.IsTerminal)

                    % We want the smallest generation number for the tree
                    if (isempty(generation_number) || (generation_number > next_branch.GenerationNumber))
                        generation_number = next_branch.GenerationNumber;
                    end
                end

                segments_to_do(end) = [];
                segments_to_do = [segments_to_do child_branches];
            end
        end

        function centre = GetCentreOfMass(point_cloud)
            cloud_points = point_cloud.Coords;
            centre = mean(cloud_points, 1);
        end

        function new_image = GetImageFromApex(template, apices)
            new_image = zeros(template.ImageSize, 'uint16');
            colour = 1;
            for apex = apices
                cloud_points = apex.PointCloud.Coords;
                if ~isempty(cloud_points)
                    cloud_points = MimImageCoordinateUtilities.PTKCoordinatesToCoordinatesMm(cloud_points);
                    point_coord = template.CoordinatesMmToGlobalCoordinates(cloud_points);
                    point_coord = template.GlobalToLocalCoordinates(point_coord);
                    indices = sub2ind(template.ImageSize, point_coord(:,1), point_coord(:,2), point_coord(:,3));
                    new_image(indices) = colour;
                end
                colour = colour + 1;
            end
        end

        function centreline_indices_local = CentrelinePointsToLocalIndices(centreline_points, template_image)
            centreline_indices_global = MimImageCoordinateUtilities.GetGlobalIndicesForPoints(centreline_points, template_image);
            centreline_indices_local = template_image.GlobalToLocalIndices(centreline_indices_global);
        end

        function closest_point = FindClosestPointInCloud(point, cloud)
            point_coords = [point.CoordX, point.CoordY, point.CoordZ];
            num_points_in_cloud = size(cloud.Coords, 1);
            distance = cloud.Coords - repmat(point_coords, [num_points_in_cloud, 1]);
            distance = sqrt(distance(:,1).^2 + distance(:,2).^2 + distance(:,3).^2);
            [~, min_index] = min(distance, [], 1);
            closest_point = cloud.Coords(min_index, :);
        end

        % Compute a normalised direction vector for a plane parallel to the
        % given vector and the direction of the given branch. If the two vectors
        % are parallel then the direction of the parent branch is used. If this
        % does not exist or is still parallel, then we choose a guaranteed
        % non-parallel vector using the null space.
        function plane_normal = GetValidPlaneNormal(vector_to_com, branch)

            if ~isempty(branch.Parent)
                parent_branch_direction = branch.Parent.Direction;
                plane_normal = cross(vector_to_com, parent_branch_direction);
            else
                branch_direction = branch.Direction;
                plane_normal = cross(vector_to_com, branch_direction);
            end

            % This is an implementation of the algorithm described in Tawhai et
            % al., 2004 for finding the plane. However, this does not work since
            % the parent branch will often be collinear with the centre of mass
            % point (since branches are created in the direction of the centre
            % of mass).
%             branch_direction = branch.Direction;
%
%             % Find the normal to the plane which passes through the centre
%             % of mass and the branch start and end points
%             plane_normal = cross(vector_to_com, branch_direction);
%
%             % If the branch direction is parallel to the vector then we try the
%             % parent direction
%             if isequal(plane_normal, [0 0 0])
%                 disp('parallel - correcting');
%                 if ~isempty(branch.Parent)
%                     parent_branch_direction = branch.Parent.Direction;
%                     plane_normal = cross(vector_to_com, parent_branch_direction);
%                 end
%             end

            % If the vectors are still parallel then we use the null space to
            % find another vector which is perpendicular
            if isequal(plane_normal, [0 0 0])
                disp('still parallel - correcting');
                plane_null_space = null(vector_to_com/norm(vector_to_com));
                perpendicular_vector = plane_null_space(:, 1);
                plane_normal = cross(vector_to_com, perpendicular_vector);
            end

            plane_normal = plane_normal / norm(plane_normal);
        end

        % Divides the point cloud in two, based on a plane perpendicular to the
        % direction of the last two branches
        function [this_plane, other_plane] = SplitPointCloud(point_cloud, normal, origin)
            points_coords = point_cloud.Coords;
            p = - dot(normal, origin);

            % Find which axis is most parallel to the plane
            [~, ordered_directions] = sort(normal, 'descend');
            main_direction = ordered_directions(1);
            other_directions = ordered_directions(2:3);
            plane_points = - (p + normal(other_directions(1))*points_coords(:, other_directions(1)) + normal(other_directions(2))*points_coords(:, other_directions(2)))/normal(main_direction);
            in_plane = points_coords(:, main_direction) < plane_points;
            this_plane = PTKCoords(points_coords(in_plane, :));
            other_plane = PTKCoords(points_coords(~in_plane, :));
        end

        function resampled_volume = CreatePointCloud(growth_volume, grid_spacing_mm)
            resampled_volume = growth_volume.Copy;
            grid_spacing = [grid_spacing_mm, grid_spacing_mm, grid_spacing_mm];
            resampled_volume.Resample(grid_spacing, '*nearest')
        end

        % Checks the branch angle of a proposed growth centre and adjusts it within tolerance, if necessary
        function end_point = CheckBranchAngleLengthAndAdjust(start_point, end_coords, parent_direction, parent_length_mm, generation_number)
            % calculate vector from apex start to the centre
            start_coords = [start_point.CoordX, start_point.CoordY, start_point.CoordZ];
            new_direction = end_coords - start_coords;

            % Record the branching length
            branch_length = norm(new_direction);

            new_direction = new_direction / norm(new_direction);

            old_direction = parent_direction;
            old_direction = old_direction / norm(old_direction);

            % determine branch angle
            dot_product = dot(new_direction, parent_direction);
            branch_angle = acos(dot_product/(norm(new_direction)*norm(parent_direction)));

            % If the branch angle is above the limit then set it to the limit
            if (branch_angle > PTKAirwayGenerator.AngleLimitRadians)
                perpendicular_one = cross(new_direction, old_direction);
                perpendicular_one = perpendicular_one/norm(perpendicular_one);
                perpendicular_two = cross(old_direction, perpendicular_one);
                perpendicular_two = perpendicular_two/norm(perpendicular_two);
                old_direction = old_direction*cos(PTKAirwayGenerator.AngleLimitRadians);
                perpendicular_two = perpendicular_two*sin(PTKAirwayGenerator.AngleLimitRadians);
                new_direction = old_direction + perpendicular_two;
            end

            % Reduce the branch length as required
            branch_length = branch_length*PTKAirwayGenerator.BranchingFraction;
            if generation_number >= PTKAirwayGenerator.BranchLengthToParentGenerationLimit
                branch_length = min(branch_length, parent_length_mm*PTKAirwayGenerator.BranchLengthToParentRatioLimit);
            end
            new_direction = new_direction * branch_length;

            % Update the centre
            end_coords = start_coords + new_direction;
            end_point = PTKPoint(end_coords(1), end_coords(2), end_coords(3));
        end

        function new_apex = AddBranchAndApex(parent_branch, cloud, lung_volume, lung_mask, generation_number, image_size, reporting)
            new_apex = [];

            % Determine the centre of the point cloud
            centre_of_mass = PTKAirwayGenerator.GetCentreOfMass(cloud);

            % The new branch starts at the end of the parent branch
            new_branch_start_point = parent_branch.EndPoint;

            % Fetch the director vector of the parent branch
            parent_direction = parent_branch.Direction;

            % Calculate the end point of the new branch. This is subject both to
            % a length limit and an angle limit.
            new_branch_end_point = PTKAirwayGenerator.CheckBranchAngleLengthAndAdjust(new_branch_start_point, centre_of_mass, parent_direction, parent_branch.Length, generation_number);

            if ~PTKAirwayGenerator.IsInsideVolume(new_branch_end_point, lung_mask)
                reporting.ShowWarning('PTKAirwayGenerator:OutsideVolume', 'Branches terminated because they grew outside the lung volume', []);
                parent_branch.IsTerminal = true;
                return;
            end

            % Create the new branch in AirwayTree
            new_branch = PTKAirwayGrowingTree(parent_branch);
            new_branch.StartPoint = new_branch_start_point;
            new_branch.EndPoint = new_branch_end_point;
            new_branch.IsGenerated = true;

            new_branch_length = MimImageCoordinateUtilities.DistanceBetweenPoints(new_branch_end_point, new_branch_start_point);

            % Find closest point in cloud and delete
            closest_point = PTKAirwayGenerator.FindClosestPointInCloud(new_branch_end_point, cloud);
            closest_point_global_coords = MimImageCoordinateUtilities.PTKCoordinatesToCoordinatesMm(closest_point);
            closest_point_global_coords = lung_volume.CoordinatesMmToGlobalCoordinates(closest_point_global_coords);
            lung_volume.SetVoxelToThis(closest_point_global_coords, 0);

            % Check if the number of points in the cloud is less than the
            % required minimum
            if (size(cloud.Coords, 1) <= PTKAirwayGenerator.PointLimitVoxels)
                new_branch.IsTerminal = true;
                reporting.ShowWarning('PTKAirwayGenerator:BelowLengthThreshold', 'Branches terminated because the number of points was below the threshold', []);
            else
                if (new_branch_length < PTKAirwayGenerator.LengthLimitMm)
                    new_branch.IsTerminal = true;
                    reporting.ShowWarning('PTKAirwayGenerator:BelowLengthThreshold', 'Branches terminated because their length was below the threshold', []);
                else
                    % Create new growth branch
                    new_apex = PTKAirwayGeneratorApex(new_branch, cloud, true);
                end
            end
        end

        function apices = AssignPointCloudToApices(apices, resampled_volume, reporting)
            if numel(apices) == 0
               reporting.Error('PTKAirwayGenerator:AssignPointCloudToApices', 'No starting points');
            end
            sample_image = zeros(resampled_volume.ImageSize, 'uint16');
            apex_start_points = zeros(numel(apices), 3);
            for apex_number = 1 : numel(apices)
                apex = apices(apex_number);
                end_point = apex.AirwayGrowingTreeSegment.EndPoint;
                end_point_global_coordinates = MimImageCoordinateUtilities.GetGlobalCoordinatesForPoints(end_point, resampled_volume);
                end_point = resampled_volume.GlobalToLocalCoordinates(end_point_global_coordinates);
                apex_start_points(apex_number, :) = [end_point(1), end_point(2), end_point(3)];
                current_value = sample_image(end_point(1), end_point(2), end_point(3));
                if current_value ~= 0
                    reporting.ShowWarning('PTKAirwayGenerator:ApexAlreadyUsed', 'The start point for allocating voxels to apices could not be assigned as the point has already been used by another apex', []);
                end
                sample_image(end_point(1), end_point(2), end_point(3)) = apex_number;
            end

            bw_image = sample_image > 0;

            [~, IDX] = bwdist(bw_image);
            mapped_image = sample_image(IDX);
            mapped_image_masked = zeros(size(bw_image), 'uint16');
            point_indices = find(resampled_volume.RawImage);
            mapped_image_masked(point_indices) = mapped_image(point_indices);

            for apex_number = 1 : numel(apices)
                local_indices = find(mapped_image_masked == apex_number);
                if ~isempty(local_indices)
                    [di, dj, dk] = ind2sub(size(mapped_image_masked), local_indices);

                    % Calculate the distance from these new points to the start
                    % point
                    root_point = apex_start_points(apex_number, :);
                    di = di - root_point(1);
                    dj = dj - root_point(2);
                    dk = dk - root_point(3);
                    dist_voxels = sqrt(di.^2 + dj.^2 + dk.^2);
                    points_too_far_away = dist_voxels > PTKAirwayGenerator.PointDistanceLimit*numel(local_indices);

                    % This is a check for distance from root point compared to
                    % parent length
%                     di = (di - root_point(1))*resampled_volume.VoxelSize(1);
%                     dj = (dj - root_point(2))*resampled_volume.VoxelSize(2);
%                     dk = (dk - root_point(3))*resampled_volume.VoxelSize(3);
%                     dist_mm = sqrt(di.^2 + dj.^2 + dk.^2);
%                     last_branch_length = apices(apex_number).AirwayGrowingTreeSegment.Length;
%                     points_too_far_away = dist_mm > PTKAirwayGenerator.PointDistanceLimit*last_branch_length;

                    number_of_points_to_remove = sum(uint8(points_too_far_away));
                    if number_of_points_to_remove > 0
                        disp([int2str(number_of_points_to_remove) ' points removed as they were too far away (generation' int2str(apices(apex_number).AirwayGrowingTreeSegment.GenerationNumber) ')']);
                    end
                    local_indices = local_indices(~points_too_far_away, :);

                    if ~isempty(local_indices)
                        global_indices =  resampled_volume.LocalToGlobalIndices(local_indices);
                        [ic, jc, kc] = resampled_volume.GlobalIndicesToCoordinatesMm(global_indices);
                        [ptk_x, ptk_y, ptk_z] = MimImageCoordinateUtilities.CoordinatesMmToPTKCoordinates(ic, jc, kc);

                        apices(apex_number).PointCloud.Coords = [ptk_x, ptk_y, ptk_z];
                    else
                        apices(apex_number).PointCloud.Coords = [];
                    end
                else
                    apices(apex_number).PointCloud.Coords = [];
                end
            end
        end

        function [apex_1, apex_2] = GrowApex(current_apex, lung_volume, lung_mask, generation_number, image_size, reporting)
            branch = current_apex.AirwayGrowingTreeSegment;

            if isempty(current_apex.PointCloud.Coords)
                branch.IsTerminal = true;
                reporting.ShowWarning('PTKAirwayGenerator:EmptyPointCloud', 'Branches terminated because they had an empty point cloud', []);
                apex_1 = [];
                apex_2 = [];
            elseif size(current_apex.PointCloud.Coords, 1) < 2
                branch.IsTerminal = true;
                reporting.ShowWarning('PTKAirwayGenerator:TooFewPointsInCloud', 'Branches terminated because there was only one point in the point cloud', []);
                apex_1 = [];
                apex_2 = [];
            else

                % Find the end coordinates for this branch
                branch_end_point = branch.EndPoint;
                branch_end_coords = [branch_end_point.CoordX, branch_end_point.CoordY, branch_end_point.CoordZ];

                % Find the centre of mass for the point cloud assigned to this
                % apex
                centre_of_mass = PTKAirwayGenerator.GetCentreOfMass(current_apex.PointCloud);

                % Create a vector from the end branch point to the centre of
                % mass
                vector_to_com = centre_of_mass - branch_end_coords;

                % Get a normalised vector perpendicular to the plane passing
                % through the centre of pass and the start and end points of the
                % branch
                plane_normal = PTKAirwayGenerator.GetValidPlaneNormal(vector_to_com, branch);

                % Split the point cloud into two, creating a new apex for each if there are enough points
                [cloud1, cloud2] = PTKAirwayGenerator.SplitPointCloud(current_apex.PointCloud, plane_normal, branch_end_coords);

                if isempty(cloud1.Coords) || isempty(cloud2.Coords)
                    reporting.ShowWarning('PTKAirwayGenerator:EmptyPointCloud', 'Branches terminated because the point clouds could not be divided into two nonempty clouds', []);
                    branch.IsTerminal = true;
                    apex_1 = [];
                    apex_2 = [];
                    return;
                end

                % Create new branches
                apex_1 = PTKAirwayGenerator.AddBranchAndApex(branch, cloud1, lung_volume, lung_mask, generation_number, image_size, reporting);
                apex_2 = PTKAirwayGenerator.AddBranchAndApex(branch, cloud2, lung_volume, lung_mask, generation_number, image_size, reporting);
            end
        end

        function [apices, initial_apex_image] = Grow(lung_volume, airway_tree, starting_segment, reporting)

            % We need to keep a copy of the lung mask for checking that airways
            % are inside. The original image will have points removed as airways are grown.
            lung_mask = lung_volume.Copy;

            reporting.ShowProgress('Growing branches');
            reporting.UpdateProgressValue(0);
            first_run = true;
            image_size = lung_volume.ImageSize;

            % Set this to something non-empty for the first run
            apices = 1;
            generation_number = 1;

            % Step through generations one by one and terminate when there are
            % no active growing branches left
            while ~isempty(generation_number)

                % Find the lowest generation to grow
                generation_number = PTKAirwayGenerator.GetMinimumActiveTerminalGeneration(airway_tree, starting_segment, reporting);
                if (first_run)
                    min_generation_number = generation_number;
                end

                % Terminate when there are no active growing branches left
                if isempty(generation_number)
                    return;
                end

                % Terminate when we have exceeded a generation threshold, and
                % report how many branches were incomplete
                if generation_number >= PTKAirwayGenerator.MaximumGenerationNumber
                    reporting.ShowMessage('PTKAirwayGenerator:IncompleteApices', [num2str(length(apices)) ' branches were incomplete when the terminal generation was reached']);
                    return
                end

                % Progress reporting
                reporting.ShowMessage('PTKAirwayGenerator:GenerationNumber', ['Generation:' int2str(generation_number)]);
                reporting.UpdateProgressValue(round(100*(generation_number - min_generation_number)/(PTKAirwayGenerator.MaximumGenerationNumber - min_generation_number)));

                % Allocate/reallocate points to the apices
                if (first_run) || ((generation_number < PTKAirwayGenerator.NumberOfGenerationsToReallocate) && (PTKAirwayGenerator.ReallocatePointsAtEachGeneration))

                    is_this_the_last_reallocation = (generation_number + 1 == PTKAirwayGenerator.NumberOfGenerationsToReallocate) || (~PTKAirwayGenerator.ReallocatePointsAtEachGeneration);

                    if is_this_the_last_reallocation
                        apices = PTKAirwayGenerator.GetApicesBelowThisBranchForTerminalSegments(starting_segment, airway_tree, generation_number, reporting);
                    else
                        apices = PTKAirwayGenerator.GetApicesBelowThisBranchForThisGeneration(starting_segment, airway_tree, generation_number, reporting);
                    end

                    num_apices = numel(apices);
                    apices = PTKAirwayGenerator.AssignPointCloudToApices(apices, lung_volume, reporting);
%                     disp(['Generation:' int2str(generation_number) ' Number of apices:' int2str(num_apices) ' Apices post-allocation:' int2str(numel(apices))]);
                    if (first_run)
                        initial_apex_image = PTKAirwayGenerator.GetImageFromApex(lung_volume, apices);
                        first_run = false;
                    end
                end

                apices_next_generation = PTKAirwayGeneratorApex.empty(50000,0);
                for apex = apices

                    % Ignore non-growing apices - they are just there to help
                    % with the point reallocation
                    if (apex.IsGrowingApex)
                        % After the final point reallocation, we may have apices
                        % with higher generation numbers than the current one - this
                        % will happen if the segmented tree has a higher number of
                        % generations than the number of generations to reallocate.
                        % We ignore any apices with a higher generation number than
                        % the current one, but save these for processing later
                        if (generation_number == apex.AirwayGrowingTreeSegment.GenerationNumber)
                            [new_apex_1, new_apex_2] = PTKAirwayGenerator.GrowApex(apex, lung_volume, lung_mask, generation_number, image_size, reporting);
                            if ~isempty(new_apex_1)
                                apices_next_generation(end+1) = new_apex_1;
                            end
                            if ~isempty(new_apex_2)
                                apices_next_generation(end+1) = new_apex_2;
                            end
                        else
                            apices_next_generation(end + 1) = apex;
                        end
                    end
                end

                % The next generation of apices is used when point reallocation
                % no longer happens (either the point reallocation generation
                % has been exceeded, or the reallocation flag is switched off).
                % The list of apices includes newly generated apices, plus any
                % terminal branhces from the segmented airways which are waiting
                % to be grown
                apices = apices_next_generation;
            end
        end
    end
end


% io_loadRFwaveform.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% [RF_struct]=io_loadRFwaveform(filename,type,off_res);
%
% DESCRIPTION:
% Initialize an RF pulse structure to contain an
% RF Pulse waveform as well as its accompanying header
% information.  This function finds the time-bandwidth
% product (tbw) and the time-w1 product (tw1) of the pulse,
% and stores this information in the header fields of the
% output RF structure.
%
% INPUTS:
% filename  = filename of RF pulse waveform text file.  Can be in Siemens
%             format (.pta), Varian/Agilent format (.RF), Bruker format
%             (.inv, .ref or .exc) or a plain text file (.txt, with two,
%             three or four columns (amplitude, phase, time-step
%             (optional),and gradient strength (optional).  Filename can
%             also be the name of a two, three or four column matlab vector
%             specifing the phase, amplitude, time vectors (optionally) and
%             gradient (optionally) of an RF pulse waveform.  Note about
%             Units: RF amplitude waveform [arbitrary units], B1 intensity
%             [kHz],  phase waveform [degrees], time step vector [arbitrary
%             units],  gradient waveform [G/cm].
% type      = Excitation ('exc'), Refocusing ('ref') or Inversion ('inv').
%             Alternatively, 'type' can specify the exact flip angle [in
%             degrees] of the pulse.  This can be useful if you want to use
%             a pulse that does not have an exact flip angle of 90 or 180
%             degrees (Thanks to Eric Plitman for help with this feature).
% off_res   = set to "1" if the centre of the RF pulse is not at 0 Hz.
%             Optional. Default=0.
%
% OUTPUTS:
% RF_struct = RF pulse waveform in FID-A rf pulse structure format.

function RF_struct=io_loadRFwaveform(filename,type,off_res);

if nargin<3
    off_res=0;
end

if isnumeric(type)
    %If 'type' was specified to be exactly 90 degrees, set the type to 'exc';
    if type==90;
        type='exc';
    end

    %If 'type' was specified to be exactly 180 degrees, set the type to 'inv'
    %(functionally there is no real difference between 'ref' and 'inv', so I
    %just chose 'Inv';
    if type==180;
        type='inv';
    end
end

%Now read in the waveform:
if isstr(filename)
    if exist(filename)
        if filename(end-3:end)=='.pta'
            disp('Siemens format .pta RF pulse file detected!! Loading waveform now.');
            rf=io_readpta(filename);
        elseif filename(end-2:end)=='.RF'
            disp('Varian/Agilent format .RF RF pulse file detected!! Loading waveform now.');
            rf=io_readRF(filename);
        elseif filename(end-3:end)=='.inv'
            disp('Bruker format .inv RF pulse file detected!! Loading waveform now.');
            rf=io_readRFBruk(filename);
        elseif filename(end-3:end)=='.rfc'
            disp('Bruker format .pta RF pulse file detected!! Loading waveform now.');
            rf=io_readRFBruk(filename);
        elseif filename(end-3:end)=='.exc'
            disp('Bruker format .exc RF pulse file detected!! Loading waveform now.');
            rf=io_readRFBruk(filename);
        elseif filename(end-3:end)=='.txt'
            disp('Basic .txt format RF pulse file detected!! Loading waveform now.');
            rf=io_readRFtxt(filename);
        else
            error('ERROR:  RF Pulse file not recognized.  Aborting!');
        end
    else
        error('ERROR:  File not found!  Aborting!');
    end
else
    if ismatrix(filename) && ndims(filename)==2 && ( 2 <= size(filename,2) <= 4 )
        disp('Input is an RF waveform already in the matlab workspace.  Loading waveform now.');
        if size(filename,2)==2
            rf=filename;
            rf(:,3)=ones(length(filename(:,1)),1);
        elseif size(filename,2)==3 || size(filename,2)==4
            rf=filename;
        end
    end
end

Tp=0.005;  %assume a 5 ms rf pulse;

%Frequency shifting the pulse to zero if off_res is 1. To be shifted back
%afterwards - PT,2021
if off_res
    %Initializations
    tmp_w1=0.03;
    tmp_min=1;

    %Looping through until a min of <-0.98 (inv/ref) or <0.2 (exc) is reached
    switch type
        case 'exc'
            min_floor = 0.2;
        case {'inv','ref'}
            min_floor = -0.98;
    end
    while tmp_min > min_floor
        [mv,sc]=bes(rf,Tp*1000,'f',tmp_w1,-5,5,1000); %running bes with bare settings
        [tmp_min,tmp_min_pos]=min(mv(3,:)); %finding the min and min position
        freq_shift=sc(tmp_min_pos)*1000;%the freq shift (in Hz) of the pulse @ 5ms
        tmp_w1=tmp_w1+0.02;
    end

    %frequency shifting the pulse to 0 Hz
    N=size(rf,1);
    dt=Tp/N;
    t=[0:dt:Tp-dt];
    phaseRamp=t*-freq_shift*360;
    rf(:,1)=rf(:,1)+phaseRamp';
end

%Find out if the pulse is phase modulated.  If it is not, then we can
%determine the time-w1 product of the pulse quite simply.  If it is phase
%modulated (adiabatic, etc) then the determination of the time-w1 product
%will need to me more interactive.
a=(round(rf(:,1))==180)|(round(rf(:,1))==0);

if sum(a)<length(rf(:,1))
    isPhsMod=true;
else
    isPhsMod=false;
end

%If there are any phase discontinuities in the phase function that are
%equal to a 360 degree jump we can remove these.  This will make it easier
%for rf_resample to do it's job later on:
jumps=diff(rf(:,1));
jumpsAbs=(abs(jumps)>355 & abs(jumps)<365);  %Assume jumps within this range are exactly = 360 degrees.
jumpIndex=find(jumpsAbs);
for n=1:length(jumpIndex)
    rf(jumpIndex(n)+1:end,1)=rf(jumpIndex(n)+1:end,1)-(360*(jumps(jumpIndex(n))/abs(jumps(jumpIndex(n)))));
end

%scale amplitude function so that maximum value is 1:
rf(:,2)=rf(:,2)./max(rf(:,2));

if ~isPhsMod
    %The pulse is not phase modulated, so we can calculate the w1max:
    %find the B1 max of the pulse in [kHz]:
    if isstr(type)
        if type=='exc'
            flipCyc=0.25; %90 degrees is 0.25 cycles;
        elseif type=='ref'
            flipCyc=0.5;  %180 degress is 0.5 cycles;
        elseif type=='inv'
            flipCyc=0.5;  %180 degrees is 0.5 cycles;
        end
    elseif isnumeric(type) %assume that a flip angle (in degrees) was given
        flipCyc=type/360;
    end

    intRF=sum(rf(:,2).*((-2*(rf(:,1)>179))+1))/length(rf(:,2));

    if intRF~=0
        w1max=flipCyc/(intRF*Tp); %w1max is in [Hz]
    else
        w1max=0;
    end
    tw1=Tp*w1max;
else
    %The pulse is phase modulated, so we will need to run some test to find
    %out the w1max;  To do this, we can plot Mz as a function of w1 and
    %find the value of w1 that results in the desired flip angle.
    [mv,sc]=bes(rf,Tp*1000,'b',0,0,5,40000);
    plot(sc,mv(3,:));
    xlabel('w1 (kHz)');
    ylabel('mz');
    w1max=input('Input desired w1max in kHz (for 5.00 ms pulse):  ');
    w1max=w1max*1000; %convert w1max to [Hz]
    tw1=Tp*w1max;
end

%now it's time to find out the time-bandwidth product:
%First make a high resolution plot the pulse profile over a wide bandwidth:
[mv,sc]=bes(rf,Tp*1000,'f',w1max/1000,-5,5,100000);
if isstr(type)
    if type=='exc'
        index=find(abs(mv(1,:)+1i*mv(2,:))>0.5);
        bw=sc(index(end))-sc(index(1))
        %plot(sc(index),mv(3,index),'.-',sc,mv(3,:));
    elseif type=='ref'
        index=find(mv(3,:)<0);
        bw=sc(index(end))-sc(index(1));
        %plot(sc(index),mv(3,index),'.-',sc,mv(3,:));
    elseif type=='inv'
        index=find(mv(3,:)<0);
        bw=sc(index(end))-sc(index(1));
        %plot(sc(index),mv(3,index),'.-',sc,mv(3,:));
    end
elseif isnumeric(type)
    mz=cos(type); %Find out the Mz value immediately following the pulse.
    thr=(1+mz)/2;  %Find out the Mz value mid-way between 1 and the mz (half-max):
    index=find(mv(3,:)<thr);  %Find the indices of the corresponding "full width"
    bw=sc(index(end))-sc(index(1));  %Now find the bandwidth at that point ("Full width at half max").
end

%Now make a very high resolution plot the pulse profile over a narrower bandwidth:
[mv,sc]=bes(rf,Tp*1000,'f',w1max/1000,-bw,bw,100000);
if isstr(type)
    if type=='exc'
        index=find(abs(mv(1,:)+1i*mv(2,:))>0.5);
        bw=sc(index(end))-sc(index(1))
        %plot(sc(index),mv(3,index),'.-',sc,mv(3,:));
    elseif type=='ref'
        index=find(mv(3,:)<0);
        bw=sc(index(end))-sc(index(1));
        %plot(sc(index),mv(3,index),'.-',sc,mv(3,:));
    elseif type=='inv'
        index=find(mv(3,:)<0);
        bw=sc(index(end))-sc(index(1));
        %plot(sc(index),mv(3,index),'.-',sc,mv(3,:));
    end
elseif isnumeric(type)
    mz=cos(type); %Find out the Mz value immediately following the pulse.
    thr=(1+mz)/2;  %Find out the Mz value mid-way between 1 and the mz (half-max):
    index=find(mv(3,:)<thr);  %Find the indices of the corresponding "full width"
    bw=sc(index(end))-sc(index(1));  %Now find the bandwidth at that point ("Full width at half max").
end

%finally, try to estimate the centre point of the RF pulse 'rfCentre'.
%This is a number between 0 and 1 that indicates where the peak of the RF
%amplitude waveform occurs.  If rfCentre = 0.5, then the pulse is symmetric.
%If rfCentre<0.5 then the peak occurs near the beginning of the pulse (i.e.
%a max phase pulse).  If rfCentre >0.5, then the peak occurs near the end
%of the pulse (i.e. min phase pulse).
maxIndex=find(rf(:,2)==max(rf(:,2)));
rfCentre=mean(maxIndex)/length(rf(:,2));

%Now store the output structure;
RF_struct.waveform=rf;
RF_struct.type=type;
RF_struct.f0=0;
if size(rf,2)>3 && any(rf(:,4))
    RF_struct.tbw='N/A - gradient modulated pulse';
    RF_struct.isGM=true;
    RF_struct.tthk=bw*Tp; %This is the time x sliceThickness product for
                         %gradient modulated pulses.  It is in units [cm.s]
else
    RF_struct.tbw=bw*Tp*1000;
    RF_struct.isGM=false;
    RF_struct.tthk='N/A - frequency selective pulse';
end
RF_struct.tw1=tw1;
RF_struct.rfCentre=rfCentre;


function [ output_meta ] = meta2sicd_rs_xml( xml_domnode, beta_domnode, noise_domnode )
%META2SICD_RS_XML Converts Radarsat product.xml description into a SICD-style metadata structure
%
% Takes as input a Document Object Model (DOM) node from the RS2
% product.xml descriptor file.
%
% This function does NOT handle ScanSAR datasets.
%
% There is an outstanding question with regard to the meaning of the
% pulseRepetitionFrequency as provided.  See comments in Timeline section
% below.
%
% Written by: Wade Schwartzkopf, NGA/Research
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

%% Setup
SECONDS_IN_A_DAY = 24*60*60;
xp=javax.xml.xpath.XPathFactory.newInstance.newXPath();

%% CollectionInfo
output_meta.CollectionInfo.CollectorName=char(xp.evaluate(...
    xpath_str({'product','sourceAttributes','satellite'}),xml_domnode));
if strcmpi(output_meta.CollectionInfo.CollectorName, 'RADARSAT-2')
    gen = 'RS2';
elseif strncmpi(output_meta.CollectionInfo.CollectorName, 'RCM', 3)
    gen = 'RCM';
end
[rawDataStartTime, rawDataStartTimeFrac] = datenum_w_frac(char(xp.evaluate(...
    xpath_str({'product','sourceAttributes','rawDataStartTime'}),...
    xml_domnode)));
if strcmp(gen,'RS2')
    output_meta.CollectionInfo.CoreName = [... % Start with NGA-like prefix
        upper(datestr(rawDataStartTime,'ddmmmyy')) 'RS02' ...
        char(xp.evaluate(xpath_str({'product','sourceAttributes','imageId'}),xml_domnode))];
elseif strcmp(gen, 'RCM')
    output_meta.CollectionInfo.CoreName = [... % Start with NGA-like prefix
        upper(datestr(rawDataStartTime,'ddmmmyy')) ...
        'RCM' output_meta.CollectionInfo.CollectorName(end) ...
        datestr(rawDataStartTime,'HHMMSS')]; % Make time of day unique identier within day
    % ScanSAR might need multiple CoreNames, one for each burst?
end
output_meta.CollectionInfo.CollectType='MONOSTATIC';
output_meta.CollectionInfo.RadarMode.ModeID=char(xp.evaluate(...
    xpath_str({'product','sourceAttributes','beamModeMnemonic'}),xml_domnode));
beamMode = char(xp.evaluate(xpath_str({...
    'product','sourceAttributes','beamMode'}),xml_domnode));
acqType = char(xp.evaluate(xpath_str({...
    'product','sourceAttributes','radarParameters','acquisitionType'}),xml_domnode));
if ((~isempty(beamMode) && strncmpi(beamMode, 'SPOTLIGHT', 9)) || ...
   (~isempty(acqType) && strncmpi(acqType, 'SPOTLIGHT', 9)) || ...
   ~isempty(strfind(output_meta.CollectionInfo.RadarMode.ModeID,'SL')))
    output_meta.CollectionInfo.RadarMode.ModeType = 'SPOTLIGHT';
elseif strcmpi(output_meta.CollectionInfo.RadarMode.ModeID(1:2), 'SC')
    error('META2SICD_RS_XML:RS_SCANSAR', 'ScanSAR mode data is not currently handled.');
else % Finally assume it's stripmap
    output_meta.CollectionInfo.RadarMode.ModeType = 'STRIPMAP';
end
if strcmp(gen, 'RS2') % All RS2 data is unclassified
    output_meta.CollectionInfo.Classification='UNCLASSIFIED';
elseif strcmp(gen, 'RCM') % RCM has this as a specific field
    output_meta.CollectionInfo.Classification=upper(char(xp.evaluate(...
        xpath_str({'product', 'securityAttributes', 'securityClassification'}), xml_domnode)));
end

%% ImageCreation
output_meta.ImageCreation.Application=char(xp.evaluate(...
    xpath_str({'product','imageGenerationParameters','generalProcessingInformation','softwareVersion'}),...
    xml_domnode));
output_meta.ImageCreation.DateTime=datenum(char(xp.evaluate(...
    xpath_str({'product','imageGenerationParameters','generalProcessingInformation','processingTime'}),...
    xml_domnode)),'yyyy-mm-ddTHH:MM:SS.FFF');
output_meta.ImageCreation.Site=char(xp.evaluate(...
    xpath_str({'product','imageGenerationParameters','generalProcessingInformation','processingFacility'}),...
    xml_domnode));
output_meta.ImageCreation.Profile='Prototype';

%% ImageData
if strcmp(gen, 'RS2')
    output_meta.ImageData.NumRows=uint32(str2double(xp.evaluate(...
       xpath_str({'product','imageAttributes','rasterAttributes','numberOfSamplesPerLine'}),...
       xml_domnode)));
    output_meta.ImageData.NumCols=uint32(str2double(xp.evaluate(...
       xpath_str({'product','imageAttributes','rasterAttributes','numberOfLines'}),...
        xml_domnode)));
elseif strcmp(gen, 'RCM')
    output_meta.ImageData.NumRows=uint32(str2double(xp.evaluate(...
       xpath_str({'product','sceneAttributes','imageAttributes','samplesPerLine'}),...
       xml_domnode)));
    output_meta.ImageData.NumCols=uint32(str2double(xp.evaluate(...
       xpath_str({'product','sceneAttributes','imageAttributes','numLines'}),...
        xml_domnode)));
end
output_meta.ImageData.FullImage=output_meta.ImageData;
output_meta.ImageData.FirstRow=uint32(0); output_meta.ImageData.FirstCol=uint32(0);
output_meta.ImageData.PixelType='RE16I_IM16I';  % RS2 always 16-bit
if strcmp(gen, 'RCM') && str2double(xp.evaluate(...  % RCM can be 16 or 32
       xpath_str({'product','imageReferenceAttributes','rasterAttributes','bitsPerSample'}),...
        xml_domnode)) == 32
    output_meta.ImageData.PixelType='RE32F_IM32F';
end
% Seems that all pixels are always valid
output_meta.ImageData.ValidData.Vertex(1).Row = uint32(0);
output_meta.ImageData.ValidData.Vertex(1).Col = uint32(0);
output_meta.ImageData.ValidData.Vertex(2).Row = uint32(0);
output_meta.ImageData.ValidData.Vertex(2).Col = output_meta.ImageData.NumCols-1;
output_meta.ImageData.ValidData.Vertex(3).Row = output_meta.ImageData.NumRows-1;
output_meta.ImageData.ValidData.Vertex(3).Col = output_meta.ImageData.NumCols-1;
output_meta.ImageData.ValidData.Vertex(4).Row = output_meta.ImageData.NumRows-1;
output_meta.ImageData.ValidData.Vertex(4).Col = uint32(0);

%% SCP
if strcmp(gen, 'RS2')
    im_at_str = 'imageAttributes';
elseif strcmp(gen, 'RCM')
    im_at_str = 'imageReferenceAttributes';
end
% There are many different equally valid options for picking the SCP point.
% One way is to chose the tie point that is closest to the image center.
num_tie_points=str2double(xp.evaluate(...
    ['count(' xpath_str({'product',im_at_str,'geographicInformation','geolocationGrid','imageTiePoint'}) ')'],...
    xml_domnode));
tiePointPixels = zeros(2,num_tie_points);
tiePointGeo    = zeros(3,num_tie_points);
for i=1:num_tie_points
    tiePointPixels(1,i) = str2double(xp.evaluate(...
            [xpath_str({'product',im_at_str,'geographicInformation','geolocationGrid','imageTiePoint'})...
            '[' num2str(i) ']' xpath_str({'imageCoordinate','pixel'})],...
            xml_domnode));
    tiePointPixels(2,i) = str2double(xp.evaluate(...
            [xpath_str({'product',im_at_str,'geographicInformation','geolocationGrid','imageTiePoint'})...
            '[' num2str(i) ']' xpath_str({'imageCoordinate','line'})],...
            xml_domnode));
    tiePointGeo(1,i) = str2double(xp.evaluate(...
            [xpath_str({'product',im_at_str,'geographicInformation','geolocationGrid','imageTiePoint'})...
            '[' num2str(i) ']' xpath_str({'geodeticCoordinate','latitude'})],...
            xml_domnode));
    tiePointGeo(2,i) = str2double(xp.evaluate(...
            [xpath_str({'product',im_at_str,'geographicInformation','geolocationGrid','imageTiePoint'})...
            '[' num2str(i) ']' xpath_str({'geodeticCoordinate','longitude'})],...
            xml_domnode));
    tiePointGeo(3,i) = str2double(xp.evaluate(...
            [xpath_str({'product',im_at_str,'geographicInformation','geolocationGrid','imageTiePoint'})...
            '[' num2str(i) ']' xpath_str({'geodeticCoordinate','height'})],...
            xml_domnode));
end
% Pick tie point closest to center for SCP
centerPoint = [double(output_meta.ImageData.NumRows-1)/2.0; ...
               double(output_meta.ImageData.NumCols-1)/2.0];
D = (tiePointPixels - repmat(centerPoint,1,num_tie_points));
[C,scp_index] = min( sqrt( sum(D.^2) ) );
output_meta.ImageData.SCPPixel.Row = uint32(tiePointPixels(1,scp_index));
output_meta.ImageData.SCPPixel.Col = uint32(tiePointPixels(2,scp_index));

% Sometimes lines up with SCP in Lockheed SICDs (and sometimes not):
% output_meta.ImageData.SCPPixel.Col = ceil(output_meta.ImageData.NumCols/2)-1;
% output_meta.ImageData.SCPPixel.Row = floor(output_meta.ImageData.NumRows/2);

%% GeoData
% All RS2 and RCM data we know use the WGS84 model, although it is stated
% in slightly different XML fields in the RS2 and RCM product.xml
output_meta.GeoData.EarthModel='WGS_84';
% Initially, we just seed this with a rough value.  Later we will put in
% something more precise.
output_meta.GeoData.SCP.LLH.Lat = tiePointGeo(1,scp_index);
output_meta.GeoData.SCP.LLH.Lon = tiePointGeo(2,scp_index);
output_meta.GeoData.SCP.LLH.HAE = tiePointGeo(3,scp_index);
pos_ecf = geodetic_to_ecf(tiePointGeo(:,scp_index));
output_meta.GeoData.SCP.ECF.X = pos_ecf(1);
output_meta.GeoData.SCP.ECF.Y = pos_ecf(2);
output_meta.GeoData.SCP.ECF.Z = pos_ecf(3);
% Corner coordinates will be computed later by derived_sicd_fields.
% Corners
% min_row=min(tiePointPixels(1,:));
% min_col=min(tiePointPixels(2,:));
% max_row=max(tiePointPixels(1,:));
% max_col=max(tiePointPixels(2,:));
% ll_index=find((tiePointPixels(1,:)==min_row)&(tiePointPixels(2,:)==min_col), 1);
% ul_index=find((tiePointPixels(1,:)==max_row)&(tiePointPixels(2,:)==min_col), 1);
% ur_index=find((tiePointPixels(1,:)==max_row)&(tiePointPixels(2,:)==max_col), 1);
% lr_index=find((tiePointPixels(1,:)==min_row)&(tiePointPixels(2,:)==max_col), 1);
% output_meta.GeoData.ImageCorners.ICP.FRFC.Lat=tiePointGeo(1,ul_index);
% output_meta.GeoData.ImageCorners.ICP.FRFC.Lon=tiePointGeo(2,ul_index);
% output_meta.GeoData.ImageCorners.ICP.FRLC.Lat=tiePointGeo(1,ur_index);
% output_meta.GeoData.ImageCorners.ICP.FRLC.Lon=tiePointGeo(2,ur_index);
% output_meta.GeoData.ImageCorners.ICP.LRLC.Lat=tiePointGeo(1,lr_index);
% output_meta.GeoData.ImageCorners.ICP.LRLC.Lon=tiePointGeo(2,lr_index);
% output_meta.GeoData.ImageCorners.ICP.LRFC.Lat=tiePointGeo(1,ll_index);
% output_meta.GeoData.ImageCorners.ICP.LRFC.Lon=tiePointGeo(2,ll_index);

%% State vectors
num_state_vectors=str2double(xp.evaluate(...
    ['count(' xpath_str({'product','sourceAttributes','orbitAndAttitude','orbitInformation','stateVector'}) ')'],...
    xml_domnode));
state_vector_T  = zeros(1,num_state_vectors);
state_vector_T_frac  = zeros(1,num_state_vectors);
state_vector_X  = zeros(1,num_state_vectors);
state_vector_Y  = zeros(1,num_state_vectors);
state_vector_Z  = zeros(1,num_state_vectors);
% state_vector_VX = zeros(1,num_state_vectors);
% state_vector_VY = zeros(1,num_state_vectors);
% state_vector_VZ = zeros(1,num_state_vectors);
for i=1:num_state_vectors
    timeStamp = char(xp.evaluate(...
        [xpath_str({'product','sourceAttributes','orbitAndAttitude','orbitInformation','stateVector'})...
        '[' num2str(i) ']' xpath_str({'timeStamp'})],...
        xml_domnode));
    [state_vector_T(i), state_vector_T_frac(i)] = datenum_w_frac(timeStamp);

    state_vector_X(i) = str2double(xp.evaluate(...
        [xpath_str({'product','sourceAttributes','orbitAndAttitude','orbitInformation','stateVector'})...
        '[' num2str(i) ']' xpath_str({'xPosition'})],...
        xml_domnode));
    state_vector_Y(i) = str2double(xp.evaluate(...
        [xpath_str({'product','sourceAttributes','orbitAndAttitude','orbitInformation','stateVector'})...
        '[' num2str(i) ']' xpath_str({'yPosition'})],...
        xml_domnode));
    state_vector_Z(i) = str2double(xp.evaluate(...
        [xpath_str({'product','sourceAttributes','orbitAndAttitude','orbitInformation','stateVector'})...
        '[' num2str(i) ']' xpath_str({'zPosition'})],...
        xml_domnode));
%     state_vector_VX(i) = str2double(xp.evaluate(...
%         [xpath_str({'product','sourceAttributes','orbitAndAttitude','orbitInformation','stateVector'})...
%         '[' num2str(i) ']' xpath_str({'xVelocity'})],...
%         xml_domnode));
%     state_vector_VY(i) = str2double(xp.evaluate(...
%         [xpath_str({'product','sourceAttributes','orbitAndAttitude','orbitInformation','stateVector'})...
%         '[' num2str(i) ']' xpath_str({'yVelocity'})],...
%         xml_domnode));
%     state_vector_VZ(i) = str2double(xp.evaluate(...
%         [xpath_str({'product','sourceAttributes','orbitAndAttitude','orbitInformation','stateVector'})...
%         '[' num2str(i) ']' xpath_str({'zVelocity'})],...
%         xml_domnode));
end
state_vector_T = round((state_vector_T-rawDataStartTime)*SECONDS_IN_A_DAY) + ... % Convert from days to secs
        (state_vector_T_frac-rawDataStartTimeFrac); % Handle fractional seconds
% sv2poly.m shows ways to determine best polynomial order, but 5th is almost always best
polyorder = min(5, numel(state_vector_T) - 1);
P_x = polyfit(state_vector_T, state_vector_X, polyorder);
P_y = polyfit(state_vector_T, state_vector_Y, polyorder);
P_z = polyfit(state_vector_T, state_vector_Z, polyorder);
output_meta.Position.ARPPoly.X = P_x(end:-1:1).';
output_meta.Position.ARPPoly.Y = P_y(end:-1:1).';
output_meta.Position.ARPPoly.Z = P_z(end:-1:1).';

%% Grid
if strcmp(char(xp.evaluate(...
    xpath_str({'product','imageGenerationParameters','generalProcessingInformation','productType'}),...
    xml_domnode)),'SLC')
    output_meta.Grid.ImagePlane = 'SLANT';
    output_meta.Grid.Type = 'RGZERO';
else
    output_meta.Grid.ImagePlane = 'GROUND';
end
output_meta.Grid.Row.SS = str2double(xp.evaluate(...
    xpath_str({'product',im_at_str,'rasterAttributes','sampledPixelSpacing'}),...
    xml_domnode));
% Col.SS is derived after DRateSFPoly below, rather than used from this
% given field, so that SICD metadata can be internally consistent.
% output_meta.Grid.Col.SS = str2double(xp.evaluate(...
%     xpath_str({'product',im_at_str,'rasterAttributes','sampledLineSpacing'}),...
%     xml_domnode));
output_meta.Grid.Row.Sgn = -1; % Always true for RS2
output_meta.Grid.Col.Sgn = -1; % Always true for RS2
fc = str2double(xp.evaluate(...
    xpath_str({'product','sourceAttributes','radarParameters','radarCenterFrequency'}),...
    xml_domnode)); % Center frequency
output_meta.Grid.Row.ImpRespBW = 2*str2double(xp.evaluate(...
    xpath_str({'product','imageGenerationParameters','sarProcessingInformation','totalProcessedRangeBandwidth'}),...
    xml_domnode))/SPEED_OF_LIGHT;
dop_bw = str2double(xp.evaluate(...
    xpath_str({'product','imageGenerationParameters','sarProcessingInformation','totalProcessedAzimuthBandwidth'}),...
    xml_domnode)); % Doppler bandwidth
[zd_last, zd_last_frac] = datenum_w_frac(char(xp.evaluate(...
    xpath_str({'product','imageGenerationParameters','sarProcessingInformation','zeroDopplerTimeLastLine'}),...
    xml_domnode)));
[zd_first, zd_first_frac] = datenum_w_frac(char(xp.evaluate(...
    xpath_str({'product','imageGenerationParameters','sarProcessingInformation','zeroDopplerTimeFirstLine'}),...
    xml_domnode)));
ss_zd_s = abs(round((zd_last-zd_first)*SECONDS_IN_A_DAY) + ... % Convert from days to secs
        (zd_last_frac-zd_first_frac))/... % Handle fractional seconds
        double(output_meta.ImageData.NumCols-1); % Image column spacing in zero doppler time (seconds)
output_meta.Grid.Row.KCtr = 2*fc/SPEED_OF_LIGHT;
output_meta.Grid.Col.KCtr = 0;
output_meta.Grid.Row.DeltaK1 = -output_meta.Grid.Row.ImpRespBW/2;
output_meta.Grid.Row.DeltaK2 = -output_meta.Grid.Row.DeltaK1;
output_meta.Grid.Row.DeltaKCOAPoly = 0;
% Constants used to compute weighting parameters
NUM_SAMPLES = 512;
OVERSAMPLE = 1024;
output_meta.Grid.Row.WgtType.WindowName = upper(char(xp.evaluate(...
    xpath_str({'product','imageGenerationParameters','sarProcessingInformation','rangeWindow','windowName'}),...
    xml_domnode)));
if strcmpi(output_meta.Grid.Row.WgtType.WindowName,'KAISER') % The usual RS2 weigting
    output_meta.Grid.Row.WgtType.Parameter.name = 'BETA';
    output_meta.Grid.Row.WgtType.Parameter.value = char(xp.evaluate(...
        xpath_str({'product','imageGenerationParameters','sarProcessingInformation','rangeWindow','windowCoefficient'}),...
        xml_domnode));
    beta_row = str2double(output_meta.Grid.Row.WgtType.Parameter.value);
    % We don't use the Mathworks Kaiser function, so we won't be dependent on the Signal Processing Toolbox
    output_meta.Grid.Row.WgtFunct = kaiser_nosptb(NUM_SAMPLES,beta_row);
    imp_resp = abs(fft(output_meta.Grid.Row.WgtFunct, round(NUM_SAMPLES*OVERSAMPLE))); % Oversampled response function
    imp_resp = imp_resp/sum(output_meta.Grid.Row.WgtFunct); % Normalize to unit peak
    ind = find(imp_resp<1/sqrt(2),1,'first')+[-1 -0]; % Samples surrounding half-power point
    ind = interp1(imp_resp(ind), ind, 1/sqrt(2)); % Linear interpolation to solve for half-power point
    row_broadening_factor = 2*(ind - 1)/OVERSAMPLE;
    output_meta.Grid.Row.ImpRespWid = row_broadening_factor/output_meta.Grid.Row.ImpRespBW;
end
output_meta.Grid.Col.WgtType.WindowName = upper(char(xp.evaluate(...
    xpath_str({'product','imageGenerationParameters','sarProcessingInformation','azimuthWindow','windowName'}),...
    xml_domnode)));

%% Radar Collection
% Ultrafine and spotlight modes have "lower" and "upper" parts to the
% pulse.
% output_meta.RadarCollection.RefFreqIndex=uint32(0); % Absence of this field means all frequencies are true values
num_pulse_parts = str2double(xp.evaluate(...
    ['count(' xpath_str({'product','sourceAttributes','radarParameters','pulseLength'}) ')'],...
    xml_domnode));
for i=1:num_pulse_parts
    output_meta.RadarCollection.Waveform.WFParameters(i).TxRFBandwidth = str2double(xp.evaluate(...
        [xpath_str({'product','sourceAttributes','radarParameters','pulseBandwidth'}) '[' num2str(i) ']'],...
        xml_domnode)); % Bandwidth
    output_meta.RadarCollection.Waveform.WFParameters(i).TxPulseLength = str2double(xp.evaluate(...
        [xpath_str({'product','sourceAttributes','radarParameters','pulseLength'}) '[' num2str(i) ']'],...
        xml_domnode));
    output_meta.RadarCollection.Waveform.WFParameters(i).RcvDemodType='CHIRP';
    sample_rate = str2double(xp.evaluate(...
        [xpath_str({'product','sourceAttributes','radarParameters','adcSamplingRate'}) '[' num2str(i) ']'],...
        xml_domnode));
    output_meta.RadarCollection.Waveform.WFParameters(i).RcvWindowLength = str2double(xp.evaluate(...
        xpath_str({'product','sourceAttributes','radarParameters','samplesPerEchoLine'}),...
        xml_domnode))/sample_rate;
    output_meta.RadarCollection.Waveform.WFParameters(i).ADCSampleRate = sample_rate;
    output_meta.RadarCollection.Waveform.WFParameters(i).RcvFMRate = 0; % True for RcvDemodType='CHIRP'
end
bw = sum([output_meta.RadarCollection.Waveform.WFParameters.TxRFBandwidth]);
output_meta.RadarCollection.TxFrequency.Min = fc-(bw/2); % fc calculated in Grid section
output_meta.RadarCollection.TxFrequency.Max = fc+(bw/2);
% Assumes pulse parts are exactly adjacent in bandwidth
output_meta.RadarCollection.Waveform.WFParameters(1).TxFreqStart = ...
    output_meta.RadarCollection.TxFrequency.Min;
for i=2:num_pulse_parts
    output_meta.RadarCollection.Waveform.WFParameters(i).TxFreqStart = ...
        output_meta.RadarCollection.Waveform.WFParameters(i-1).TxFreqStart + ...
        output_meta.RadarCollection.Waveform.WFParameters(i-1).TxRFBandwidth;
end
% Polarization
pols = textscan(char(xp.evaluate(...
    xpath_str({'product','sourceAttributes','radarParameters','polarizations'}),...
    xml_domnode)),'%s');
pols = pols{1};
M = struct('H','H','V','V','C','RHC');
tx_pols = unique(cellfun(@(x) x(1), pols));
for i=1:numel(pols)
    output_meta.RadarCollection.RcvChannels.ChanParameters(i).TxRcvPolarization = ...
        [M.(pols{i}(1)) ':' M.(pols{i}(2))];
end
if isscalar(tx_pols) % Only one transmit polarization
    output_meta.RadarCollection.TxPolarization = M.(tx_pols);
else % Multiple transmit polarizations
    output_meta.RadarCollection.TxPolarization = 'SEQUENCE';
    for i = 1:numel(tx_pols)
        output_meta.RadarCollection.TxSequence.TxStep(i).TxPolarization = M.(tx_pols(i));
    end
end
% Another way to get polarimetric channels:
% num_pol_bands = str2double(xp.evaluate(...
%     ['count(' xpath_str({'product','imageAttributes','fullResolutionImageData'}) ')'],...
%     xml_domnode));
% for i=1:num_pol_bands
%     pol = char(xp.evaluate(...
%         [xpath_str({'product','imageAttributes','fullResolutionImageData'}) '[' num2str(i) ']/@pole'],...
%         xml_domnode));
% end

%% Timeline
output_meta.Timeline.CollectStart = rawDataStartTime + (rawDataStartTimeFrac/SECONDS_IN_A_DAY);
if strcmp(gen, 'RS2')
    prf_xp_str = {'pulseRepetitionFrequency'};
elseif strcmp(gen, 'RCM')
    prf_xp_str = {'prfInformation', 'pulseRepetitionFrequency'};
end
prf = str2double(xp.evaluate(...
    xpath_str([{'product','sourceAttributes', 'radarParameters'} prf_xp_str]),...
    xml_domnode));
num_lines_entries = str2double(xp.evaluate(...
    ['count(' xpath_str({'product','imageGenerationParameters','sarProcessingInformation','numberOfLinesProcessed'}) ')'],...
    xml_domnode));
num_lines_processed = zeros(num_lines_entries,1);
for i = 1:num_lines_entries
    num_lines_processed(i) = str2double(xp.evaluate(...
        [xpath_str({'product','imageGenerationParameters','sarProcessingInformation','numberOfLinesProcessed'}) '[@pole="' pols{i} '"]'],...
        xml_domnode));
end
num_lines_processed = num_lines_processed(1) * numel(tx_pols);
if num_lines_entries ~= numel(pols) || ~all(num_lines_processed == num_lines_processed(1))
    % This should never happen, but we'll throw an error if it does.
    warning('META2SICD_RS_XML:UnableToComputeCollectDuration', 'Unhandled data condition.');
end
prf = prf * num_pulse_parts;
if num_pulse_parts==2 && ...
        strcmp(output_meta.CollectionInfo.RadarMode.ModeType,'STRIPMAP')
    % Why????
    % This seems to be necessary to make CollectDuration match CA ranges
    % and to make 1/prf roughly equal to ss_zd_s (which is generally true
    % for STRIPMAP).  But we already doubled the prf above to account for
    % the pulse parts (to make real vs effective prf), so why do we have to
    % do it again? And why don't we have to do it for SPOTLIGHT?
    prf = 2*prf;
end
output_meta.Timeline.CollectDuration = num_lines_processed/prf;
output_meta.Timeline.IPP.Set.TStart = 0;
output_meta.Timeline.IPP.Set.TEnd = 0; % Apply real value later.  Just a placeholder.
output_meta.Timeline.IPP.Set.IPPStart = uint32(0);
output_meta.Timeline.IPP.Set.IPPEnd = uint32(num_lines_processed);
output_meta.Timeline.IPP.Set.IPPPoly = [0; prf];
output_meta.Timeline.IPP.Set.TEnd = output_meta.Timeline.CollectDuration;

%% Image Formation
output_meta.ImageFormation.RcvChanProc = ...
    struct('NumChanProc', uint32(1), ... % Assumes not a MODEX collect
    'PRFScaleFactor', 1/max(num_pulse_parts, numel(tx_pols))); % Either polarimetric or multi-step, but not both.
output_meta.ImageFormation.ImageFormAlgo = 'RMA';
output_meta.ImageFormation.TStartProc = 0;
output_meta.ImageFormation.TEndProc = output_meta.Timeline.CollectDuration;
output_meta.ImageFormation.TxFrequencyProc.MinProc = ...
    output_meta.RadarCollection.TxFrequency.Min;
output_meta.ImageFormation.TxFrequencyProc.MaxProc = ...
    output_meta.RadarCollection.TxFrequency.Max;
output_meta.ImageFormation.STBeamComp = 'GLOBAL';
output_meta.ImageFormation.ImageBeamComp = 'SV';
output_meta.ImageFormation.AzAutofocus = 'NO';
output_meta.ImageFormation.RgAutofocus = 'NO';

%% RMA.INCA
output_meta.RMA.RMAlgoType = 'OMEGA_K';
output_meta.RMA.ImageType = 'INCA';
output_meta.SCPCOA.SideOfTrack = char(xp.evaluate(...
    xpath_str({'product','sourceAttributes','radarParameters','antennaPointing'}),...
    xml_domnode));  % Should always be right looking for RCM
output_meta.SCPCOA.SideOfTrack = upper(output_meta.SCPCOA.SideOfTrack(1));
if output_meta.SCPCOA.SideOfTrack=='L'
    ss_zd_s = -ss_zd_s;
    % In addition to left/right, RS2 data can independently be in
    % increasing/decreasing line order.
    if (round((zd_first-zd_last)*SECONDS_IN_A_DAY) + ...
        (zd_first_frac-zd_last_frac)) < 0 % zd_last occurred after zd_first
        zd_first = zd_last;
        zd_first_frac = zd_last_frac;
    end
    look = 1;
else
    if (round((zd_first-zd_last)*SECONDS_IN_A_DAY) + ...
        (zd_first_frac-zd_last_frac)) > 0 % zd_last occurred before zd_first
        zd_first = zd_last;
        zd_first_frac = zd_last_frac;
    end
    look = -1;
end
% Zero doppler time of SCP relative to collect start
zd_t_scp = round((zd_first-rawDataStartTime)*SECONDS_IN_A_DAY) + ... % Convert days to seconds
    (zd_first_frac-rawDataStartTimeFrac) + ... % Handle fractional seconds
    (double(output_meta.ImageData.SCPPixel.Col) * ss_zd_s);
if strcmp(gen, 'RS2')
    near_range = str2double(xp.evaluate(...
        xpath_str({'product','imageGenerationParameters','sarProcessingInformation','slantRangeNearEdge'}),...
        xml_domnode)); % in meters
elseif strcmp(gen, 'RCM')
    near_range = str2double(xp.evaluate(...
        xpath_str({'product','sceneAttributes','imageAttributes','slantRangeNearEdge'}),...
        xml_domnode)); % in meters
end
output_meta.RMA.INCA.R_CA_SCP = near_range + ...
    (double(output_meta.ImageData.SCPPixel.Row)*output_meta.Grid.Row.SS);
output_meta.RMA.INCA.FreqZero = fc;
% Doppler Rate (We do this first since some other things are dependent on
% it.)
% For the purposes of the DRateSFPoly computation, we ignore any
% changes in velocity over the azimuth dimension.
pos_coefs = [P_x(:) P_y(:) P_z(:)];
% Velocity is derivate of position.
vel_coefs=pos_coefs(1:end-1,:).*repmat(((size(pos_coefs,1)-1):-1:1)',[1 3]);
vel_x = polyval(vel_coefs(:,1), zd_t_scp);
vel_y = polyval(vel_coefs(:,2), zd_t_scp);
vel_z = polyval(vel_coefs(:,3), zd_t_scp);
vm_ca_sq = vel_x.^2 + vel_y.^2 + vel_z.^2; % Magnitude of the velocity squared
r_ca = [output_meta.RMA.INCA.R_CA_SCP; 1]; % Polynomial representing range as a function of range distance from SCP
if strcmp(gen, 'RS2')
    drc_xp_str = {'product','imageGenerationParameters','dopplerRateValues', 'dopplerRateValuesCoefficients'};
elseif strcmp(gen, 'RCM')
    drc_xp_str = {'product','dopplerRate','dopplerRateEstimate', 'dopplerRateCoefficients'};
end
drr_xp_str = [drc_xp_str{1:(end-1)} {'dopplerRateReferenceTime'}];
dop_rate_coefs = str2num(xp.evaluate(xpath_str(drc_xp_str),... % Multiple numbers.  We need str2num instead of str2double
    xml_domnode)); % Shifted (origin at dop_rate_ref_t, not SCP) and scaled (sec, not m) version of SICD DopCentroidPoly
dop_rate_ref_t = str2double(xp.evaluate(xpath_str(drr_xp_str),...
    xml_domnode)); % Reference time of Doppler rate polynomial
dop_rate_coefs_shifted = polyshift(dop_rate_coefs, ... % Shift so SCP is reference
    (output_meta.RMA.INCA.R_CA_SCP*2/SPEED_OF_LIGHT) - ... % SICD reference time (SCP)
    dop_rate_ref_t); % Reference time of native Doppler Centroid polynomial
dop_rate_coefs_scaled = dop_rate_coefs_shifted .*  ... % Scale from seconds to meters
    (2/SPEED_OF_LIGHT) .^ (0:(length(dop_rate_coefs)-1));
output_meta.RMA.INCA.DRateSFPoly = - conv(dop_rate_coefs_scaled.',r_ca) * ... % Multiplication of two polynomials is just a convolution of their coefficients
    SPEED_OF_LIGHT / (2 * fc * vm_ca_sq(1)); % Assumes a SGN of -1

%% Fields dependent on Doppler rate
% This computation of SS is actually better than the claimed SS
% (sampledLineSpacing) in many ways, because this makes all of the metadata
% internally consistent.  This must be the sample spacing exactly at SCP
% (which is the definition for SS in SICD), if the other metadata from
% which is it computed is correct and consistent. Since column SS can vary
% slightly over a RGZERO image, we don't know if the claimed sample spacing
% in the native metadata is at our chosen SCP, or another point, or an
% average across image or something else.
output_meta.Grid.Col.SS = sqrt(vm_ca_sq(1)) * abs(ss_zd_s) * ...
    output_meta.RMA.INCA.DRateSFPoly(1,1);
output_meta.Grid.Col.ImpRespBW = dop_bw*abs(ss_zd_s)/output_meta.Grid.Col.SS; % Convert to azimuth spatial bandwidth (cycles per meter)
output_meta.RMA.INCA.TimeCAPoly = [zd_t_scp; ss_zd_s/output_meta.Grid.Col.SS];
if strcmpi(output_meta.Grid.Col.WgtType.WindowName,'KAISER') % The usual RS2 weigting
    output_meta.Grid.Col.WgtType.Parameter.name = 'BETA';
    output_meta.Grid.Col.WgtType.Parameter.value = char(xp.evaluate(...
        xpath_str({'product','imageGenerationParameters','sarProcessingInformation','azimuthWindow','windowCoefficient'}),...
        xml_domnode));
    beta_col = str2double(output_meta.Grid.Col.WgtType.Parameter.value);
    % We don't use the Mathworks Kaiser function, so we won't be dependent on the Signal Processing Toolbox
    output_meta.Grid.Col.WgtFunct = kaiser_nosptb(NUM_SAMPLES,beta_col);
    imp_resp = abs(fft(output_meta.Grid.Col.WgtFunct, round(NUM_SAMPLES*OVERSAMPLE))); % Oversampled response function
    imp_resp = imp_resp/sum(output_meta.Grid.Col.WgtFunct); % Normalize to unit peak
    ind = find(imp_resp<1/sqrt(2),1,'first')+[-1 -0]; % Samples surrounding half-power point
    ind = interp1(imp_resp(ind), ind, 1/sqrt(2)); % Linear interpolation to solve for half-power point
    col_broadening_factor = 2*(ind - 1)/OVERSAMPLE;
    output_meta.Grid.Col.ImpRespWid = col_broadening_factor/output_meta.Grid.Col.ImpRespBW;
end

%% Doppler Centroid
if strcmp(gen, 'RS2')
    dc_xp_str = {'product','imageGenerationParameters','dopplerCentroid'};
elseif strcmp(gen, 'RCM')
    dc_xp_str = {'product','dopplerCentroid','dopplerCentroidEstimate'};
end
dop_cent_coefs = str2num(xp.evaluate(... % Multiple numbers.  We need str2num instead of str2double
    xpath_str([dc_xp_str,{'dopplerCentroidCoefficients'}]),...
    xml_domnode)); % Shifted (origin at dop_cent_ref_t, not SCP) and scaled (sec, not m) version of SICD DopCentroidPoly
dop_cent_ref_t = str2double(xp.evaluate(...
    xpath_str([dc_xp_str,{'dopplerCentroidReferenceTime'}]),...
    xml_domnode)); % Reference time of Doppler Centroid polynomial
dop_cent_coefs_shifted = polyshift(dop_cent_coefs, ... % Shift so SCP is reference
    (output_meta.RMA.INCA.R_CA_SCP*2/SPEED_OF_LIGHT) - ... % SICD reference time (SCP)
    dop_cent_ref_t); % Reference time of native Doppler Centroid polynomial
dop_cent_coefs_scaled = dop_cent_coefs_shifted .*  ... % Scale from seconds to meters
    (2/SPEED_OF_LIGHT) .^ (0:(length(dop_cent_coefs)-1));
output_meta.RMA.INCA.DopCentroidPoly=dop_cent_coefs_scaled.';
% Adjust Doppler Centroid for spotlight
if strcmp(output_meta.CollectionInfo.RadarMode.ModeType,'SPOTLIGHT')
    [dop_est, dop_est_frac] = datenum_w_frac(char(xp.evaluate(...  % Doppler estimate time
        xpath_str([dc_xp_str 'timeOfDopplerCentroidEstimate']), xml_domnode)));
    dop_est_t = abs(round((dop_est - rawDataStartTime)*SECONDS_IN_A_DAY) + ... % Convert from days to secs
        (dop_est_frac - rawDataStartTimeFrac)); % Handle fractional seconds
    dop_est_col = (dop_est_t - zd_t_scp)/ss_zd_s; % This is the column where the doppler centroid was computed.
    % Column-dependent variation in DopCentroidPoly due to spotlight
    output_meta.RMA.INCA.DopCentroidPoly(1,2) = ...
        -look * fc * (2 / SPEED_OF_LIGHT) * sqrt(vm_ca_sq(1)) / ...
        output_meta.RMA.INCA.R_CA_SCP;
    % dopplerCentroid in native metadata was defined at specific column,
    % which might not be our SCP column.  Adjust so that SCP column is
    % correct.
    output_meta.RMA.INCA.DopCentroidPoly(1,1) = ...
        output_meta.RMA.INCA.DopCentroidPoly(1,1) - ...
        (output_meta.RMA.INCA.DopCentroidPoly(1,2) * ...
        dop_est_col * output_meta.Grid.Col.SS);
end
output_meta.Grid.Col.DeltaKCOAPoly = ...
    output_meta.RMA.INCA.DopCentroidPoly * ss_zd_s / output_meta.Grid.Col.SS;
% Compute Col.DeltaK1/K2 from DeltaKCOAPoly
% This is not always straightforward to do generically for any possible
% DeltaKCOAPoly 2D polynomial, since you would have to compute all 2D roots
% and edge cases.  However, for the RS case, this can be solved exactly,
% since its usually a 1D polynomial.  Even the spotlight case only brings
% in a linear variation in the second dimensions, so its still easily
% solved.
% Min/max in row/range must exist at edges or internal local min/max
minmax = roots(polyder(output_meta.Grid.Col.DeltaKCOAPoly(end:-1:1,1)));
rg_bounds_m = (double([0 (output_meta.ImageData.NumRows-1)]) - ...
     double(output_meta.ImageData.SCPPixel.Row)) * output_meta.Grid.Row.SS;
possible_bounds_rg = [rg_bounds_m minmax(minmax>min(rg_bounds_m) & minmax<max(rg_bounds_m))];
% Constant or (linearly increasing\decreasing for spotlight) in column, so
% edges must contain max/min.
possible_bounds_az = (double([0 (output_meta.ImageData.NumCols-1)]) - ...
     double(output_meta.ImageData.SCPPixel.Col)) * output_meta.Grid.Col.SS;
possible_bounds_deltak = sicd_polyval2d(output_meta.Grid.Col.DeltaKCOAPoly, ...
    possible_bounds_az, possible_bounds_rg);
output_meta.Grid.Col.DeltaK1 = min(possible_bounds_deltak(:)) - ...
    (output_meta.Grid.Col.ImpRespBW/2);
output_meta.Grid.Col.DeltaK2 = max(possible_bounds_deltak(:)) + ...
    (output_meta.Grid.Col.ImpRespBW/2);
% Wrapped spectrum
if (output_meta.Grid.Col.DeltaK1 < -(1/output_meta.Grid.Col.SS)/2) || ...
        (output_meta.Grid.Col.DeltaK2 > (1/output_meta.Grid.Col.SS)/2)
    output_meta.Grid.Col.DeltaK1 = -(1/output_meta.Grid.Col.SS)/2;
    output_meta.Grid.Col.DeltaK2 = -output_meta.Grid.Col.DeltaK1;
end
%% TimeCOAPoly
% TimeCOAPoly=TimeCA+(DopCentroid/dop_rate)
% Since we can't evaluate this equation analytically, we will evaluate
% samples of it across our image and fit a 2D polynomial to it.
POLY_ORDER = 2; % Order of polynomial which we want to compute
grid_samples = POLY_ORDER + 1; % in each dimension
coords_az_m = linspace(-double(output_meta.ImageData.SCPPixel.Col),...
    double(output_meta.ImageData.NumCols-output_meta.ImageData.SCPPixel.Col-1), grid_samples) * ...
    output_meta.Grid.Col.SS;
coords_rg_m = linspace(-double(output_meta.ImageData.SCPPixel.Row),...
    double(output_meta.ImageData.NumRows-output_meta.ImageData.SCPPixel.Row-1), grid_samples) * ...
    output_meta.Grid.Row.SS;
timeca_sampled = sicd_polyval2d(output_meta.RMA.INCA.TimeCAPoly(:).',coords_az_m,coords_rg_m);
dopcentroid_sampled = sicd_polyval2d(output_meta.RMA.INCA.DopCentroidPoly,coords_az_m,coords_rg_m);
doprate_sampled = sicd_polyval2d(dop_rate_coefs_scaled.',coords_az_m,coords_rg_m);
timecoapoly_sampled = timeca_sampled+(dopcentroid_sampled./doprate_sampled);
% Least squares fit for 2D polynomial
% A*x = b
[coords_az_m, coords_rg_m] = ndgrid(coords_az_m, coords_rg_m);
a = zeros(grid_samples^2, (POLY_ORDER+1)^2);
for i = 0:POLY_ORDER
    for j = 0:POLY_ORDER
        a(:,i*(POLY_ORDER+1)+j+1) = (coords_rg_m(:).^j).*(coords_az_m(:).^i);
    end
end
b_coa = zeros((POLY_ORDER+1)^2,1);
for i=1:((POLY_ORDER+1)^2)
   b_coa(i)=sum(timecoapoly_sampled(:).*a(:,i)); % center of aperture
end
A=zeros((POLY_ORDER+1)^2);
for i=1:((POLY_ORDER+1)^2)
    for j=1:((POLY_ORDER+1)^2)
        A(i,j)=sum(a(:,i).*a(:,j));
    end
end
old_warning_state=warning('off','MATLAB:nearlySingularMatrix');
x=A\b_coa; % MATLAB often flags this as badly scaled, but results still appear valid
warning(old_warning_state);
output_meta.Grid.TimeCOAPoly=reshape(x, POLY_ORDER+1, POLY_ORDER+1);
if strcmp(output_meta.CollectionInfo.RadarMode.ModeType,'SPOTLIGHT')
    output_meta.Grid.TimeCOAPoly = output_meta.Grid.TimeCOAPoly(1);
    % This field required to compute TimeCOAPoly, but not allowed for
    % spotlight in SICD.
    output_meta.RMA.INCA = rmfield(output_meta.RMA.INCA, 'DopCentroidPoly');
else % This field also not allowed for spotlight in SICD.
    output_meta.RMA.INCA.DopCentroidCOA=true;
end

%% GeoData
% Now that sensor model fields have been populated, we can populate
% GeoData.SCP more precisely.
ecf = point_image_to_ground([output_meta.ImageData.SCPPixel.Row;output_meta.ImageData.SCPPixel.Col],output_meta);
output_meta.GeoData.SCP.ECF.X=ecf(1);
output_meta.GeoData.SCP.ECF.Y=ecf(2);
output_meta.GeoData.SCP.ECF.Z=ecf(3);
llh=ecf_to_geodetic([output_meta.GeoData.SCP.ECF.X output_meta.GeoData.SCP.ECF.Y output_meta.GeoData.SCP.ECF.Z]);
output_meta.GeoData.SCP.LLH.Lat=llh(1);
output_meta.GeoData.SCP.LLH.Lon=llh(2);
output_meta.GeoData.SCP.LLH.HAE=llh(3);

%% Radiometric
if exist('beta_domnode','var')
    % Offset always zero for SLC, so we only need gains
    betas = str2num(xp.evaluate(xpath_str({'lut','gains'}),beta_domnode)); %#ok<ST2NM>
    % Of the provided LUTs, we really only work with beta here, since it is
    % the radiometric term most commonly kept constant, and the others
    % (sigma/gamma) can always be derived from it.
    % if any(strcmp({'Constant-beta', 'Point Target', 'Point Target-1', ...
    %         'Calibration-1', 'Calibration-2', 'Ship-1', 'Ship-2', ...
    %         'Ship-3', 'Unity'}, ... % Known modes with constant beta
    %         xp.evaluate(xpath_str({'product','imageGenerationParameters', ...
    %         'sarProcessingInformation','lutApplied'}), xml_domnode)))
    if all(betas==betas(1)) % In case we missed some modes, this condition may be more reliable
        output_meta.Radiometric.BetaZeroSFPoly = 1/betas(1)^2;
    else  % Otherwise fit a 1D polynomial in range
        % RS2 has value for every row
        coords_rg_m = (double(0:(output_meta.ImageData.NumRows-1)) - ...
            double(output_meta.ImageData.SCPPixel.Row)) * output_meta.Grid.Row.SS;
        if strcmp(gen, 'RCM') % RCM subsamples the rows
            rng_indices = ((str2double(xp.evaluate(xpath_str({'lut','pixelFirstAnglesValue'}), beta_domnode)):...
                ... % Should be this, but simulated datasets are inconsistent with spec document
                ... % crm_indices = (xp.evaluate(xpath_str({'lut','pixelFirstLutValue'}), beta_domnode):...
                (str2double(xp.evaluate(xpath_str({'lut','numberOfValues'}), beta_domnode))-1)) * ... % First row is zero
                str2double(xp.evaluate(xpath_str({'lut','stepSize'}), beta_domnode))) + 1;
            coords_rg_m = coords_rg_m(rng_indices);
        end
        % For the datasets we have seen, this function is very close to
        % linear.  For the "Mixed" LUT, there is a tiny linear piecewise
        % deviation from a single overall linear.
        betapoly = polyfit(coords_rg_m, 1./betas.^2, 2);
        output_meta.Radiometric.BetaZeroSFPoly = betapoly(end:-1:1).';
    end
    % RCS, Sigma, and Gamma will be computed below in derived_sicd_fields
    % derived_sicd_fields.
    output_meta.Radiometric.NoiseLevel.NoiseLevelType = 'ABSOLUTE';
    if strcmp(gen, 'RS2')
        beta0_str = [xpath_str({'product', 'sourceAttributes', ...
            'radarParameters','referenceNoiseLevel'}) ...
            '[@incidenceAngleCorrection="Beta Nought"]'];
        noise_domnode = xml_domnode;  % RS2 noise is in main product.xml
    elseif strcmp(gen, 'RCM') % RCM noise is in separate file
        beta0_str = [xpath_str({'noiseLevels', 'referenceNoiseLevel'}) ...
            '/*[text()=''Beta Nought'']/..'];
    end
    if exist('noise_domnode','var')
        pfv = str2double(xp.evaluate([beta0_str ...
            '/*[local-name()=''pixelFirstNoiseValue'']'], noise_domnode)); % Index of first row defined to be zero
        step = str2double(xp.evaluate([beta0_str ...
            '/*[local-name()=''stepSize'']'], noise_domnode));
        beta0s = str2num(xp.evaluate([beta0_str ...
            '/*[local-name()=''noiseLevelValues'']'], noise_domnode)); %#ok<ST2NM>
        range_coords = output_meta.Grid.Row.SS * ...
            ((((1:numel(beta0s))-1) * step) + pfv - double(output_meta.ImageData.SCPPixel.Row));
        noisepoly = polyfit(range_coords, beta0s - (10*log10(polyval(...
            output_meta.Radiometric.BetaZeroSFPoly(end:-1:1), range_coords))), 2);
        output_meta.Radiometric.NoiseLevel.NoisePoly = noisepoly(end:-1:1).';
    end
end

%% SCPCOA
% All of these fields are derivable for more fundamental fields.
output_meta = derived_sicd_fields(output_meta);

%% Process fields specific to each polarimetric band
band_independent_meta = output_meta; % Values that are consistent across all bands
grouped_meta = cell(numel(pols),1);
for i=1:numel(pols)
    output_meta = band_independent_meta;

    output_meta.ImageFormation.RcvChanProc.ChanIndex = i;
    output_meta.ImageFormation.TxRcvPolarizationProc = ...
        output_meta.RadarCollection.RcvChannels.ChanParameters(i).TxRcvPolarization;

    grouped_meta{i} = output_meta;
end
output_meta = grouped_meta; % Cell array with metadata struct for each band

end

% Creates an XPath query that is a namespace insensitive search for an XML
% heirachy.  Input is a cell array of strings specifying the XML fields,
% starting with the root node and working its way down the XML tree to the
% leaf nodes.  This is required since RS2 XML files specify a default
% namespace, so XPath queries with no namespace speficied will not work.
% (That only searches for elements associated with no namespace at all.)
% Note: This seems to be an issue only in MATLAB 2010a and above, as
% previous versions used another XPATH library that was more tolerant.
function out_str = xpath_str(in_cell_array)
    out_str='';
    for j=1:length(in_cell_array)
        out_str=[out_str '/*[local-name()=''' in_cell_array{j} ''']'];
    end
end

% MATLAB's datenum function won't handle precise times down under a
% millisecond, because 1) It won't accept a format with more than 3 .FFF in
% the string description of the date format and 2) the resulting serial
% date number is stored in days from 00-JAN-0000 and just doesn't have
% enough bits to handle fractional seconds to the level we want.  Here we
% handle the fractional seconds separately so we can read date with the
% precision we need.
function [datenum_s, datenum_frac] = datenum_w_frac(datestring)
    datenum_s = datenum(datestring,'yyyy-mm-ddTHH:MM:SS');
    datenum_frac = str2double(regexp(datestring,'\.\d*','match'));
    if isnan(datenum_frac), datenum_frac = 0; end;
end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////