%##########################################################################
% 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 = 2200;
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;
state = zeros(1,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;
state_appearance = zeros(1,DIM);
counts = 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.15; y(1) = 0.3; 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;

% QLearning process
for kk = 1:DIM,
    
    if kk ~= 1,
       if kk == 50000
           kk
           x(kk) = (randi(10)+10)*0.01;
           y(kk) = 0.3;
           fi(kk) = -pi/2;
       elseif kk == 100000
           kk
           x(kk) = 0.15;
           y(kk) = 0.3;
           fi(kk) = -pi/2;
       elseif kk == 150000
           kk
           x(kk) = 0.15;
           y(kk) = 0.9;
           fi(kk) = pi/3;
       elseif kk == 200000
           kk
           x(kk) = 0.15;
           y(kk) = 0.9;
           fi(kk) = 2*pi/3;
       elseif kk == 250000
           kk
           x(kk) = 0.15;
           y(kk) = 0.3;
           fi(kk) = 3*pi/2;
       elseif kk == 300000
           kk
           x(kk) = 0.15;
           y(kk) = 0.3;
           fi(kk) = -pi/3;
       elseif kk == 350000
           kk
           x(kk) = 0.1;
           y(kk) = 0.9;
           fi(kk) = pi/2;
       elseif kk == 400000
           kk
           x(kk) = 0.1;
           y(kk) = 0.9;
           fi(kk) = pi/3;
       elseif kk == 450000
           kk
           x(kk) = 0.1;
           y(kk) = 0.3;
           fi(kk) = -pi/2;
       elseif kk == 500000
           kk
           x(kk) = 0.15;
           y(kk) = 0.3;
           fi(kk) = -pi/3;
       elseif kk == 550000
           kk
           x(kk) = 0.2;
           y(kk) = 0.9;
           fi(kk) = pi/2;
       elseif kk == 600000
           kk
           x(kk) = 0.2;
           y(kk) = 0.9;
           fi(kk) = 2*pi/3;
       elseif kk == 650000
           kk
           x(kk) = 0.2;
           y(kk) = 0.3;
           fi(kk) = 3*pi/2;
       elseif kk == 700000
           kk
           x(kk) = 0.2;
           y(kk) = 0.3;
           fi(kk) = -pi/2;
       else
           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("Backwards")
               if kk <= 100000
                   x(kk) = (randi(10)+10)*0.01;
                   y(kk) = 0.3;
                   fi(kk) = -pi/2;
               elseif kk <= 150000 && kk > 100000
                   x(kk) = 0.15;
                   y(kk) = 0.3;
                   fi(kk) = -pi/2;
               elseif kk <= 200000 && kk > 150000
                   x(kk) = 0.15;
                   y(kk) = 0.9;
                   fi(kk) = pi/3;
               elseif kk <= 250000 && kk > 200000
                   x(kk) = 0.15;
                   y(kk) = 0.9;
                   fi(kk) = 2*pi/3;
               elseif kk <= 300000 && kk > 250000
                   x(kk) = 0.15;
                   y(kk) = 0.3;
                   fi(kk) = 3*pi/2;
               elseif kk <= 350000 && kk > 300000
                   x(kk) = 0.15;
                   y(kk) = 0.3;
                   fi(kk) = -pi/3;
               elseif kk <= 400000 && kk > 450000
                   x(kk) = 0.1;
                   y(kk) = 0.9;
                   fi(kk) = pi/2;
               elseif kk <= 450000 && kk > 400000
                   x(kk) = 0.1;
                   y(kk) = 0.9;
                   fi(kk) = pi/3;
               elseif kk <= 500000 && kk > 450000
                   x(kk) = 0.1;
                   y(kk) = 0.3;
                   fi(kk) = -pi/2;
               elseif kk <= 550000 && kk > 500000
                   x(kk) = 0.1;
                   y(kk) = 0.3;
                   fi(kk) = -pi/3;
               elseif kk <= 600000 && kk > 550000
                   x(kk) = 0.2;
                   y(kk) = 0.9;
                   fi(kk) = pi/2;
               elseif kk <= 650000 && kk > 600000
                   x(kk) = 0.2;
                   y(kk) = 0.9;
                   fi(kk) = 2*pi/3;
               elseif kk <= 700000 && kk > 650000
                   x(kk) = 0.2;
                   y(kk) = 0.3;
                   fi(kk) = 3*pi/2;
               else
                   x(kk) = 0.2;
                   y(kk) = 0.3;
                   fi(kk) = -pi/2;
               end
           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");
               if kk <= 100000
                   x(kk) = (randi(10)+10)*0.01;
                   y(kk) = 0.9;
                   fi(kk) = pi/2;
               elseif kk <= 150000 && kk > 100000
                   x(kk) = 0.15;
                   y(kk) = 0.3;
                   fi(kk) = -pi/2;
               elseif kk <= 200000 && kk > 150000
                   x(kk) = 0.15;
                   y(kk) = 0.9;
                   fi(kk) = pi/3;
               elseif kk <= 250000 && kk > 200000
                   x(kk) = 0.15;
                   y(kk) = 0.9;
                   fi(kk) = 2*pi/3;
               elseif kk <= 300000 && kk > 250000
                   x(kk) = 0.15;
                   y(kk) = 0.3;
                   fi(kk) = 3*pi/2;
               elseif kk <= 350000 && kk > 300000
                   x(kk) = 0.15;
                   y(kk) = 0.3;
                   fi(kk) = -pi/3;
               elseif kk <= 400000 && kk > 450000
                   x(kk) = 0.1;
                   y(kk) = 0.9;
                   fi(kk) = pi/2;
               elseif kk <= 450000 && kk > 400000
                   x(kk) = 0.1;
                   y(kk) = 0.9;
                   fi(kk) = pi/3;
               elseif kk <= 500000 && kk > 450000
                   x(kk) = 0.1;
                   y(kk) = 0.3;
                   fi(kk) = -pi/2;
               elseif kk <= 550000 && kk > 500000
                   x(kk) = 0.1;
                   y(kk) = 0.3;
                   fi(kk) = -pi/3;
               elseif kk <= 600000 && kk > 550000
                   x(kk) = 0.2;
                   y(kk) = 0.9;
                   fi(kk) = pi/2;
               elseif kk <= 650000 && kk > 600000
                   x(kk) = 0.2;
                   y(kk) = 0.9;
                   fi(kk) = 2*pi/3;
               elseif kk <= 700000 && kk > 650000
                   x(kk) = 0.2;
                   y(kk) = 0.3;
                   fi(kk) = 3*pi/2;
               else
                   x(kk) = 0.2;
                   y(kk) = 0.3;
                   fi(kk) = -pi/2;
               end
           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")
               if kk <= 100000
                   x(kk) = (randi(10)+10)*0.01;
                   y(kk) = 0.9;
                   fi(kk) = pi/2;
               elseif kk <= 150000 && kk > 100000
                   x(kk) = 0.15;
                   y(kk) = 0.3;
                   fi(kk) = -pi/2;
               elseif kk <= 200000 && kk > 150000
                   x(kk) = 0.15;
                   y(kk) = 0.9;
                   fi(kk) = pi/3;
               elseif kk <= 250000 && kk > 200000
                   x(kk) = 0.15;
                   y(kk) = 0.9;
                   fi(kk) = 2*pi/3;
               elseif kk <= 300000 && kk > 250000
                   x(kk) = 0.15;
                   y(kk) = 0.3;
                   fi(kk) = 3*pi/2;
               elseif kk <= 350000 && kk > 300000
                   x(kk) = 0.15;
                   y(kk) = 0.3;
                   fi(kk) = -pi/3;
               elseif kk <= 400000 && kk > 450000
                   x(kk) = 0.1;
                   y(kk) = 0.9;
                   fi(kk) = pi/2;
               elseif kk <= 450000 && kk > 400000
                   x(kk) = 0.1;
                   y(kk) = 0.9;
                   fi(kk) = pi/3;
               elseif kk <= 500000 && kk > 450000
                   x(kk) = 0.1;
                   y(kk) = 0.3;
                   fi(kk) = -pi/2;
               elseif kk <= 550000 && kk > 500000
                   x(kk) = 0.1;
                   y(kk) = 0.3;
                   fi(kk) = -pi/3;
               elseif kk <= 600000 && kk > 550000
                   x(kk) = 0.2;
                   y(kk) = 0.9;
                   fi(kk) = pi/2;
               elseif kk <= 650000 && kk > 600000
                   x(kk) = 0.2;
                   y(kk) = 0.9;
                   fi(kk) = 2*pi/3;
               elseif kk <= 700000 && kk > 650000
                   x(kk) = 0.2;
                   y(kk) = 0.3;
                   fi(kk) = 3*pi/2;
               else
                   x(kk) = 0.2;
                   y(kk) = 0.3;
                   fi(kk) = -pi/2;
                end
           elseif (Dist_IR_sensor(1,kk-1)>=1.2)||(Dist_IR_sensor(2,kk-1)>=1.2)||(Dist_IR_sensor(3,kk-1)>=1.2)||(Dist_IR_sensor(4,kk-1)>=1.2)||(Dist_IR_sensor(5,kk-1)>=1.2)||(Dist_IR_sensor(6,kk-1)>=1.2)
               disp('red')
               if kk <= 100000
                   x(kk) = (randi(10)+10)*0.01;
                   y(kk) = 0.9;
                   fi(kk) = pi/2;
               elseif kk <= 150000 && kk > 100000
                   x(kk) = 0.15;
                   y(kk) = 0.3;
                   fi(kk) = -pi/2;
               elseif kk <= 200000 && kk > 150000
                   x(kk) = 0.15;
                   y(kk) = 0.9;
                   fi(kk) = pi/3;
               elseif kk <= 250000 && kk > 200000
                   x(kk) = 0.15;
                   y(kk) = 0.9;
                   fi(kk) = 2*pi/3;
               elseif kk <= 300000 && kk > 250000
                   x(kk) = 0.15;
                   y(kk) = 0.3;
                   fi(kk) = 3*pi/2;
               elseif kk <= 350000 && kk > 300000
                   x(kk) = 0.15;
                   y(kk) = 0.3;
                   fi(kk) = -pi/3;
               elseif kk <= 400000 && kk > 450000
                   x(kk) = 0.1;
                   y(kk) = 0.9;
                   fi(kk) = pi/2;
               elseif kk <= 450000 && kk > 400000
                   x(kk) = 0.1;
                   y(kk) = 0.9;
                   fi(kk) = pi/3;
               elseif kk <= 500000 && kk > 450000
                   x(kk) = 0.1;
                   y(kk) = 0.3;
                   fi(kk) = -pi/2;
               elseif kk <= 550000 && kk > 500000
                   x(kk) = 0.1;
                   y(kk) = 0.3;
                   fi(kk) = -pi/3;
               elseif kk <= 600000 && kk > 550000
                   x(kk) = 0.2;
                   y(kk) = 0.9;
                   fi(kk) = pi/2;
               elseif kk <= 650000 && kk > 600000
                   x(kk) = 0.2;
                   y(kk) = 0.9;
                   fi(kk) = 2*pi/3;
               elseif kk <= 700000 && kk > 650000
                   x(kk) = 0.2;
                   y(kk) = 0.3;
                   fi(kk) = 3*pi/2;
               else
                   x(kk) = 0.2;
                   y(kk) = 0.3;
                   fi(kk) = -pi/2;
                end   
           else
               currentState = newState;
               z = rand();
               if z <= (1/log(kk))
                   MOTION_type = randi(9);
               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(1,kk)=get_State(sensor_Levels(:,kk));
               newState = state(1,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    
    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(1,kk)=get_State(sensor_Levels(:,kk));
    newState = state(1,kk);
    new_appearance = find(state==newState);
   auxiliar = prod(size(new_appearance));
   if auxiliar==1
       %disp("aux")
        state_appearance(1,kk)=counts+1;
        counts=counts+1;
   else
       state_appearance(1,kk)=state_appearance(1,kk-1);
   end
    
end
%%
hf=figure
hf=colordef(hf,'white');
hf.Color='w';
plot(1:DIM,state_appearance),hold on
plot(1:DIM,e_greedy), legend('annealing e-greedy','e-greedy','FontSize',16), xlabel('Nº of iterations','FontSize',16), ylabel('Number of states visited','FontSize',16)
set(gca,'FontSize',12)
%%
disp('Press a key to start the graphical animation...')
%##########################################################################
% GRAPHICAL ANIMATION OF THE ROBOT'S MOTION
%##########################################################################
for kk = 1:10:10,
    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
H1=plot(x(34950:34980),y(34950:34980),'r.')
set(H1,'LineWidth',2)

for jj=1:650
                    if(jj <= 630)
                        error_p1(jj) = abs(0.15-x(jj));
                        error_o1(jj) = abs(fi(jj)+pi/2)*180/pi;
                    else
                        error_p1(jj) = abs(0.15-y(jj));
                        error_o1(jj) = abs(fi(jj))*180/pi;
                    end
end

for jj=1:30
                    if(jj <= 11)
                        error_p3(jj) = abs(0.15-x(jj+34950));
                        error_o3(jj) = abs(fi(jj+34950)+pi/2)*180/pi;
                    else
                        error_p3(jj) = abs(0.15-y(jj+34950));
                        error_o3(jj) = abs(fi(jj+34950))*180/pi;
                    end
end
%%
hf=figure
hf=colordef(hf,'white');
hf.Color='w';
subplot(1,2,1),plot(1:length(error_p1),error_p1,'b.-'),hold on
plot(1:length(error_p3),error_p3,'r.-'),hold on
plot(1:length(error_pf),error_pf,'g.-'),legend('Beginning of learning','Halfway of learning', 'After learning','FontSize',16),xlabel('Number of iterations','FontSize',20),ylabel('Position Error (m) ','FontSize',20), axis([0 650 0 0.25])
set(gca,'FontSize',20)


subplot(1,2,2),plot(1:length(error_o1),error_o1,'b.-'),hold on
plot(1:length(error_o3),error_o3,'r.-'),hold on
plot(1:length(error_of),error_of,'g.-'),legend('Beginning of learning','Halfway of learning', 'After learning','FontSize',16),xlabel('Number of iterations','FontSize',20),ylabel('Orientation Error (deg)','FontSize',20), axis([0 650 0 90])
set(gca,'FontSize',20)

%% Pos learning 
x(1) = 0.15; y(1) = 0.3; fi(1) = -pi/2; % Initial robot's configuration
%QTablecorridor = load('QTableCorridor.mat');

for kk = 1:30,
    
    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:30

                    if(jj <= 20)
                        error_pf(jj) = abs(0.15-x(jj));
                        error_of(jj) = abs(fi(jj)+pi/2)*180/pi;
                    else
                        error_pf(jj) = abs(0.15-y(jj));
                        error_of(jj) = abs(fi(jj))*180/pi;
                    end

end

for kk = 1:10:30,
    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
H1=plot(x(1:30),y(1:30),'r.')
set(H1,'LineWidth',0.02)


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

%%
IniParam        % Script 
%##########################################################################
% GENERATE THE ANGULAR VELOCITIES @WHEELS
%##########################################################################
% Virtual time
Ts = 3e-3;
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;
state = zeros(1,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.51; y(1) = 0.1; 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;
episodes = 30;
A=[0 0;0 1.2; 0 0;1.2 0;0.3 0;0.3 0;0.6 0;0.3 0.6];
B=[0 1.2;1.2 1.2;1.2 0;1.2 1.2; 0.3 0.3;0.6 0;0.6 0.6;0.6 0.6];
rewardSum = zeros(1,episodes);
iterations = zeros(1,episodes);

for ii = 1:episodes
    kk = 1;
    rewardSum(ii) = 0;
    iterations(ii) = 0;
    x(kk) = (randi(20)+30)*0.01;
    y(kk) = 0.1;
    while(x(kk) >= 0.3)
         %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) = 0.45;
                       y(kk) = 0.1;
                       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) = 0.45;
                       y(kk) = 0.1;
                       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) = 0.45;
                    y(kk) = 0.1;
                    fi(kk) = pi/2;
               elseif (Dist_IR_sensor(1,kk-1)>=1.1)||(Dist_IR_sensor(2,kk-1)>=1.1)||(Dist_IR_sensor(3,kk-1)>=1.1)||(Dist_IR_sensor(4,kk-1)>=1.1)||(Dist_IR_sensor(5,kk-1)>=1.1)||(Dist_IR_sensor(6,kk-1)>=1.1)

               else
                   
                   currentState = newState;
                   z=rand();
                   if z<=0.1
                       MOTION_type = randi(9);
                   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(1,kk)=get_State(sensor_Levels(:,kk));
                   newState = state(1,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(1,kk)=get_State(sensor_Levels(:,kk));
     newState = state(1,kk);
     if(newState==3823 || newState==3840 || newState==2799 || newState==3808 || newState==3552)
         GraphicalAnimation(A,B,...                          % Maze layout
                            [x(kk) y(kk)],fi(kk),...         % Robot location
                            IPoint_IR_sensor(:,:,kk),t)  % IR sensor data
         pause
     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(x(kk)<=0.3)
                disp('Terminal');
                x(kk) = 0.45;
                y(kk) = 0.1;
                fi(kk)=pi/2;
        end
end

plot(2:episodes,rewardSum(2:30))




