%##########################################################################
% File: MazeSimulator.m
% Author: Filipe Silva
% Date: 15-04-2019
% Purpose: this script simulates the motion of a mobile robot in a maze, 
%           as well as the infrared sensor readings (Sharp GP2Y=A41SK0F)  
%##########################################################################
clear all
close all
clc
%##########################################################################
% PARAMETER INITIALIZATION SCRIPT
%##########################################################################
%%
IniParam        % Script 
%##########################################################################
% GENERATE THE ANGULAR VELOCITIES @WHEELS
%##########################################################################
% Virtual time
Ts = 3e-2;
Texe = 10;
t = 0:Ts:Texe;
ANG_vel = 10; 
DLevels = [0.055 0.125 0.185 0.255]
%MOTION_type = 0;    % 0: left-rotation about itself
                    % 1: right-rotation about itself
                    % 2: moving forward along an oscillatory path
%[t,w_R,w_L] = WheelMotionGenerator(Ts,Texe,ANG_vel,MOTION_type);  
DIM = length(t);
disp('Simulating the robot motion and the ID sensor data acquisition...')
%##########################################################################
% SIMULATE THE ROBOT'S MOTION
%##########################################################################
% Robot localization (x,y,fi), "ground-truth" --> pre-allocation for speed
% Sensor distance & intersection point --> pre-allocation for speed
NUM_IR_sensors = length(IR_sensor_ori);
Dist_IR_sensor = ones(NUM_IR_sensors,DIM);
IPoint_IR_sensor = zeros(NUM_IR_sensors,2,DIM);
sensor_Levels=zeros(NUM_IR_sensors,DIM);
nLevels = 4;
episodes = 300;
state = zeros(episodes,DIM);
nStates = nLevels^size(sensor_Levels,1);
nActions = 9;
QTable = zeros(nStates,nActions);
reward = zeros(1,DIM);
alpha = 0.1;
gamma = 0.9;
currentState = 0;
newState = 0;
Action_Taken= zeros(1,DIM);
Action_Taken(1)=1;
x = zeros(1,DIM);
y = zeros(1,DIM);
fi = zeros(1,DIM);

x(1) = 0.6; y(1) = 0.4; fi(1) = (pi)/2; % Initial robot's configuration

% Simulate the robot's motion and the ID sensor data acquisition
Ts=3e-2;
Texe=3e-2;

state_appearance = zeros(1,episodes);
counts = 0;
A = [0 0; 0 1.2; 0 0; 1.2 0; 0 0.45; 0.45 0; 0 0.75; 0.45 0.75; 0.75 0; 0.75 0.45; 0.75 0.75; 0.75 0.75;0.75 0.45]; 
B = [0 1.2; 1.2 1.2; 1.2 0; 1.2 1.2; 0.45 0.45; 0.45 0.45; 0.45 0.75; 0.45 1.2; 0.75 0.45; 1.2 0.45; 1.2 0.75; 0.75 1.2;0.75 0.75];
                        
rewardSum = zeros(1,episodes);
iterations = zeros(1,episodes);

