%Project_1_step_1s

% Define satelite locations and time corrections A,B,C,t
A_1 = 15600;
B_1 = 7540;
C_1 = 10380;
t_1 = .0593200;
A_2 = 11760;
B_2 = 2750;
C_2 = 16190;
t_2 = .0519200;
A_3 = 11610;
B_3 = 14630;
C_3 = 7680;
t_3 = .0624200;
A_4 = 15170;
B_4 = 610;
C_4 = 13320;
t_4 = .0557100;
c = 299792.458;
% Define the simplified consatants

K_1 = A_1^2-A_2^2+B_1^2-B_2^2 +C_1^2-C_2^2 + t_2^2-t_1^2;
K_2 = A_1^2-A_3^2+B_1^2-B_3^2 +C_1^2-C_3^2 + t_3^2-t_1^2;
K_3 = A_1^2-A_4^2+B_1^2-B_4^2 +C_1^2-C_4^2 + t_4^2-t_1^2;


Q_3 = (t_3-t_1)*(C_2-C_1) - (t_2-t_1)*(C_3-C_1);
Q_4 = (t_4-t_1)*(C_2-C_1) - (t_2-t_1)*(C_4-C_1);


% K_1, K_2, Q_3, Q_4, X_1, X_2, Y_1, Y_2
X_1 = 2*Q_4*(t_2-t_1)*(A_3-A_1) - (t_3-t_1)*(A_2-A_1);
X_2 = 2*Q_3*(t_2-t_1)*(A_4-A_1) - (t_4-t_1)*(A_2-A_1);
Y_1 = 2*Q_4*(t_2-t_1)*(B_3-B_1) + (t_3-t_1)*(B_2-B_1);
Y_2 = 2*Q_3*(t_2-t_1)*(B_4-B_1) + (t_4-t_1)*(B_2-B_1);

% alpha_1, alpha_2, alpha_3, Beta_1, Beta_2, Beta_3
alpha_1 = 2*(X_2-X_1) / (Y_1 - Y_2);
Beta_1 = ( K_3*2*Q_3*(t_2-t_1) + K_1*(t_4 - t_1) - K_2*2*Q_4*(t_2 - t_1) - K_1*(t_3 - t_1)  )  / (Y_1 - Y_2);
alpha_2 =(2*(t_2-t_1)*(A_4-A_1)-2*(t_4-t_1)*(A_2-A_1) +alpha_1*2*((t_2-t_1)*(B_4-B_1)-(t_4-t_1)*(B_2-B_1)))/ (2*((t_4 - t_1)*(C_2 - C_1) - (t_2 - t_1)*(C_4 - C_1))) ;
Beta_2 = (2*Beta_1*((t_2-t_1)*(B_4-B_1)-(t_4-t_1)*(B_2-B_1))  +  (t_2-t_1)*K_2-(t_4-t_1)*K_1) / (2*((t_4 - t_1)*(C_2 - C_1) - (t_2 - t_1)*(C_4 - C_1))) ;
alpha_3 = (2*(A_2-A_1)+2*alpha_1*(B_2-B_1)+2*alpha_2*(C_2-C_1))/(2*c^2*(t_2-t_1));
Beta_3 = (2*Beta_1*(B_2-B_1)+2*Beta_2*(C_2-C_1)+K_1)/(2*c^2*(t_2-t_1));

% Theta, Psi, gamma
Theta = A_1^2 + (Beta_1-B_1)^2 + (Beta_2 - C_1)^2 - c^2*(t_1 + Beta_3)^2;
Psi = -2*A_1 + 2*(Beta_1 - B_1) + 2*(Beta_2 - C_1) + 2*alpha_3*c^2*(t_1+Beta_3);
gamma = 1 + alpha_1^2 + alpha_2^2 - c^2*alpha_3^2;


% Run quadratic

x_solution_1 = (-Psi + sqrt(Psi^2-4*gamma*Theta))/(2*gamma)
x_solution_2 = (-Psi - sqrt(Psi^2-4*gamma*Theta))/(2*gamma)

% Find y
y = alpha_1*x_solution_1 + Beta_1
z = alpha_2*x_solution_1 + Beta_2
d = alpha_3*x_solution_1 + Beta_3