clear
clc
% SOUŘADNICE PERMANENTNÍCH REFERENČNÍCH STANIC ETRF2000
CPAR = [3949919.079 1116467.046 4865832.535];
TUBO = [4001470.600 1192345.308 4805795.319];
KUNZ = [4037498.108 1097034.031 4798909.116];
CADM = [3999588.775 1190109.750 4807943.393];
% VÝPOČET VEKTORŮ
x0 = [TUBO(1) - CADM(1);KUNZ(1) - CADM(1);CPAR(1) - CADM(1)];
y0 = [TUBO(2) - CADM(2);KUNZ(2) - CADM(2);CPAR(2) - CADM(2)];
z0 = [TUBO(3) - CADM(3);KUNZ(3) - CADM(3);CPAR(3) - CADM(3)];

p = 1;
for yr = 2018 : 2021
    
%% IMPORT DATA
% TUBO
ftubo = fopen(['data/' num2str(yr) '_TUBO.txt']);
ttubo = textscan(ftubo,'%s %s %f %f %f %s %f %f %f %f %f', 'HeaderLines',1, 'Delimiter', ' ');
fclose(ftubo);
tuboDates = erase(ttubo{:,1}, ['CADM' num2str(yr)]);
tuboSites = extractBetween(ttubo{:,2},1,4);
tuboX = ttubo{:,3};
tuboY = ttubo{:,4};
tuboZ = ttubo{:,5};
tuboDurat = duration(replace(ttubo{:,6}, ',', '.'), 'InputFormat', 'hh:mm:ss.S', 'Format', 's');  
tuboHZ = ttubo{:,8};  
tuboV = ttubo{:,9};  
tuboRMS = ttubo{:,10};  

% CPAR
fcpar = fopen(['data/' num2str(yr) '_CPAR.txt']);
tcpar = textscan(fcpar,'%s %s %f %f %f %s %f %f %f %f %f', 'HeaderLines',1, 'Delimiter', ' ');
fclose(fcpar);
cparDates = erase(tcpar{:,1}, ['CADM' num2str(yr)]);
cparSites = extractBetween(tcpar{:,2},1,4);
cparX = tcpar{:,3};
cparY = tcpar{:,4};
cparZ = tcpar{:,5};
cparDurat = duration(replace(tcpar{:,6}, ',', '.'), 'InputFormat', 'hh:mm:ss.S', 'Format', 's'); 
cparHZ = tcpar{:,8};  
cparV = tcpar{:,9};
cparRMS = tcpar{:,10};  

% KUNZ
fkunz = fopen(['data/' num2str(yr) '_KUNZ.txt']);
tkunz = textscan(fkunz,'%s %s %f %f %f %s %f %f %f %f %f', 'HeaderLines',1, 'Delimiter', ' ');
fclose(fkunz);
kunzDates = erase(tkunz{:,1}, ['CADM' num2str(yr)]);
kunzSites = extractBetween(tkunz{:,2},1,4);
kunzX = tkunz{:,3};
kunzY = tkunz{:,4};
kunzZ = tkunz{:,5};
kunzDurat = duration(replace(tkunz{:,6}, ',', '.'), 'InputFormat', 'hh:mm:ss.S', 'Format', 's');   
kunzHZ = tkunz{:,8};  
kunzV = tkunz{:,9};
kunzRMS = tkunz{:,10};  
    

%% CAT ARRAYS
% USPOŘÁDÁNÍ VEKTORŮ
dates = vertcat(tuboDates, kunzDates, cparDates);
sites = vertcat(tuboSites, kunzSites, cparSites);
durat = vertcat(tuboDurat, kunzDurat, cparDurat);
X     = vertcat(tuboX, kunzX, cparX);
Y     = vertcat(tuboY, kunzY, cparY);
Z     = vertcat(tuboZ, kunzZ, cparZ);
HZ   = vertcat(tuboHZ, kunzHZ, cparHZ);
V   = vertcat(tuboV, kunzV, cparV);
RMS   = vertcat(tuboRMS, kunzRMS, cparRMS); 


%% DAILY PROCESSING
%PŘEVOD DO JD
begDate = datetime(yr, 1 , 1);
endDate = datetime(yr, 12 , 31);

begMjd = juliandate(begDate) - 2400000.5;
endMjd = juliandate(endDate) - 2400000.5;

