% Parametri:
% velocità lineare desiderata
% velocità angolare desiderata
% mistanze misurate
% sensori utilizzati
% figura su cui plottare l'orizzonte

function [v_eff,th_eff]=obst_av(v_des,th_des,Distanze,Sensori,fig)


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

if fig~=0
    figure(fig)
    clf
    %axis([-180 180 0 1])
    hold on
end


nd=360;
Orizz=zeros(nd+1,4);  % base, attrattivo, repulsivo, somma
Orizz(:,1)=[-180:360/nd:180]';


% Orizzonte attrattivo
%th_des=160*pi/180;

Awide=50;
des=mod(round(mod(th_des,2*pi)/pi*nd/2+nd/2),nd)+1;
for i=1:nd+1
    dist=distanza(i,des,nd);
    if dist<=(Awide)
        Orizz(i,2)=(Awide-dist)/Awide;
    end
end


if fig~=0
    plot(Orizz(:,1),Orizz(:,2),'g')
    plot([Orizz(des,1) Orizz(des,1)],[0 1],'--g')
end


% Orizzonte repulsivo
Rwide=80;
for j=1:num_sens
    if Distanze(j)~=inf
        for i=1:nd+1
        des=mod(round(mod(-Sensori(j,5),2*pi)/pi*nd/2+nd/2),nd)+1;
        dist=distanza(i,des,nd);
            if dist<=(Rwide)
                ampiezza=max((3-Distanze(j))/3,0);
                Orizz(i,3)=max(Orizz(i,3),(Rwide-dist)/Rwide*ampiezza);
            end
        end
    end
end

if fig~=0
    plot(Orizz(:,1),Orizz(:,3),'b')
end



% Orizzonte somma
for i=1:nd+1
    Orizz(i,4)=max(Orizz(i,3),1-Orizz(i,2));
end

if fig~=0
    plot(Orizz(:,1),Orizz(:,4),'r')
end


% Defuzzification
[minimo,indice]=min(Orizz(:,4));
th_eff=-Orizz(indice,1)*pi/180;
v_eff=(1-sqrt(minimo));
if v_eff<0.1
    v_eff=0;
end
v_eff=v_des*v_eff;


