%##########################################################################
% 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-3;
Texe = 50;
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);
nActions = 9;
nCheckPoints = 7;
nOptions = 3;
%HierarchicalQTable = load('HRL_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) = .4; y(1) = 0.15; fi(1) = pi; % Initial robot's configuration

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

for kk = 1:DIM,
    if kk ~=1
    if kk > 15
        
       if ((x(kk-1)>=0.9 && x(kk-1)<=1.2) && (round(y(kk-1),2)==0.59 && round(y(kk-15),2)==0.61)) %% CheckPoint 1
           disp('Entered CheckPoint 1');
           [dum,ind]=max(HierarchicalQTable(1,:));
           if(ind == 1)
               QTable = door_front.QTable;
           elseif(ind == 2)
               QTable = door_right.QTable;
           else
               QTable = door_left.QTable;
           end
       elseif ((x(kk-1)>=1.19 && x(kk-10)<=1.21) && (y(kk-1)>=0.3 && y(kk-1)<=0.6)) %% CheckPoint 2
           disp('Entered CheckPoint 2');
           [dum,ind]=max(HierarchicalQTable(2,:));
           if(ind == 1)
               QTable = door_front.QTable;
           elseif(ind == 2)
               QTable = door_right.QTable;
           else
               QTable = door_left.QTable;
           end
       elseif ((x(kk-1)>=0.9 && x(kk-1)<=1.2) && (y(kk-10)>=0.29 && y(kk-1)<=0.31)) %% CheckPoint 3 
           disp('Entered CheckPoint 3');
           [dum,ind]=max(HierarchicalQTable(3,:));
           if(ind == 1)
               QTable = door_front.QTable;
           elseif(ind == 2)
               QTable = door_right.QTable;
           else
               QTable = door_left.QTable;
           end
       elseif ((x(kk-1)>0 && x(kk-1)<=0.3) && (round(y(kk-20),2)==0.59 && round(y(kk-1),2)==0.6)) %% CheckPoint 4
           disp('Entered CheckPoint 4');
           [dum,ind]=max(HierarchicalQTable(4,:));
           if(ind == 1)
               QTable = door_front.QTable;
           elseif(ind == 2)
               disp('right')
               QTable = door_right.QTable;
           else
               QTable = door_left.QTable;
           end 
        elseif ((x(kk-1)>=0 && x(kk-1)<=0.3)) && ((round(y(kk-1),2)==0.89) && round(y(kk-2),2)==0.90) %% CheckPoint 5
           disp('Entered CheckPoint 5')
           [dum,ind]=max(HierarchicalQTable(5,:));
           if(ind == 1)
               QTable = door_front.QTable;
           elseif(ind == 2)
               QTable = door_right.QTable;
           else
               QTable = door_left.QTable;
           end
       elseif (round(x(kk-1),2) ==0.29 && round(x(kk-2),2)==0.3 && (y(kk-1)>=0.6 && y(kk-1)<=0.9)) %% CheckPoint 6 
           x(kk-1)
           x(kk-10)
           disp('Entered CheckPoint 6');
           [dum,ind]=max(HierarchicalQTable(6,:));
           if(ind == 1)
               QTable = door_front.QTable;
           elseif(ind == 2)
               QTable = door_right.QTable;
           else
               QTable = door_left.QTable;
           end
       elseif ((x(kk-1)>=0.91 && x(kk-1)<=1.2) && (y(kk-10)>=0.59 && y(kk-1)<=0.61 && y(kk-10)< y(kk-1))) || ((x(kk-10)>=1.19 && x(kk-1)<=1.21 && x(kk-10)<x(kk-1)) && (y(kk-1)>=0.3 && y(kk-1)<=0.6)) || ((x(kk-1)>=0.9 && x(kk-1)<=1.2) && (y(kk-1)>=0.29 && y(kk-10)<=0.31) && y(kk-10)>y(kk-1)) || ((x(kk-1)>=0 && x(kk-1)<=0.3) && (y(kk-1)>=0.59 && y(kk-10)<=0.61 && y(kk-10)>y(kk-1))) || ((x(kk-1)>=0 && x(kk-1)<=0.3) && (y(kk-10)>=0.89 && y(kk-1)<=0.91 && y(kk-10)<y(kk-1))) || ((x(kk-10)>=0.29 && x(kk-1)<=0.31 && x(kk-10)<x(kk-1)) && (y(kk-1)>=0.6 && y(kk-1)<=0.9)) || ((x(kk-1)>=0.88 && x(kk-10)<=0.9 && x(kk-10)>x(kk-1)) && (y(kk-1)>=0.3 && y(kk-1)<=0.6))%% CheckPoint 1 ao contrário
           disp('Corridor')
           QTable = corridor.QTable;
               
       elseif ((x(kk-1)>=0.3 && x(kk-1)<=0.35) && (y(kk-1)>=0.3 && y(kk-1)<=0.6)) %% GoalCell
           disp('Entered the goal');
           break;
       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")
%                
%            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");
%                
%            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")
%                
%            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)
%     
            %disp('else')
       end
    end
               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

               [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

    [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

%%
disp('Press a key to start the graphical animation...')
%##########################################################################
% GRAPHICAL ANIMATION OF THE ROBOT'S MOTION
%##########################################################################
for jj =1:11:1
    GraphicalAnimation(A,B,...                          % Maze layout
                       [x(jj) y(jj)],fi(jj),...         % Robot location
                       IPoint_IR_sensor(:,:,jj),t)  % IR sensor data
    if jj == 1, pause, else pause(0.001), end    
end
H1 = plot(x(1:kk),y(1:kk),'r.')
set(H1,'LineWidth',2)
%%
x(1) = 0.1; y(1) = 0.8; fi(1) = -pi/3; % Initial robot's configuration

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

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])

%%
%##########################################################################
% 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:DIM ,
    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])

