% Jakub Vynikal, 3.3.2021
%% CONF
clc;clear; format long g; tic
addpath(genpath('bin'))
addpath(genpath('m_map'))
addpath(genpath('data'))
m_proj('Mollweide','clongitude',0);
ro = pi/180;
G = 6.6743015e-11;

%konfigurace
ref_grav_model_name='eigen-6s';
nmax = 20;
dt = 10; %nemenit
dny = 3; % 1 - 15
Ntroj = 1280;
noise = -2; % cm, exponent -2/-1/0

addpath(genpath(sprintf('noise_%g', noise)))
Nx = 112;
it = 0;

%ewh
troj = load(sprintf('fichier_maillage_sphere_%05d.dat',Ntroj));
lontd=troj(:,1); lattd=troj(:,2); surf=troj(:,end);
ewh = fce_ewh_trojuhelniky_01(lontd,lattd,nmax,0);

%transformace stredu trojuhelniku
Re = 6378136.3;
xt = Re*cosd(lattd).*cosd(lontd);
yt = Re*cosd(lattd).*sind(lontd);
zt = Re*sind(lattd);

%hmotnosti trojuhelniku
mt = ewh .* surf * 1000;

%nacteni souradnic druzic
if dny == 15
    Nfile = length(dir(fullfile(sprintf('noise_%g', noise),'*.dat')));
else
    Nfile = dny*length(dir(fullfile(sprintf('noise_%g', noise),'*20150509*.dat')));
end
dat_shc = zeros(Nfile*Nx,14);

%% CYCLE
F = eye(Ntroj+1); R = 1; Pini = 0; Pini2 = Inf;

At = zeros(Ntroj+1); WAt = At; Alt = zeros(Ntroj+1,1); Alt_shc = Alt;
while abs((Pini-Pini2)/Pini) > 0.01 && it < 100
    Pini2 = Pini;
    P = F*Pini*F'; Sum_r2 = 0; xh_kf = zeros(Ntroj+1,1);
for f = 1:Nfile
    file = dir(fullfile(sprintf('noise_%g', noise),sprintf('*0%04g_20*.dat', f)));
    fileW = dir(fullfile(sprintf('noise_%g', noise),'W',sprintf('*0%04g_20*.dat', f)));
    dif = load(fullfile(file.folder, file.name));
    dx = dif(:,3); dy = dif(:,4); dz = dif(:,5);
    dax = dif(:,6); day = dif(:,7); daz = dif(:,8);
    Wdx = load(fullfile(fileW(1).folder, fileW(1).name)); Wdy = load(fullfile(fileW(2).folder, fileW(2).name)); Wdz = load(fullfile(fileW(3).folder, fileW(3).name));
    