for ii = 1:episodes
    kk = 1;
    x(kk) = (randi(10)+55)*0.01;
    y(kk) = 0.4;
    fi(kk)=pi/2;
    [Dist_IR_sensor(:,kk),IPoint_IR_sensor(:,:,kk)] = ...
     IR_SensorData(A,B,x(kk),y(kk),fi(kk),IR_sensor_ori);
     sensor_Levels(:,kk)= getSensorLevels(Dist_IR_sensor(:,kk),DLevels);
     state(ii,kk)=get_State(sensor_Levels(:,kk));
     newState = state(ii,kk);
    x(2)=x(1);
    y(2)=y(1);
    fi(2)=fi(1);
    [Dist_IR_sensor(:,2),IPoint_IR_sensor(:,:,2)] = ...
     IR_SensorData(A,B,x(2),y(2),fi(2),IR_sensor_ori);
     sensor_Levels(:,2)= getSensorLevels(Dist_IR_sensor(:,2),DLevels);
     state(ii,2)=get_State(sensor_Levels(:,2));
     newState = state(ii,2);
    rewardSum(ii) = 0;
    iterations(ii) = 0;
    kk=kk+1;
    while(y(kk-1) <= 0.76)
         %disp(x(kk));
         kk = kk+1;
         if kk ~= 2,
               
               if (Dist_IR_sensor(1,kk-1)<=0.09)||((Dist_IR_sensor(3,kk-1)<=0.09)&&(Dist_IR_sensor(2,kk-1)<=0.09))||((Dist_IR_sensor(5,kk-1)<=0.09)&&(Dist_IR_sensor(6,kk-1)<=0.09))||(Dist_IR_sensor(3,kk-1)<=0.09)||(Dist_IR_sensor(5,kk-1)<=0.09)||(Dist_IR_sensor(6,kk-1)<=0.09)||(Dist_IR_sensor(2,kk-1)<=0.09)
                   disp('Back');
 
                       x(kk) = (randi(10)+55)*0.01;
                       y(kk) = 0.4;
                       fi(kk) = pi/2;
                       

               elseif(Dist_IR_sensor(1,kk-1)<=0.06)||(Dist_IR_sensor(2,kk-1)<=0.06)||(Dist_IR_sensor(3,kk-1)<=0.06)||(Dist_IR_sensor(4,kk-1)<=0.06)||(Dist_IR_sensor(5,kk-1)<=0.06)||(Dist_IR_sensor(6,kk-1)<=0.06)
                   disp("xxx");
                       x(kk) = (randi(10)+55)*0.01;
                       y(kk) = 0.4;
                       fi(kk) = pi/2;
                       
               elseif ((Dist_IR_sensor(4,kk-1)<=0.09)&&(Dist_IR_sensor(3,kk-1)<=0.09))||((Dist_IR_sensor(4,kk-1)<=0.09)&&(Dist_IR_sensor(5,kk-1)<=0.09))
                   disp("Frontwards")
                    x(kk) = (randi(10)+55)*0.01;
                    y(kk) = 0.4;
                    fi(kk) = pi/2;
                    
               else
                   
                   currentState = newState;
                   z = rand();
                   if z <= (1/log((ii+1)^5))
                   %if z <= 0.1 
                       MOTION_type = randi(9);
                       %disp("random action")
                   else
                       [aux,ind]=max(QTable(currentState,:));
                       MOTION_type = ind;
                   end
                   Action_Taken(1,kk)=MOTION_type;
                   [t,w_R,w_L] = WheelMotionGenerator(Ts,Texe,ANG_vel,MOTION_type); 
                   % Update the robot's localization (numerical integration)
                   x(kk) = x(kk-1)+R_wheels*cos(fi(kk-1))*Ts*(w_R(1)+w_L(1))/2;
                   y(kk) = y(kk-1)+R_wheels*sin(fi(kk-1))*Ts*(w_R(1)+w_L(1))/2;
                   fi(kk) = fi(kk-1)+R_wheels*Ts*(w_R(1)-w_L(1))/L_wheels;
                   if fi(kk) > pi, fi(kk) = fi(kk)-2*pi; end
                   if fi(kk) < -pi, fi(kk) = fi(kk)+2*pi; end

                   [Dist_IR_sensor(:,kk),IPoint_IR_sensor(:,:,kk)] = ...
                   IR_SensorData(A,B,x(kk),y(kk),fi(kk),IR_sensor_ori);
                   sensor_Levels(:,kk)= getSensorLevels(Dist_IR_sensor(:,kk),DLevels);
                   state(ii,kk)=get_State(sensor_Levels(:,kk));
                   newState = state(ii,kk);
                   reward(1,kk) = rewardFunction(newState,MOTION_type);
                   QTable(currentState,MOTION_type) = QTable(currentState,MOTION_type) + alpha*(reward(1,kk) + (gamma*max(QTable(newState,:)))-QTable(currentState,MOTION_type));    
               end
               
        end
        
        
       
        % Read IR sensor data
     [Dist_IR_sensor(:,kk),IPoint_IR_sensor(:,:,kk)] = ...
     IR_SensorData(A,B,x(kk),y(kk),fi(kk),IR_sensor_ori);
     sensor_Levels(:,kk)= getSensorLevels(Dist_IR_sensor(:,kk),DLevels);
     state(ii,kk)=get_State(sensor_Levels(:,kk));
     newState = state(ii,kk);
     
     
     new_appearance = find(state==newState);
     auxiliar = prod(size(new_appearance));
     if auxiliar==1
        state_appearance(1,ii)=counts+1;
        counts=counts+1;   
     else
         if ii > 1
           state_appearance(1,ii)=counts;
         end
     end
     
     rewardSum(ii) = rewardSum(ii) + reward(1,kk);
     iterations(ii) = iterations(ii)+1;
    end
    
