%##########################################################################
% 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
DIM = 0;
Ts = 3e-3;
Texe = 0.12;
ANG_vel = 10;
MOTION_type = 4;    % 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 = 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
x = zeros(1,DIM);
y = zeros(1,DIM);
fi = zeros(1,DIM);
x(1) = 0.15; y(1) = 0.4; fi(1) = pi/2; % Initial robot's configuration
% Sensor distance & intersection point --> pre-allocation for speed
M = length(IR_sensor_ori);
Dist_IR_sensor = zeros(M,DIM);
IPoint_IR_sensor = zeros(M,2,DIM);
% Simulate the robot's motion and the ID sensor data acquisition
for kk = 1:DIM,
    if kk ~= 1,
       % 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;
       fi(kk) = fi(kk-1)+R_wheels*Ts*(w_R(kk-1)-w_L(kk-1))/L_wheels;
       if fi(kk) > pi, fi(kk) = fi(kk)-pi; end
       if fi(kk) < -pi, fi(kk) = fi(kk)+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);
end
disp('Press a key to start the graphical animation...'), pause
%##########################################################################
% GRAPHICAL ANIMATION OF THE ROBOT'S MOTION
%##########################################################################
for kk = 1:10:DIM,
    GraphicalAnimation(A,B,[x(kk) y(kk)],fi(kk),...
                       IPoint_IR_sensor(:,:,kk),t(kk))
    if kk == 1, pause, else pause(0.001), end    
end
%##########################################################################
% ANALYZE THE DISTANCE MEASURES
%##########################################################################
SENSOR_lim = [ 0.04 0.30];
SENSOR_flag = 1;    % 0: no limit sensor; 1: 40 cm max range 
%ILog = (D_s1234 < SENSOR_lim(1) | D_s1234 > SENSOR_lim(2));
%D_s1234(ILog) = NaN;
figure
%plot(t(1:500),D_s1234(1,1:500),'k', t(1:500),D_s1234(1,1:500),'r.')
subplot(6,1,1) 
plot(t,Dist_IR_sensor(1,:),'k', t,Dist_IR_sensor(1,:),'r.')
xlabel('Time (s)'), ylabel('Distance measure (m)')
title('Front IR-sensor'), axis([0 1.7 0.0 0.30])
%figure
subplot(6,1,2)
plot(t,Dist_IR_sensor(2,:),'k', t,Dist_IR_sensor(2,:),'r.')
xlabel('Time (s)'), ylabel('Distance measure (m)')
title('Front-Left IR-sensor'), axis([0 1.7 0.0 0.30])
%figure
subplot(6,1,3)
plot(t,Dist_IR_sensor(3,:),'k', t,Dist_IR_sensor(3,:),'r.')
xlabel('Time (s)'), ylabel('Distance measure (m)')
title('Left IR-sensor'), axis([0 1.7 0.0 0.30])
%figure
subplot(6,1,4)
plot(t,Dist_IR_sensor(4,:),'k', t,Dist_IR_sensor(4,:),'r.')
xlabel('Time (s)'), ylabel('Distance measure (m)')
title('Back IR-sensor'), axis([0 1.7 0.0 0.30])
%figure
subplot(6,1,5)
plot(t,Dist_IR_sensor(5,:),'k', t,Dist_IR_sensor(5,:),'r.')
xlabel('Time (s)'), ylabel('Distance measure (m)')
title('Right IR-sensor'), axis([0 1.7 0.0 0.30])
%figure
subplot(6,1,6)
plot(t,Dist_IR_sensor(6,:),'k', t,Dist_IR_sensor(6,:),'r.')
xlabel('Time (s)'), ylabel('Distance measure (m)')
title('Front-Right IR-sensor'), axis([0 1.7 0.0 0.30])
% Discretize states
DLevels = [0.05 0.12 0.18 0.25]
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;

figure, subplot(6,1,1)
stem(t,L_s1234(1,:),'r.')
xlabel('Time (s)'), ylabel('Discrete state')
title('FRONT IR-sensor'), axis([0 1.7 0.0 4])

subplot(6,1,2)
stem(t,L_s1234(2,:),'r.')
xlabel('Time (s)'), ylabel('Discrete state')
title('Front-Left IR-sensor'), axis([0 1.7 0.0 4])
%figure
subplot(6,1,3)
stem(t,L_s1234(3,:),'r.')
xlabel('Time (s)'), ylabel('Discrete level')
title('Left IR-sensor'), axis([0 1.7 0.0 4])
%figure
subplot(6,1,4)
stem(t,L_s1234(4,:),'r.')
xlabel('Time (s)'), ylabel('Discrete level')
title('Back IR-sensor'), axis([0 1.7 0.0 4])
%figure
subplot(6,1,5)
stem(t,L_s1234(5,:),'r.')
xlabel('Time (s)'), ylabel('Discrete level')
title('Right IR-sensor'), axis([0 1.7 0.0 4])
%figure
subplot(6,1,6)
stem(t,L_s1234(6,:),'r.')
xlabel('Time (s)'), ylabel('Discrete level')
title('Front-Right IR-sensor'), axis([0 1.7 0.0 4])
% Automatic level extraction
USED_levels = L_s1234(:,1);
for kk = 2:size(L_s1234,2), 
    if ~all(L_s1234(:,kk)==L_s1234(:,kk-1)), 
        USED_levels = [USED_levels L_s1234(:,kk)];
    end
    %pause
end
USED_levels
DIM = size(USED_levels,2);
% Extract the sequence of states
STATE_signature = zeros(1,DIM);
for kk = 1:DIM,
    D4 = USED_levels(1,kk);
    D3 = USED_levels(2,kk);
    D2 = USED_levels(3,kk);
    D1 = USED_levels(4,kk);
    STATE_signature(kk) = (D4-1)*4^3+(D3-1)*4^2+(D2-1)*4^1+(D1-1)*4^0+1;
end
STATE_signature
% State transition to next state
STATE = zeros(DIM,1);
NEXT_states= zeros(DIM,5);
for kk = 1:DIM-1,
    if sum(STATE_signature(kk) == STATE(1:kk-1)) > 0, 
        continue, 
    else
        STATE(kk) = STATE_signature(kk);    
    end
    if sum(STATE_signature(kk) == STATE(1:kk-1)) > 1, continue, end
    I = find(STATE_signature == STATE(kk));
    if STATE_signature(end) == STATE(kk),
       AUX_states = STATE_signature(I(1:end-1)+1);
    else
        AUX_states = STATE_signature(I+1);
    end
    for ii = 1:length(AUX_states),
        if ii > length(AUX_states), break, end
        I = find(AUX_states==AUX_states(ii));
        if length(I) > 1,
           AUX_states(I(2:end)) = [];
        end
        %NEXT_states, pause
    end
    NEXT_states(kk,1:length(AUX_states)) = sort(AUX_states);
end
ILog = STATE == 0;
STATE(ILog) = [];
NEXT_states(ILog,:) = [];
[STATE_order,I] = sort(STATE)
NEXT_states_order = NEXT_states(I,:)
% State transition from previous state

% Sub-task SIGNATURE
% SIGNATURE1 = D_s1234(1,:)+D_s1234(2,:)+D_s1234(3,:)+D_s1234(4,:);
% figure
% plot(t,SIGNATURE1,'k', t,SIGNATURE1,'r.')
% xlabel('Time (s)'), ylabel('SIGNATURE1 (m)')
% title('PTODUCT IR-sensors'), %axis([0 7 0.04 0.30])
%#######################################################################END