%##########################################################################
% 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]
A = [0 0;0 0;0.3 0;0 2];
B = [0 2;0.3 0;0.3 2;0.3 2];    
%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,1);
IPoint_IR_sensor = zeros(NUM_IR_sensors,2,1);
sensor_Levels=zeros(NUM_IR_sensors,1);
nLevels = 4;
nStates = nLevels^size(sensor_Levels,1);
nActions = 11;
rewardFunction1 = zeros(1,nStates);
rewardFunction2 = zeros(1,nStates);
rewardFunction3 = zeros(nStates,nActions);
xl = [-0.15:0.001:0.15];
fi_l = [-pi/2:pi/360:pi/2];

xg = xl + 0.15;
fi_g = fi_l + pi/2;
yg = 1;
count = 1;
state = zeros(1,length(xl)*length(fi_l));
for ii=1:length(xg)
    for jj=1:length(fi_g)
        [Dist_IR_sensor(:,1),IPoint_IR_sensor(:,:,1)] = ...
        IR_SensorData(A,B,xg(ii),yg,fi_g(jj),IR_sensor_ori);
        sensor_Levels(:,1)= getSensorLevels(Dist_IR_sensor(:,1),DLevels);
        state(1,count)=get_State(sensor_Levels(:,1));
        if(state(1,count)>0)
%             if(xl(ii)>=-0.09 && xl(ii)<=0.09)
%                 rewardFunction1(1,state(1,count)) = (-500*xl(ii).^2)+0.5;
              rewardFunction2(1,state(1,count)) =-500 *((xl(ii)^2)+0.0159*(fi_l(jj)^2))+1.5;
%                 aux = rewardFunction2(1,state(1,count));
%             else
%                 rewardFunction1(1,state(1,count)) = -10;
%             end
              rewardFunction3(state(1,count),:) =-500*((xl(ii)^2)+0.0159*(fi_l(jj)^2))+2;
              if (fi_l(jj) < pi/2 && fi_l(jj) > 0) && (xl(ii) <= -0.04)
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-20 -20 -20 -20 -20 -20 -20 -20 -19 5 -20];
              %elseif (fi_l(jj) <= pi/12 && fi_l(jj) > 0) && (xl(ii) < -0.05)
                  %rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-5 -4 -2 -1 1 -10 -15 -17 -19 -5 -10];
              elseif (fi_l(jj) <= 0 && fi_l(jj) > -pi/4) && (xl(ii) <= -0.04)
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-2 5 -20 -20 -20 -20 -20 -20 -20 -20 -20];
              elseif (fi_l(jj) <= -pi/4 && fi_l(jj) >= -pi/2) && (xl(ii) <= -0.04)
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-20 -20 -20 -20 -20 -4 -3 -2 5 -20 -20];
              elseif (fi_l(jj) <= pi/2 && fi_l(jj) > pi/6) && (xl(ii) > -0.04 && xl(ii) < 0)
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-20 -20 -20 -20 5 -20 -20 -20 -20 -10 -20];
              elseif (fi_l(jj) <= pi/6 && fi_l(jj) > 0) && (xl(ii) > -0.04 && xl(ii) < 0)
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-3 5 -2 -20 -20 -20 -20 -20 -20 -20 -20];
              elseif (fi_l(jj) <= 0 && fi_l(jj) > -pi/4) && (xl(ii) > -0.04 && xl(ii) < 0)
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [1 5 5 -20 -20 -20 -20 -20 -20 -20 -20];
              elseif (fi_l(jj) <= -pi/4 && fi_l(jj) >= -pi/2) && (xl(ii) > -0.04 && xl(ii) < 0)
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-20 -20 -20 -20 -20 -20 -20 5 5 -20 -20];
              elseif (fi_l(jj) <= pi/2 && fi_l(jj) >= pi/4) && (xl(ii) >= 0 && xl(ii) < 0.04)
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-20 -20 -3 -2 5 -20 -20 -20 -20 -20 -20];
              elseif (fi_l(jj) < pi/4 && fi_l(jj) > 0) && (xl(ii) >= 0 && xl(ii) < 0.04)
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-1 -20 -20 -20 -20 5 -20 -20 -20 -20 -5];
              elseif (fi_l(jj) < 0 && fi_l(jj) >= -pi/6) && (xl(ii) >= 0 && xl(ii) < 0.04)
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-20 -20 -20 -20 -20 4 5 -20 -20 -20 -20];
              elseif (fi_l(jj) < -pi/6 && fi_l(jj) >= -pi/2) && (xl(ii) >= 0 && xl(ii) < 0.04)
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-20 -20 -20 -20 -20 -20 -20 -20 5 -20 -20];
              elseif (fi_l(jj) <= pi/2 && fi_l(jj) >= pi/4) && (xl(ii) >= 0.04)
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-20 -20 -20 -2 5 -20 -20 -20 -20 -20 -20];
              elseif (fi_l(jj) < pi/4 && fi_l(jj) >= 0) && (xl(ii) >= 0.04)
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-2 -20 -20 -20 -20 1 -20 -20 -20 -20 -20];
              %elseif (fi_l(jj) < 0 && fi_l(jj) >= -pi/12) && (xl(ii) > 0.05)
                  %rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-5 -10 -15 -17 -19 -4 -2 -1 1 -10 -5];
              else  
                  rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-20 -20 -20 -20 -20 -20 -20 -20 -20 -20 5];
              end