%     aux1 = 1;
%     v = 0;
%     c = unique(state(1,10:kk));
%     for i = 3:length(c)
%         if rewardFunction(c(i),:) == rewardFunction(1,:)
%             v(aux1) = c(i);
%             aux1 = aux1 +1;
%         end
%     end
%     for i = 1:length(v)
%         v(i)
%         position = find(state==v(i));
%         GraphicalAnimation(A,B,...                          % Maze layout
%                            [x(position(1)) y(position(1))],fi(position(1)),...         % Robot location
%                            IPoint_IR_sensor(:,:,position(1)),t)  % IR sensor data
%         pause
%     end
        if(y(kk-1)>=0.76)
                disp('Terminal');
%                 H1 = figure(1); clf, hold on, set(H1,'Color',[1 1 1])
            if(ii==1)
                for jj=1:kk
                        error_p1x(jj) = abs(0.6-x(jj));
                        error_o1x(jj) = abs(fi(jj)-pi/2)*180/pi;
                end
            elseif(ii==3)
                for jj=1:kk
                    error_p3x(jj) = abs(0.6-x(jj));
                    error_o3x(jj) = abs(fi(jj)-pi/2)*180/pi;
                end
            end  
                
                x(kk) = (randi(10)+55)*0.01;
                y(kk) = 0.4;
                fi(kk)=pi/2;
        end
end

annealing=state_appearance;

Ts = 3e-2;
Texe = 10;
t = 0:Ts:Texe;
ANG_vel = 10; 
DLevels = [0.055 0.125 0.185 0.255]
%MOTION_type = 0;    % 0: left-rotation about itself
                    % 1: right-rotation about itself
                    % 2: moving forward along an oscillatory path
%[t,w_R,w_L] = WheelMotionGenerator(Ts,Texe,ANG_vel,MOTION_type);  
DIM = length(t);
disp('Simulating the robot motion and the ID sensor data acquisition...')
%##########################################################################
% SIMULATE THE ROBOT'S MOTION
%##########################################################################
% Robot localization (x,y,fi), "ground-truth" --> pre-allocation for speed
% Sensor distance & intersection point --> pre-allocation for speed
NUM_IR_sensors = length(IR_sensor_ori);
Dist_IR_sensor = ones(NUM_IR_sensors,DIM);
IPoint_IR_sensor = zeros(NUM_IR_sensors,2,DIM);
sensor_Levels=zeros(NUM_IR_sensors,DIM);
nLevels = 4;
episodes = 300;
state = zeros(episodes,DIM);
nStates = nLevels^size(sensor_Levels,1);
nActions = 9;
QTable = zeros(nStates,nActions);
reward = zeros(1,DIM);
alpha = 0.1;
gamma = 0.9;
currentState = 0;
newState = 0;
Action_Taken= zeros(1,DIM);
Action_Taken(1)=1;
x = zeros(1,DIM);
y = zeros(1,DIM);
fi = zeros(1,DIM);

x(1) = 0.6; y(1) = 0.4; fi(1) = (pi)/2; % Initial robot's configuration

% Simulate the robot's motion and the ID sensor data acquisition
Ts=3e-2;
Texe=3e-2;

state_appearance = zeros(1,episodes);
counts = 0;
                      
rewardSum = zeros(1,episodes);
iterations = zeros(1,episodes);

