format long
c = 299792.458;
sat_1 = [15600; 7540; 20140];
A_1 = sat_1(1,1);
B_1 = sat_1(2,1);
C_1 = sat_1(3,1);
sat_2 = [18760; 2750; 18610];
A_2 = sat_2(1,1);
B_2 = sat_2(2,1);
C_2 = sat_2(3,1);
sat_3 = [17610; 14630; 13480];
A_3 = sat_3(1,1);
B_3 = sat_3(2,1);
C_3 = sat_3(3,1);
sat_4 = [19170; 610; 18390];
A_4 = sat_4(1,1);
B_4 = sat_4(2,1);
C_4 = sat_4(3,1);
t_1 = 0.07074;
t_2 = 0.07220;
t_3 = 0.07690;
t_4 = 0.07242;
w_1 = -c^2*(t_1^2 - t_4^2)  + A_1^2 - A_4^2 + B_1^2 - B_4^2 + C_1^2 - C_4^2;
w_2 = -c^2*(t_1^2 - t_3^2)  + A_1^2 - A_3^2 + B_1^2 - B_3^2 + C_1^2 - C_3^2;
w_3 = -c^2*(t_1^2 - t_2^2)  + A_1^2 - A_2^2 + B_1^2 - B_2^2 + C_1^2 - C_2^2;

u_x_1 = 2*(A_4 - A_1);
u_x_2 = 2*(A_3 - A_1);
u_x_3 = 2*(A_2 - A_1);

u_y_1 = 2*(B_4 - B_1);
u_y_2 = 2*(B_3 - B_1);
u_y_3 = 2*(B_2 - B_1);

u_z_1 = 2*(C_4 - C_1);
u_z_2 = 2*(C_3 - C_1);
u_z_3 = 2*(C_2 - C_1);

u_d_1 = -2*c^2*(t_4 - t_1);
u_d_2 = -2*c^2*(t_3 - t_1);
u_d_3 = -2*c^2*(t_2 - t_1);

Mx1 = [u_y_1, u_z_1, u_x_1; u_y_2, u_z_2, u_x_2; u_y_3, u_z_3, u_x_3];
ax = det(Mx1);

Mx2 = [u_y_1, u_z_1, u_d_1; u_y_2, u_z_2, u_d_2; u_y_3, u_z_3, u_d_3];
bx = det(Mx2);

Mx3 = [u_y_1, u_z_1, w_1; u_y_2, u_z_2, w_2; u_y_3, u_z_3, w_3];
cx = det(Mx3);

My1 = [u_x_1, u_z_1, u_y_1; u_x_2, u_z_2, u_y_2; u_x_3, u_z_3, u_y_3];
ay = det(My1);

My2 = [u_x_1, u_z_1, u_d_1; u_x_2, u_z_2, u_d_2; u_x_3, u_z_3, u_d_3];
by = det(My2);

My3 = [u_x_1, u_z_1, w_1; u_x_2, u_z_2, w_2; u_x_3, u_z_3, w_3];
cy = det(My3);

Mz1 = [u_x_1, u_y_1, u_z_1; u_x_2, u_y_2, u_z_2; u_x_3, u_y_3, u_z_3];
az = det(Mz1);

Mz2 = [u_x_1, u_y_1, u_d_1; u_x_2, u_y_2, u_d_2; u_x_3, u_y_3, u_d_3];
bz = det(Mz2);

Mz3 = [u_x_1, u_y_1, w_1; u_x_2, u_y_2, w_2; u_x_3, u_y_3, w_3];
cz = det(Mz3);

q_a = (bx/ax)^2 + (by/ay)^2 + (bz/az)^2 - c^2;
q_b = 2*(bx/ax)*(cx/ax + A_1) + 2*(by/ay)*(cy/ay + B_1) + 2*(bz/az)*(cz/az + C_1) + 2*c^2*t_1;
q_c = (cx/ax + A_1)^2 + (cy/ay + B_1)^2 + (cz/az + C_1)^2 - c^2*t_1^2;

if q_b > 0
	d_pos = - ( ( q_b + sqrt( q_b^2 - 4 * q_a * q_c ) ) / (2*q_a) );
	d_neg = - ( ( 2 * q_c ) / ( q_b + sqrt( q_b^2 - 4 * q_a * q_c ) ) );
else
	d_pos = ( -q_b + sqrt( q_b^2 - 4 * q_a * q_c ) ) / ( 2 * q_a );
	d_neg = ( 2 * q_c ) / ( -q_b + sqrt( q_b^2 - 4 * q_a * q_c) );
end

x_sol_pos = -(cx / ax + (bx / ax) * d_pos);
x_sol_neg = -(cx / ax + (bx / ax) * d_neg);
y_sol_pos = -(cy / ay + (by / ay) * d_pos);
y_sol_neg = -(cy / ay + (by / ay) * d_neg);
z_sol_pos = -(cz / az + (bz / az) * d_pos);
z_sol_neg = -(cz / az + (bz / az) * d_neg);

final_ans_pos = [x_sol_pos; y_sol_pos; z_sol_pos; d_pos]
dis_pos = sqrt(x_sol_pos^2 + y_sol_pos^2 + z_sol_pos^2) - 6370
final_ans_neg = [x_sol_neg; y_sol_neg; z_sol_neg; d_neg]
dis_neg = sqrt(x_sol_neg^2 + y_sol_neg^2 + z_sol_neg^2) - 6370