% falta incluir incerteza nas altitudes dos pontos inicial e final

% script de ajustamento por minimos quadrados de uma linha de nivelamento
% geometrico cujo erro de fecho e inferior a tolerancia

clc 
clear all;

format long

% estrutura do ficheiro de uma linha de nivelamento

% cada linha do ficheiro tem 5 campos
% coluna{1} coluna{2} coluna{3} coluna{4} coluna{5}

% MN=nome marca de nivelamento
% LR=leitura rectaguarda
% DR=distancia rectaguarda
% altitude=altitude da marca de nivelamento
% -1=caracter identificador de inicio de linha
% P=nome do ponto
% LF=leitura frente
% DF=distancia frente
% sigma_aparelho=desvio padrao por km de nivelemento duplo
%
% MN1,LR,DR,altitude INICIAL,-1
% P1,LR,DR,LF,DF
% ...
% PN,LR,DR,LF,DF
% MN2,altitude FINAL,sigma_aparelho (mm/km),LF,DF

% exemplo (MN1_P5_2020_2021.txt)

% "MN1",0.9909,10.33,80.765,-1
% "P1",1.5155,17.22,1.4968,10.28
% "AUX1",1.4860,17.80,1.5020,16.50
% "P2",0.1893,4.08,2.0468,16.45
% "G",2.1506,19.95,2.5906,11.83
% "AUX2",2.3108,15.43,0.0552,16.58
% "AUX3",1.3629,13.03,1.0433,17.42
% "D",0.7229,20.35,0.6956,15.04
% "AUX4",1.3887,23.44,1.4085,18.91
% "AUX5",1.6074,12.88,0.7586,20.06
% "E",0.4274,17.78,1.4870,10.91
% "P9",1.2011,22.08,1.7132,22.40
% "AUX6",0.9901,18.67,1.7015,23.04
% "AUX7",1.1994,8.59,1.7052,21.76
% "AUX8",1.2188,23.55,3.2020,11.07
% "AUX9",1.3638,29.53,1.3558,24.88
% "AUX10",1.0647,21.64,1.1807,26.34
% "P3",1.8809,20.50,1.3267,19.39
% "P",1.2709,25.00,0.5613,22.34
% "AUX11",0.4605,25.27,1.8617,25.61
% "AUX12",1.1617,10.30,1.0208,16.40
% "P5",76.947,0.7,1.0643,10.49

disp '*********************************************************************'
disp '*                                                                   *'
disp '*        Ajustamento de uma linha de nivelamento geometrico         *'
disp '*                                                                   *'
disp '*********************************************************************'                                                                 

disp '   '

% variancia a priori

sigma_priori_2 = 1;

% leitura do nome do ficheiro de dados

ifile = input('Ficheiro de dados: ', 's');
fprintf ('  \n')

% abertura do ficheiro de dados

fid = fopen(ifile,'r');

% leitura do ficheiro de dados

coluna = textscan(fid,'%q %f %f %f %f','Delimiter',',');

% determinacao do numero de linhas do ficheiro

num_linhas = length(coluna{2});

% calcular desniveis e distancias entre miras

for i = 1:num_linhas-1
    desnivel(i) = coluna{2}(i)-coluna{4}(i+1);
    distancia(i) = coluna{3}(i)+coluna{5}(i+1);
end

l = desnivel';
distancia = distancia';

desnivel_total = sum(desnivel);

ef = coluna{4}(1)-coluna{2}(num_linhas)+desnivel_total;

fprintf('Erro de fecho da linha (m) = %6.4f\n',ef); 

% calculo da tolerancia (m) para o erro de fecho da linha c/ formulas dos slides 94 e 96

% Zeiss Dini, Wild NA2

disp ('    ')