for ii = 1:episodes
    kk = 1;
    x(kk) = (randi(10)+55)*0.01;
    y(kk) = 0.4;
    fi(kk)=pi/2;
    [Dist_IR_sensor(:,kk),IPoint_IR_sensor(:,:,kk)] = ...
     IR_SensorData(A,B,x(kk),y(kk),fi(kk),IR_sensor_ori);
     sensor_Levels(:,kk)= getSensorLevels(Dist_IR_sensor(:,kk),DLevels);
     state(ii,kk)=get_State(sensor_Levels(:,kk));
     newState = state(ii,kk);
    x(2)=x(1);
    y(2)=y(1);
    fi(2)=fi(1);
    [Dist_IR_sensor(:,2),IPoint_IR_sensor(:,:,2)] = ...
     IR_SensorData(A,B,x(2),y(2),fi(2),IR_sensor_ori);
     sensor_Levels(:,2)= getSensorLevels(Dist_IR_sensor(:,2),DLevels);
     state(ii,2)=get_State(sensor_Levels(:,2));
     newState = state(ii,2);
    rewardSum(ii) = 0;
    iterations(ii) = 0;
    kk=kk+1;
    while(y(kk-1) <= 0.76)
         %disp(x(kk));
         kk = kk+1;
         if kk ~= 2,
               
               if (Dist_IR_sensor(1,kk-1)<=0.09)||((Dist_IR_sensor(3,kk-1)<=0.09)&&(Dist_IR_sensor(2,kk-1)<=0.09))||((Dist_IR_sensor(5,kk-1)<=0.09)&&(Dist_IR_sensor(6,kk-1)<=0.09))||(Dist_IR_sensor(3,kk-1)<=0.09)||(Dist_IR_sensor(5,kk-1)<=0.09)||(Dist_IR_sensor(6,kk-1)<=0.09)||(Dist_IR_sensor(2,kk-1)<=0.09)
                   disp('Back');
 
                       x(kk) = (randi(10)+55)*0.01;
                       y(kk) = 0.4;
                       fi(kk) = pi/2;
                       

               elseif(Dist_IR_sensor(1,kk-1)<=0.06)||(Dist_IR_sensor(2,kk-1)<=0.06)||(Dist_IR_sensor(3,kk-1)<=0.06)||(Dist_IR_sensor(4,kk-1)<=0.06)||(Dist_IR_sensor(5,kk-1)<=0.06)||(Dist_IR_sensor(6,kk-1)<=0.06)
                   disp("xxx");
                       x(kk) = (randi(10)+55)*0.01;
                       y(kk) = 0.4;
                       fi(kk) = pi/2;
                       
               elseif ((Dist_IR_sensor(4,kk-1)<=0.09)&&(Dist_IR_sensor(3,kk-1)<=0.09))||((Dist_IR_sensor(4,kk-1)<=0.09)&&(Dist_IR_sensor(5,kk-1)<=0.09))
                   disp("Frontwards")
                    x(kk) = (randi(10)+55)*0.01;
                    y(kk) = 0.4;
                    fi(kk) = pi/2;
                    
               else
                   
                   currentState = newState;
                   z = rand();
                   %if z <= (1/log((ii+1)^5))
                   if z <= 0.1 
                       MOTION_type = randi(9);
                       %disp("random action")
                   else
                       [aux,ind]=max(QTable(currentState,:));
                       MOTION_type = ind;
                   end
                   Action_Taken(1,kk)=MOTION_type;
                   [t,w_R,w_L] = WheelMotionGenerator(Ts,Texe,ANG_vel,MOTION_type); 
                   % Update the robot's localization (numerical integration)
                   x(kk) = x(kk-1)+R_wheels*cos(fi(kk-1))*Ts*(w_R(1)+w_L(1))/2;
                   y(kk) = y(kk-1)+R_wheels*sin(fi(kk-1))*Ts*(w_R(1)+w_L(1))/2;
                   fi(kk) = fi(kk-1)+R_wheels*Ts*(w_R(1)-w_L(1))/L_wheels;
                   if fi(kk) > pi, fi(kk) = fi(kk)-2*pi; end
                   if fi(kk) < -pi, fi(kk) = fi(kk)+2*pi; end

                   [Dist_IR_sensor(:,kk),IPoint_IR_sensor(:,:,kk)] = ...
                   IR_SensorData(A,B,x(kk),y(kk),fi(kk),IR_sensor_ori);
                   sensor_Levels(:,kk)= getSensorLevels(Dist_IR_sensor(:,kk),DLevels);
                   state(ii,kk)=get_State(sensor_Levels(:,kk));
                   newState = state(ii,kk);
                   reward(1,kk) = rewardFunction(newState,MOTION_type);
                   QTable(currentState,MOTION_type) = QTable(currentState,MOTION_type) + alpha*(reward(1,kk) + (gamma*max(QTable(newState,:)))-QTable(currentState,MOTION_type));    
               end
               
        end
        
        
       
        % Read IR sensor data
     [Dist_IR_sensor(:,kk),IPoint_IR_sensor(:,:,kk)] = ...
     IR_SensorData(A,B,x(kk),y(kk),fi(kk),IR_sensor_ori);
     sensor_Levels(:,kk)= getSensorLevels(Dist_IR_sensor(:,kk),DLevels);
     state(ii,kk)=get_State(sensor_Levels(:,kk));
     newState = state(ii,kk);
     
     new_appearance = find(state==newState);
     auxiliar = prod(size(new_appearance));
     if auxiliar==1
        state_appearance(1,ii)=counts+1;
        counts=counts+1;   
     else
         if ii > 1
           state_appearance(1,ii)=counts;
         end
     end
     
     rewardSum(ii) = rewardSum(ii) + reward(1,kk);
     iterations(ii) = iterations(ii)+1;
    end
    

        if(y(kk-1)>=0.76)
                disp('Terminal');
