%% Car - Simulation
%
% Assignment 2 in Fuzzy systems
% 
% author:
%   Christos Choutouridis ΑΕΜ 8997
%   cchoutou@ece.auth.gr
%
clear; clc; close all;

global Speed_mag;
global Ts;


% Configuration
% ----------------------
Speed_mag = 0.05;
Ts = 1;

% Givents
% ----------------------
corners = [5 1 ; 6 2 ; 7 3];
x_init  = 3.8;
y_init  = 0.5;
x_final = 10;
y_final = 3.2;
theta_init = [-45 0 45];


% Saturation between min: m and max: M
sat = @(x, m, M) max(m, min(M, x));

% Load control
flc = readfis('CarFLC');

t = 0:Ts:250;
for i = 1:length(theta_init)
    x = x_init;
    y = y_init;
    theta = theta_init(i);

    X = zeros(size(t));
    Y = zeros(size(t));

    End=length(t);
    for k=1:End
        [dh, dv] = wallDistance(corners, x, y);
        if isfinite(dh) && ~isfinite(dv) && (dh <=0 || dv <= 0)
            End = k-1;
            fprintf('Car crashed!! - Stop simulation');
            break;
        end
        
        dtheta = evalfis(flc, [sat(dh, 0, 1) sat(dv, 0, 1) theta]);
        [x, y, theta] = moveCar(x, y, theta, dtheta);
        X(k) = x; Y(k) = y;
        
        if x >= x_final
            End = k-1;
            break;
        end
    end
    
    figure(i); clf; hold on; grid on; axis equal;
    xlim([2 11]); ylim([-1 4]);
    xlabel('x [m]'); ylabel('y [m]');
    title( ...
        "Car movement $(\theta_0 =" + theta_init(i)+ "^{\circ})$", ...
        'Interpreter','latex', ...
        'FontSize', 16);
    
    % Walls
    fill([5 6 6 5],[0 0 1 1],[0.5 0.5 0.5],'FaceAlpha',0.4,'EdgeColor','none');
    fill([6 7 7 6],[0 0 2 2],[0.5 0.5 0.5],'FaceAlpha',0.4,'EdgeColor','none');
    fill([7 10 10 7],[0 0 3 3],[0.5 0.5 0.5],'FaceAlpha',0.4,'EdgeColor','none');
    
    plot(X(1:End), Y(1:End), '-', 'LineWidth', 1.5);
    
    hold on;
    plot(X(1),   Y(1),   'go', 'MarkerFaceColor', 'g'); % begin
    plot(X(End), Y(End), 'ro', 'MarkerFaceColor', 'r'); % end
    hold off;

    filename = sprintf('CarMovement_theta%d.png', theta_init(i));
    saveas(gcf, filename);
end