% EKF
function [NewEstPath,CovOut,Errori]=EKF_est(EKF_enable,r,Sensori,...
                                            InterDist,InterAngle,Landmarks,Mis_Land,...
                                            EstPath,CovPred_all,Ambiente,Distanze,Parete,Q,C,R,dt,v_eff,th_eff,...
                                            sens_offset,interlacciamento)


                                        
%if min(size(EstPath))==1
%    EstPath=EstPath';
%end
                                            
num_sens=size(Sensori); 
num_sens=num_sens(1);

num_pareti=size(Ambiente);
num_pareti=num_pareti(1);

Correzione=[];
corr_index=0;

num_rob=max(size(InterDist));

% La covarianza predetta del robot attuale
CovPred=squeeze(CovPred_all(r,:,:));

% Abilitazione degli algoritmi per EKF
laser_su_parete=1;
distanza_da_robot=2;
angolo_da_robot=4;
distanza_da_landmark=8;
angolo_da_landmark=16;

% Default
NewEstPath=EstPath(r,:);
CovOut=CovPred;

Errori_dist=zeros(num_rob,1);     
Errori_ang=zeros(num_rob,1);
Errori_distL=zeros(num_rob,1);     
Errori_angL=zeros(num_rob,1);


%--------------------------------------------------------------------------
% PREDIZIONE
%--------------------------------------------------------------------------

%Xpred(1,1)=EstPath(r,1)+dt*v_eff*cos(EstPath(r,3)+th_eff*dt/2);
%Xpred(2,1)=EstPath(r,2)+dt*v_eff*sin(EstPath(r,3)+th_eff*dt/2);
%Xpred(3,1)=EstPath(r,3)+th_eff*dt;

% La predizione è già stata fatta fuori
Xpred(1,1)=EstPath(r,1);
Xpred(2,1)=EstPath(r,2);
Xpred(3,1)=EstPath(r,3);



%La CovPred è già stata calcolata

%JX=[1  0  -dt*v_eff*sin(Xpred(3,1)+th_eff*dt/2)
%    0  1   dt*v_eff*cos(Xpred(3,1)+th_eff*dt/2)
%    0  0   1                                      ];

%JU=[ dt*cos(Xpred(3,1)+th_eff*dt/2)  -dt^2/2*v_eff*sin(Xpred(3,1)+th_eff*dt/2)
%     dt*sin(Xpred(3,1)+th_eff*dt/2)   dt^2/2*v_eff*cos(Xpred(3,1)+th_eff*dt/2)
%     0                                  dt                                        ];
 
%CovPred=JX*CovIn*JX'+JU*C*JU'+Q;

% Posizione stimata del robot
Prx=Xpred(1);
Pry=Xpred(2);
Prt=Xpred(3);

%--------------------------------------------------------------------------
% INNOVAZIONE
%--------------------------------------------------------------------------
JH=[];                    %jacobiano delle letture convalidate
R_inn=[];                 % matrice covarianza errore sulle misure
sv=0;                     % righe dello jacobiano riempite
JHrow=[0 0 0];            % riga generica dello jacobiano
V=[];

%--------------------------------------------------------------------------
% Innovazione Laser su Parete
%--------------------------------------------------------------------------

if bitand(EKF_enable,laser_su_parete) ~= 0
    %disp('laser')

% Leggo le misure del robot predizione

[DistanzeHp,PareteHp]=get_measures(Ambiente,Sensori,Xpred);

