% experiment 3 
% Martin Radvansky 
% Diplomova prace 2022
% Vlastni optimalizace FIS Sugeno, vystup je ulozen do slozky .\Optimal
% Vytvoreni Sugeno modelu pro všechny řidiče
% Spousti se v ramci RunOptimalizace.bat 
% Vstupuji parametry RunID - skupina, NumCores - pocet jader


vstup_ix = RunID;
pocetProc = NumCores;

%clc
%clear
%close all
%clear global


Optimal_dir = '.\Optimal\'; %slozka kde jsou optimalizovane modely pro vsechny 38 ridice
All_spline_vzdal = load('.\Data\All_SplineMean.csv');
All_wheel_angle = load('.\Data\All_WheelAngleMean.csv');
All_wheel_angle = All_wheel_angle.*900;  %hodnota je ve stupnich


%oprava chybnych hodnot u zaznamu rizeni - nahradime prumernou hodnotou okoli
values = sort(All_wheel_angle(1,:));  %2x hodnota -5
values(1:10)
ix_min1 = find(All_wheel_angle(1,:) == min(All_wheel_angle(1,:)));
for i = 1:length(ix_min1)
   All_wheel_angle(1,ix_min1(i))= mean([All_wheel_angle(1,ix_min1(i)-1) All_wheel_angle(1,ix_min1(i)+1)]);   
end

values = sort(All_wheel_angle(23,:)); %2x hodnota 5
values(740:751)
ix_max23 = find(All_wheel_angle(23,:) == max(All_wheel_angle(23,:)));
for i = 1:length(ix_max23)
   All_wheel_angle(23,ix_max23(i))= mean([All_wheel_angle(1,ix_max23(i)-1) All_wheel_angle(1,ix_max23(i)+1)]);
end

values = sort(All_wheel_angle(38,:));  %hodnota -5
values(1:10)
ix_min38 = find(All_wheel_angle(38,:) == min(All_wheel_angle(38,:)));
for i = 1:length(ix_min38)
   All_wheel_angle(38,ix_min38(i))= mean([All_wheel_angle(1,ix_min38(i)-1) All_wheel_angle(1,ix_min38(i)+1)]);
end

%celkovy prumer pro vytvoreni vystupnich linearnich funkci
mean_Spline = mean(All_spline_vzdal);

%prumerne natoceni od spline
mean_Angle = mean(All_wheel_angle);

%vytvorime koeficienty pro linearni funkce na vystupu FIS na zaklade
%prumeru nalezeneho rucne z prechodove charakterisiky
body_primek_x = [0.26082,0.281282; 0.221405,1.456938; 1.096494,2.41825; 2.125,3.58228; 3.50339,3.52858];
body_primek_y = [1.218314,-3.54056; -2.4496,-8.62085; -8.623555,-4.16386; -6.50468,16.168; 16.168,-1.20091];

%koeficienty k a q pro každou přímku -  vystupni funkci y=kx+q
coef_p1 = polyfit(body_primek_x(1,:),body_primek_y(1,:),1);
coef_p2 = polyfit(body_primek_x(2,:),body_primek_y(2,:),1);
coef_p3 = polyfit(body_primek_x(3,:),body_primek_y(3,:),1);
coef_p4 = polyfit(body_primek_x(4,:),body_primek_y(4,:),1);
coef_p5 = polyfit(body_primek_x(5,:),body_primek_y(5,:),1);

%Vytvorime FIS sugeno pro kazdeho ridice

%FIS pro prumerny prubeh
minmaxI = [min(mean_Spline) max(mean_Spline)]; 
minmaxO = [min(mean_Angle) max(mean_Angle)]; 
fis_mean = make_raw_fis_sug(minmaxI, minmaxO,body_primek_x);

fis_mean = napln_fis_primkyS(fis_mean,body_primek_x,body_primek_y);
fis_eval = evalfis(fis_mean,mean_Spline);

% **** Pro kazdy FIS provedeme PSO na vstupni funkce prislusnosti a vahy **
%Ladime pomoci PSO na nejlepsi vysledek -> nejmensi RMSE
%definovane zakladni hodnoty odvozene od zlomu primek

global zakl_hodnoty;
global fss;
global vahy;
global data_wheel;
global data_spline;

if (pocetProc=4)
	if (vstup_ix==1)
		ridici=[1 2 3 4 5 6 7 8 9];
		indexy_Rid = "1_9";
	elseif (vstup_ix==2)
		ridici=[10 11 12 13 14 15 16 17 18];
		indexy_Rid = "10_18";
	elseif (vstup_ix==3)
		ridici=[19 20 21 22 23 24 25 26 27];    
		indexy_Rid = "19_27";
	elseif (vstup_ix==4)
		ridici=[28 29 30 31 32 33 34 35 36 37 38];
		indexy_Rid = "28_38";
	end
