clc;
clear all;
close all;
tic;

disp('*****************************************');
disp('***     MATLAB - SIMULATION V0.2      ***');
disp('*****************************************');

%% Constantes
largeur = 210;
Longueur = 300;
vitesse_son = 340;
f_ech = 123.077e3;
longueur_sbpa = 2047;
%load donnee.mat
%% Coordonn閑s des balises
B1 = [0 largeur/2];
B2 = [Longueur 0];
B3 = [Longueur largeur];
        
%% Param鑤res
x_robot = 1.70;  %max 300    milieu de terrain : 150
y_robot = 1.05;  %max 210    milieu de terrain : 105
k=0;

bar = waitbar(0,'Simulation en cours...');

for x_robot = [0:1:Longueur-1]
    for y_robot = [0:1:largeur-1]
        k=k+1;
        waitbar(((x_robot)*largeur+(y_robot))/(Longueur*largeur));
        
        %% Calcul des distances
        dist(1) = sqrt((x_robot)^2 + (y_robot - largeur/2)^2);
        dist(2) = sqrt((x_robot-Longueur)^2 + (y_robot)^2);
        dist(3) = sqrt((x_robot-Longueur)^2 + (y_robot - largeur)^2);

        %% Calcul d閜hasage sur les balises en 閏hantillons -> pour la cr閍tion des
        %% signaux
        deph_ech = round(dist*1e-2/vitesse_son*f_ech);
        if max(deph_ech) > 2*longueur_sbpa
            deph_ech = deph_ech - min(deph_ech);
        end
        deph_ech(find(deph_ech > 2*longueur_sbpa - 1)) = 1022;

        %% Calcul des d閏alages temporels -> pour comparaison apr鑣 corr閘ation
        dec_temps(1,((x_robot+1)*largeur+(y_robot+1))) = (dist(1)*1e-2/vitesse_son) - (dist(2)*1e-2/vitesse_son);    % 1-2
        dec_temps(2,((x_robot+1)*largeur+(y_robot+1))) = (dist(2)*1e-2/vitesse_son) - (dist(3)*1e-2/vitesse_son);    % 2-3
        dec_temps(3,((x_robot+1)*largeur+(y_robot+1))) = (dist(3)*1e-2/vitesse_son) - (dist(1)*1e-2/vitesse_son);    % 3-1

        %% Calcul des d閏alages en nombre d'閏hantillons -> pour comparaison apr鑣
        %% corr閘ation
        dec_ech = round(dec_temps*f_ech);

        %% Calcul des diff閞ences de distances
        dist_cal = dec_temps*vitesse_son;

%         %% Affichage
%         s = sprintf('Donn閑s : ');
%         disp(s);
%         s = sprintf('Coordonn閑s du robot : %g ; %g', x_robot,y_robot);
%         disp(s);
%         s = sprintf('Diff閞ences de distances : \n\t d1-d2 : %g m ; \n\t d2-d3 : %g m ; \n\t d3-d1 : %g m ;', dist_cal(1), dist_cal(2), dist_cal(3));
%         disp(s);
%         s = sprintf('Diff閞ences temporelles : \n\t t1-t2 = %g ms ; \n\t t2-t3 = %g ms ; \n\t t3-t1 = %g ms ;', dec_temps(1)*1e3,dec_temps(2)*1e3,dec_temps(3)*1e3);
%         disp(s);
%         s = sprintf('Diff閞ences d''閏hantillons : \n\t B1-B2 : %g 閏hantillons ; \n\t B2-B3 : %g 閏hantillons ; \n\t B3-B1 : %g 閏hantillons ;', dec_ech(1), dec_ech(2), dec_ech(3));
%         disp(s);
%         disp('.');
%         disp('*****************************************');
%         disp('SIMULATION');
%         disp('*****************************************');

        %% Cr閍tion des signaux ultrasons
        %sim ech 1023 points entre 0 et 1023 d'amplitude
        ech=randn([longueur_sbpa 1])*1023;
        ech = ech -mean(ech);

        %ech = ech - 512;
        modele = ech(1:longueur_sbpa)';
        size_modele = length(modele);

        signal(1,1:deph_ech(1)+1) = modele(size_modele-deph_ech(1):size_modele);
        signal(1,deph_ech(1)+2:size_modele) = modele(1:size_modele-deph_ech(1)-1);

        signal(2,1:deph_ech(2)+1) = modele(size_modele-deph_ech(2):size_modele);
        signal(2,deph_ech(2)+2:size_modele) = modele(1:size_modele-deph_ech(2)-1);

        signal(3,1:deph_ech(3)+1) = modele(size_modele-deph_ech(3):size_modele);
        signal(3,deph_ech(3)+2:size_modele) = modele(1:size_modele-deph_ech(3)-1);

        %% Affichage des signaux re鐄s par les balises :
