%MainSimulation program: It runs a simulation and saves it in the
%                        SimulationData file

% APRENDIZAJE AUTOMÁTICO (G. Pajares y J. M. de la Cruz, Eds.)
% Editorial: RA-MA
% Capítulo 8: Monte Carlo II
% Autores: Eva Besada-Portas, Sergey M. Plis

clear;close all;

Data %Initial variables

%Drawing light and ceiling
[lxm,lxM,lym,lyM]=MapLimits;
x=lxm:0.1:lxM; 
y=lym:0.1:lyM;
[XX,YY]=meshgrid(x,y);
h=Height(0,XX,YY);
mesh(XX,YY,h);
Pos=MapLight(0);
hold on;
plot3(Pos(:,1),Pos(:,2),repmat(2,size(Pos,1),1),'oy')
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Techo');
zlim([0,3]);

%Simulation
N=size(uc,3);
[t,xreal,yreal]=Simulation(N,x0,uc,dt,covrobot,covceil,light);

%Printing results
x=squeeze(xreal);
figure;
plot(x(1,:),x(2,:),'LineWidth',2)
hold on;
MapLight(1);
Height(1);
xlabel('X (m)','FontSize',12);ylabel('Y (m)','FontSize',12);
axis square

figure;
y=squeeze(yreal);
subplot(2,1,1);
plot(t(2:end),y(1,2:end),'o-','MarkerFaceColor','b','MarkerSize',4)
ylabel('Distancia techo (m)','FontSize',12);
subplot(2,1,2);
plot(t(2:end),y(2,2:end),'o-','MarkerFaceColor','b','MarkerSize',4)
ylabel('Detecta foco luz','FontSize',12);
xlabel('Tiempo (s)','FontSize',12);

save SimulaData xreal yreal uc dt covrobot covceil light t