function out=ModelSensor(call,x,covceil,light,sen,y)

%ModelSensor    Function that implements the measurement probability model of the robot
%
% out=ModelSensor(call,x,covceil,light,sen,y)
% call: value used to define the behaviour of the function
% x: state(x,y,theta) associated with the current time
% covceil: covariance (error) in the measurement of the ceiling
% light: information associated with the lighting model
%   light(1): limit
%   light(2): l2
%   light(3): l3
% sen: array that identifies which sensors are used to calculate the
%      probabilities
%      1: only ceiling height
%      2: only lights
%      [1,2]: both sensors
% y: measurement values (provided when call ==1) (height,light)
% out: returned value, dependent on call
%   If call==0, it returns the measurements (height,light)
%   If call==1, it returns the probability of the measurements given the
%               state. It calculates it using the sensors selected by sen.
%

% 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


N=size(x,1);

LightMarks=MapLight(0); %Get the positions of the lights

limit=light(1);
l2=light(2);
l1=light(3);

switch(call)
    case 0, %sample
       
        out(:,1)=Height(0,x(:,1),x(:,2))+gsamp(0,covceil,N);
        
        light=LightToMarks(x(:,1:2),LightMarks,limit);
        prob=(light-l1)/(l2-l1);
        prob(prob<0)=0;
        prob(prob>1)=1;
        out(:,2)=rand(N,1)<=prob;
      
    case 1,
        out=ones(size(x,1),1);
        
        if any(sen==1)
            err2=y(:,1)-Height(0,x(:,1),x(:,2));
            out=out.*exp(-err2.^2/2/covceil);
        end
        
        if any(sen==2)
            
            light=LightToMarks(x(:,1:2),LightMarks,limit); %Defined later on
            prob3=(light-l1)/(l2-l1);
            prob3(prob3<0)=0;
            prob3(prob3>1)=1;
            if y(:,2)==0
                prob3=1-prob3;
            end
             
            out=out.*prob3;
             
        end
        %Outside the map, the probability is zero
        [lxm,lxM,lym,lyM]=MapLimits;
        out(x(:,1)>lxM)=0;
        out(x(:,2)>lyM)=0;
        out(x(:,1)<lxm)=0;
        out(x(:,2)<lxm)=0;
        
    otherwise,1
        error('Not valid information');
end


function d=LightToMarks(x,PosMark,limit)

N=size(PosMark,1);
NP=size(x,1);
d=zeros(NP,1);
for i=1:N
    x1=x(:,1)-PosMark(i,1);
    y1=x(:,2)-PosMark(i,2);
    de=x1.^2+y1.^2;
    d=d+exp(-de/limit);
end

