function [Distanze,Parete]=get_measures(Ambiente, Sensori,PathRobot)

%Sensori:
% 1- tipo (1:laser, 2:sonar)
% 2- angolo del sensore
% 3- apertura (solo per sonar)
% 4- range
% 5- direzione sulla quale è disposto il sensore rispetto al robot
% 6- distanza dal centro del robot

seg=size(Ambiente);seg=seg(1);

num_sens=size(Sensori); num_sens=num_sens(1);

Distanze=zeros(1,num_sens);


% Coeff rette ambiente
CoAm=zeros(seg,3);
for k=1:seg
    CoAm(k,1)=-(Ambiente(k,2)-Ambiente(k,4));
    CoAm(k,2)=(Ambiente(k,1)-Ambiente(k,3));
    CoAm(k,3)=Ambiente(k,3)*(Ambiente(k,2)-Ambiente(k,4))-Ambiente(k,4)*(Ambiente(k,1)-Ambiente(k,3));         
end


% Posizione der Robot
Prx=PathRobot(1);
Pry=PathRobot(2);
Prt=PathRobot(3);
    
    
for j=0:num_sens-1 % Per ogni sensore
        
    
    % Posizione del sensore
    Th=Sensori(j+1,2)+Prt;    
    Psx=Prx+Sensori(j+1,6)*cos(Th);
    Psy=Pry+Sensori(j+1,6)*sin(Th);
    %plot(Psx,Psy,'g.')
    % Coeff retta sensore
    as=-(Psy-Pry);
    bs=(Psx-Prx);
    cs=Prx*(Psy-Pry)-Pry*(Psx-Prx);

    Distanze(j+1)=inf;


    for k=1:seg % Per ogni segmento
        distP(k)=inf;
        distOrt(k)=inf;
        % Test per determinare la distanza del segmento dal centro del
        % robot
        distRS=sqrt((Prx-Ambiente(k,5))^2+(Pry-Ambiente(k,6))^2);
        if distRS<Sensori(j+1,4)+Ambiente(k,7)/2
        
            %Test per decidere se esiste intersezione
            % Determiniamo il punto di intersezione        
            A=[as bs; CoAm(k,1) CoAm(k,2)];
            C=[-cs -CoAm(k,3)]';
            if abs(det(A)) > 10e-5
                v=inv(A)*C;
                Pix=v(1);
                Piy=v(2);
                % Vediamo se è davandi o dietro al sensore: 
                % il centro del robot NON deve stare in mezzo:
                if ((Prx-Pix)*(Prx-Psx)<=0) & ((Pry-Piy)*(Pry-Psy)<=0)
                    intersezione=0;
                else
                    intersezione=1;
                end
            else
                Pix=inf;
                Piy=inf;
                intersezione=0;
            end
        else
            Pix=inf;
            Piy=inf;
            intersezione=0;
        end
            

        if intersezione==1
                
                % Se rimane all'interno del segmento
                if ((Pix-Ambiente(k,1))*(Pix-Ambiente(k,3))<=0) & ((Piy-Ambiente(k,2))*(Piy-Ambiente(k,4))<=0)
                % Calcoliamo la distanza tra il sensore e l'intersezione (Ps->Pi)
                
                %plot(Pix,Piy,'g.')

                distP(k)=sqrt((Pix-Psx)^2+(Piy-Psy)^2);

                %  Calcoliamo la distanza ortogonale tra il sensore ed il segmento
                distOrt(k)=abs(CoAm(k,1)*Psx+CoAm(k,2)*Psy+CoAm(k,3))/sqrt(CoAm(k,1)^2+CoAm(k,2)^2);
                    
                % Angolo tra i due segmenti
                ang_ret=acos(distOrt(k)/distP(k));

                % Se l'angolo è minore di "ang_max"
                if abs(ang_ret)>Sensori(j+1,3)/2;
                    distOrt(k)=inf;
                end

            end
            
        end % intersezione==1
            
    end % per ogni segmento k
  

% Calcola l'indice del segmento più vicino
[dd,kk]=min(distP);
    
Parete(j+1,1)=kk;
Parete(j+1,2)=Pix;
Parete(j+1,3)=Piy;
    
if Sensori(j+1,1)==1 % Laser
    Distanze(j+1)=distP(kk);
else                 % Sonar
    Distanze(j+1)=distOrt(kk);
end
    

if Distanze(j+1)>Sensori(j+1,4)
    Distanze(j+1)=inf;
end
    
end % per ogni sensore j
    
    