% Per ogni sensore
for i=1:num_sens

    if (Distanze(i)~=inf)&(DistanzeHp(i)~=inf) % se c'è stata riflessione entro il range del sensore su una parete

        % Coefficenti della retta parete
        k=PareteHp(i,1);
        a=-(Ambiente(k,2)-Ambiente(k,4));
        b=(Ambiente(k,1)-Ambiente(k,3));
        c=Ambiente(k,3)*(Ambiente(k,2)-Ambiente(k,4))-Ambiente(k,4)*(Ambiente(k,1)-Ambiente(k,3)); 
        
        raggio=Sensori(i,6);
        ang_sens=Sensori(i,2);
        
        
        % Posizione stimata del laser
        Th=ang_sens+Prt;    
        Psx=Prx+raggio*cos(Th);
        Psy=Pry+raggio*sin(Th);
               
        % Distanza sensore parete
        dist_sp=abs(a*Psx+b*Psy+c)/sqrt(a^2+b^2);
        
        % Ristanza Robot Parete
        dist_rp=abs(a*Prx+b*Pry+c)/sqrt(a^2+b^2);
        
        % Calcola la riga dello Jacobiano di h

    
        % Calcolo Jacobiano
        % | num(x) /  den(x) |
        den=(a*cos(Th)+b*sin(Th));
        num=a*Psx+b*Psy+c;
    
        % derivate
        d_num= -a*raggio*sin(Th)+b*raggio*cos(Th);
        d_den= -a*sin(Th)+b*cos(Th);

        % SE LE RETTE NON SONO PARALLELE AND LATO DELLA PARETE QUELLO GIUSTO!!!!
        if (den~=0) & (dist_sp<dist_rp) & (dist_rp>raggio)
            
                
            JHrow(1)=sign(num/den)*(a/den);
            JHrow(2)=sign(num/den)*(b/den);    
            JHrow(3)=sign(num/den)*((d_num*den-d_den*num)/(den^2));    

            sv=sv+1;
            JH(sv,:)=JHrow;
            R_inn(sv)=R(1);  % elemento relativo a distanza da parete
            V(sv,1)=(Distanze(i)-abs(num/den));
        end
        
    end % se la distanza è nel range

end % per ogni sensore laser


end % abilitazione

%--------------------------------------------------------------------------
% Innovazione Distanza da Robot
%--------------------------------------------------------------------------

if bitand(EKF_enable,distanza_da_robot) ~= 0
        %disp('distanza')

   
for i=1:num_rob
    if ((i~=r) & (InterDist(r,i)~=inf) & (InterDist(r,i)~=0))
        % Posizione dell'altro robot
        PrVx=EstPath(i,1);
        PrVy=EstPath(i,2);
        %PrVt=EstPath(i,3);
    
        est_dist=sqrt((Prx-PrVx)^2+(Pry-PrVy)^2);
        
        if est_dist>0
        
            JHrow(1)=(Prx-PrVx)/est_dist;
            JHrow(2)=(Pry-PrVy)/est_dist;    
            JHrow(3)=0;    
    
            sv=sv+1;
            JH(sv,:)=JHrow;
            R_inn(sv)=R(2);  % elemento relativo a distanza da robot
            V(sv,1)=(InterDist(r,i)-est_dist);
            %disp(['est_dist(' num2str(r) ',' num2str(i) ')=' num2str(est_dist)])
            %disp(['errore distanza=' num2str(V(sv,1))])
            Errori_dist(i)=V(sv,1);
            
            corr_index=corr_index+1;
            Correzione(corr_index,:)=[i 0 1 sv 0];
            
        end
        
    end
end % per ogni robot

end % enable

%--------------------------------------------------------------------------
% Innovazione Angolo da Robot
%--------------------------------------------------------------------------

if bitand(EKF_enable,angolo_da_robot) ~= 0
        %disp('angolo')
        

