%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Homework #7
%%% Exercise #3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Set variables
x_bks=-109.9;
y_bks=97.3;
x_cmb=54.7;
y_cmb=114.9;
x_farb=-178.1;
y_farb=77.5;
x_mhc=-57.1;
y_mhc=38.0;
x_pkd=42.4;
y_pkd=-117.1;

x0=-100; % x0=-121E
y0=-100; % y0=37N

vp=6.4;
vs=3.7;

t_bks=26.5;
t_cmb=26.6;
t_farb=25.5;
t_mhc=12.1;
t_pkd=20.0;

%% Matrix d
F_bks=((x_bks-x0)^2+(y_bks-y0)^2)^0.5/vp;
F_cmb=((x_cmb-x0)^2+(y_cmb-y0)^2)^0.5/vp;
F_farb=((x_farb-x0)^2+(y_farb-y0)^2)^0.5/vp;
F_mhc=((x_mhc-x0)^2+(y_mhc-y0)^2)^0.5/vp;
F_pkd=((x_pkd-x0)^2+(y_pkd-y0)^2)^0.5/vp;

d=[t_bks-F_bks;t_cmb-F_cmb;t_farb-F_farb;t_mhc-F_mhc;t_pkd-F_pkd];

%RMS
rms0=((t_bks-F_bks)^2+(t_cmb-F_cmb)^2+(t_farb-F_farb)^2+(t_mhc-F_mhc)^2+(t_pkd-F_pkd)^2)^0.5;

%% Matrix A
A_bks_x=(x0-x_bks)/((x_bks-x0)^2+(y_bks-y0)^2)^.5;
A_cmb_x=(x0-x_cmb)/((x_cmb-x0)^2+(y_cmb-y0)^2)^.5;
A_farb_x=(x0-x_farb)/((x_farb-x0)^2+(y_farb-y0)^2)^.5;
A_mhc_x=(x0-x_mhc)/((x_mhc-x0)^2+(y_mhc-y0)^2)^.5;
A_pkd_x=(x0-x_pkd)/((x_pkd-x0)^2+(y_pkd-y0)^2)^.5;

A_bks_y=(y0-y_bks)/((x_bks-x0)^2+(y_bks-y0)^2)^.5;
A_cmb_y=(y0-y_cmb)/((x_cmb-x0)^2+(y_cmb-y0)^2)^.5;
A_farb_y=(y0-y_farb)/((x_farb-x0)^2+(y_farb-y0)^2)^.5;
A_mhc_y=(y0-y_mhc)/((x_mhc-x0)^2+(y_mhc-y0)^2)^.5;
A_pkd_y=(y0-y_pkd)/((x_pkd-x0)^2+(y_pkd-y0)^2)^.5;

A=[A_bks_x A_bks_y;A_cmb_x A_cmb_y;A_farb_x A_farb_y;A_mhc_x A_mhc_y;A_pkd_x A_pkd_y]./vp;