%                 H1 = figure(1); clf, hold on, set(H1,'Color',[1 1 1])
            if(ii==1)
                for jj=1:kk
                        error_p1x(jj) = abs(0.6-x(jj));
                        error_o1x(jj) = abs(fi(jj)-pi/2)*180/pi;
                end
            elseif(ii==100)
                for jj=1:kk
                    error_p3x(jj) = abs(0.6-x(jj));
                    error_o3x(jj) = abs(fi(jj)-pi/2)*180/pi;
                end
            end    
                x(kk) = (randi(10)+55)*0.01;
                y(kk) = 0.4;
                fi(kk)=pi/2;
        end
end

%%

x(1) = 0.6; y(1) = 0.4; fi(1) = pi/2; % Initial robot's configuration
 
for kk = 1:70,
    
    if kk ~= 1,
       
               currentState = newState;
               [aux,ind]=max(QTable(currentState,:));
               MOTION_type = ind;
               Action_Taken(1,kk)=MOTION_type;
               [t,w_R,w_L] = WheelMotionGenerator(Ts,Texe,ANG_vel,MOTION_type); 
               % Update the robot's localization (numerical integration)
               x(kk) = x(kk-1)+R_wheels*cos(fi(kk-1))*Ts*(w_R(1)+w_L(1))/2;
               y(kk) = y(kk-1)+R_wheels*sin(fi(kk-1))*Ts*(w_R(1)+w_L(1))/2;
               fi(kk) = fi(kk-1)+R_wheels*Ts*(w_R(1)-w_L(1))/L_wheels;
               if fi(kk) > pi, fi(kk) = fi(kk)-2*pi; end
               if fi(kk) < -pi, fi(kk) = fi(kk)+2*pi; end
    end
    % Read IR sensor data
    [Dist_IR_sensor(:,kk),IPoint_IR_sensor(:,:,kk)] = ...
    IR_SensorData(A,B,x(kk),y(kk),fi(kk),IR_sensor_ori);
    sensor_Levels(:,kk)= getSensorLevels(Dist_IR_sensor(:,kk),DLevels);
    %reward(1,kk)=RewardFunction(sensor_Levels(:,kk),MOTION_type);
    state(1,kk)=get_State(sensor_Levels(:,kk));
    newState = state(1,kk);
    
end

for jj=1:kk
    error_pfinal(jj) = abs(0.6-x(jj));
    error_ofinal(jj) = abs(fi(jj)-pi/2)*180/pi;
end
disp('Press a key to start the graphical animation...')
%##########################################################################
% GRAPHICAL ANIMATION OF THE ROBOT'S MOTION
%##########################################################################
for kk = 1:10:70,
    GraphicalAnimation(A,B,...                          % Maze layout
                       [x(kk) y(kk)],fi(kk),...         % Robot location
                       IPoint_IR_sensor(:,:,kk),t)  % IR sensor data
    if kk == 1, pause, else pause(0.001), end    
end
hold on
H1=plot(x(1:kk),y(1:kk))
set(H1,'LineWidth',2)
%%

