% experiment 1 Sugeno optimalizace 
% Martin Radvansky 
% Diplomova prace 2022
% Skript pro optimalizaci Sugena - % automat 100x autotunem vygeneruje 100 FIS modelu
clc
clear
close all
clear global

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

prum_spline = mean(spline_vzdal,1);
prum_wheel_angle = mean(wheel_angle,1)*900;
delka_dat = size(spline_vzdal,2);

plot(prum_spline,prum_wheel_angle,'color','blue','linewidth',2);

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;

minmaxI = [min(prum_spline) max(prum_spline)]; 
minmaxO = [min(prum_wheel_angle) max(prum_wheel_angle)]; 
fis_avg = make_raw_fis_sug(minmaxI, minmaxO, spline_x);

fis_avg = napln_fis_primkyS(fis_avg,spline_x,wheel_y);
vykresli_mfsS(fis_avg,prum_spline);

predicted = evalfis(fis_avg,prum_spline);
rmse = sqrt(sum((predicted'-prum_wheel_angle).^2));

plot(1:751, predicted,'color','red','linewidth',2);
hold on;
plot(1:751, prum_wheel_angle,'color','blue','linewidth',2);
text(500,0.1,sprintf('RMSE %2.3f',rmse));

vykresli_vystup(prum_spline,prum_wheel_angle,predicted',rmse);

%Vytvorime optimalizaci co to udela
%ziskame fuzzySetting
[in,~,~] = getTunableSettings(fis_avg);
options = tunefisOptions('Method','ga');
options.MethodOptions.MaxGenerations = 200;
warning('off');
rng('default')  % pro opakovatelnost
cesta = ".\sug100\";
for i = 1:100
  [fisout,optimout] = tunefis(fis_avg,[in],prum_spline,prum_wheel_angle,options);
  
  fis_name = sprintf('%s_Sugeno_%d.fis',cesta,i);
  writeFIS(fisout,fis_name);
  info_save = sprintf('%s_Sugeno_info_%d.mat',cesta,i);
  save(info_save,'optimout');

  predicted = evalfis(fisout,prum_spline);
  rmse = sqrt(sum((predicted'-prum_wheel_angle).^2));
  
  %Ulozeni obrazku
  f = figure('visible','off');
  plot(prum_spline,prum_wheel_angle,'color','blue','linewidth',2);
    grid on;
    hold on;
    plot(prum_spline,predicted,'color','red','linewidth',2);
    title('Závislost úhlu natočení volantu na vzdálenosti od spline');
    xlabel('Vzdálenost of spline [m]');
    ylabel('Úhel natočení volantu');
    legend('Original','Fuzzy model','Location','southeast');
    hold off;
    nazev_plotu = sprintf('%sprevodni_sugeno_%d.jpg',cesta,i);
    saveas(f,nazev_plotu);

    %obrazek 2
    plot(1:size(prum_wheel_angle,2),prum_wheel_angle,'color','blue','linewidth',2); % prumer
    grid on;
    hold on;
    plot(1:size(prum_wheel_angle,2),predicted,'color','red','linewidth',2);
    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_sugeno_%d.jpg',cesta,i);
    saveas(f,nazev_plotu); 
    disp(sprintf('**** Krok: %d  ****',i));
end

predicted = evalfis(fisout,prum_spline);
rmse = sqrt(sum((predicted'-prum_wheel_angle).^2));
vykresli_vystup(prum_spline,prum_wheel_angle,predicted',rmse);