for i=1:num_rob
    if ((i~=r) & (InterDist(i,r)~=inf) & (InterDist(i,r)~=0))
        % Posizione dell'altro robot
        PrVx=EstPath(i,1);
        PrVy=EstPath(i,2);
        %PrVt=EstPath(i,3);
    
        
        
        Theta_dist=atan2((Pry-PrVy),(Prx-PrVx));
        est_angle=Theta_dist-Prt; %+pi;
        %est_angle=mod(est_angle+pi,2*pi)-pi;
        est_angle=mod(est_angle,2*pi)-pi;
        
        JHrow(1)=-(Pry-PrVy)/((Prx-PrVx)^2+(Pry-PrVy)^2);
        JHrow(2)=(Prx-PrVx)/((Prx-PrVx)^2+(Pry-PrVy)^2);    
        JHrow(3)=-1;    
    
        sv=sv+1;
        JH(sv,:)=JHrow;
        R_inn(sv)=R(3);  % elemento relativo a distanza da robot
        V(sv,1)=mod(InterAngle(r,i)-est_angle+pi,2*pi)-pi;

        
        
        %disp(['est_angle(' num2str(r) ',' num2str(i) ')=' num2str(est_angle*180/pi)])
        %disp(['errore di angolo=' num2str(V(sv,1)*180/pi)])
        
        Errori_ang(i)=V(sv,1);
        
        j=find(not([Correzione(:,1)]-i));
        if isempty(j)
            corr_index=corr_index+1;
            Correzione(corr_index,:)=[i 1 0 0 sv];
        else
            Correzione(j,2)=1;
            Correzione(j,5)=sv;
        end
        


            
      
    end
end % per ogni robot




end % enable


%--------------------------------------------------------------------------
% Innovazione Distanza da LandMark
%--------------------------------------------------------------------------

if bitand(EKF_enable,distanza_da_landmark) ~= 0
        %disp('distanza')

[len,fields] = size(Mis_Land);

if len>0 % se vede almeno un landmark
    
% Posizione del sensore
Psensx=Prx+sens_offset(1)*cos(Prt+sens_offset(2));
Psensy=Pry+sens_offset(1)*sin(Prt+sens_offset(2));

    
for i=1:len

        % Posizione del landmak
        PrVx=Landmarks(Mis_Land(i,1),1);
        PrVy=Landmarks(Mis_Land(i,1),2);
    
        
        est_dist=sqrt((Psensx-PrVx)^2+(Psensy-PrVy)^2);
        
        if est_dist>0
        
            JHrow(1)=(Psensx-PrVx)/est_dist;
            JHrow(2)=(Psensy-PrVy)/est_dist;    
            JHrow(3)=-sens_offset(1)*(sin(Prt+sens_offset(2))*(Prx-PrVx)-cos(Prt+sens_offset(2))*(Pry-PrVy))/est_dist;    
    
            sv=sv+1;
            JH(sv,:)=JHrow;
            R_inn(sv)=R(4);  % elemento relativo a distanza da landmark
            V(sv,1)=(Mis_Land(i,2)-est_dist);
            %disp(['distanza landmark-> robot:' num2str(r) ' landmark:' num2str(Mis_Land(i,1))...
            %      ' stima:' num2str(est_dist), ' misura:' num2str(Mis_Land(i,2))])
            Errori_distL(i)=V(sv,1);
            
        end
        
end % per ogni landmark visibile

end % enable

end % vede almeno un landmark

%--------------------------------------------------------------------------
% Innovazione Angolo da LandMark
%--------------------------------------------------------------------------


if bitand(EKF_enable,angolo_da_landmark) ~= 0
        %disp('distanza')

[len,fields] = size(Mis_Land);

if len>0 % se vede almeno un landmark
    