hf=figure
hf=colordef(hf,'white');
hf.Color='w';
plot(1:300,state_appearance(1:300)),hold on
plot(1:300,annealing), legend('e-greedy','annealing e-greedy','FontSize',16), xlabel('Nº of episodes','FontSize',20), ylabel('Number of states visited','FontSize',20),axis([0 305 0 140])
set(gca,'FontSize',20)
%%

hf=figure
hf=colordef(hf,'white');
hf.Color='w';
plot(1:length(error_p1x),error_p1x,'b.-'),hold on
plot(1:length(error_p3),error_p3,'r.-'),hold on
plot(1:35,error_pfinal(1:35),'g.-'),legend('1º episode','100º episode', 'After learning','FontSize',16),xlabel('Number of iterations','FontSize',20),ylabel('Error of the position of the robot (m)','FontSize',20)
set(gca,'FontSize',20)

hf=figure
hf=colordef(hf,'white');
hf.Color='w';
plot(1:length(error_p1x),error_o1x,'b.-'),hold on
plot(1:length(error_o3),error_o3,'r.-'),hold on
plot(1:35,error_ofinal(1:35),'g.-'),legend('1º episode','100º episode', 'After learning','FontSize',16),xlabel('Number of iterations','FontSize',20),ylabel('Error of the orientation of the robot (º)','FontSize',20)
set(gca,'FontSize',20)


%%
disp('Press a key to start the graphical animation...')
%##########################################################################
% GRAPHICAL ANIMATION OF THE ROBOT'S MOTION
%##########################################################################
for kk = 1:10:600
    GraphicalAnimation(A,B,...                          % Maze layout
                       [x(kk) y(kk)],fi(kk),...         % Robot location
                       IPoint_IR_sensor(:,:,kk),t)  % IR sensor data
    if kk == 1, pause, else pause(0.001), end    
end

%%
x(1) = 0.5984; y(1) = 0.3956; fi(1) = pi/2; % Initial robot's configuration
A = [0 0; 0 1.2; 0 0; 1.2 0; 0 0.45; 0.45 0; 0 0.75; 0.45 0.75; 0.75 0; 0.75 0.45; 0.75 0.75; 0.75 0.75;0.45 0.45]; 
B = [0 1.2; 1.2 1.2; 1.2 0; 1.2 1.2; 0.45 0.45; 0.45 0.45; 0.45 0.75; 0.45 1.2; 0.75 0.45; 1.2 0.45; 1.2 0.75; 0.75 1.2;0.45 0.75];
  
for kk = 1:600,
    
    if kk ~= 1,
       
               currentState = newState;
               [aux,ind]=max(QTable(currentState,:));
               MOTION_type = ind;
               Action_Taken(1,kk)=MOTION_type;
               [t,w_R,w_L] = WheelMotionGenerator(Ts,Texe,ANG_vel,MOTION_type); 
               % Update the robot's localization (numerical integration)
               x(kk) = x(kk-1)+R_wheels*cos(fi(kk-1))*Ts*(w_R(1)+w_L(1))/2;
               y(kk) = y(kk-1)+R_wheels*sin(fi(kk-1))*Ts*(w_R(1)+w_L(1))/2;
               fi(kk) = fi(kk-1)+R_wheels*Ts*(w_R(1)-w_L(1))/L_wheels;
               if fi(kk) > pi, fi(kk) = fi(kk)-2*pi; end
               if fi(kk) < -pi, fi(kk) = fi(kk)+2*pi; end
    end
    % Read IR sensor data
    [Dist_IR_sensor(:,kk),IPoint_IR_sensor(:,:,kk)] = ...
    IR_SensorData(A,B,x(kk),y(kk),fi(kk),IR_sensor_ori);
    sensor_Levels(:,kk)= getSensorLevels(Dist_IR_sensor(:,kk),DLevels);
    %reward(1,kk)=RewardFunction(sensor_Levels(:,kk),MOTION_type);
    state(1,kk)=get_State(sensor_Levels(:,kk));
    newState = state(1,kk);
    
end

disp('Press a key to start the graphical animation...')
%##########################################################################
% GRAPHICAL ANIMATION OF THE ROBOT'S MOTION
%##########################################################################
for kk = 1:10:600,
    GraphicalAnimation(A,B,...                          % Maze layout
                       [x(kk) y(kk)],fi(kk),...         % Robot location
                       IPoint_IR_sensor(:,:,kk),t)  % IR sensor data
    if kk == 1, pause, else pause(0.001), end    