if (coluna{3}(num_linhas)==1.3)
    K=4.5/206265;
    tol=2.6*sqrt(sum(distancia'.^2))*K;
end

% Zeiss Dini c/mira invar

if (coluna{3}(num_linhas)==0.7)
    K=2.5/206265;
    tol=2.6*sqrt(sum(distancia'.^2))*K;
end

% Wild NAK2

if (coluna{3}(num_linhas)==0.3)
    K=1.0/206265;
    tol=2.6*sqrt(sum(distancia'.^2))*K;
end

% Wild N3

if (coluna{3}(num_linhas)==0.2)
    K=0.5/206265;
    tol=2.6*sqrt(sum(distancia'.^2))*K;
end

fprintf ('Tolerancia (m) para o erro de fecho da linha c/ formulas dos slides 94 e 96 = %6.4f\n',tol)

disp ('    ')

% calculo da tolerancia para o erro de fecho da linha c/ formulas do slide 91

desenvolvimento_km = sum(distancia)/1000;

fprintf('Desenvolvimento da linha (km) = %6.3f\n',desenvolvimento_km); 

num_desniveis = num_linhas-1;

fprintf('Numero de desniveis da linha = %3.0f\n',num_desniveis); 

num_desniveis_km = num_desniveis/desenvolvimento_km;

fprintf('Numero de desniveis/km = %3.1f\n',num_desniveis_km); 

disp ('    ')

if num_desniveis_km >= 16
    tol1 = sqrt(36*num_desniveis+num_desniveis^2/16)/1000;
    tol2 = sqrt(9*num_desniveis+num_desniveis^2/16)/1000;
    tol3 = 2*sqrt(num_desniveis)/1000;
else
    tol1 = 4*sqrt(36*desenvolvimento_km+desenvolvimento_km^2)/1000;
    tol2 = 4*sqrt(9*desenvolvimento_km+desenvolvimento_km^2)/1000;
    tol3 = 8*sqrt(desenvolvimento_km)/1000;
end

fprintf ('Tolerancia baixa (m) para o erro de fecho da linha c/ formulas do slide 91 = %6.4f\n',tol1)
fprintf ('Tolerancia media (m) para o erro de fecho da linha c/ formulas do slide 91 = %6.4f\n',tol2)
fprintf ('Tolerancia  alta (m) para o erro de fecho da linha c/ formulas do slide 91 = %6.4f\n',tol3)

disp ('   ')

% aceitar/rejeitar obs.

if (abs(ef) < tol)
    disp '|erro de fecho|<tolerancia => Aceitar observacoes, seguir para ajustamento.'
else
    disp '|erro de fecho|>tolerancia => Rejeitar observacoes, repetir trabalho de campo.'
    disp ('    ')
    opcao = input('Quer alterar o valor da tolerancia (s/n)? ','s');
    if opcao == 's';
        tol = input ('    Nova tolerancia: ');
    else
        disp ('   ')
        disp ('Fim.')
        return
    end
end

% incerteza do aparelho (mm/km nivelamento duplo -> m/km nivelamento duplo)

s_aparelho = coluna{3}(num_linhas)/1000; %m

% inicio do ciclo de ajustamento dos desniveis

flag = 0;
num = 1;
while flag == 0
    
    v = zeros(num_linhas-1,1);
    B = zeros(num_linhas-1,num_linhas-2);
    d = zeros(num_linhas-1,1);

    d(1,1) = -coluna{4}(1);
    d(num_linhas-1,1) = coluna{2}(num_linhas);

    f = d-l;

    for i = 1:num_linhas-2;
        B(i,i) = -1;
        B(i+1,i) = 1;
    end

    B;

% matriz das distancias entre miras (soma da distancia atras e distancia a frente) dist(14,14)

    dist = zeros(num_linhas-1,num_linhas-1);
    for i = 1:num_linhas-1
        dist(i,i) = distancia(i);
    end
    
% variancia de cada desnivel mm**2

    s_desniveis_2 = (s_aparelho*sqrt(2*dist/1000))^2;

% pesos de cada desnivel

    pesos = zeros(num_linhas-1,num_linhas-1);
    for i = 1:num_linhas-1
        pesos(i,i) = 1/s_desniveis_2(i,i);
    end

    pesos;

% ajustamento por minimos quadrados

    N = B'*pesos*B;
    t = B'*pesos*f;

% calculo dos parametros
    
    delta = inv(N)*t;
        
% calculo da matriz cofactor dos parametros
        
    Q_delta = inv(N);

% calculo dos residuos

    v = f-B*delta;

% observacoes ajustadas

    l_ajustadas = l+v;
        
    disp ('    ')
    ef1 = coluna{4}(1)-coluna{2}(num_linhas)+sum(l_ajustadas);
    if num == 1;
        fprintf('Erro de fecho da linha apos ajustamento (m) = %6.4f\n',ef1); 
    end
        
% calculo da matriz cofactor das observacoes ajustadas

    Q_l_ajustadas = B*inv(N)*B';

% redundancia ou graus de liberdade (n. observacoes-n.parametros)

    [nlinhas,ncolunas] = size(B);
        
    r = nlinhas-ncolunas;

% calculo da variancia a posteriori

    sigma_posteriori_2 = v'*pesos*v/r;

% calculo da matriz de variancias-covariancias dos parametros

    sigma_delta = sigma_posteriori_2*Q_delta;

% calculo da matriz de variancias-covariancias das observacoes ajustadas

    sigma_l_ajustadas = sigma_posteriori_2*Q_l_ajustadas;

% distribuicao do qui-quadrado a 0.05%

    qui = icdf('chi2',1-0.05,r);

% teste da razao de variancias
    
    disp ('    ')

    if (sigma_posteriori_2*r/sigma_priori_2 < qui)            
% sigma02 e s02 estatisticamente semelhantes 
        fprintf('Teste da razao das variancias: aceitar o ajustamento. %s\n');
        disp ('    ');
        disp ('    ');
        for i = 1:num_linhas-2
            fprintf('altitude ajustada do ponto %s %s %5.3f %s %5.3f\n',coluna{1}{i+1},' = ',delta(i),'±',sqrt(sigma_delta(i,i)));
        end
        disp ('   ')
        fprintf('Incerteza nominal do aparelho = %3.1f %s\n',coluna{3}(num_linhas),'mm/km niv. duplo'); 
        fprintf('Incerteza real do aparelho    = %3.1f %s\n',s_aparelho*1000,'mm/km niv. duplo');
        disp('    ')
        resultado = 0;
    else
        fprintf('Teste da razao das variancias: rejeitar o ajustamento. %s\n')
        resultado = 1;
    end
 
    if resultado == 0
        flag = 1;
    else
% degradar incerteza instrumental
        fprintf('Degradar incerteza nominal do aparelho 0.5 mm/km. %3.0f\n');  
        num = num+1;
        s_aparelho = s_aparelho+0.0005;
    end
       
end        
            
disp('Fim.')