classdef HelperNTNPositionEstimate
    %HelperNTNPositionEstimate Class defining the supporting functions used
    %in the receiver position estimation in NTN example
    %
    %   Note: This is an undocumented class and its API and/or
    %   functionality may change in subsequent releases.

    % Copyright 2024-2025 The MathWorks, Inc.

    methods (Static)
        function out = instantposition(s, v, r, rDot)
            % OUT = instantposition(S,V,R,RDOT) returns the possible
            % position OUT using satellite position S, satellite velocity
            % V, range R, and range-rate RDOT. S and V are in Earth
            % Centered Earth Fixed (ECEF) coordinate system. R is in m and
            % RDOT is in m/s. OUT is a 3-by-2 matrix with each column
            % corresponding to a possible position in LLA (Latitude,
            % Longitude, Altitude).

            tolerance = 1e-4;
            max_iter_refine = 1000;
            % Earth Parameters
            r_E = 6378137;  % Equatorial radius (meters)
            r_p = 6356752;  % Polar radius (meters)

            % Initialize two r_c values with random values
            r_c_1 = (r_E - r_p) * rand();
            r_c_2 = r_c_1; % Starting both from the same value (can be different if needed)

            % Refinement loop for two solutions
            for iter = 1:max_iter_refine
                prev_r_c_1 = r_c_1;
                prev_r_c_2 = r_c_2;

                % Compute geometric parameters
                r_s2 = dot(s,s);   % Squared norm of position vector

                % Linear system for refinement
                A = [s(1) s(2); v(1) v(2)];
                b_1 = 0.5*(r_s2 + r_c_1^2 - r^2);
                b_2 = 0.5*(r_s2 + r_c_2^2 - r^2);
                c = dot(s,v) - r*rDot;

                % Solve the parameters for r_c_1 and r_c_2
                mat_1 = [b_1, s(3); c, v(3)];
                mat_2 = [b_2, s(3); c, v(3)];

                params_1 = A \ mat_1;
                params_2 = A \ mat_2;

                % Extract parameters for the first solution
                alpha_1 = params_1(1);
                beta_1 = params_1(3);
                gamma_1 = params_1(2);
                delta_1 = params_1(4);

                % Extract parameters for the second solution
                alpha_2 = params_2(1);
                beta_2 = params_2(3);
                gamma_2 = params_2(2);
                delta_2 = params_2(4);

                % Calculate discriminant for both solutions
                numerator_1 = alpha_1 * beta_1 + gamma_1 * delta_1;
                discriminant_1 = numerator_1^2 - (1 + delta_1^2 + beta_1^2) * (alpha_1^2 + gamma_1^2 - r_c_1^2);

                numerator_2 = alpha_2 * beta_2 + gamma_2 * delta_2;
                discriminant_2 = numerator_2^2 - (1 + delta_2^2 + beta_2^2) * (alpha_2^2 + gamma_2^2 - r_c_2^2);

                % Handle negative discriminant by regenerating r_c
                if discriminant_1 < 0
                    r_c_1 = r_p + (r_E - r_p) * rand();
                    continue;
                end
                if discriminant_2 < 0
                    r_c_2 = r_p + (r_E - r_p) * rand();
                    continue;
                end

                % Compute z-coordinates for both solutions
                z_hat_1 = (numerator_1 + sqrt(discriminant_1)) / (1 + delta_1^2 + beta_1^2);
                z_hat_2 = (numerator_2 - sqrt(discriminant_2)) / (1 + delta_2^2 + beta_2^2);

                % Compute x and y coordinates for both solutions
                x_hat_1 = alpha_1 - beta_1 * z_hat_1;
                y_hat_1 = gamma_1 - delta_1 * z_hat_1;

                x_hat_2 = alpha_2 - beta_2 * z_hat_2;
                y_hat_2 = gamma_2 - delta_2 * z_hat_2;

                % Convert to LLA
                lla_1 = ecef2llaLocal([x_hat_1; y_hat_1; z_hat_1]);
                lla_2 = ecef2llaLocal([x_hat_2; y_hat_2; z_hat_2]);

                % Recalculate r_c for refined solutions
                dr_1 = sind(lla_1(1))^2 + cosd(lla_1(1))^2 * (r_p / r_E)^2;
                r_c_1 = sqrt(r_p^2 / dr_1);

                dr_2 = sind(lla_2(1))^2 + cosd(lla_2(1))^2 * (r_p / r_E)^2;
                r_c_2 = sqrt(r_p^2 / dr_2);

                % Check convergence for both solutions
                if abs(r_c_1 - prev_r_c_1) < tolerance && abs(r_c_2 - prev_r_c_2) < tolerance
                    break;
                end
            end
            out = [];
            if discriminant_1 > 0
                out = lla_1(:);
            end
            if discriminant_2 > 0
                out = [out(:) lla_2(:)];
            end
        end

        function error = positionerror(originalPos,estimatedPos)
            % ERR = positionerror(ORIGINALPOS,ESTIMATEDPOS) computes the
            % position errors ERR for given original position ORIGINALPOS
            % and estimated positions ESTIMATEDPOS. Original position and
            % estimated positions are in LLA. ESTIMATEDPOS is 3-by-N matrix
            % with N being the number of positions.

            % Convert the positions to ECEF
            posECEF = lla2ecefLocal([originalPos estimatedPos]);
            error = vecnorm(posECEF(:,1)-posECEF(:,2:end));
        end
    end
end

function lla = ecef2llaLocal(ecef)
% Output Latitude (degrees), Longitude (degrees), and Altitude (meters)

tmp = matlabshared.orbit.internal.Transforms.itrf2geographic(ecef);
% Convert to degrees and get the LLA coordinates
lla = [rad2deg(tmp(1,1:end)); ...
    rad2deg(tmp(2,1:end)); ...
    tmp(3,1:end)];

end

function ecef = lla2ecefLocal(lla)
% Output ECEF: X (meters), Y (meters), and Z (meters)

% Convert to radians
lla = [deg2rad(lla(1,1:end)); ...
    deg2rad(lla(2,1:end)); ...
    lla(3,1:end)];
% Get ECEF coordinates
ecef = matlabshared.orbit.internal.Transforms.geographic2itrf(lla);

end