end
hold on
H1=plot(x(1:kk),y(1:kk))
set(H1,'LineWidth',2)

figure
stem((1:kk),state(1,1:kk),'r.')
xlabel('Iterations'), ylabel('Discrete state')
str = sprintf('Number of State')
title(str), %axis([5 6 0.0 4])
%%
%##########################################################################
% ANALYZE THE DISTANCE MEASURES
%##########################################################################
% Discrete states

IL_1 = Dist_IR_sensor >= DLevels(1) & Dist_IR_sensor < DLevels(2);
IL_2 = Dist_IR_sensor >= DLevels(2) & Dist_IR_sensor < DLevels(3);
IL_3 = Dist_IR_sensor >= DLevels(3) & Dist_IR_sensor < DLevels(4);
IL_4 = Dist_IR_sensor >= DLevels(4);
L_s1234 = zeros(size(Dist_IR_sensor));
L_s1234(IL_1) = 1;
L_s1234(IL_2) = 2;
L_s1234(IL_3) = 3;
L_s1234(IL_4) = 4;

Texe=5;
t=0:Ts:Texe;
figure
for kk = 1:NUM_IR_sensors,
    subplot(NUM_IR_sensors/2,2,kk)
    stem(t,L_s1234(kk,:),'r.')
    xlabel('Time (s)'), ylabel('Discrete state')
    str = sprintf('IR-sensor%d: %0.3g deg',kk,IR_sensor_ori(kk)*180/pi);
    title(str), %axis([5 6 0.0 4])
end
%#######################################################################END


%% Analyze the ambiguity of the states


clear all
close all
clc
%##########################################################################
% PARAMETER INITIALIZATION SCRIPT
%##########################################################################
IniParam        % Script 
%##########################################################################
% GENERATE THE ANGULAR VELOCITIES @WHEELS
%##########################################################################
% Virtual time
Ts = 3e-3;
Texe = 1.65;
t = 0:Ts:Texe;
ANG_vel = 10;
DLevels = [0.055 0.12 0.18 0.25]
%MOTION_type = 0;    % 0: left-rotation about itself
                    % 1: right-rotation about itself
                    % 2: moving forward along an oscillatory path