%                 if(xl(ii)<=-0.04 && fi_l(jj) > 0)
%                     rewardFunction3(state(1,count),10) = rewardFunction3(state(1,count),10)+20;
%                 end
%                 if(xl(ii)>=0.04 && fi_l(jj) < 0)
%                     rewardFunction3(state(1,count),11) = rewardFunction3(state(1,count),11)+20;
%                 end
%                 
                    
        end
        count = count + 1;
    end
end

%%
IniParam        % Script 
%##########################################################################
% GENERATE THE ANGULAR VELOCITIES @WHEELS
%##########################################################################
% Virtual time
Ts = 3e-2;
Texe = 400;
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;

nStates = nLevels^size(sensor_Levels,1);
nActions = 11;
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.15; y(1) = 0.3; fi(1) = (pi)/2; % Initial robot's configuration
[Dist_IR_sensor(:,1),IPoint_IR_sensor(:,:,1)] = ...
                   IR_SensorData(A,B,x(1),y(1),fi(1),IR_sensor_ori);
% Simulate the robot's motion and the ID sensor data acquisition
Ts=3e-2;
Texe=3e-2;
episodes = 100;
state = zeros(episodes,DIM);
A = [0 0;0 0;1.2 0;0 1.2;0 0;0 1.2;0.3 0];
B = [0 1.2;1.2 0;1.2 1.2;1.2 1.2;0.3 0;0.3 1.2;0.3 1.2]; 
rewardSum = zeros(1,episodes);
iterations = zeros(1,episodes);

