% experiment 1 Sugeno optimalizace 
% Martin Radvansky 
% Diplomova prace 2022

%Nacteni a vyhodnoceni hodnot, zobrazeni RMSE pro Sugeno FIS optimalizace
%Optimalizace se provedly na prumernem modelu ze 7 mereni
%Generovani obrazku a sestaveni zpravy z optimalizace

%vstup: v podslozce Output_100 musi existovat soubory z optimalizace a to
%fis s nalezenym optimem a info.mat s informacemi o prubehu optimalizace
%ve slozce Data lezi zdrojova csv jedno pro vzdalenost od splien a
%jedno pro otoceni volantem.

%vystup: v podslozce images budou vytvoreny obrazky a textovy soubor s
%podrobnostmi k prubehu optimalizace.
clc
clear
close all
clear global

cesta = ".\Output_100";
obrazkydir = ".\Images";
if (~exist(obrazkydir, 'dir'))
       mkdir(obrazkydir);
end
soubor_mfs = "Sugeno_";
soubor_mfs_info = "Sugeno_info_";

spline_vzdal = load('.\Data\Export_J1_Dist.csv');
spline_mean = mean(spline_vzdal);
wheel_angle = load('.\Data\Export_J1_Wheel.csv');
wheel_mean = mean(wheel_angle);

MyFolderInfo = dir(sprintf("%s\\*.fis",cesta));

%prochazime optimalizaci pravidly

RMSE_mfs = [];
Cas_Opti_mfs = [];
Pocet_Fun_mfs = [];

for i = 1:100
  %funkce prislusnosti
  model_mfs = readfis(sprintf("%s\\%s%d.fis",cesta,soubor_mfs,i));    
  tuned_info_mfs = load(sprintf("%s\\%s%d.mat",cesta,soubor_mfs_info,i));
  RMSE_mfs(end+1) = tuned_info_mfs.optimout.tuningOutputs.fval;
  Cas_Opti_mfs(end+1) = tuned_info_mfs.optimout.totalRuntime;
  Pocet_Fun_mfs(end+1) = tuned_info_mfs.optimout.totalFcnCount;
end
figure(1);
plot(RMSE_mfs);

%Zobrazeni boxplotu pro nejlepsi RMSE ze vsech populaci *******************
figure(1);
boxplot(RMSE_mfs,'Labels',{'Pravidla'});
title('Sugeno optimalizace');
ylabel('RMSE [{\circ}]');
saveas(gcf,sprintf("%s\\opti_sug_rul_bp.png",obrazkydir))

% *************************************************************************

%Nejlepsi a nejhorsi RMSE jejich index
minRMSE_mfs = min(RMSE_mfs);
maxRMSE_mfs = max(RMSE_mfs);

ix_minRMSE_mfs = find(RMSE_mfs == minRMSE_mfs);
ix_maxRMSE_mfs = find(RMSE_mfs == maxRMSE_mfs);

%Sugeno optimalizace funkci prislusnosti ********************************
%min RMSE nejlepsi
fis_opti_mfs = readfis(sprintf("%s\\%s%d.fis",cesta,soubor_mfs,ix_minRMSE_mfs(1)));  
vystup_eval = evalfis(fis_opti_mfs,spline_mean);
 plot(1:length(spline_mean),wheel_mean*900,'LineWidth',1.4); % prumer
    grid on;
    hold on;
    plot(1:length(spline_mean),vystup_eval,'LineWidth',1.4);
    title('Natočení volantu v čase, nejlepší RMSE');
    xlabel('Čas [20 ms]');
    ylabel('Úhel natočení volantu [{\circ}]');
    legend('Original','Fuzzy model','Location','southeast');
    hold off; 
saveas(gcf,sprintf("%s\\sug_best_mfs_t.png",obrazkydir));

%prevodni charakteristika
plot(spline_mean,wheel_mean*900,'LineWidth',1.4);
    grid on;
    hold on;
    plot(spline_mean,vystup_eval,'LineWidth',1.4);
    title('Závislost úhlu natočení volantu na vzdálenosti od spline');
    xlabel('Vzdálenost od spline [m]');
    ylabel('Úhel natočení volantu [{\circ}]');
    legend('Original','Fuzzy model','Location','southeast');
    hold off;
saveas(gcf,sprintf("%s\\sug_best_mfs_prev.png",obrazkydir));

%ulozime obrazky funkci prislusnosti pro nejlepsi vysledek
vykresli_mfs_sugeno(fis_opti_mfs,sprintf("%s\\sug_best_mfs_input.png",obrazkydir));