for mjd = begMjd : endMjd
    date = datetime(mjd + 2400000.5,'convertfrom','juliandate');
    doy  = num2str(day(date, 'dayofyear'),'%03.f');
    %POČET MĚŘENÍ
    idx = find(strcmp(dates, doy));
    
    if(isempty(idx))
        continue;
    end
    
    q = 1;
    %MATICE PLÁNU A
    A = zeros(length(idx));
    
    for i = 1 : length(idx)
        A(q, 1) = -1;
        A(q+1, 2) = -1;
        A(q+2, 3) = -1;
        
        site = sites{idx(i)};
        if (strcmp(site,'TUBO'))
            j = 1;
        elseif (strcmp(site,'KUNZ'))
            j = 2;
        else
            j = 3;
        end
        %MATICE REDUKOVANÝCH MĚŘENÍ
        lc(q)   = x0(j) - X(idx(i));
        lc(q+1) = y0(j) - Y(idx(i));
        lc(q+2) = z0(j) - Z(idx(i));
        
        %DÉLKA MEZI STANICEMI
        DST = sqrt(X(idx(i))^2 + Y(idx(i))^2 + Z(idx(i))^2);
        arclength = seconds(durat(idx(i)));
        baselineAcc = sqrt(HZ(idx(i))^2 + V(idx(i))^2) ;
        if (j == 1)
           baselineAcc = baselineAcc / sqrt(3);
        end
         
        P(q)   = 1/ (baselineAcc * arclength / 86400)^2;
        P(q+1) = 1/ (baselineAcc * arclength / 86400)^2;
        P(q+2) = 1/ (baselineAcc * arclength / 86400)^2;
        
        
        q = q + 3;
    end
 
    
    W = diag(P);   
    %PŘÍRŮSTKY
    dx = -inv(A' * W * A) * A' * W * lc';
    %OPRAVY
    v = A*dx + lc';
    
    %% VARIANCES
    m0 = sqrt((v' * W * v) / (length(lc) - 3));
    Qx = inv(A' * W * A);
    Sx = sqrt(diag(m0^2 * Qx)');
    
    XX(p, 1) = CADM(1) + dx(1);
    XX(p, 2) = CADM(2) + dx(2);
    XX(p, 3) = CADM(3) + dx(3);
    DT(p) = date;
    M0(p) = m0;
    SX(p,:) = Sx;
    p = p +1;
    
    clear A;
    clear W;
    clear lc;
    clear P;
    clear dx;
end

clear HZ;
clear V;
clear RMS;
clear X;
clear Y;
clear Z;
clear dates;
clear sites;
clear durat;
clear cparDates cparSites cparX cparY cparZ cparDurat cparHZ cparV cparRMS;
clear tuboDates tuboSites tuboX tuboY tuboZ tuboDurat tuboHZ tuboV tuboRMS;
clear kunzDates kunzSites kunzX kunzY kunzZ kunzDurat kunzHZ kunzV kunzRMS;
end

%TRANSFORMACE DO ENU
meanX = mean(XX(:,1));
meanY = mean(XX(:,2));
meanZ = mean(XX(:,3));
ref = referenceEllipsoid('grs80');
[meanB meanL meanH] = ecef2geodetic(ref,meanX, meanY, meanZ);
[E N U] = ecef2enu(XX(:,1), XX(:,2), XX(:,3), meanB, meanL, meanH, ref);
%VYKRESLENÍ VÝSLEDKU
figure(1)
plot (DT, U,':','Color','#00B000');
hold on;

plot (DT, E,':','Color','red');
hold on

plot (DT, N,':','Color','blue');
legend("UP","EAST","NORTH")
hold on
title("Before correction")
ylim([-0.01 0.01]);
xlabel("Date")
ylabel("Deviation from average [m]")
ytickformat('%.3f')


%% EXPORT DATA
out = fopen('CADM.out','w');
fprintf(out,'   DATE       X(ECEF)      Y(ECEF)      Z(ECEF)      sX      sY      sZ      m0\r\n');

for i = 1 : length(XX(:,1))
    fprintf(out, '%s %0.4f %0.4f %0.4f %0.5f %0.5f %0.5f %0.5f\r\n', ...
        datestr(DT(i),'yyyy-mm-dd'), XX(i,1), XX(i,2), XX(i,3), SX(i,1), SX(i,2), SX(i,3), M0(i));
    
end
fclose(out);
