% Carica l'ambiente
% clear all
%load AmbienteQuadrato

clear all



   
interlacciamento=0;     

plotout=1;
movieout=0;

% Parametri
n_it=500;           % numero di passi
raggio=0.25;        % raggio del robot
n_rob=6;

num_dist_rob=0;


[Ambiente,EstPath,EKF_enable]=carica_simulazione3(n_rob);


%---------------------------------------------------------
% Colori
rob_col(1,:)='r.';
rob_col(2,:)='g.';
rob_col(3,:)='b.';
rob_col(4,:)='c.';
rob_col(5,:)='m.';
rob_col(6,:)='y.';



%---------------------------------------------------------
PathRobot(1)=0;
PathRobot(2)=-4;
PathRobot(3)=0;    




% Matrici di Covarianza dello stato
for k=1:n_rob
    Cov(k,:,:)=0.1*eye(3);
    CovPred(k,:,:)=0.1*eye(3);
end

% Covarianza disturbi stato, ingresso e misure
sigma_dx=0.0001;
sigma_du=0.0001;
%sigma_dm=0.1;

Q=sigma_dx*eye(3,3);    % Predizione stato
C=sigma_du*eye(2,2);    % Predizione ingressi

for k=1:n_rob
    R(k,1)=0.001;     % laser su parete
end




% Disturbo sulla velocità di rotazione
dist_th(1)=0;
dist_th(2)=0;
dist_th(3)=0;



%dist_th=[0 0 0];

% Parametri del moto desiderato

v_des=0.1;
th_des=0.0;
v_eff=0;
th_eff=0;

% 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;


% Intervallo di campionamento
dt=1;  % secondi
t=0;


%---------------------------------------------------------------
% Matrice dei Sensori Laser e Sonar (uguali per tutti i robot)
num_sens=8;
for k=1:num_sens
    Sensori(k,1)=1;                 % 1- tipo (1:laser, 2:sonar)
    Sensori(k,2)=k*(2*pi/num_sens); % 2- angolo del sensore
    Sensori(k,3)=55*pi/180;         % 3- apertura (solo per sonar)
    Sensori(k,4)=2;                 % 4- range
    Sensori(k,5)=k*(2*pi/num_sens); % 5- direzione sulla quale è disposto il sensore rispetto al robot
    Sensori(k,6)=raggio;            % 6- distanza dal centro del robot
end

%---------------------------------------------------------------
% Sensore di distanza e di angolo dai landmark
% il sensore è spostato rispetto al centro del robot ma il suo zero
% coincide con la direzione di avanzamento del robot stesso

sens_offset(1)=0;   % distanza dal centro
sens_offset(2)=0;   % angolo rispetto alla direzione del robot





%---------------------------------------------------------------
% Plot della mappa
if plotout
    figure(1)
    set(gcf,'Position',[323   184   635   506])
    clf
    mappa(Ambiente,1)
    axis('equal')
    axis([-6 6 -6 6])
    axis('image')
    hold on
end
    



if movieout
    figure(1)
    mov = avifile('navigation.avi')
    mov.Quality = 100;
    mov.Fps=10;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Loop principale
