% Mar/2023
% falta incluir incerteza nas cotas dos pontos inicial e final

% script de ajustamento por minimos quadrados de uma rede de nivelamento
% geometrico constituida por linhas que se cruzam, em que cada linha foi
% previamente ajustada

clc 
clear all;

format short

% 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
% cota=cota 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,COTA INICIAL,-1
% P1,LR,DR,LF,DF
% ...
% PN,LR,DR,LF,DF
% MN2,COTA FINAL,sigma_aparelho (mm/km),LF,DF

% exemplo T1_C1p_MN1_P5.txt

% "T1",0.135,27.00,80.108,-1
% "P7",1.378,17.90,1.989,27.30
% "C",1.517,14.40,1.429,22.50
% "P",1.819,20.30,1.732,16.20
% "B",1.348,28.10,1.237,17.90
% "A",0.993,18.10,1.541,29.40
% "AUX1",1.281,24.00,1.740,19.90
% "P8",0.779,26.10,1.178,22.70
% "AUX2",1.143,24.90,1.322,27.50
% "AUX3",1.347,15.60,1.595,24.70
% "P3",3.170,22.70,1.418,15.30
% "AUX4",1.792,19.00,0.380,17.80
% "P1",1.400,22.10,1.002,19.00
% "AUX5",1.411,13.80,1.310,22.30
% "C1P",80.562,0.005,1.195,14.1
% "MN1",1.1174,10.36,80.765,-1
% "P1",1.5842,28.08,1.6218,10.84
% "AUX6",1.5322,11.39,1.4932,15.46
% "P2",0.6471,2.82,2.1721,12.85
% "G",2.2010,25.37,3.4562,21.68
% "AUX7",3.3001,14.87,0.4928,16.94
% "AUX8",1.4772,15.48,1.3272,17.86
% "D",0.6820,25.08,0.7093,7.12
% "AUX9",1.4317,29.27,1.4093,31.88
% "E",1.3547,11.17,1.4466,16.95
% "AUX10",1.1827,13.29,1.7274,7.78
% "P9",1.1754,28.65,1.2932,12.09
% "AUX11",0.7752,28.89,1.8443,28.83
% "AUX12",1.2267,26.74,1.8573,24.17
% "P8",0.8029,18.49,1.8473,21.75
% "AUX13",1.0529,22.49,1.5329,21.31
% "AUX14",1.0492,31.25,1.0342,29.93
% "P3",2.2330,22.46,1.4082,25.05
% "P",1.5707,25.82,0.9143,20.60
% "AUX15",1.1062,16.28,1.5260,27.11
% "AUX16",1.4898,26.27,2.4231,26.49
% "P5",76.947,0.0007,1.2735,16.46 

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

disp '   '

% 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_total_linhas_ficheiro=length(coluna{2});

fprintf('Numero total de linhas do ficheiro = %2.0f\n',num_total_linhas_ficheiro); 

disp ('   ')

% identificar indices de mudanca de linha

k=1;
mudanca_linha(k)=1;
for i=2:num_total_linhas_ficheiro
    if coluna{5}(i)==-1
        k=k+1;
        mudanca_linha(k)=i;
    end
    i=i+1;
end
mudanca_linha(k+1)=num_total_linhas_ficheiro+1;
    
fprintf('Mudanca de linha da rede = %2.0f\n',mudanca_linha); 

num_linhas_rede=length(mudanca_linha)-1;

disp ('   ')

fprintf('Numero de linhas na rede = %2.0f\n',num_linhas_rede)

% calcular desniveis e distancias entre miras

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

desnivel=desnivel';
distancia=distancia';

disp ('   ')

fprintf('Numero de desniveis da rede = %2.0f\n',length(desnivel)); 

disp ('   ')

num_desniveis=length(desnivel);

% identificacao dos parametros: retirar pontos de cota conhecida e pontos
% repetidos; para o efeito, a) construir um vector auxiliar sem os pontos 
% de cota conhecida b) e sobre este vector identificar os pontos repetidos

% a)

j=1;
for k=1:length(mudanca_linha)-1;
    for i=mudanca_linha(k)+1:mudanca_linha(k+1)-2;
        parametro_com_repetidos(j)=coluna{1}(i);
        j=j+1;
    end
end

parametro_com_repetidos';

% b)  

parametro=unique(parametro_com_repetidos,'stable');

parametro';

% vector auxiliar

k=1;
for j=1:num_linhas_rede;
    for i=mudanca_linha(j)+1:mudanca_linha(j+1)-1;
        auxiliar(k)=coluna{1}(i);
        k=k+1;
    end
end

auxiliar';

% construcao da matriz B

B=zeros(length(auxiliar),length(parametro));

% tf=strcmp(s1,s2)
% tf=strcmp(s1,s2) compares s1 and s2 and returns 1 (true) if the two are
% identical and 0 (false) otherwise. Text is considered identical if the 
% size and content of each are the same. The return result tf is of data 
% type logical.
% The input arguments can be any combination of string arrays, character 
% vectors, and cell arrays of character vectors.

for i=1:length(parametro);
    string=parametro(i);
    for j=i:length(auxiliar);
        teste=strcmp(string,auxiliar(j));
        % diferentes
        if teste == 1
            B(j,i)=-1;
            B(j+1,i)=1;
        end   
    end
end

B

% construcao da matriz dos pesos

pesos=zeros(length(desnivel),length(desnivel));
w=0;
for k=1:length(mudanca_linha)-1;
    s_aparelho=coluna{3}(mudanca_linha(k+1)-1)/1000;
    for i=mudanca_linha(k)-w:mudanca_linha(k+1)-2-w;
            s_desniveis_2(i,i)=(s_aparelho*sqrt(2*distancia(i)/1000))^2;
            pesos(i,i)=1/s_desniveis_2(i,i);
    end
    w=w+1;
end

pesos';

% ajustamento por minimos quadrados

% vector das cotas dos pontos extremos das linhas

w=0;
for k=1:length(mudanca_linha)-1;
    cotas_extremos(mudanca_linha(k)-w)=coluna{4}(mudanca_linha(k));
    cotas_extremos(mudanca_linha(k+1)-2-w)=-coluna{2}(mudanca_linha(k+1)-1);
    w=w+1;
end

cotas_extremos=cotas_extremos';

termos_independentes=cotas_extremos+desnivel;

N=B'*pesos*B;
t=B'*pesos*(-termos_independentes);

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

% calculo dos residuos

v=-termos_independentes-B*delta;

desnivel_ajustado=desnivel+v;
        
% 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;

for i=1:length(parametro)
    fprintf('Cota ajustada do ponto %s %s %5.3f %s %5.3f\n',parametro{i},' = ',delta(i),'±',sqrt(sigma_delta(i,i)));
end

% calculo da matriz de variancias-covariancias das observacoes ajustadas

sigma_l_ajustadas=sigma_posteriori_2*Q_l_ajustadas;
 
disp ('    ')

disp('Fim.')