%     Nx = length(dx);
    rst = zeros(Nx,Ntroj); ax = rst; ay = ax; az = ax;
    for j = 1:Nx
        for k = 1:Ntroj
            rst(j,k) = sqrt((dx(j)-xt(k))^2+(dy(j)-yt(k))^2+(dz(j)-zt(k))^2);
            ax(j,k) = mt(k)/rst(j,k)^3 * -(dx(j)-xt(k));
            ay(j,k) = mt(k)/rst(j,k)^3 * -(dy(j)-yt(k));
            az(j,k) = mt(k)/rst(j,k)^3 * -(dz(j)-zt(k));
        end
    end
    
    l1 = sum(ax,2); l2 = sum(ay,2); l3 = sum(az,2);
    A = [ax;ay;az];
    l = [l1;l2;l3];
    A = [A./ewh',ones(3*Nx,1)];
    dA = A'*A;
    At = At+dA;
    dAl = A'*l;
    Alt = Alt+dAl;
    
    add = 1e-6*ones(Nx,1)/G;
    WA = [Wdx*[ax./ewh',add];Wdy*[ay./ewh',add];Wdz*[az./ewh',add]]*G;
    l_shc = [Wdx*dax;Wdy*day;Wdz*daz];
    
    if it == 0
        WdA = WA'*WA;
        WAt = WAt+WdA;
        dAl_shc = WA'*l_shc;
        Alt_shc = Alt_shc+dAl_shc;
        
        in = (f-1)*Nx+1;
        dat_shc(in:f*Nx,:) = dif;

    elseif it == 1
        lh = WA*xh_gls;
        rez = l_shc-lh;
        Sum_r2 = Sum_r2+sum(rez.^2);

    else
        Ap = WA;
        lp = l_shc;
        nu = lp-Ap*xh_kf;
        R = eye(3*Nx);
        S = R+Ap*P*Ap';
        K = P*Ap'*inv(S);
        xh_kf = xh_kf+K*nu;
        P = P-K*S*K';
    end
end

if it == 0
    xh_gls = WAt\Alt_shc;
    Pini = 1e5;
elseif it == 1
    mse = Sum_r2/(Nx*Nfile*3-Ntroj-1)
    Pini = (xh_gls'*xh_gls)/(mse*(Ntroj+1));
    xh_shc = xh_gls;
else
    xh_shc = xh_kf;
    Pini = (xh_shc'*xh_shc)/(mse*(Ntroj+1));
end
(Pini-Pini2)/Pini
it = it+1;
end

ewh_gls = xh_gls(1:end-1);
ewh_kf = xh_kf(1:end-1);
xh = At\Alt; ewh_h = xh(1:end-1);
Sx_ols = sqrt(mse)*sqrt(diag(inv(WAt))); M_gls = 2*Sx_ols(1:end-1);
Sx_kf = sqrt(mse)*sqrt(diag(P)); M_kf = 2*Sx_kf(1:end-1);
er_gls = ewh-xh_gls(1:end-1);
er_kf = ewh-xh_kf(1:end-1);
rmse_gls = sqrt(sum(er_gls.^2)/Ntroj);
rmse_kf = sqrt(sum(er_kf.^2)/Ntroj);

bool_gls = zeros(Ntroj,1); bool_kf = bool_gls;
for i = 1:Ntroj
    if abs(er_gls(i))<M_gls(i)
        bool_gls(i) = 1;
    end
    if abs(er_kf(i))<M_kf(i)
        bool_kf(i) = 1;
    end
end

pct_gls = sum(bool_gls)/Ntroj; pct_kf = sum(bool_kf)/Ntroj;

xs_shc = dat_shc(:,3); ys_shc = dat_shc(:,4); zs_shc = dat_shc(:,5);
Nsat = length(xs_shc);

%transformace souradnic druzic
fi = atan(zs_shc./sqrt(xs_shc.^2+ys_shc.^2))/ro;
lam = atan2(ys_shc,xs_shc)/ro;

%% GRAPHS
%graf drahy druzice
figure(1)
m_proj('Mollweide','clongitude',0);
m_plot(lam,fi,'r.')
m_coast('color','black');
m_grid('ytick',3,'yticklabels',[],'xticklabels',[]);
m_grid('xtick',3,'yticklabels',[],'xticklabels',[]);
title({lomitka(sprintf('Obeh druzice (%gs, %gd)', dt, dny))});
w=1000; h=600; set(gcf, 'WindowStyle','normal','Position',[1,1,w,h],'PaperPositionMode','auto', 'PaperUnits','points','PaperSize',[w,h]);

%troj ref
figure(2)
hold on
krok=360/nmax/50;
[xq,yq] = meshgrid(-180:krok:180, -90:krok:90);
vq = griddata(lontd,lattd,ewh*100,xq,yq,'nearest');
m_pcolor(xq,yq,vq)
m_coast('color','black');
m_grid('ytick',3,'yticklabels',[],'xticklabels',[]);
m_grid('xtick',3,'yticklabels',[],'xticklabels',[]);
shading interp;
cptcmap_pm=lower('BlueWhiteOrangeRed');
colormap(cptcmap_pm);
title({'Interpolated seasonal EWH';lomitka(sprintf('[Eigen6S, nmax=%d, krok=%g, troj=%g, dt=%g, d=%g]',nmax,krok,Ntroj,dt,dny))});
w=1000; h=600; set(gcf, 'WindowStyle','normal','Position',[1,1,w,h],'PaperPositionMode','auto', 'PaperUnits','points','PaperSize',[w,h]);
ewh_clbr=25.9;
caxis(ewh_clbr*[-1 1])
set(colorbar,'ytick',(-20:10:20));
xlabel(colorbar,'cm');
clbr_modify(.5/.8,.75/.8,.99);

%troj GLS
figure(3)
hold on
krok=360/nmax/50;
[xq,yq] = meshgrid(-180:krok:180, -90:krok:90);
vq = griddata(lontd,lattd,ewh_gls*100,xq,yq,'nearest');
m_pcolor(xq,yq,vq)
m_coast('color','black');
m_grid('ytick',3,'yticklabels',[],'xticklabels',[]);
m_grid('xtick',3,'yticklabels',[],'xticklabels',[]);
shading interp;
cptcmap_pm=lower('BlueWhiteOrangeRed');
colormap(cptcmap_pm);
title({'Interpolated seasonal EWH (computed from SHC with GLS)';lomitka(sprintf('[Eigen6S, nmax=%d, krok=%g, troj=%g, dt=%g, d=%g]',nmax,krok,Ntroj,dt,dny))});
w=1000; h=600; set(gcf, 'WindowStyle','normal','Position',[1,1,w,h],'PaperPositionMode','auto', 'PaperUnits','points','PaperSize',[w,h]);
ewh_clbr=25.9;
caxis(ewh_clbr*[-1 1])
set(colorbar,'ytick',(-20:10:20));
xlabel(colorbar,'cm');
clbr_modify(.5/.8,.75/.8,.99);

%troj KF
figure(4)
hold on
krok=360/nmax/50;
[xq,yq] = meshgrid(-180:krok:180, -90:krok:90);
vq = griddata(lontd,lattd,ewh_kf*100,xq,yq,'nearest');
m_pcolor(xq,yq,vq)
m_coast('color','black');
m_grid('ytick',3,'yticklabels',[],'xticklabels',[]);
m_grid('xtick',3,'yticklabels',[],'xticklabels',[]);
shading interp;
cptcmap_pm=lower('BlueWhiteOrangeRed');
colormap(cptcmap_pm);
title({'Interpolated seasonal EWH (computed from SHC with KF)';lomitka(sprintf('[Eigen6S, nmax=%d, krok=%g, troj=%g, dt=%g, d=%g, Pini=%.1g]',nmax,krok,Ntroj,dt,dny,Pini))});
w=1000; h=600; set(gcf, 'WindowStyle','normal','Position',[1,1,w,h],'PaperPositionMode','auto', 'PaperUnits','points','PaperSize',[w,h]);
ewh_clbr=25.9;
caxis(ewh_clbr*[-1 1])
set(colorbar,'ytick',(-20:10:20));
xlabel(colorbar,'cm');
clbr_modify(.5/.8,.75/.8,.99);

%chyby GLS
figure(5)
hold on
krok=360/nmax/50;
[xq,yq] = meshgrid(-180:krok:180, -90:krok:90);
vq = griddata(lontd,lattd,M_gls*100,xq,yq,'nearest');
m_pcolor(xq,yq,vq)
m_coast('color','black');
m_grid('ytick',3,'yticklabels',[],'xticklabels',[]);
m_grid('xtick',3,'yticklabels',[],'xticklabels',[]);
shading interp;
colormap(flipud(hot));
title({'95% error of GLS computation';lomitka(sprintf('[Eigen6S, nmax=%d, krok=%g, troj=%g, dt=%g, d=%g, std_avg=%.2gcm, mse=%.3g]',nmax,krok,Ntroj,dt,dny,mean(M_gls*100),mse))});
w=1000; h=600; set(gcf, 'WindowStyle','normal','Position',[1,1,w,h],'PaperPositionMode','auto', 'PaperUnits','points','PaperSize',[w,h]);
ewh_clbr=25;
caxis(ewh_clbr*[0 1])
set(colorbar,'ytick',(-20:10:20));
xlabel(colorbar,'cm');
clbr_modify(.5/.8,.75/.8,.99);

%chyby KF
figure(6)
hold on
krok=360/nmax/50;
[xq,yq] = meshgrid(-180:krok:180, -90:krok:90);
vq = griddata(lontd,lattd,M_kf*100,xq,yq,'nearest');
m_pcolor(xq,yq,vq)
m_coast('color','black');
m_grid('ytick',3,'yticklabels',[],'xticklabels',[]);
m_grid('xtick',3,'yticklabels',[],'xticklabels',[]);
shading interp;
colormap(flipud(hot));
title({'95% error of KF computation';lomitka(sprintf('[Eigen6S, nmax=%d, krok=%g, troj=%g, dt=%g, d=%g, std_avg=%.2gcm, mse=%.3g]',nmax,krok,Ntroj,dt,dny,mean(M_kf*100),mse))});
w=1000; h=600; set(gcf, 'WindowStyle','normal','Position',[1,1,w,h],'PaperPositionMode','auto', 'PaperUnits','points','PaperSize',[w,h]);
ewh_clbr=25;
caxis(ewh_clbr*[0 1])
set(colorbar,'ytick',(-20:10:20));
xlabel(colorbar,'cm');
clbr_modify(.5/.8,.75/.8,.99);

%uspesne odhady gls
figure(7)
hold on
krok=360/nmax/50;
[xq,yq] = meshgrid(-180:krok:180, -90:krok:90);
vq = griddata(lontd,lattd,bool_gls,xq,yq,'nearest');
m_pcolor(xq,yq,vq)
m_coast('color','black');
m_grid('ytick',3,'yticklabels',[],'xticklabels',[]);
m_grid('xtick',3,'yticklabels',[],'xticklabels',[]);
shading interp;
colormap(flipud(cool));
title({'Do GLS results fit in confidence interval?';'blue=yes; pink=no';lomitka(sprintf('[nmax=%d, krok=%g, troj=%g, dt=%g, d=%g, std_avg=%.2gcm, mse=%.3g, pct=%.3g]',nmax,krok,Ntroj,dt,dny,mean(M_gls*100),mse,pct_gls*100))});
w=1000; h=600; set(gcf, 'WindowStyle','normal','Position',[1,1,w,h],'PaperPositionMode','auto', 'PaperUnits','points','PaperSize',[w,h]);

%uspesne odhady kf
figure(8)
hold on
krok=360/nmax/50;
[xq,yq] = meshgrid(-180:krok:180, -90:krok:90);
vq = griddata(lontd,lattd,bool_kf,xq,yq,'nearest');
m_pcolor(xq,yq,vq)
m_coast('color','black');
m_grid('ytick',3,'yticklabels',[],'xticklabels',[]);
m_grid('xtick',3,'yticklabels',[],'xticklabels',[]);
shading interp;
colormap(flipud(cool));
title({'Do KF results fit in confidence interval?';'blue=yes; pink=no';lomitka(sprintf('[nmax=%d, krok=%g, troj=%g, dt=%g, d=%g, std_avg=%.2gcm, mse=%.3g, pct=%.3g]',nmax,krok,Ntroj,dt,dny,mean(M_kf*100),mse,pct_kf*100))});
w=1000; h=600; set(gcf, 'WindowStyle','normal','Position',[1,1,w,h],'PaperPositionMode','auto', 'PaperUnits','points','PaperSize',[w,h]);

%histogram
edge = -50:1:50;
figure(9)
histogram(er_gls*100,edge)
hold on
title({'Histogram of errors xt-xh (GLS)';lomitka(sprintf('[nmax=%d, troj=%g, dt=%g, d=%g, std.d.=%gcm]',nmax,Ntroj,dt,dny,100*rmse_gls))});
xlabel('Velikost chyby (cm)')
xlim([-25 25])
ylim([0 300])

figure(10)
histogram(er_kf*100,edge)
hold on
title({'Histogram of errors xt-xh (KF)';lomitka(sprintf('[nmax=%d, troj=%g, dt=%g, d=%g, std.d =%.2gcm]',nmax,Ntroj,dt,dny,100*rmse_kf))});
xlabel('Velikost chyby (cm)')
xlim([-25 25])
ylim([0 300])
toc
save(sprintf('vysledky_%g_%g_%g.mat', Ntroj, dny, noise))