clear
clc

%% IMPORT DATA

finp = fopen('CADM.out'); 
tinp = textscan(finp, '%s %f %f %f %f %f %f %f', 'HeaderLines', 1);
fclose(finp);

date = datetime(tinp{:,1});
x    = tinp{:,2};
y    = tinp{:,3};
z    = tinp{:,4};
sx   = tinp{:,5};
sy   = tinp{:,6};
sz   = tinp{:,7};
m0   = tinp{:,8};


%% CONVERT TO HORIZONTAL COORDINATES
ref = referenceEllipsoid('grs80');
meanX = mean(x);
meanY = mean(y);
meanZ = mean(z);
[B L H] = ecef2geodetic(ref, meanX, meanY, meanZ);
[E N U] = ecef2enu(x, y, z, B, L, H, ref);

DD = sqrt(E.^2 + N.^2 + U.^2);
%% INITIAL OUTLIER REJECTION
idxOutl = isoutlier(DD,'movmedian',20);
date(idxOutl) = [];
x(idxOutl) = [];
y(idxOutl) = [];
z(idxOutl) = [];
sx(idxOutl) = [];
sy(idxOutl) = [];
sz(idxOutl) = [];
m0(idxOutl) = [];
E(idxOutl) = [];
N(idxOutl) = [];
U(idxOutl) = [];
DD(idxOutl) = [];


%% TREND / VARIATIONS ESTIMATION
epoch = juliandate(date) - juliandate(date(1));
ENU = [E N U];

for c = 1 : 3
        for i = 1 : length(epoch)
            A(i,1) = 1;  
            A(i,2) = epoch(i);
            A(i,3) = sin(2* pi * 1 /365.25 * epoch(i));
            A(i,4) = cos(2* pi * 1 /365.25 * epoch(i));
            A(i,5) = sin(2* pi * 2 /365.25 * epoch(i));
            A(i,6) = cos(2* pi * 2 /365.25 * epoch(i));
            l0(i,1) = ENU(1,c) -  ENU(i,c); 
        end
        
        dx = -inv(A'*A)*A'*l0;
        X(:,c) = dx;
        v = A * dx + l0;
        m0apos(c) = sqrt((v' * v) / (length(l0) - 6));
        clear A l0;
        
          
           TS(:,c) =ENU(1,c)+X(1,c)+ X(2,c)*epoch + X(3,c)*sin(2* pi * 1 /365.25 * epoch) + X(4,c)*cos(2* pi * 1 /365.25 * epoch) + ...
               X(5,c)*sin(2* pi * 2 /365.25 * epoch) + X(6,c)*cos(2* pi * 1 /365.25 * epoch);

end

DF = TS - ENU;
DF3D = sqrt(DF(:,1).^2 + DF(:,2).^2 + DF(:,3).^2);


%% FINAL OTLIER DETECTION / REJECTION
COV = cov(DF);

for i = 1 : length(DF(:,1))
    
 XC = [DF(i,1); DF(i,2); DF(i,3)];
 K = XC' * inv(COV) * XC;
 
  
 if K <= 2.8^2
    TEST{i} = 'ok';
    OUTL{i} = 'ok';
 elseif K > 2.8^2 && K <= 3.4^2
    TEST{i} = 'strag';
    OUTL{i} = 'outl';
 else
    TEST{i} = 'outl'; 
    OUTL{i} = 'outl';
 end

end
%VYŘAZENÍ OUTL DALŠÍ PODMÍNKOU
idxOutl = contains(TEST, 'outl');
idxStrag = contains(TEST, 'strag');
idx = contains(OUTL, 'outl');
date(idx) = [];
x(idx) = [];
y(idx) = [];
z(idx) = [];
sx(idx) = [];
sy(idx) = [];
sz(idx) = [];
m0(idx) = [];
E(idx) = [];
N(idx) = [];
U(idx) = [];
DD(idx) = [];

%% FINAL TREND / VARIATIONS ESTIMATION
epoch = juliandate(date) - juliandate(date(1));
ENU2 = [E N U];
DT=date;
for c = 1 : 3
        for i = 1 : length(epoch)
            A(i,1) = 1;  
            A(i,2) = epoch(i);
            A(i,3) = sin(2* pi * 1 /365.25 * epoch(i));
            A(i,4) = cos(2* pi * 1 /365.25 * epoch(i));
            A(i,5) = sin(2* pi * 2 /365.25 * epoch(i));
            A(i,6) = cos(2* pi * 2 /365.25 * epoch(i));
            l0(i,1) = ENU2(1,c) -  ENU2(i,c); 
        end
        
        dx = -inv(A'*A)*A'*l0;
        X2(:,c) = dx;
        v = A * dx + l0;
        m0apos2(c) = sqrt((v' * v) / (length(l0) - 6));
        clear A l0;
        
          
           TS2(:,c) =ENU2(1,c)+X(1,c)+ X(2,c)*epoch + X(3,c)*sin(2* pi * 1 /365.25 * epoch) + X(4,c)*cos(2* pi * 1 /365.25 * epoch) + ...
               X(5,c)*sin(2* pi * 2 /365.25 * epoch) + X(6,c)*cos(2* pi * 1 /365.25 * epoch);

end

DF2 = TS2 - ENU2;
DF3D2 = sqrt(DF2(:,1).^2 + DF2(:,2).^2 + DF2(:,3).^2);


%% PLOT
figure(2)
% VYKRESLENÍ VÝSLEDKU
plot (DT, U,':','Color','#00B000');
hold on;
g1=plot(DT,TS2(:,3),'Color','#00B000','LineWidth',2);
hold on;
plot (DT, E,':','Color','red');
hold on
g2=plot(DT,TS2(:,1),'Color','red','LineWidth',2);
hold on
plot (DT, N,':','Color','blue');
hold on;
g3=plot(DT,TS2(:,2),'Color','blue','LineWidth',2);
legend([g1 g2 g3],{'UP', 'EAST ', 'NORTH'})
hold on
title("After correction")
xlabel("Date")
ylabel("Deviation from average [m]")
ylim([-0.01 0.01]);
ytickformat('%.3f')