elseif (pocetProc=6)
	if (vstup_ix==1)
		ridici=[1 2 3 4 5 6];
		indexy_Rid = "1_6";
	elseif (vstup_ix==2)
		ridici=[7 8 9 10 11 12];
		indexy_Rid = "7_12";
	elseif (vstup_ix==3)
		ridici=[ 13 14 15 16 17 18];    
		indexy_Rid = "13_18";
	elseif (vstup_ix==4)
		ridici=[19 20 21 22 23 24];
		indexy_Rid = "19_24";	
	elseif (vstup_ix==5)
		ridici=[ 25 26 27 28 29 30];
		indexy_Rid = "25_30";	
	elseif (vstup_ix==6)
		ridici=[31 32 33 34 35 36 37 38];
		indexy_Rid = "30_38";
	end
end


Celkove_Vysledky = {size(All_spline_vzdal,1),4}; %38 ridicu, matice tf, info tf, matice vah, info vah
for ridic = ridici
    %data jednotlivych ridicu
    data_wheel = All_wheel_angle(ridic,:);
    data_spline = All_spline_vzdal(ridic,:);
        
    pole_koef = zeros(10,15);
    info_koef = zeros(10,2);
    pole_vah = zeros(10,5);
    info_vah = zeros(10,2);
    pocet_cyklu = 10;
    warning('off');
    rng(123456); %nastaveni RNG pro reprodukovatelnost
    for c = 1:pocet_cyklu    
        %leva,stred,prava,sirka_zmeny
        zakl_hodnoty = [ body_primek_x(1,1) mean(body_primek_x(1,:)) body_primek_x(1,2) body_primek_x(1,2) ...
                         body_primek_x(2,1) mean(body_primek_x(2,:)) body_primek_x(2,2) body_primek_x(2,2)-body_primek_x(2,1) ...
                         body_primek_x(3,1) mean(body_primek_x(3,:)) body_primek_x(3,2) body_primek_x(3,2)-body_primek_x(3,1) ...
                         body_primek_x(4,1) mean(body_primek_x(4,:)) body_primek_x(4,2) body_primek_x(4,2)-body_primek_x(4,1) ...
                         body_primek_x(5,1) mean(body_primek_x(5,:)) body_primek_x(5,2) body_primek_x(5,2)-body_primek_x(5,1)];                 
        nastaveni = [0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5];
        vahy = [1 1 1 1 1];
        
        %FIS pro prumerny prubeh
        minmaxI = [min(data_spline) max(data_spline)]; 
        minmaxO = [min(data_wheel) max(data_wheel)]; 
        fis_mean = make_raw_fis_sug(minmaxI, minmaxO, body_primek_x);
        fis_mean = napln_fis_primkyS(fis_mean,body_primek_x,body_primek_y);

        fss = fis_mean; %vytvoreny FIS
        fss = ComputeIntMF(fss, nastaveni, zakl_hodnoty);
        fss = nastav_vahy(fss,vahy);

        [BestSol BestRMSE timePSO]= DoPSO();
        pole_koef(c,:) = BestSol.Position;
        info_koef(c,:) = [BestRMSE, timePSO];

        %nastavime fss na nejlepsi nastaveni z funkci prislusnosti
        fss = ComputeIntMF(fss, BestSol.Position, zakl_hodnoty);
        fss = nastav_vahy(fss,vahy);
		
        %--------------------------------------------------------------------------
        %Ted dame dalsi PSO a to na vahy jednotlivych vystupnich funkci
        %rng(18042022); %nastaveni RNG pro reprodukovatelnost
        [BestSol_w BestRMSE_w timePSO_w]= DoPSO_Weight();
        pole_vah(c,:) = BestSol_w.Position;
        info_vah(c,:) = [BestRMSE_w, timePSO_w];
        disp(sprintf("*** Iterace: %d ... RMSE tvf: %f  RMSE vaha: %f",c,BestRMSE,BestRMSE_w));  
    end %hlavni cyklus 100x PSO FP a pro kazdy nejlepsi 100x PSO vahy
    Celkove_Vysledky{ridic,1}=pole_koef;
    Celkove_Vysledky{ridic,2}=info_koef;
    Celkove_Vysledky{ridic,3}=pole_vah;
    Celkove_Vysledky{ridic,4}=info_vah;
end %38 ridicu
save(sprintf("%sRidici_%s.mat",Optimal_dir,indexy_Rid),'Celkove_Vysledky');
save(sprintf("%sEnvRidici_%s.mat",Optimal_dir,indexy_Rid));