%-----------------------------------------------------------
% Carlos Cabral 2004 - Dep.UA

% extracçao de todas as features 
%-----------------------------------------------------------
clear all; close all; clc
%-----------------------------------------------------------
% Import data to workspace 
% load('Laser_data25032003'); % original data of the laser captured in 25-07-2003
load('Laser_data25032003_inv_17Hz'); % original inverse data of the laser captured in 25-07-2003
%-----------------------------------------------------------

% (1)start ---- DEFINE PARAM ------------------------------------------------------------------
% programa recebe dados em cm e (1x364) com os tres primeiros campos sao as horas; exemplo a=[18 37 43 480 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 387 388 388 382 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 818 452 428 432 431 426 421 424 426 431 434 436 439 442 445 447 450 453 457 460	463	466	470	473	477	480	484	488	492	496	500	505	510	513	518	523	527	532	537	542	547	553	559	565	571	577	303	300	298	297	295	293	291	290	288	286	284	284	282	281	279	278	277	278	281	287	291	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	818	803	796	794	792	790	790	788	787	818	818	818	818	818	818	818	818	818	818	818	818	818	819	818	818	818	819	818	819	819	819	819	819	818	818	818	818	818	818	818	818	818	819	818	818	818	818	819	818	819	819	819	819	805	794	790	788	791	796	819	819	819	819	819	819	818	819	818	819	819	819	819	819	819	819	819	819	819	819	819	819	819	818	819	819	819	819	819	819	819	819	819	819	819	819	819	819	307	302	299	298	297	296	296	295	296	297	298	300	302	305	310	818	819	819	819	819	819	819	819	819	819	819	819	819	819	819	819	819	819	819	819	819	819	819	819	819	819	819	818	819	819	819	819	819	819	819	819	819	819	818	818	818	818	819	819	819	819	819	819	818	818	818	819	819	819	819	819	819	819	819	819	819	819	819	819	819	818	819	819	819	819	819	819	819	819	819	819	819	819	819	818	819	819	819	819	819	819	819	819	819	819	819	819	819	819	819]
% (1)end --------------------------------------------------------------------------------------
tic
for i=1:7365 %to search an individual line (3624) (7365) 
    
    Scan=i   
    [cart_data , data]=CONVERTDATA2CART(a,i); % -----extracçao e conversao dos dados----------- 

    % (2)START~~~~~~~~~~~~~~~~~~~~~DETECÇAO RECTANGULARES~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
        [feature_pontos, RECT_feature_bit_control]=GRADMETHOD_IDEMFEATURESr(data); % -----calculo do gradiente entre duas medidas consecutivas e identificaçao de features-----
        coord_rect=[0 0 0 1];  % ultima coluna distingue entre pilares rectangular 1 e pilar circular 0
        %coord_rect -- 1ª e 2ª col= X e Y do ponto de intercepçao das duas rectas; 3ª col= angulo do segmento maior; 4ª col= tipo de pilar 1-rectangular 0-circular 
        [coord_rect]=TESTCANDIDATOPILARRECT(RECT_feature_bit_control, feature_pontos, data, cart_data, coord_rect); % -----Teste para candidato a pilar rectangular-----     
        coord_rect(find(coord_rect(:,2)==0),:)=[]; % funçao para eliminar o que efectivamente nao interessa
    % (2)END~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
       
    % (3)START~~~~~~~~~~~~~~~~~~~~~DETECÇAO CIRCULARES~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~          
        [feature_pontos, RECT_feature_bit_control]=GRADMETHOD_IDEMFEATURESc(data); % -----identificaçao de features usando o metodo do gradiente----
        coord_circ=[0 0 0 0]; % ultima coluna distingue entre pilares rectangular 1 e pilar circular 0
        %coord_circ -- 1ª e 2ª col= X e Y do ponto central do pilar circular; 3ª col= raio da circunferencia obtida; 4ª col= tipo de pilar 1-rectangular 0-circular 
        [coord_circ]=TESTCANDIDATOPILARCIRC(RECT_feature_bit_control,feature_pontos,data,cart_data, coord_circ); % -----Teste para candidato a pilar circular-----
	    coord_circ(find(coord_circ(:,3)==0),:)=[]; % funçao para eliminar o que efectivamente nao interessa
    % (3)END~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
     
    % (4)START~~~~~~~~~~~~~~~~REFERENCIAÇAO/LOCALIZAÇAO DOS PILARES~~~~~~~~~~~~~~~~~~~~~~~~~~~~          
        % 4ª coluna - pilar rectangular 1  - pilar circular 0
        coord_pilares=[coord_circ;coord_rect]; % junçao de todos os pilares (primeiro os circulares depois os rectangulares)
        coord_loc_robot =[NaN NaN];
        [coord_loc_robot] = QUANT_PILARES4LOCALIZACAO(coord_pilares, coord_loc_robot); % funçao para localizaçao local do robot
    % (4)END~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

    % (5)START~~~~~~~~~~~~~~~~~~~~REPRESENTAÇAO~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%         REPRESENT(coord_circ, cart_data, coord_loc_robot);
%         pause(0.01)
    % (5)END~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    
end 
toc