for ii = 1:episodes
    clear error
    %x(1) = (randi(10)+10)*0.01;
    x(1)=0.2;
    y(1) = 0.33;
    %fi(1)=(randi(314))*0.01;
    fi(1)=2;
    error(1)=abs(fi(1)-pi/2);
    [Dist_IR_sensor(:,1),IPoint_IR_sensor(:,:,1)] = ...
                   IR_SensorData(A,B,x(1),y(1),fi(1),IR_sensor_ori);
    sensor_Levels(:,1)= getSensorLevels(Dist_IR_sensor(:,1),DLevels);
    state(ii,1)=get_State(sensor_Levels(:,1));
    newState = state(ii,1);
    kk = 2;
    rewardSum(ii) = 0;
    iterations(ii) = 0;
    %x(kk) = (randi(10)+10)*0.01;
    x(2) = 0.2;
    aux=x(1);
    y(kk) = 0.33;
    %fi(kk)=(randi(314))*0.01;
    fi(kk)=2;
    error(2)=abs(fi(2)-pi/2);
    aux=fi(1); 
    [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);
    while(y(kk-1) <= 0.97 || y(kk-1) >=0.1)
        if kk >=300000
            break;
        end
         if kk ~= 1,           
               if (Dist_IR_sensor(1,kk-1)<=0.067)||((Dist_IR_sensor(3,kk-1)<=0.067)&&(Dist_IR_sensor(2,kk-1)<=0.067))||((Dist_IR_sensor(5,kk-1)<=0.067)&&(Dist_IR_sensor(6,kk-1)<=0.067))||(Dist_IR_sensor(3,kk-1)<=0.067)||(Dist_IR_sensor(5,kk-1)<=0.067)||(Dist_IR_sensor(6,kk-1)<=0.067)||(Dist_IR_sensor(2,kk-1)<=0.067)
                   %disp('Back');
                   x(kk) = x(1); 
                   y(kk) = 0.4;
                   fi(kk) = fi(1);
               elseif(Dist_IR_sensor(1,kk-1)<=0.067)||(Dist_IR_sensor(2,kk-1)<=0.067)||(Dist_IR_sensor(3,kk-1)<=0.067)||(Dist_IR_sensor(4,kk-1)<=0.067)||(Dist_IR_sensor(5,kk-1)<=0.067)||(Dist_IR_sensor(6,kk-1)<=0.067)
                   disp("xxx");
                    x(kk) = 0.15;
                       y(kk) = 0.3;
                       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.15;
                    y(kk) = 0.3;
                    fi(kk) = pi/2;
                    
               else
                   %disp('entrei')
                   currentState = newState;
                   z=rand();
                   if z<=0.1
                       MOTION_type = randi(11);
                   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;
                   %error(kk)=abs(fi(kk)-pi/2);
                   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);
                   if(newState > 0)
                       reward(1,kk) = rewardFunction3(newState,MOTION_type);
%                        if(reward(1,kk) ==0)
%                            GraphicalAnimation(A,B,...                          % Maze layout
%                             [x(kk) y(kk)],fi(kk),...         % Robot location
%                             IPoint_IR_sensor(:,:,kk),t)  % IR sensor data
%                        end
                       %reward(1,kk) = rewardFunction2(newState);
                       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(ii,kk)=get_State(sensor_Levels(:,kk));
     newState = state(ii,kk);
     rewardSum(ii) = rewardSum(ii) + reward(1,kk);
     iterations(ii) = iterations(ii)+1;
     
     kk = kk+1;
     if(y(kk-1)>=0.97 || y(kk-1)<=0.1)
        disp('Terminal');
        if ii == 1
            for jj=1:kk
                error_p1(jj) = abs(0.15-x(jj));
                error_o1(jj) = abs(fi(jj)-pi/2)*180/pi;
            end
        elseif ii == 10
            for jj=1:kk
                error_p100(jj) = abs(0.15-x(jj));
                error_o100(jj) = abs(fi(jj)-pi/2)*180/pi;
            end
        end
        %plot(1:length(error),error,'r.'),pause 
        if ii==1
            for ll = kk-10:10:kk-1,
                GraphicalAnimation(A,B,...                          % Maze layout
                                   [x(ll) y(ll)],fi(ll),...         % Robot location
                                   IPoint_IR_sensor(:,:,ll),t)  % IR sensor data
                if ll == 1,, else pause(0.001), end    
            end
            H1=plot(x(1:kk),y(1:kk),'r.')
            set(H1,'LineWidth',2),pause
        end

        break;
     end
   end
                
end
    disp('left')
%%
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_p100),error_p100,'r.-'),hold on
plot(1:80,error_p(1:80),'g.-'),legend('1º Episode','10º Episode', 'After learning','FontSize',16),xlabel('Number of iterations','FontSize',20),ylabel('Position Error (m) ','FontSize',20),axis([0 200 0 0.2])
set(gca,'FontSize',20)

