close all
clear
clc

addpath(genpath('C:\Users\z5030440\Documents\GitHub\geetools\functions\xyz2spz'))

sl_gt = load('sl_gt.mat')
sl_sds = load('sl_sds.mat')

sl_sds = sl_sds.arr
sl_gt = sl_gt.arr
for i = 1:length(sl_sds)
    sds.x = sl_sds{i}(:,1)
    sds.y = sl_sds{i}(:,2)
    sds.z = zeros(length(sl_sds{i}(:,1)),1)
    
    gt.x = sl_gt{i}(:,1)
    gt.y = sl_gt{i}(:,2)
    gt.z = zeros(length(sl_gt{i}(:,1)),1)
    
    outsds = xyz2spz(sds,'NARRA')
    outgt = xyz2spz(gt,'NARRA')
    
    figure
    hold on
    grid on
    box on
    plot(outsds.s, outsds.p, 'b-', 'linewidth',2)
    plot(outgt.s, outgt.p, 'r-', 'linewidth',2)
    xlabel('s [m]')
    ylabel('p [m]')
    title('Horizontal comparison in spz coordinates')
    legend({'SDS', 'contour at tide level'})
    
    xq = nanmin(outsds.s):10:nanmax(outsds.s)
    [gt_s idx_gt] = unique(outgt.s)
    gt_p = outgt.p(idx_gt)
    gt_p_int = interp1(gt_s, gt_p, xq)
    [sds_s idx_sds] = unique(outsds.s)
    sds_p = outsds.p(idx_sds)
    sds_p_int = interp1(sds_s, sds_p, xq)
    diff_p = sds_p_int - gt_p_int;
    
    sds_av(i) = median(sds_p_int(~(sds_p_int > median(sds_p_int) + 2*std(sds_p_int) | sds_p_int < median(sds_p_int) - 2*std(sds_p_int))))
    gt_p_int(isnan(gt_p_int)) = []
    gt_av(i) = median(gt_p_int(~(gt_p_int > median(gt_p_int) + 2*std(gt_p_int) | gt_p_int < median(gt_p_int) - 2*std(gt_p_int))))
    
    
    idx_nan = isnan(diff_p)
    diff_p(idx_nan) = []
    xq(idx_nan) = []    
    idx_outlier = diff_p > median(diff_p) + 2*std(diff_p) | diff_p < median(diff_p) - 2*std(diff_p)
    diff_p(idx_outlier) = []
    xq(idx_outlier) = []  
    
    p_rmse(i) = sqrt(mean(diff_p.^2))
    p_mean(i) = mean(diff_p)
    p_std(i) = std(diff_p)
    p_q90(i) = quantile(abs(diff_p), 0.9)
    
end

clearvars -except sds_av gt_av p_rmse p_mean p_std p_q90

save 'hor_error.mat'