%         figure('Name', 'Signaux ultrasons re鐄s par les balises', 'NumberTitle','off');
%         subplot(4,1,1);
%         plot(modele);
%         title('Signal 閙it par le robot');
%         subplot(4,1,2);
%         plot(signal(1,:));
%         title('Balise 1');
%         subplot(4,1,3);
%         plot(signal(2,:));
%         title('Balise 2');
%         subplot(4,1,4);
%         plot(signal(3,:));
%         title('Balise 3');

        %% Corr閘ation des signaux
        correlation(1,:)=xcorr(signal(1,:), signal(2,:));     %1 - 2
        correlation(2,:)=xcorr(signal(2,:), signal(3,:));     %2 - 3
        correlation(3,:)=xcorr(signal(3,:), signal(1,:));     %3 - 1

        %% Affichage des corr閘ations
%         x_corr = [-size_modele+1:size_modele-1];
%         figure('Name', 'Convolutions', 'NumberTitle','off');
%         subplot(3,1,1);
%         plot(x_corr,correlation(1,:));
%         title('Corr閘ation de B1 et B2');
%         subplot(3,1,2);
%         plot(x_corr,correlation(2,:));
%         title('Corr閘ation de B2 et B3');
%         subplot(3,1,3);
%         plot(x_corr,correlation(3,:));
%         title('Corr閘ation de B3 et B1');

        %% Recherche du maximum de corr閘ation
        [m idx] = max(correlation(1,:));
        dec_ech_corr(1) = idx - size_modele;
        [m idx] = max(correlation(2,:));
        dec_ech_corr(2) = idx - size_modele;
        [m idx] = max(correlation(3,:));
        dec_ech_corr(3) = idx - size_modele;

        %% Calcul des diff閞ences de temps
        temps_corr = dec_ech_corr/f_ech;

        %% Calcul d'erreur
%        err_temps = abs((temps_corr-dec_temps)./dec_temps);

        %% Calcul des diff閞ences de distances
        dist_corr = temps_corr*vitesse_son;

        %% Calcul d'erreur
        err_dist(x_robot+1,y_robot+1) = max(abs((dist_corr-dist_cal(:,k)')./dist_cal(:,k)'));

        %% Affichage r閟ultat
%         disp('.');
%         s = sprintf('Calcul : ');
%         disp(s);
%         s = sprintf('Diff閞ences d''閏hantillons : \n\t B1-B2 : %g 閏hantillons ; \n\t B2-B3 : %g 閏hantillons ; \n\t B3-B1 : %g 閏hantillons ;', dec_ech_corr(1), dec_ech_corr(2), dec_ech_corr(3));
%         disp(s);
%         s = sprintf('Diff閞ences temporelles : \n\t t1-t2 = %g ms ; \n\t t2-t3 = %g ms ; \n\t t3-t1 = %g ms ;', temps_corr(1)*1e3,temps_corr(2)*1e3,temps_corr(3)*1e3);
%         disp(s);
%         s = sprintf('\t Erreur temporelle max : %g %% ; %g ms', max(err_temps)*100, max(err_temps)*1000);
%         disp(s);
%         s = sprintf('Diff閞ences de distances : \n\t d1-d2 : %g m ; \n\t d2-d3 : %g m ; \n\t d3-d1 : %g m ;', temps_corr(1)*vitesse_son, temps_corr(2)*vitesse_son, temps_corr(3)*vitesse_son);
%         disp(s);
%         s = sprintf('\t Erreur de distance max : %g %% ; %g mm', max(err_dist)*100, max(err_dist)*1000);
%         disp(s);
%         s = sprintf('Coordonn閑s du robot : ?? ; ??');
%         disp(s);

    end
end

close(bar);

figure;
mesh(err_dist);
figure;
hold on;
plot( dec_temps(1,:), 'color',[1 0 0]);
plot( dec_temps(2,:), 'color',[0 1 0]);
plot( dec_temps(3,:), 'color',[0 0 1]);

t = toc;
s = sprintf('Temps de calcul Matlab : %g s, soit %g min',t, t/60);
disp(s);