subplot(1,2,2),plot(1:length(error_o1),error_o1,'b.-'),hold on
plot(1:length(error_o100),error_o100,'r.-'),hold on
plot(1:80,error_of(1:80),'g.-'),legend('1º Episode','10º Episode', 'After learning','FontSize',16),xlabel('Number of iterations','FontSize',20),ylabel('Orientation Error (deg)','FontSize',20),axis([0 200 0 130])
set(gca,'FontSize',20)
%%
figure
plot(1:DIM,state_appearance),hold on
plot(1:DIM,e_greedy), legend('annealing e-greedy','e-greedy'), xlabel('Nº of iterations'), ylabel('Number of states visited')
%%
disp('Press a key to start the graphical animation...')
%##########################################################################
% GRAPHICAL ANIMATION OF THE ROBOT'S MOTION
%##########################################################################
for kk = 1:10:54,
    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:kk),y(1:kk),'r.')
set(H1,'LineWidth',2)

%%
x(1) = 0.14; y(1) = 0.5; fi(1) =1.5; % Initial robot's configuration
%QTablecorridor = load('QTableCorridor.mat');
%A = [0 0;0 0;1.5 0;0 1.5;0 0;0 1.5;0.3 0];
%B = [0 1.5;1.5 0;1.5 1.5;1.5 1.5;0.3 0;0.3 1.5;0.3 1.5]; 
A = [0 0;0 0;0 2;2 0;0 0;0.3 0;0 2;0.3 1.7;0.9 1.7];
B = [0 2;2 0;2 2;2 2;0.3 0;0.3 1.7;0.9 2;0.9 1.7;0.9 2]; 
for kk = 1:150
    
    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;
               %error(kk) = abs(fi(kk)-pi/2);
               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 ll = 1:10:kk,
    GraphicalAnimation(A,B,...                          % Maze layout
                       [x(ll) y(ll)],fi(ll),...         % Robot location
                       IPoint_IR_sensor(:,:,ll),t)  % IR sensor data
    if ll == 1, pause, else pause(0.001), end    
end
H1=plot(x(1:kk),y(1:kk),'r.')
set(H1,'LineWidth',0.02)

for jj=1:kk
                error_p(jj) = abs(0.15-x(jj));
                error_of(jj) = abs(fi(jj)-pi/2)*180/pi;
end
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])

%%

%%
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]
A = [0 0;0 0;0 2;2 0;0 0;0.3 0;0 2;0.3 1.7;0.9 1.7];
B = [0 2;2 0;2 2;2 2;0.3 0;0.3 1.7;0.9 2;0.9 1.7;0.9 2];    
%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,1);
IPoint_IR_sensor = zeros(NUM_IR_sensors,2,1);
sensor_Levels=zeros(NUM_IR_sensors,1);
nLevels = 4;
nStates = nLevels^size(sensor_Levels,1);
nActions = 11;
%rewardFunction4 = rewardFunction3;
rewardFunction5 = rewardFunction4;
xl = [-0.094:0.005:0.094];
fi_l = [-pi/2:pi/50:3*pi/2];
yl = [0:0.005:0.204];
xg = xl + 0.15;
fi_g = fi_l + pi/2;
yg = yl + 1.7;
count = 1;
state = zeros(1,length(xl)*length(fi_l)*length(yl));
for ii=1:length(xg)
    for jj=1:length(fi_g)
        for xx=1:length(yg)
            [Dist_IR_sensor(:,1),IPoint_IR_sensor(:,:,1)] = ...
            IR_SensorData(A,B,xg(ii),yg(xx),fi_g(jj),IR_sensor_ori);
            sensor_Levels(:,1)= getSensorLevels(Dist_IR_sensor(:,1),DLevels);
            state(1,count)=get_State(sensor_Levels(:,1));
            if(state(1,count)>0)
                if(rewardFunction5(state(1,count),:) == 0)
                     if(((xl(ii) > -0.05 && xl(ii) < 0.05) && yl(xx) < 0.2) || (xl(ii) >= 0.05 && (yl(xx) > 0 && yl(xx) < 0.2)))
                         if(fi_g(jj) >= 2.0 && fi_g(jj) <= 5.5)
                             rewardFunction5(state(1,count),:) = -750*(xl(ii).^2+(yl(xx)).^2);
                             rewardFunction5(state(1,count),10) = -10;
                             rewardFunction5(state(1,count),11) = -10;
                         else
                            rewardFunction5(state(1,count),:) = -750*(xl(ii).^2+(yl(xx)-0.103).^2);
                         end
                     else
                         rewardFunction5(state(1,count),:) = -1500*(xl(ii).^2+(yl(xx).^2));
                     end
