format long
c = 299792.458;
d_1 = 0.0001;
delta_t = 10^-8;

init_vec = [0; 0; 6370; 0];
true_sol = [0; 0; 6370; 0.0001];

n_vals = [4, 5, 6, 7, 8, 9, 10, 11, 12, 16, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95, 100];
n_value = zeros(length(n_vals),1);
cond_value = zeros(length(n_vals),1);
pos_err = zeros(length(n_vals),1);
emf = zeros(length(n_vals),1);

for n_idx = 1:length(n_vals)
    n = n_vals(n_idx);
    h1 = (pi / 2) / n;
    h2 = (2*pi) / n;
    rho = 26570;
    phi = [0:n-1]*h1;
    theta = [0:n-1]*h2;
	sat_A = zeros(1,n);
	sat_B = zeros(1,n);
	sat_C = zeros(1,n);
	R = zeros(1,n);
	t = zeros(1,n);
	for i=1:n
		sat_A(i) = rho*cos(phi(1,i))*cos(theta(1,i));
		sat_B(i) = rho*cos(phi(1,i))*sin(theta(1,i));
		sat_C(i) = rho*sin(phi(1,i));
		R(i) = sqrt(sat_A(i)^2 + sat_B(i)^2 + (sat_C(i) - 6370)^2);
		t(i) = d_1 + R(i)/c;
    end

    maxerr = 0;
    maxposerr = 0;
    for randiter = 1:100
        k = 100;
        f = zeros(1,n);
        init_vec = [0; 0; 6370; 0];
        time_error = delta_t * ((rand(1, n) > 0.5) * 2 - 1);

        for iter=1:k
            x = init_vec(1,1);
            y = init_vec(2,1);
            z = init_vec(3,1);
            d = init_vec(4,1);
            for i=1:n
                f(i) = (x - sat_A(i))^2 + (y - sat_B(i))^2 + (z - sat_C(i))^2 - (c*(t(i) + time_error(i) - d))^2;
            end
            f_sol = transpose(f);

            Jacobian = zeros(n,4);
            for i=1:n
                Jacobian(i,1) = 2*(x - sat_A(i));
                Jacobian(i,2) = 2*(x - sat_B(i));
                Jacobian(i,3) = 2*(x - sat_C(i));
                Jacobian(i,4) = 2*c^2*(t(i) + time_error(i) - d);
            end

            A = transpose(Jacobian)*Jacobian; 
            b = -(transpose(Jacobian)*f_sol);
            v = A \ b;

            init_vec = init_vec + v;
        end
        comp_vec = init_vec;
        
        maxerr = max(maxerr, max(abs(comp_vec - true_sol)));
        maxposerr = max(maxposerr, norm(comp_vec - true_sol));
    end
    
	%cond_value
	pos_err(n_idx,1) = maxposerr;
	n_value(n_idx,1) = n;
    emf(n_idx,1) = maxerr / (c * delta_t);
    
end


n_value
pos_err
emf

scatter(n_value, emf);
title('Accuracy Gains from Increased Satellites');
xlabel('Number of Satellites');
ylabel('Error Magnification Factor');