%% Calculation of the x matrix: dx and dz
x=(A'*A)^(-1)*A'*d;
x1=x0+x(1,1);
y1=y0+x(2,1);

x1_lon=x1/89-121
y1_lat=y1/111+37


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Second iteration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Matrix d
F_bks=((x_bks-x1)^2+(y_bks-y1)^2)^0.5/vp;
F_cmb=((x_cmb-x1)^2+(y_cmb-y1)^2)^0.5/vp;
F_farb=((x_farb-x1)^2+(y_farb-y1)^2)^0.5/vp;
F_mhc=((x_mhc-x1)^2+(y_mhc-y1)^2)^0.5/vp;
F_pkd=((x_pkd-x1)^2+(y_pkd-y1)^2)^0.5/vp;

d=[t_bks-F_bks;t_cmb-F_cmb;t_farb-F_farb;t_mhc-F_mhc;t_pkd-F_pkd];

%RMS
rms1=((t_bks-F_bks)^2+(t_cmb-F_cmb)^2+(t_farb-F_farb)^2+(t_mhc-F_mhc)^2+(t_pkd-F_pkd)^2)^0.5;

%% Matrix A
A_bks_x=(x1-x_bks)/((x_bks-x1)^2+(y_bks-y1)^2)^.5;
A_cmb_x=(x1-x_cmb)/((x_cmb-x1)^2+(y_cmb-y1)^2)^.5;
A_farb_x=(x1-x_farb)/((x_farb-x1)^2+(y_farb-y1)^2)^.5;
A_mhc_x=(x1-x_mhc)/((x_mhc-x1)^2+(y_mhc-y1)^2)^.5;
A_pkd_x=(x1-x_pkd)/((x_pkd-x1)^2+(y_pkd-y1)^2)^.5;

A_bks_y=(y1-y_bks)/((x_bks-x1)^2+(y_bks-y1)^2)^.5;
A_cmb_y=(y1-y_cmb)/((x_cmb-x1)^2+(y_cmb-y1)^2)^.5;
A_farb_y=(y1-y_farb)/((x_farb-x1)^2+(y_farb-y1)^2)^.5;
A_mhc_y=(y1-y_mhc)/((x_mhc-x1)^2+(y_mhc-y1)^2)^.5;
A_pkd_y=(y1-y_pkd)/((x_pkd-x1)^2+(y_pkd-y1)^2)^.5;

A=[A_bks_x A_bks_y;A_cmb_x A_cmb_y;A_farb_x A_farb_y;A_mhc_x A_mhc_y;A_pkd_x A_pkd_y]./vp;

%% Calculation of the x matrix: dx and dz
x=(A'*A)^(-1)*A'*d;
x2=x1+x(1,1);
y2=y1+x(2,1);

x2_lon=x2/89-121
y2_lat=y2/111+37

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Third iteration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Matrix d
F_bks=((x_bks-x2)^2+(y_bks-y2)^2)^0.5/vp;
F_cmb=((x_cmb-x2)^2+(y_cmb-y2)^2)^0.5/vp;
F_farb=((x_farb-x2)^2+(y_farb-y2)^2)^0.5/vp;
F_mhc=((x_mhc-x2)^2+(y_mhc-y2)^2)^0.5/vp;
F_pkd=((x_pkd-x2)^2+(y_pkd-y2)^2)^0.5/vp;

d=[t_bks-F_bks;t_cmb-F_cmb;t_farb-F_farb;t_mhc-F_mhc;t_pkd-F_pkd];

%RMS
rms2=((t_bks-F_bks)^2+(t_cmb-F_cmb)^2+(t_farb-F_farb)^2+(t_mhc-F_mhc)^2+(t_pkd-F_pkd)^2)^0.5;

%% Matrix A
A_bks_x=(x2-x_bks)/((x_bks-x2)^2+(y_bks-y2)^2)^.5;
A_cmb_x=(x2-x_cmb)/((x_cmb-x2)^2+(y_cmb-y2)^2)^.5;
A_farb_x=(x2-x_farb)/((x_farb-x2)^2+(y_farb-y2)^2)^.5;
A_mhc_x=(x2-x_mhc)/((x_mhc-x2)^2+(y_mhc-y2)^2)^.5;
A_pkd_x=(x2-x_pkd)/((x_pkd-x2)^2+(y_pkd-y2)^2)^.5;

A_bks_y=(y2-y_bks)/((x_bks-x2)^2+(y_bks-y2)^2)^.5;
A_cmb_y=(y2-y_cmb)/((x_cmb-x2)^2+(y_cmb-y2)^2)^.5;
A_farb_y=(y2-y_farb)/((x_farb-x2)^2+(y_farb-y2)^2)^.5;
A_mhc_y=(y2-y_mhc)/((x_mhc-x2)^2+(y_mhc-y2)^2)^.5;
A_pkd_y=(y2-y_pkd)/((x_pkd-x2)^2+(y_pkd-y2)^2)^.5;

A=[A_bks_x A_bks_y;A_cmb_x A_cmb_y;A_farb_x A_farb_y;A_mhc_x A_mhc_y;A_pkd_x A_pkd_y]./vp;

%% Calculation of the x matrix: dx and dz
x=(A'*A)^(-1)*A'*d;
x3=x2+x(1,1);
y3=y2+x(2,1);

x3_lon=x3/89-121
y3_lat=y3/111+37

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Fourth iteration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Matrix d
F_bks=((x_bks-x3)^2+(y_bks-y3)^2)^0.5/vp;
F_cmb=((x_cmb-x3)^2+(y_cmb-y3)^2)^0.5/vp;
F_farb=((x_farb-x3)^2+(y_farb-y3)^2)^0.5/vp;
F_mhc=((x_mhc-x3)^2+(y_mhc-y3)^2)^0.5/vp;
F_pkd=((x_pkd-x3)^2+(y_pkd-y3)^2)^0.5/vp;

d=[t_bks-F_bks;t_cmb-F_cmb;t_farb-F_farb;t_mhc-F_mhc;t_pkd-F_pkd];

%RMS
rms3=((t_bks-F_bks)^2+(t_cmb-F_cmb)^2+(t_farb-F_farb)^2+(t_mhc-F_mhc)^2+(t_pkd-F_pkd)^2)^0.5;

%% Matrix A
A_bks_x=(x3-x_bks)/((x_bks-x3)^2+(y_bks-y3)^2)^.5;
A_cmb_x=(x3-x_cmb)/((x_cmb-x3)^2+(y_cmb-y3)^2)^.5;
A_farb_x=(x3-x_farb)/((x_farb-x3)^2+(y_farb-y3)^2)^.5;
A_mhc_x=(x3-x_mhc)/((x_mhc-x3)^2+(y_mhc-y3)^2)^.5;
A_pkd_x=(x3-x_pkd)/((x_pkd-x3)^2+(y_pkd-y3)^2)^.5;

A_bks_y=(y3-y_bks)/((x_bks-x3)^2+(y_bks-y3)^2)^.5;
A_cmb_y=(y3-y_cmb)/((x_cmb-x3)^2+(y_cmb-y3)^2)^.5;
A_farb_y=(y3-y_farb)/((x_farb-x3)^2+(y_farb-y3)^2)^.5;
A_mhc_y=(y3-y_mhc)/((x_mhc-x3)^2+(y_mhc-y3)^2)^.5;
A_pkd_y=(y3-y_pkd)/((x_pkd-x3)^2+(y_pkd-y3)^2)^.5;

A=[A_bks_x A_bks_y;A_cmb_x A_cmb_y;A_farb_x A_farb_y;A_mhc_x A_mhc_y;A_pkd_x A_pkd_y]./vp;

%% Calculation of the x matrix: dx and dz
x=((A'*A)^(-1))*A'*d;
x4=x3+x(1,1);
y4=y3+x(2,1);

x4_lon=x4/89-121
y4_lat=y4/111+37

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Fifth iteration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Matrix d
F_bks=((x_bks-x4)^2+(y_bks-y4)^2)^0.5/vp;
F_cmb=((x_cmb-x4)^2+(y_cmb-y4)^2)^0.5/vp;
F_farb=((x_farb-x4)^2+(y_farb-y4)^2)^0.5/vp;
F_mhc=((x_mhc-x4)^2+(y_mhc-y4)^2)^0.5/vp;
F_pkd=((x_pkd-x4)^2+(y_pkd-y4)^2)^0.5/vp;

d=[t_bks-F_bks;t_cmb-F_cmb;t_farb-F_farb;t_mhc-F_mhc;t_pkd-F_pkd];

%RMS
rms4=((t_bks-F_bks)^2+(t_cmb-F_cmb)^2+(t_farb-F_farb)^2+(t_mhc-F_mhc)^2+(t_pkd-F_pkd)^2)^0.5;

%% Matrix A
A_bks_x=(x4-x_bks)/((x_bks-x4)^2+(y_bks-y4)^2)^.5;
A_cmb_x=(x4-x_cmb)/((x_cmb-x4)^2+(y_cmb-y4)^2)^.5;
A_farb_x=(x4-x_farb)/((x_farb-x4)^2+(y_farb-y4)^2)^.5;
A_mhc_x=(x4-x_mhc)/((x_mhc-x4)^2+(y_mhc-y4)^2)^.5;
A_pkd_x=(x4-x_pkd)/((x_pkd-x4)^2+(y_pkd-y4)^2)^.5;

A_bks_y=(y4-y_bks)/((x_bks-x4)^2+(y_bks-y4)^2)^.5;
A_cmb_y=(y4-y_cmb)/((x_cmb-x4)^2+(y_cmb-y4)^2)^.5;
A_farb_y=(y4-y_farb)/((x_farb-x4)^2+(y_farb-y4)^2)^.5;
A_mhc_y=(y4-y_mhc)/((x_mhc-x4)^2+(y_mhc-y4)^2)^.5;
A_pkd_y=(y4-y_pkd)/((x_pkd-x4)^2+(y_pkd-y4)^2)^.5;

A=[A_bks_x A_bks_y;A_cmb_x A_cmb_y;A_farb_x A_farb_y;A_mhc_x A_mhc_y;A_pkd_x A_pkd_y]./vp;

%% Calculation of the x matrix: dx and dz
x=((A'*A)^(-1))*A'*d;
x5=x4+x(1,1);
y5=y4+x(2,1);

x5_lon=x5/89-121
y5_lat=y5/111+37

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Sixth iteration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Matrix d
F_bks=((x_bks-x5)^2+(y_bks-y5)^2)^0.5/vp;
F_cmb=((x_cmb-x5)^2+(y_cmb-y5)^2)^0.5/vp;
F_farb=((x_farb-x5)^2+(y_farb-y5)^2)^0.5/vp;
F_mhc=((x_mhc-x5)^2+(y_mhc-y5)^2)^0.5/vp;
F_pkd=((x_pkd-x5)^2+(y_pkd-y5)^2)^0.5/vp;

d=[t_bks-F_bks;t_cmb-F_cmb;t_farb-F_farb;t_mhc-F_mhc;t_pkd-F_pkd];

%RMS
rms5=((t_bks-F_bks)^2+(t_cmb-F_cmb)^2+(t_farb-F_farb)^2+(t_mhc-F_mhc)^2+(t_pkd-F_pkd)^2)^0.5;

%% Matrix A
A_bks_x=(x5-x_bks)/((x_bks-x5)^2+(y_bks-y5)^2)^.5;
A_cmb_x=(x5-x_cmb)/((x_cmb-x5)^2+(y_cmb-y5)^2)^.5;
A_farb_x=(x5-x_farb)/((x_farb-x5)^2+(y_farb-y5)^2)^.5;
A_mhc_x=(x5-x_mhc)/((x_mhc-x5)^2+(y_mhc-y5)^2)^.5;
A_pkd_x=(x5-x_pkd)/((x_pkd-x5)^2+(y_pkd-y5)^2)^.5;

A_bks_y=(y5-y_bks)/((x_bks-x5)^2+(y_bks-y5)^2)^.5;
A_cmb_y=(y5-y_cmb)/((x_cmb-x5)^2+(y_cmb-y5)^2)^.5;
A_farb_y=(y5-y_farb)/((x_farb-x5)^2+(y_farb-y5)^2)^.5;
A_mhc_y=(y5-y_mhc)/((x_mhc-x5)^2+(y_mhc-y5)^2)^.5;
A_pkd_y=(y5-y_pkd)/((x_pkd-x5)^2+(y_pkd-y5)^2)^.5;

A=[A_bks_x A_bks_y;A_cmb_x A_cmb_y;A_farb_x A_farb_y;A_mhc_x A_mhc_y;A_pkd_x A_pkd_y]./vp;

%% Calculation of the x matrix: dx and dz
x=((A'*A)^(-1))*A'*d;
x6=x5+x(1,1);
y6=y5+x(2,1);

x6_lon=x6/89-121
y6_lat=y6/111+37


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Seventh iteration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Matrix d
F_bks=((x_bks-x6)^2+(y_bks-y6)^2)^0.5/vp;
F_cmb=((x_cmb-x6)^2+(y_cmb-y6)^2)^0.5/vp;
F_farb=((x_farb-x6)^2+(y_farb-y6)^2)^0.5/vp;
F_mhc=((x_mhc-x6)^2+(y_mhc-y6)^2)^0.5/vp;
F_pkd=((x_pkd-x6)^2+(y_pkd-y6)^2)^0.5/vp;

d=[t_bks-F_bks;t_cmb-F_cmb;t_farb-F_farb;t_mhc-F_mhc;t_pkd-F_pkd];

%RMS
rms6=((t_bks-F_bks)^2+(t_cmb-F_cmb)^2+(t_farb-F_farb)^2+(t_mhc-F_mhc)^2+(t_pkd-F_pkd)^2)^0.5;

%% Matrix A
A_bks_x=(x6-x_bks)/((x_bks-x6)^2+(y_bks-y6)^2)^.5;
A_cmb_x=(x6-x_cmb)/((x_cmb-x6)^2+(y_cmb-y6)^2)^.5;
A_farb_x=(x6-x_farb)/((x_farb-x6)^2+(y_farb-y6)^2)^.5;
A_mhc_x=(x6-x_mhc)/((x_mhc-x6)^2+(y_mhc-y6)^2)^.5;
A_pkd_x=(x6-x_pkd)/((x_pkd-x6)^2+(y_pkd-y6)^2)^.5;

A_bks_y=(y6-y_bks)/((x_bks-x6)^2+(y_bks-y6)^2)^.5;
A_cmb_y=(y6-y_cmb)/((x_cmb-x6)^2+(y_cmb-y6)^2)^.5;
A_farb_y=(y6-y_farb)/((x_farb-x6)^2+(y_farb-y6)^2)^.5;
A_mhc_y=(y6-y_mhc)/((x_mhc-x6)^2+(y_mhc-y6)^2)^.5;
A_pkd_y=(y6-y_pkd)/((x_pkd-x6)^2+(y_pkd-y6)^2)^.5;

A=[A_bks_x A_bks_y;A_cmb_x A_cmb_y;A_farb_x A_farb_y;A_mhc_x A_mhc_y;A_pkd_x A_pkd_y]./vp;

%% Calculation of the x matrix: dx and dz
x=((A'*A)^(-1))*A'*d;
x7=x6+x(1,1);
y7=y6+x(2,1);

x7_lon=x7/89-121
y7_lat=y7/111+37


rms=[rms0 rms1 rms2 rms3 rms4 rms5 rms6]
plot(rms,'o')