for i=1:n_it
    
    disp(['######### iterazione ' num2str(i) ' ##########'])
    
    t=t+dt;
    

    if mod(i,1)==0
        plotout=1;
    else
        plotout=0;
    end
    
    
       
    
    % Aggiorna la posizione del robot reali
        
    % Disturbo sull'ingresso
    dist_th=sqrt(sigma_du)*randn;
    dist_v=sqrt(sigma_du)*randn;

    % Nuova posizione Reale
    Th=PathRobot(3)+(th_eff+dist_th)*dt/2;
    PathRobot_next(1)=PathRobot(1)+(v_eff+dist_v)*dt*cos(Th);
    PathRobot_next(2)=PathRobot(2)+(v_eff+dist_v)*dt*sin(Th);   
    PathRobot_next(3)=PathRobot(3)+(th_eff+dist_th)*dt;



    if plotout
        figure(1)
        if exist('rob_h')
            set(rob_h,'Visible','off')
            set(rob_lab_h,'Visible','off')
        end
        [Segmenti_Robot,rob_h,rob_lab_h]=show_car(PathRobot_next,'b',1,plotout,0);
    end
        
     
        

    % Calcolo della posizione stimata e della matrice di covarianza (predizione)
    for r=1:n_rob
        [Xpred,CovPred_r]=EKF_est_predizione(r,EstPath,squeeze(Cov(r,:,:)),Q,C,dt,v_eff,th_eff);

        EstPath(r,:)=Xpred;
        CovPred(r,:,:)=CovPred_r;            
    end
    
    
    %----------------------------------------------------------------------
    %----------------------------------------------------------------------
    % Sezione misure
    
    
    
    % Qui si può usare anche la FOX
    [Distanze,Parete]=get_measures(Ambiente,Sensori,PathRobot_next);
    Distanze_n=Distanze+randn*sqrt(R(r,1));
     
     
    % Correzione
    for r=1:n_rob
    
            
        
        [NewEstPath,CovOut]=EKF_est_correzione(EKF_enable,r,Sensori,[],[],[],[],...
                                    EstPath,CovPred,...
                                    Ambiente,Distanze_n,Parete,Q,C,R,dt,v_eff,th_eff,...
                                    sens_offset,interlacciamento);                            
                                
        EstPath(r,:)=NewEstPath';
        Cov(r,:,:)=CovOut;
    
        
        
        %------------------------------------------------------------------
        %------------------------------------------------------------------
        % NAVIGAZIONE
        
        % Obstacle Avoidance
        [v_eff,th_eff]=obst_av(v_des,th_des,Distanze,Sensori,0);
        th_eff=th_eff*0.5;
        
       

        
        
        
    end % per ogni robot
    
  
   % Plot del Robot stimato
   for r=1:n_rob
        if plotout
            figure(1)
            if exist('rob_hs') & (length(rob_hs)>=r)
                set(rob_hs(r),'Visible','off')
                set(rob_lab_hs(r),'Visible','off')
            end
            [Segmenti_Robot,rob1_h,rob_lab1_h]=show_car(EstPath(r,:),'r',r,plotout,0);
            rob_hs(r)=rob1_h;
            rob_lab_hs(r)=rob_lab1_h;
        end
   end
        
    %------------------------------------------------------------------
    % Plot delle misure di distanza
    if plotout
        figure(1)
        for k=1:num_sens

            Th=Sensori(k,2)+PathRobot_next(3);
            Psx=PathRobot_next(1)+Sensori(k,6)*cos(Th);
            Psy=PathRobot_next(2)+Sensori(k,6)*sin(Th);

            if bitand(EKF_enable,laser_su_parete) ~= 0
                switch Sensori(k,1)
                    case 1
                    % Intersezione in linea (Laser)
                    if exist('sonar_h') & (size(sonar_h,2)>=k)
                         set(sonar_h(k),'Visible','off')
                    end
                    if Distanze(k)~=inf
                        sonar_h(k)=plot(Psx+Distanze(k)*cos(Th),Psy+Distanze(k)*sin(Th),'rx');
                    else
                        sonar_h(k)=0;
                    end


                    % Intesezione normale (Sonar)
                    case 2
                    if exist('sonar_h') & (size(sonar_h,2)>=k)
                         set(sonar_h(k),'Visible','off')
                    end
                    if Distanze(k)~=inf
                        sonar_h(k)=plot(Psx+Distanze(k)*cos(Th),Psy+Distanze(k)*sin(Th),'b.')
                    else
                        sonar_h(k)=0;
                    end
                end
            end
        end
    end     


%----------------------------------------    
% Update
PathRobot=PathRobot_next;
    
%----------------------------------------
% Manage figures
if plotout
    
    figure(1)
    drawnow
    pause(0)
end

if movieout
    mov = addframe(mov,getframe(gca));
end

    

end % per ogni passo i

if movieout
    mov = close(mov);
end