% Posizione del sensore
Psenst=Prt+sens_offset(2);
Psensx=Prx+sens_offset(1)*cos(Psenst);
Psensy=Pry+sens_offset(1)*sin(Psenst);

    
for i=1:len

        % Posizione del landmak
        PrVx=Landmarks(Mis_Land(i,1),1);
        PrVy=Landmarks(Mis_Land(i,1),2);
    
        Theta_dist=atan2((Psensy-PrVy),(Psensx-PrVx));
        est_angle=Theta_dist-Psenst+pi;
        est_angle=mod(est_angle+pi,2*pi)-pi;
        
        est_dist2=((Psensx-PrVx)^2+(Psensy-PrVy)^2);
        
        JHrow(1)=-(Psensy-PrVy)/est_dist2;
        JHrow(2)=(Psensx-PrVx)/est_dist2;
        %JHrow(3)=-1;
        JHrow(3)=-(sens_offset(1)*(cos(Psenst)*(Prx-PrVx)+sin(Psenst)*(Pry-PrVy))+(Prx^2-2*Prx*PrVx+PrVy^2+PrVx^2+ Pry^2-2*Pry*PrVy))/est_dist2;
        
        
        
        
        
    
        sv=sv+1;
        JH(sv,:)=JHrow;
        R_inn(sv)=R(5);  % elemento relativo ad angolo da landmark
        V(sv,1)=mod(Mis_Land(i,3)-est_angle+pi,2*pi)-pi;

        
       %disp(['angolo landmark-> robot:' num2str(r) ' landmark:' num2str(Mis_Land(i,1))...
       %      ' stima:' num2str(est_angle*180/pi), ' misura:' num2str(Mis_Land(i,3)*180/pi)...
       %      ' errore:' num2str(V(sv,1)*180/pi) ])

       Errori_angL(i)=V(sv,1);
                
end % per ogni landmark visibile

end % enable

end % vede almeno un landmark



%--------------------------------------------------------------------------
% CORREZIONE
%--------------------------------------------------------------------------
if sv~=0
    
    R_cov=diag(R_inn);
    
    %------------------------------------------------------
    % Modifica alla matrice R causata dall'interlacciamento
    if interlacciamento
    for i=1:corr_index
        if Correzione(corr_index,2)+Correzione(corr_index,3)==1

            sv_rob=Correzione(corr_index,4)+Correzione(corr_index,5);
            robot=Correzione(corr_index,1);
            R_cov(sv_rob,sv_rob)=R_cov(sv_rob,sv_rob)+JH(sv_rob,:)*squeeze(CovPred_all(robot,:,:))*JH(sv_rob,:)';
            
        else
            
            sv_rob_a=Correzione(corr_index,4);
            sv_rob_b=Correzione(corr_index,5);
            robot=Correzione(corr_index,1);
            
            R_app=[R_cov(sv_rob_a,sv_rob_a)  R_cov(sv_rob_a,sv_rob_b) 
            R_cov(sv_rob_b,sv_rob_a)         R_cov(sv_rob_b,sv_rob_b)];
                       
            JH_app=[JH(sv_rob_a,:)
                    JH(sv_rob_b,:)];
                       
            R_app=R_app+JH_app*squeeze(CovPred_all(robot,:,:))*JH_app';
            
            R_cov(sv_rob_a,sv_rob_a)=R_app(1,1);
            R_cov(sv_rob_a,sv_rob_b)=R_app(1,2);
            R_cov(sv_rob_b,sv_rob_a)=R_app(2,1);
            R_cov(sv_rob_b,sv_rob_b)=R_app(2,2);
        end
        
    end
    end
    %------------------------------------------------------
    

    % Matrice di innovazione
    S=JH*CovPred*JH'+R_cov;

    % Validation gate

    % Guadagno di Kalman
    K= CovPred*JH'*inv(S);
        
    %K*S*K'

    % Aggiornamento
    NewEstPath=Xpred+K*V;

    CovOut=CovPred-K*S*K';
    %CovOut=CovPred-K*JH*CovPred;
else
    NewEstPath=Xpred;

    CovOut=CovPred;
end




%figure(2)
%plot(Errori_ang*180/pi,colore(r,'*'))

%figure(3)
%plot(Errori_dist,colore(r,'*'))


Errori=norm(Errori_ang)+norm(Errori_dist)+norm(Errori_angL)+norm(Errori_distL);
%figure(4)
%plot(r,Errori,colore(r,'*'))


