% experiment 1 
% Martin Radvansky 
% Diplomova prace 2022
% skript, ktery poskytne veskere obrazky k experimentu 1, tykajici se rucniho ladeni Mamdani a Sugeno lze poustet po
% sekcich, kde je uvedeno, co skript v dane casti ulozi do prislusne slozky
% ploty_fuzzy_porovnani. 
% TUTO CAST VZDY SPUSTIT
clc
clear
close all
clear global
spline_vzdal = load('.\fuzzy_data\Export_J1_Dist!.csv');
wheel_angle = load('.\fuzzy_data\Export_J1_Wheel!.csv');
prum_spline = mean(spline_vzdal,1);
prum_wheel_angle = mean(wheel_angle,1);
delka_dat = size(spline_vzdal,2);
slozka_ulozeni = '.\ulozene_obrazky'; % slozka, kam se maji ukladat vysledky
% body pro zapsani predpisu primek na vystup Sugena
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]; 
fuzzy_model = readfis('sugeno_model');
%% SUGENO RUCNE PRVNI GRAF, prevodni charakteristika
fuzzy_model = readfis('sugeno_model');
chyba = 0;
wheel_aprox = evalfis(fuzzy_model,prum_spline);
chyba = sqrt(sum(((prum_wheel_angle.*900)-(wheel_aprox'.*900)).^2));    % RMSE
plot(prum_spline,prum_wheel_angle.*900,'LineWidth',1.5);
title('Převodní charakteristika úhlu natočení volantu na vzdálenosti od spline');
xlabel('Vzdálenost od spline [m]');
ylabel('Úhel natočení volantu [°]');
grid on;
hold on;
plot(prum_spline,wheel_aprox.*900,'LineWidth',1.5);
legend({'Original','Aproximace'},'Position',[0.47 0.79 0.1 0.1]); %[left bottom width height].
hold off;
saveas(gcf,sprintf("%s\\sugeno_man_prevodni.png",slozka_ulozeni));
%% druhy graf SUGENO RUCNE otoceni volantu v case
plot(1:delka_dat,prum_wheel_angle.*900,'LineWidth',1.5); % prumer
title('Závislost natočení volantu na čase');
xlabel('Čas [Vzorek x 20 ms]');
ylabel('Úhel natočení volantu [°]');
grid on;
hold on;
plot(1:delka_dat,wheel_aprox.*900,'LineWidth',1.5);
legend({'Original','Aproximace'},'Position',[0.47 0.79 0.1 0.1]); %[left bottom width height].
hold off;
saveas(gcf,sprintf("%s\\sugeno_man_cas.png",slozka_ulozeni));
%% vykresleni MFs z nacteneho sugeno modelu
vykresli_mfs_sugeno(fuzzy_model);
saveas(gcf,sprintf("%s\\sugeno_man_mfs.png",slozka_ulozeni));
%% vykresleni originalu a jeho primek pouzite pro aproximaci SUGENO
plot(prum_spline,prum_wheel_angle,'LineWidth',1.5);
grid on;
hold on;
plot(spline_x(1,:),wheel_y(1,:),'r','LineWidth',1.2);
plot(spline_x(2,:),wheel_y(2,:),'r','LineWidth',1.2);
plot(spline_x(3,:),wheel_y(3,:),'r','LineWidth',1.2);
plot(spline_x(4,:),wheel_y(4,:),'r','LineWidth',1.2);
plot(spline_x(5,:),wheel_y(5,:),'r','LineWidth',1.2);
title('Data originál a přímky pro FIS Sugeno');
xlabel('Vzdálenost od spline [m]');
ylabel('Natočení volantu (raw) [-]');
saveas(gcf,sprintf("%s\\sugeno_man_primky.png",slozka_ulozeni));
%% *********** test Mamdani rucne ***********************************************************************
fuzzy_model = readfis('sedm_1_work901');
chyba = 0;
wheel_aprox = evalfis(fuzzy_model,prum_spline);
chyba = sqrt(sum((prum_wheel_angle.*900-wheel_aprox').^2));    % RMSE
plot(prum_spline,prum_wheel_angle.*900);
grid on;
hold on;
plot(prum_spline,wheel_aprox);
title('Převodní charakteristika úhlu natočení volantu na vzdálenosti od spline');
xlabel('Vzdálenost od spline [m]');
ylabel('Úhel natočení volantu [°]');
legend({'Original','Aproximace'},'Position',[0.47 0.79 0.1 0.1]); %[left bottom width height].
hold off;
saveas(gcf,sprintf("%s\\mamdani_man_prevodni.png",slozka_ulozeni));
%% zobrazeni mamdani rucne - otoceni volantu v case
plot(1:delka_dat,prum_wheel_angle.*900); % prumer
grid on;
hold on;
title('Závislost natočení volantu na čase');
xlabel('Čas [Vzorek x 20 ms]');
ylabel('Úhel natočení volantu [°]');
plot(1:delka_dat,wheel_aprox);
legend({'Original','Aproximace'},'Position',[0.47 0.79 0.1 0.1]); %[left bottom width height].
hold off;
saveas(gcf,sprintf("%s\\mamdani_man_cas.png",slozka_ulozeni));
%% vykresleni MFs z nacteneho mamdani modelu
vykresli_mfs_mamdani(fuzzy_model,slozka_ulozeni);
%% %% test optimalizace mamdani Pravidla zkouska optimalizace na malem poctu opakovani
% global real_otoceni;
% real_otoceni = prum_wheel_angle;
% fuzzy_model = readfis('sedm_1_work');
% [in,out,rule] = getTunableSettings(fuzzy_model);
% options = tunefisOptions('OptimizationType','tuning','UseParallel',false, 'NumMaxRules',49);
% rng(12345);
% [fisTRuleTuned,info] = tunefis(fuzzy_model,rule,@(fuzzy_model)cost_rmse(fuzzy_model,prum_spline),options);
% %writeFIS(fisTRuleTuned,'tunedfis_rules');
% %% test optimalizace mamdani INPUT / OUTPUTS Mfs
% warning('off');
% global real_otoceni;
% real_otoceni = prum_wheel_angle;
% fuzzy_model = readfis('tunedfis_rules');
% [in,out,rule] = getTunableSettings(fuzzy_model);
% options = tunefisOptions('OptimizationType','tuning','UseParallel',false);
% [fisTRuleTuned,info] = tunefis(fuzzy_model,[in,out],@(fuzzy_model)cost_rmse(fuzzy_model,prum_spline),options);
% %writeFIS(fisTRuleTuned,'tunedfis_mfs');
% %% Zobrazeni Tuned modelu
% %fuzzy_model = readfis('tunedfis_rules');
% fuzzy_model = readfis('tunedfis_mfs');
% chyba = 0;
% wheel_aprox = evalfis(fuzzy_model,prum_spline);
% chyba = sqrt(sum((prum_wheel_angle-wheel_aprox').^2));    % RMSE
% plot(prum_spline,prum_wheel_angle);
% grid on;
% hold on;
% plot(prum_spline,wheel_aprox);
% hold off;
% 
% %%
% plot(1:delka_dat,prum_wheel_angle); % prumer
% grid on;
% hold on;
% plot(1:delka_dat,wheel_aprox);
% %% automat 10x autotune
% clc
% clear
% close all
% clear global
% spline_vzdal = load('D:\Projekty\Diplomka\Martin_Ing_R\matlab\fuzzy_data_AGAIN\Export_J1_Dist!.csv');
% wheel_angle = load('D:\Projekty\Diplomka\Martin_Ing_R\matlab\fuzzy_data_AGAIN\Export_J1_Wheel!.csv');
% prum_spline = mean(spline_vzdal,1);
% prum_wheel_angle = mean(wheel_angle,1);
% delka_dat = size(spline_vzdal,2);
% rng(12345);
% global real_otoceni;
% real_otoceni = prum_wheel_angle;
% warning('off');
% slozka_optimalizace = "D:\Projekty\Diplomka\Martin_Ing_R\matlab\optimalizace\";
% if ~isfolder(slozka_optimalizace)
%   errorMessage = sprintf('Error: The following folder does not exist:\n%s', slozka_optimalizace);
%   uiwait(warndlg(errorMessage));
%   return;
% end
% for i =1:15
%     %optimalizace mamdani Pravidla
%     fuzzy_model = readfis('sedm_1_work');
%     [in,out,rule] = getTunableSettings(fuzzy_model);
%     options = tunefisOptions('OptimizationType','tuning','UseParallel',false, 'NumMaxRules',49);
%     [fisTRuleTuned,info] = tunefis(fuzzy_model,rule,@(fuzzy_model)cost_rmse(fuzzy_model,prum_spline),options);
%     
%     fis_name_rules = sprintf('%stuned_fis_rules_%d.fis',slozka_optimalizace,i);
%     writeFIS(fisTRuleTuned,fis_name_rules);
%     info_save = sprintf('%stuned_info_rules_%d.mat',slozka_optimalizace,i);
%     save(info_save,'info');
%     
%     f = figure('visible','off');
%     wheel_aprox = evalfis(fisTRuleTuned,prum_spline);
%     plot(prum_spline,prum_wheel_angle);
%     grid on;
%     hold on;
%     plot(prum_spline,wheel_aprox);
%     title('Závislost úhlu natočení volantu na vzdálenosti od spline');
%     xlabel('Vzdálenost od spline [m]');
%     ylabel('Úhel natočení volantu');
%     legend('Original','Fuzzy model','Location','southeast');
%     hold off;
%     nazev_plotu = sprintf('%sprevodni_rules_%d.jpg',slozka_optimalizace,i);
%     saveas(f,nazev_plotu);
%     
%     %obrazek 2
%     plot(1:delka_dat,prum_wheel_angle); % prumer
%     grid on;
%     hold on;
%     plot(1:delka_dat,wheel_aprox);
%     title('Natočení volantu v čase');
%     xlabel('Čas [Vzorek x 20 ms]');
%     ylabel('Úhel natočení volantu');
%     legend('Original','Fuzzy model','Location','southeast');
%     hold off; 
%     nazev_plotu = sprintf('%scasova_rules_%d.jpg',slozka_optimalizace,i);
%     saveas(f,nazev_plotu);
%     
%     %optimalizace mamdani INPUT / OUTPUTS Mfs
%     fuzzy_model = fisTRuleTuned;
%     [in,out,rule] = getTunableSettings(fuzzy_model);
%     options = tunefisOptions('OptimizationType','tuning','UseParallel',false);
%     [fisTRuleTuned,info] = tunefis(fuzzy_model,[in,out],@(fuzzy_model)cost_rmse(fuzzy_model,prum_spline),options);
%     
%     fis_name_mfs = sprintf('%stuned_fis_mfs_%d.fis',slozka_optimalizace,i);  
%     writeFIS(fisTRuleTuned,fis_name_mfs);
%     info_save = sprintf('%stuned_info_mfs_%d.mat',slozka_optimalizace,i);
%     save(info_save,'info');
%     
%     wheel_aprox = evalfis(fisTRuleTuned,prum_spline);
%     plot(prum_spline,prum_wheel_angle);
%     grid on;
%     hold on;
%     plot(prum_spline,wheel_aprox);
%     title('Závislost úhlu natočení volantu na vzdálenosti od spline');
%     xlabel('Vzdálenost od spline [m]');
%     ylabel('Úhel natočení volantu');
%     legend('Original','Fuzzy model','Location','southeast');
%     hold off;
%     nazev_plotu = sprintf('%sprevodni_mfs_%d.jpg',slozka_optimalizace,i);
%     saveas(f,nazev_plotu);
%     
%     %obrazek 2
%     plot(1:delka_dat,prum_wheel_angle); % prumer
%     grid on;
%     hold on;
%     plot(1:delka_dat,wheel_aprox);
%     title('Natočení volantu v čase');
%     xlabel('Čas [Vzorek x 20 ms]');
%     ylabel('Úhel natočení volantu');
%     legend('Original','Fuzzy model','Location','southeast');
%     hold off; 
%     nazev_plotu = sprintf('%scasova_mfs_%d.jpg',slozka_optimalizace,i);
%     saveas(f,nazev_plotu);
%     disp(sprintf('**** Krok: %d  ****',i));
% end