%-----------------------------------------------------------
% Carlos Cabral 2003 - Dep.UA

% ficheiro usado na durante a captura dos dados para analise visual (em
% tempo real) 
%-----------------------------------------------------------
clear all; close all; clc
%-----------------------------------------------------------

while (1)
%----------------------------------------------------------
% import data to workspace 
%[a] = textread('C:\Documents and Settings\cabral\Desktop\lixo\laser00.txt','','delimiter',',');
try
   a=dlmread('C:\Documents and Settings\cabral\Desktop\lixo\scan.txt',',', [0,0,0,360]);

catch
end
tic

%----------------------------------------------------------
[linha coluna]=size(a);
r=200; %raio do pilar circular
for i=1:linha%1654 to search on all lines
    clear coord_circ
    number_of_scan=i;
    % -----extracçao e conversao dos dados------ 
    [cart_data , data]=CONVERTDATA2CART(a,i);
    % -----calculo do gradiente entre duas medidas consecutivas e identificaçao de features-----
    [feature_pontos, i2]=GRADMETHOD_IDEMFEATURES(data);
    
    % -----Teste para candidato a pilar circular-----
    [i8,cond,feature_pontos]=TESTCANDIDATOPILARCIRC(i2,feature_pontos,data,cart_data,r);

    %----------------Aplicaçao do metodo de Reimann (to fit a circle to 2D points)---------
    if i8 == 1
        coord_circ=[0 0 0];    
    else
        for i6=1:length(cond(:,1))
            % condiçao para poder ser candidato a pilar circular usando 3*sigma (desvio padrao)
            if (cond(i6,1) >= (1.003-0.0273) && cond(i6,1) <= (1.003+0.0273) && i2 ~= 1 && not(isempty(feature_pontos)))

                %-----Teste de Reimann-----
                pontos=cart_data(feature_pontos(i6,1):feature_pontos(i6,2),:)';
                c=POINTS2CIRC_FIT_REIMANN(pontos);
  
                %--------------------------------------------------------------------------------------------------------
                %Triagem apos aplicar o metodo de Reimann
                
                    %-----Pseudo coeficiente de correlaçao (erro medio dos pontos à circunferencia)-----
                    [c,cond]=PSEUDOCOEFCORR(pontos,c,i6,cond);
                
                    %-----Quando a distancia ao centro for menor que a media das distancia que constituem o elemento-----  
                    dist_centro=sqrt(c(1)^2+c(2)^2);
                    dist_mean=mean(data(feature_pontos(i6,1):feature_pontos(i6,2)));
                    if dist_centro < dist_mean 
                        c=[0 0 0];
                        cond(i6,:)=zeros(1,length(cond(1,:)));
                        disp('Nao considerado triagem Reimann centro < media pontos')
                    end
                                
                    %-----Raio compreendido entre um intervalo(usando o raio medio e 2*sigma (desvio padrao))-----
                    if or(c(3) < 209.5557-2*10.1447, c(3) > 209.5557+2*10.1447)
                        c=[0 0 0];
                        cond(i6,:)=zeros(1,length(cond(1,:)));
                        disp('Nao considerado triagem Reimann 2sigma (mais largo ou mais estreito que deveria)')
                    end
                    
                    coord_circ(i6,:)=c;
%                 % ----------------------------------------------------------------------------------------------------------
%                 % representaçao
%                 o=0:pi/100:2*pi;
%                 xc=c(1)+c(3)*sin(o);
%                 yc=c(2)+c(3)*cos(o);
%                 figure(2)
%                 plot(cart_data(:,1),cart_data(:,2),cart_data(:,1),cart_data(:,2),'.',0,0,'ro',xc,yc,'r'), grid on, axis ('equal'),axis([-8300 8300 -2000 9000])               
       
            else
                coord_circ(i6,:)=[0 0 0];
                cond(i6,:)=zeros(1,length(cond(1,:)));
                %disp('Nao considerado triagem Gradiente (mais largo ou mais estreito que deveria)')
            end
        end
    end
    %-----calculo das distancias entre os centros dos pilares estimados-----
    [combinacoes,coord_circ]=DISTANCIAS_DOSCENTROS(coord_circ);
toc
    %-----representacao-----
    REPRESENT(coord_circ,cart_data)   
    
%     classifica(i)=input(' Classificaçao: '); %0-Nao ha pilares a detectar    1-Bom (detectou)   2-Nao Detectou    3-Falsos pilares   4-Nao detectou todos
    pause(0.5)
end

end
%----------------------------------------------------------------------------------------------------------------------------------------------------
clear c combinacoes dist_centro dist_mean i i2 i6 i8 number_of_scan r pontos cart_data data coord_circ