%                   rewardFunction4(state(1,count),:) =-500*((xl(ii)^2)+0.0159*(fi_l(jj)^2))+2;
%                   if (fi_l(jj) < pi/2 && fi_l(jj) > 0) && (xl(ii) <= -0.04)
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [-20 -20 -20 -20 -20 -20 -20 -20 -19 5 -20];
%                   %elseif (fi_l(jj) <= pi/12 && fi_l(jj) > 0) && (xl(ii) < -0.05)
%                       %rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-5 -4 -2 -1 1 -10 -15 -17 -19 -5 -10];
%                   elseif (fi_l(jj) <= 0 && fi_l(jj) > -pi/4) && (xl(ii) <= -0.04)
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [-2 5 -20 -20 -20 -20 -20 -20 -20 -10 -10];
%                   elseif (fi_l(jj) <= -pi/4 && fi_l(jj) >= -pi/2) && (xl(ii) <= -0.04)
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [-20 -20 -20 -20 -20 -4 -3 -2 5 -10 -10];
%                   elseif (fi_l(jj) <= pi/2 && fi_l(jj) > pi/6) && (xl(ii) > -0.04 && xl(ii) < 0)
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [-20 -20 -20 -20 5 -20 -20 -20 -20 -1 -10];
%                   elseif (fi_l(jj) <= pi/6 && fi_l(jj) > 0) && (xl(ii) > -0.04 && xl(ii) < 0)
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [-3 5 -2 -20 -20 -20 -20 -20 -20 -20 -20];
%                   elseif (fi_l(jj) <= 0 && fi_l(jj) > -pi/4) && (xl(ii) > -0.04 && xl(ii) < 0)
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [1 5 -20 -20 -20 -20 -20 -20 -20 -20 -20];
%                   elseif (fi_l(jj) <= -pi/4 && fi_l(jj) >= -pi/2) && (xl(ii) > -0.04 && xl(ii) < 0)
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [-20 -20 -20 -20 -20 -20 -20 5 5 -20 -20];
%                   elseif (fi_l(jj) <= pi/2 && fi_l(jj) >= pi/4) && (xl(ii) >= 0 && xl(ii) < 0.04)
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [-20 -20 -3 -2 5 -20 -20 -20 -20 -20 -20];
%                   elseif (fi_l(jj) < pi/4 && fi_l(jj) >= 0) && (xl(ii) >= 0 && xl(ii) < 0.04)
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [-1 -20 -20 -20 -20 5 -20 -20 -20 -20 -5];
%                   elseif (fi_l(jj) < 0 && fi_l(jj) >= -pi/6) && (xl(ii) >= 0 && xl(ii) < 0.04)
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [-20 -20 -20 -20 -20 4 5 -20 -20 -20 -20];
%                   elseif (fi_l(jj) < -pi/6 && fi_l(jj) >= -pi/2) && (xl(ii) >= 0 && xl(ii) < 0.04)
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [-20 -20 -20 -20 -20 -20 -20 -20 5 -20 -20];
%                   elseif (fi_l(jj) <= pi/2 && fi_l(jj) >= pi/4) && (xl(ii) >= 0.04)
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [-20 -20 -20 -2 5 -20 -20 -20 -20 -20 -20];
%                   elseif (fi_l(jj) < pi/4 && fi_l(jj) >= 0) && (xl(ii) >= 0.04)
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [-2 -20 -20 -20 -20 1 -20 -20 -20 -20 -20];
%                   %elseif (fi_l(jj) < 0 && fi_l(jj) >= -pi/12) && (xl(ii) > 0.05)
%                       %rewardFunction3(state(1,count),:) = rewardFunction3(state(1,count),:) + [-5 -10 -15 -17 -19 -4 -2 -1 1 -10 -5];
%                   else  
%                       rewardFunction4(state(1,count),:) = rewardFunction4(state(1,count),:) + [-20 -20 -20 -20 -20 -20 -20 -20 -20 -20 5];
%                   end

                end
            end
            count = count + 1;
        end
    end