%[t,w_R,w_L] = WheelMotionGenerator(Ts,Texe,ANG_vel,MOTION_type);  
DIM = length(t);
disp('Simulating the robot motion and the ID sensor data acquisition...')
%##########################################################################
% SIMULATE THE ROBOT'S MOTION
%##########################################################################
% Robot localization (x,y,fi), "ground-truth" --> pre-allocation for speed
% Sensor distance & intersection point --> pre-allocation for speed
NUM_IR_sensors = length(IR_sensor_ori);
Dist_IR_sensor = ones(NUM_IR_sensors,DIM);
IPoint_IR_sensor = zeros(NUM_IR_sensors,2,DIM);
sensor_Levels=zeros(NUM_IR_sensors,DIM);
nLevels = 4;
state = zeros(1,DIM);
nStates = nLevels^size(sensor_Levels,1);
currentState = 0;
newState = 0;
x = zeros(1,DIM);
y = zeros(1,DIM);
fi = zeros(1,DIM);
pos=[0.075 0.1 0.15 0.20 0.225];
for i=1:5
    x(1) = pos(i); y(1) = 0.6; fi(1) = (pi)/2; % Initial robot's configuration

    % Simulate the robot's motion and the ID sensor data acquisition
    Ts=3e-3;
    Texe=3e-3;
    MOTION_type = 11;
    for kk = 1:DIM,

        if kk ~= 1,
           [t,w_R,w_L] = WheelMotionGenerator(Ts,Texe,ANG_vel,MOTION_type); 
           % Update the robot's localization (numerical integration)
           %x(kk) = x(kk-1)+R_wheels*cos(fi(kk-1))*Ts*(w_R(kk-1)+w_L(kk-1))/2;
           %y(kk) = y(kk-1)+R_wheels*sin(fi(kk-1))*Ts*(w_R(kk-1)+w_L(kk-1))/2;
           x(kk) = x(kk-1)+R_wheels*cos(fi(kk-1))*Ts*(w_R(1)+w_L(1))/2;
           y(kk) = y(kk-1)+R_wheels*sin(fi(kk-1))*Ts*(w_R(1)+w_L(1))/2;
           fi(kk) = fi(kk-1)+R_wheels*Ts*(w_R(1)-w_L(1))/L_wheels;
           if fi(kk) > pi, fi(kk) = fi(kk)-2*pi; end
           if fi(kk) < -pi, fi(kk) = fi(kk)+2*pi; end
        end
           [Dist_IR_sensor(:,kk),IPoint_IR_sensor(:,:,kk)] = ...
           IR_SensorData(A,B,x(kk),y(kk),fi(kk),IR_sensor_ori);
           sensor_Levels(:,kk)= getSensorLevels(Dist_IR_sensor(:,kk),DLevels);
           state(1,kk)=get_State(sensor_Levels(:,kk));
           newState = state(1,kk);
    end

    %##########################################################################
    % ANALYZE THE DISTANCE MEASURES
    %##########################################################################
    % Discrete states

    IL_1 = Dist_IR_sensor >= DLevels(1) & Dist_IR_sensor < DLevels(2);
    IL_2 = Dist_IR_sensor >= DLevels(2) & Dist_IR_sensor < DLevels(3);
    IL_3 = Dist_IR_sensor >= DLevels(3) & Dist_IR_sensor < DLevels(4);
    IL_4 = Dist_IR_sensor >= DLevels(4);
    L_s1234 = zeros(size(Dist_IR_sensor));
    L_s1234(IL_1) = 1;
    L_s1234(IL_2) = 2;
    L_s1234(IL_3) = 3;
    L_s1234(IL_4) = 4;

    Texe=1.65;
    t=0:Ts:Texe;
    figure
    for zz = 1:NUM_IR_sensors,
        subplot(NUM_IR_sensors/2,2,zz)
        stem(fi,L_s1234(zz,:),'r.')
        xlabel('Orientation (fi)'), ylabel('Discrete state')
        str = sprintf('IR-sensor%d: %0.3g deg with x = %f',kk,IR_sensor_ori(zz)*180/pi, x(1));
        title(str), %axis([5 6 0.0 4])
    end

    figure
    stem((1:kk),state(1,1:kk),'r.')
    xlabel('Iteration'), ylabel('Discrete state')
    str = sprintf('Number of State with x = %f',x(1));
    title(str), %axis([5 6 0.0 4])
end
%%
for kk = 1:10: 1438,
    GraphicalAnimation(A,B,...                          % Maze layout
                       [x(kk) y(kk)],fi(kk),...         % Robot location
                       IPoint_IR_sensor(:,:,kk),t)  % IR sensor data
    if kk == 1, pause, else pause(0.001), end    
end

%%
%##########################################################################
% ANALYZE THE DISTANCE MEASURES
%##########################################################################
% Discrete states

IL_1 = Dist_IR_sensor >= DLevels(1) & Dist_IR_sensor < DLevels(2);
IL_2 = Dist_IR_sensor >= DLevels(2) & Dist_IR_sensor < DLevels(3);
IL_3 = Dist_IR_sensor >= DLevels(3) & Dist_IR_sensor < DLevels(4);
IL_4 = Dist_IR_sensor >= DLevels(4);
L_s1234 = zeros(size(Dist_IR_sensor));
L_s1234(IL_1) = 1;
L_s1234(IL_2) = 2;
L_s1234(IL_3) = 3;
L_s1234(IL_4) = 4;

Texe=1.65;
t=0:Ts:Texe;
figure
for kk = 1:NUM_IR_sensors,
    subplot(NUM_IR_sensors/2,2,kk)
    stem(t,L_s1234(kk,:),'r.')
    xlabel('Time (s)'), ylabel('Discrete state')
    str = sprintf('IR-sensor%d: %0.3g deg',kk,IR_sensor_ori(kk)*180/pi);
    title(str), %axis([5 6 0.0 4])
end

figure
stem((1:kk),state(1,1:kk),'r.')
xlabel('Time (s)'), ylabel('Discrete state')
str = sprintf('Number of State')
title(str), %axis([5 6 0.0 4])