%max RMSE nejhorsi
fis_opti_mfs = readfis(sprintf("%s\\%s%d.fis",cesta,soubor_mfs,ix_maxRMSE_mfs(1)));  
vystup_eval = evalfis(fis_opti_mfs,spline_mean);
 plot(1:length(spline_mean),wheel_mean*900,'LineWidth',1.4); % prumer
    grid on;
    hold on;
    plot(1:length(spline_mean),vystup_eval,'LineWidth',1.4);
    title('Natočení volantu v čase, nejlepší RMSE');
    xlabel('Čas [Vzorek x 20 ms]');
    ylabel('Úhel natočení volantu [{\circ}]');
    legend('Original','Fuzzy model','Location','southeast');
    hold off; 
saveas(gcf,sprintf("%s\\sug_worst_mfs_t.png",obrazkydir));

%prevodni charakteristika
plot(spline_mean,wheel_mean*900,'LineWidth',1.4);
    grid on;
    hold on;
    plot(spline_mean,vystup_eval,'LineWidth',1.4);
    title('Závislost úhlu natočení volantu na vzdálenosti od spline');
    xlabel('Vzdálenost od spline [m]');
    ylabel('Úhel natočení volantu [{\circ}]');
    legend('Original','Fuzzy model','Location','southeast');
    hold off;
saveas(gcf,sprintf("%s\\sug_worst_mfs_prev.png",obrazkydir));
    
%Zapis RMSE hodnot do textoveho souboru
fid=fopen(sprintf("%s\\Sugeno_Gen_RMSE.txt",obrazkydir),'w');

fprintf(fid, sprintf("Optimalizace Funkci příslusnosti RMSE min: %f \n",minRMSE_mfs));
fprintf(fid, sprintf("Optimalizace Funkci příslušnosti RMSE min: %f \n",maxRMSE_mfs));
fprintf(fid, sprintf("Optimalizace Prumerna doba jedne generace (s): %f \n",mean(Cas_Opti_mfs)));
fprintf(fid, sprintf("Optimalizace Prumerny pocet volani fitness funkci: %f \n",mean(Pocet_Fun_mfs)));

%pravidla - ty nemenime jsou stale stejna - primky
min_fis_opti_rules = readfis(sprintf("%s\\%s%d.fis",cesta,soubor_mfs,ix_minRMSE_mfs(1)));
min_fis_opti_rules_info = load(sprintf("%s\\%s%d.mat",cesta,soubor_mfs_info,ix_minRMSE_mfs(1)));
fprintf(fid, "\nNejlepsi RMSE pravidla: \n");
for i = 1:length(min_fis_opti_rules.Rules)
    fprintf(fid, sprintf("%d.  %s\n",i,min_fis_opti_rules.Rules(1,i).Description));
    fprintf(fid,sprintf("  out -> mf%d(x) = %f * x + %f \n",i, ...
       min_fis_opti_rules.Outputs.MembershipFunctions(1, i).Parameters(1),...
       min_fis_opti_rules.Outputs.MembershipFunctions(1, i).Parameters(2)));
end

fclose(fid);
%vykresli vystupni primky
spline_x = [0.123874,0.360958;0.310935,1.79768;1.58951,2.92432;2.66264,3.56705;3.54655,3.62013];
wheel_y =[-0.000274284,-0.0111101;-0.010596,-0.0148844;-0.016238,0.00635665;-0.000811686,0.0336922;0.0340542,0.000112334]; 
wheel_y = wheel_y*900;

plot(spline_mean,wheel_mean*900,'LineWidth',1.4,'Color','blue');
    grid on;
    hold on;
    x = [spline_x(1,1) spline_x(1,2)];
    y = [wheel_y(1,1) wheel_y(1,2)];    
    p1=line(x,y,'LineWidth',1.6,'Color','red'); 
    
    x = [spline_x(2,1) spline_x(2,2)];
    y = [wheel_y(2,1) wheel_y(2,2)];
    p2=line(x,y,'LineWidth',1.6,'Color','green'); 
    
    x = [spline_x(3,1) spline_x(3,2)];
    y = [wheel_y(3,1) wheel_y(3,2)];
    p3=line(x,y,'LineWidth',1.6, 'Color','cyan'); 
    
    x = [spline_x(4,1) spline_x(4,2)];
    y = [wheel_y(4,1) wheel_y(4,2)];
    p4=line(x,y,'LineWidth',1.6, 'Color','yellow');
    
    x = [spline_x(5,1) spline_x(5,2)];
    y = [wheel_y(5,1) wheel_y(5,2)];
    p5=line(x,y,'LineWidth',1.6, 'Color','magenta');
    
    title('Výstupní lineární funkce FIS Sugeno');
    xlabel('Vzdálenost od spline [m]');
    ylabel('Úhel natočení volantu [{\circ}]');
    legend('Original','Fuzzy model','Location','north');
    legend([p1,p2,p3,p4,p5],'out Mf1',' out Mf2',' out Mf3',' out Mf4','out Mf5','NumColumns',4);
    hold off;
saveas(gcf,sprintf("%s\\sug_linear_out_prev.png",obrazkydir));