end
%%
IniParam        % Script 
%##########################################################################
% GENERATE THE ANGULAR VELOCITIES @WHEELS
%##########################################################################
% Virtual time
Ts = 3e-2;
Texe = 20000;
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);
A = [0 0;0 0;0 2;2 0;0 0;0.3 0;0 2;0.3 1.7;0.9 1.7];
B = [0 2;2 0;2 2;2 2;0.3 0;0.3 1.7;0.9 2;0.9 1.7;0.9 2]; 
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(250,DIM);
nStates = nLevels^size(sensor_Levels,1);
nActions = 11;
QTable = load('QTable.mat');
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.35; y(1) = (randi(180)+10)*0.01; fi(1) = (pi)/2; % Initial robot's configuration
[Dist_IR_sensor(:,1),IPoint_IR_sensor(:,:,1)] = ...
                   IR_SensorData(A,B,x(1),y(1),fi(1),IR_sensor_ori);
% Simulate the robot's motion and the ID sensor data acquisition
Ts=3e-2;
Texe=3e-2;
episodes = 300;

rewardSum = zeros(1,episodes);
iterations = zeros(1,episodes);

for ii = 1:episodes
    x(1) = (randi(10)+10)*0.01;
    y(1) = 1.7;
    aux=y(1)
    fi(1)=(randi(20)+145)*0.01;
    [Dist_IR_sensor(:,1),IPoint_IR_sensor(:,:,1)] = ...
                   IR_SensorData(A,B,x(1),y(1),fi(1),IR_sensor_ori);
    sensor_Levels(:,1)= getSensorLevels(Dist_IR_sensor(:,1),DLevels);
    state(ii,1)=get_State(sensor_Levels(:,1));
    newState = state(ii,1);
    kk = 2;
    rewardSum(ii) = 0;
    iterations(ii) = 0;
    x(kk) = (randi(10)+10)*0.01;
    y(kk) = 1.7;
    aux=y(1)
    fi(kk)=(randi(20)+145)*0.01;
    [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);
    while(x(kk-1) <= 0.31 || y(kk-1) >=1.1)
        if kk >=50000
            break;
        end
         if kk ~= 1,           
               if (Dist_IR_sensor(1,kk-1)<=0.067)||((Dist_IR_sensor(3,kk-1)<=0.067)&&(Dist_IR_sensor(2,kk-1)<=0.067))||((Dist_IR_sensor(5,kk-1)<=0.067)&&(Dist_IR_sensor(6,kk-1)<=0.067))||(Dist_IR_sensor(3,kk-1)<=0.067)||(Dist_IR_sensor(5,kk-1)<=0.067)||(Dist_IR_sensor(6,kk-1)<=0.067)||(Dist_IR_sensor(2,kk-1)<=0.067)
                   %disp('Back');
                   x(kk) = x(1); 
                   y(kk) = y(1);
                   fi(kk) = fi(1);
               elseif(Dist_IR_sensor(1,kk-1)<=0.067)||(Dist_IR_sensor(2,kk-1)<=0.067)||(Dist_IR_sensor(3,kk-1)<=0.067)||(Dist_IR_sensor(4,kk-1)<=0.067)||(Dist_IR_sensor(5,kk-1)<=0.067)||(Dist_IR_sensor(6,kk-1)<=0.067)
                   disp("xxx");
                    x(kk) = x(1);
                       y(kk) = y(1);
                       fi(kk) = fi(1);
                       
               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) = x(1);
                    y(kk) = y(1);
                    fi(kk) = fi(1);
                    
               else
                   %disp('entrei')
                   currentState = newState;
                   z=rand();
                   if z<=0.1
                       MOTION_type = randi(11);
                   else
                       [aux,ind]=max(QTable.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);
                   if(newState > 0)
                       reward(1,kk) = rewardFunction5(newState,MOTION_type);
                       %reward(1,kk) = rewardFunction3(newState);
                       QTable.QTable(currentState,MOTION_type) = QTable.QTable(currentState,MOTION_type) + alpha*(reward(1,kk) + (gamma*max(QTable.QTable(newState,:)))-QTable.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(ii,kk)=get_State(sensor_Levels(:,kk));
     newState = state(ii,kk);
     
     rewardSum(ii) = rewardSum(ii) + reward(1,kk);
     iterations(ii) = iterations(ii)+1;
     
     kk = kk+1;
     if(x(kk-1)>=0.31 || y(kk-1)<=1.1)
        disp('Terminal');
%         if(mod(ii,20)==0)
%             for ll = kk-2:10:kk-1,
%                 GraphicalAnimation(A,B,...                          % Maze layout
%                                    [x(ll) y(ll)],fi(ll),...         % Robot location
%                                    IPoint_IR_sensor(:,:,ll),t)  % IR sensor data
%                 if ll == 1,, else pause(0.001), end    
%             end
%             H1=plot(x(1:kk),y(1:kk),'r.')
%             set(H1,'LineWidth',2),pause
%         end
             if(ii==1)
                for jj=1:kk
                    if(jj <= kk/6)
                        error_p1(jj) = abs(0.15-x(jj));
                        error_o1(jj) = abs(fi(jj)-pi/2)*180/pi;
                    else
                        error_p1(jj) = abs(1.85-y(jj));
                        error_o1(jj) = abs(fi(jj))*180/pi;
                    end
                end
            elseif(ii==100)
                for jj=1:kk
                    if(jj <= kk/6)
                        error_p3(jj) = abs(0.15-x(jj));
                        error_o3(jj) = abs(fi(jj)-pi/2)*180/pi;
                    else
                        error_p3(jj) = abs(1.85-y(jj));
                        error_o3(jj) = abs(fi(jj))*180/pi;
                    end
                end
            end
        break;
     end
   end
end                
    disp('left')
%%
x(1) = 0.15; y(1) = 1.7; fi(1) = pi/2; % Initial robot's configuration
 
for kk = 1:90,
    
    if kk ~= 1,
       
               currentState = newState;
               [aux,ind]=max(QTable.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
                    if(jj <= kk/6)
                        error_pfinal(jj) = abs(0.15-x(jj));
                        error_ofinal(jj) = abs(fi(jj)-pi/2)*180/pi;
                    else
                        error_pfinal(jj) = abs(1.85-y(jj));
                        error_ofinal(jj) = abs(fi(jj))*180/pi;
                    end
end
disp('Press a key to start the graphical animation...')
%##########################################################################
% GRAPHICAL ANIMATION OF THE ROBOT'S MOTION
%##########################################################################
for kk = 1:10:40,
    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';
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_ofinal),error_pfinal,'g.-'),legend('1º episode','100º episode', 'After learning','FontSize',16),xlabel('Number of iterations','FontSize',20),ylabel('Position Error (m) ','FontSize',20),axis([0 200 0 0.2])
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_ofinal),error_ofinal,'g.-'),legend('1º episode','100º episode', 'After learning','FontSize',16),xlabel('Number of iterations','FontSize',20),ylabel('Orientation Error (deg)','FontSize',20),axis([0 200 0 140])
set(gca,'FontSize',20)
