%% Plot NWSA cost function Global auf die x-Achse bezogen
% Während eines Breakpoints in BerechneSpline_Rotor ausführen, damit die
% entsprechend Daten alle da sind.
clear all

%% Skript für Plotbasics
basics_plot

%% Beginn

P = [-8 8 8 -8 -8;0 0 3 3 0];

r = 0.45;
z = [-8;-5];

%Needle Winding Simulation Algorithm
%Anhand der aktuellen Konfiguration werden einzelne Drähte abgelegt,
%in dem die optimale Poition für diesen Draht gesucht wird.

W_opt = [0;0];
fval_opt = inf;
wire_index = 1;
calc_index = 1;
NShift_iter_old = NaN;
NShift_iter = 1;

safety = 0.00;
W0 = [0;max(P(2,:))-3*r];
% z = [-4.54;-25];

%z = [0;-100000]


W_iter = [];
% Output_Function_ga_fmincon = @(x1,x2,x3)outfun_ga_fmincon(x1,x2,x3,W_iter,P,r,safety);
mycost1 = @(w) cost1(reshape(w,size(W0)),[],P,r,z);
options = optimset('MaxIter',5000,'PlotFcns',[]);

nlc = @(w) costInpoly(reshape(w,size(W0)),P,r);

% gaoptions = gaoptimset('TolFun',1e-14,'Display','iter','HybridFcn',{@FMINCON,options},'OutputFcns',[],'PopulationSize',200);

gaoptions = gaoptimset('TolFun',1e-14,'Display','iter','OutputFcns',[],'PopulationSize',200);
fminconoptions = optimoptions('fmincon','Display','iter');
[W] = fmincon(mycost1,[r;0],[],[],[],[],[min(P(1,:)),min(P(2,:))+r],[max(P(1,:)),max(P(2,:))-r],nlc,fminconoptions)
% [W,fval] = ga(mycost1,2,[],[],[],[],[],[],[],gaoptions);
% W = fminsearch(mycost1,W, options);

W=reshape(W,size(W0));
figure(1)
clf
plot(P(1,:),P(2,:));
hold on
text(5,14,'NWSA');
circle(W(1,:),W(2,:),r,'black');
%                     circle(z(1),z(2),norm(z - W(:,end)),[0.75;0.75;0.75])
%                     text(W(1,1),W(2,1),num2str(1));
%                     text(W(1,2),W(2,2),num2str(2));
axis equal

%% Algorithmus
flag = 1;

while flag == 1
    %% Fmincon with 6 starting location
    if numel(wire_index)>110
        test=0;
    end
    fval_iter = inf;
    for index_iter = 1:numel(wire_index) %Alle vorhandenen Drähte durchgehen
        if wire_index(index_iter) == 1  % Falls am aktuellen Draht noch einer dranpasst
            W_iter = [W(:,1:index_iter-1) W(:,index_iter+1:end) W(:,index_iter)];
            if calc_index(index_iter) == 1  %#ok<ALIGN> %Falls eine Neu-Berechnung erforderlich ist
                %index_iter %#ok<NOPRT>
                Output_fun_FMinCon = @(alpha,optimValues,state) outfun(alpha,optimValues,state,W_iter,P,r,safety);
                mycost = @(alpha) cost(alpha,W_iter,P,r,safety,z);%
                %mynonLinCon = @(alpha) nonLinConstraints(alpha,W_iter,P,r,safety);
                %Output_Function_ga_fmincon = @(x1,x2,x3)outfun_ga_fmincon(x1,x2,x3,W_iter,P,r);
                % Genetic Algorithm mit Nebenbedingungen
                fminconoptions = optimoptions('fmincon','MaxIter',10000,'Display','none','MaxFunEvals',10000000,...
                    'OutputFcn',[],'TolX',1e-15,'TolCon',1e-8);
                %gaoptions = gaoptimset('EliteCount',1,'TolFun',1e-14,'Display','off','HybridFcn',[],'OutputFcns',[],...
                %    'PopulationSize',50,'InitialPopulation',[pi/3 -pi/3 -pi:(2*pi/47):pi]');
                %[alpha_iter,fval] = ga(mycost,1,[],[],[],[],-pi,pi,[],gaoptions);
                %fval_best = Inf;
                alphaiter=zeros(6,1);
                fval=zeros(6,1);
                parfor tryiter=1:6
                    [alphaiter(tryiter),fval(tryiter)] = fmincon(mycost,(tryiter*pi/3-pi),[],[],[],[],-2*pi,2*pi,[],fminconoptions);
                end
                [fval_best,minidx] = min(fval);
                alpha_iter_best = alphaiter(minidx);
                
                
                
                W_iter = [W_iter W_iter(:,end)+[cos(alpha_iter_best)*2*(r+safety);sin(alpha_iter_best)*2*(r+safety)]]; %#ok<AGROW>
                
                if costIntersect(W_iter,r)+costInpoly(W_iter,P,r) > 10  %beim aktuellen Draht kann keine valide Position mehr angefügt werden
                    W_iter = W_iter(:,1:end-1);  %#ok<NASGU>
                    wire_index(index_iter) = 0; %#ok<AGROW>
                else %Aktueller Draht ist möglich
                    Top_alpha(index_iter) = alpha_iter_best; %#ok<AGROW> %Ergebnis zwischenspeichern
                    
                    if fval_best < fval_iter %Falls dieser Draht besser ist als der alte
                        fval_iter = fval_best;
                        %alpha_iter_opt = alpha_iter_best; %#ok<NASGU>
                        W_iter_opt = W_iter;
                    end
                end
            else %Es ist keine Berechnung erforderlich  calc_index(index_iter) == 0
                TopCost = cost(Top_alpha(index_iter),W_iter,P,r,safety,z);
                if TopCost < fval_iter %Falls dieser Draht besser ist als der alte
                    fval_iter = TopCost;
                    %alpha_iter_opt = Top_alpha(index_iter);
                    W_iter_opt = [W_iter W_iter(:,end)+[cos(Top_alpha(index_iter))*2*(r+safety);sin(Top_alpha(index_iter))*2*(r+safety)]];
                end
            end   %Berechnung erforderlich?
            
        end %Falls an diesem Wire noch was angelegt werden kann
        
    end  %alle Wire_index sind durch
    
    
    
    if sum(wire_index) == 0     %Wenn es keine Draht mehr gibt, an dem angehängt werden kann.
        flag = 0;       %Fertig
        
    else
        W = [W W_iter_opt(:,end)];
        
        d = squareform(pdist(W'));  %Alle benachbarten Drähte neu berechnen lassen
        [row,col] = find(d<4.1*r); %Wenn sie näher als 5*radius dran sind.
        calc_index = zeros(size(calc_index));
        for i=1:size(row,1)
            if row(i) ~= col(i) %falls zwei unterschiedliche Drähte so nah aneinander liegen
                if col(i)==size(d,2) %Nur Distanzen von letzen Draht anschauen
                    calc_index(row(i)) = 1; %dann sollen sie neu berechnet werden.
                end
            end
        end
        
        wire_index = [wire_index 1]; %#ok<AGROW>
        calc_index = [calc_index 1]; %#ok<AGROW>
        %
        if NShift_iter == NShift_iter_old   %Falls nicht beim ersten mal
            figure(1)
            circle(W(1,end),W(2,end),r,'black');
            %                     frame = getframe(gcf); % 'gcf' can handle if you zoom in to take a movie.
            %                     writeVideo(writerObj, frame);
            %                     circle(z(1),z(2),norm(z - W(:,end)),[0.75;0.75;0.75])
            %                     text(W(1,end),W(2,end),num2str(size(W,2)));
            drawnow
        else %Falls das erste mal die aktuelle Konfiguration gezeichnet wird.
            figure(1)
            clf
            plot(P(1,:),P(2,:));
            hold on
            text(5,14,'NWSA');
            circle(W(1,:),W(2,:),r,'black');
            %                     circle(z(1),z(2),norm(z - W(:,end)),[0.75;0.75;0.75])
            %                     text(W(1,1),W(2,1),num2str(1));
            %                     text(W(1,2),W(2,2),num2str(2));
            axis equal
            %                     axis([min(P(1,:))-1 max(P(1,:))+1 min(P(2,:))-1 max(P(2,:))+1])
            %                     frame = getframe(gcf); % 'gcf' can handle if you zoom in to take a movie.
            %                     writeVideo(writerObj, frame);
            drawnow
        end
        %                 NShift_iter_old = NShift_iter;
        %             end
    end  %Alle möglichen Drähte abgelegt
    
    
    
    if size(W_opt,2)<size(W,2)
        W_opt = W;
        fval_opt = fval;
    elseif size(W_opt,2) == size(W,2)
        if fval < fval_opt   %aktuelle Position ist besser
            W_opt = W;
            fval_opt = fval;
        end
    end
    
    
    % hold off
    % close(writerObj); % Saves the movie.
    
end




%%

function [cWindingRS,cWindingTH,cWindingLS,cWindingBH] = NWSA_Rotor(slotData,wireData,whData)

end




close all

alphaDeg = 50:1:180;
alpha = alphaDeg.*pi/180;

r = [cos(alpha);sin(alpha)];

r = r(2,:)+1;

plot(alphaDeg,r,'Color',myLineOne)

set(gcf,'defaulttextinterpreter','none')

hold on

plot([min(alphaDeg) min(alphaDeg)],[min(r) max(r)],'LineStyle','--','Color',myGray50)
plot([max(alphaDeg) max(alphaDeg)],[min(r) max(r)],'LineStyle','--','Color',myGray50)

text(min(alphaDeg)-0.1,1.3,['Infeasible', char(10), 'Region'],'Rotation',90,'VerticalAlignment','bottom')

text(max(alphaDeg)+0.1,1.3,['Infeasible', char(10), 'Region'],'Rotation',90,'VerticalAlignment','top')

plot([alphaDeg(1),alphaDeg(end)],[r(1),r(end)],'Marker','o','LineStyle','none','Color',myLineOne)

plot([alphaDeg(end),alphaDeg(end)+5],[r(end),r(end)+0.12],'Color',myGray25)
text(alphaDeg(end)+2,r(end)+0.17,['Global', char(10), 'Minimum'])


text(alphaDeg(1)+0.02,r(1)-0.07,['Local', char(10), 'Minimum'])


box off

set(gca,'XLim',[0,360])
try
    
    for i = 0:5
        switch i
            case 0
                
                plot([i*60,i*60+6],[min(r),min(r)+0.12],'Color',myGray25)
                text(i*60,min(r)+0.14,['$\alpha_{\text{init},',num2str(i),'}$'],'HorizontalAlignment','left')
            case 1
                plot([i*60,i*60],[min(r),min(r)+0.06],'Color',myGray25)
                text(i*60,min(r)+0.08,['$\alpha_{\text{init},',num2str(i),'}$'],'HorizontalAlignment','center')
            case 2
                plot([i*60,i*60],[min(r),min(r)+0.06],'Color',myGray25)
                text(i*60,min(r)+0.08,['$\alpha_{\text{init},',num2str(i),'}$'],'HorizontalAlignment','center')
            case 3
                plot([i*60,i*60-20],[min(r),min(r)+0.12],'Color',myGray25)
                text(i*60-14,min(r)+0.14,['$\alpha_{\text{init},',num2str(i),'}$'],'HorizontalAlignment','right')
            case 4
                plot([i*60,i*60+20],[min(r),min(r)+0.12],'Color',myGray25)
                text(i*60+14,min(r)+0.14,['$\alpha_{\text{init},',num2str(i),'}$'],'HorizontalAlignment','left')
            case 5
                plot([i*60,i*60],[min(r),min(r)+0.06],'Color',myGray25)
                text(i*60,min(r)+0.08,['$\alpha_{\text{init},',num2str(i),'}$'],'HorizontalAlignment','center')
                
        end
    end
    
    % for i = 0:5
    %     %     plot([i*360/6,i*360/6],[min(r),max(r)],,'LineStyle','--','Color',myGray25)
    %     %     text(i*360/6,0.02
    %
    %     if find(get(gca, 'XTick') == i*360/6)
    %
    %     else
    %
    %         set(gca, 'XTick', sort([i*360/6, get(gca, 'XTick')]));
    %         k = find(get(gca, 'XTick') == i*360/6);
    %         l = get(gca,'XTickLabel');
    %         l = {l{1:k-1},[],l{k:end}};
    %         set(gca,'XTickLabel',l);
    %     end
    %     xlims = get(gca,'XLim');
    %
    %     j=1;
    %     stillrunning = 1;
    %     ticks = get(gca, 'XTick');
    %     labels = get(gca,'XTickLabel');
    %     while stillrunning
    %         if ticks(j) == i*360/6
    %             if j == 1
    %                 tickresult = {string(['$\alpha_{init,',num2str(i),'}$'])};
    %             else
    %                 tickresult(end+1) = {string(['$\alpha_{init,',num2str(i),'}$'])};
    %             end
    %
    %         else
    %             if j == 1
    %                 tickresult = {num2str(labels{j})};
    %             else
    %                 tickresult(end+1) = {num2str(labels{j})};
    %             end
    %
    %         end
    %         j = j+1;
    %         if j > numel(labels)
    %             stillrunning = 0;
    %         end
    %     end
    %
    %     set(gca,'XTickLabel',tickresult);
    %
    %
    % end
    
    xlabel('$\alpha_{k,i}$ {[\SI{}{\degree}]}')
    ylabel('Normalized Value $\tilde{J}_{j,i}/\min\tilde{J}_{j,i}$')
catch
    blub =0;
end

%% Tikzn

matlab2tikz('filename','fitnessNWSA.tex',...
    'height', '\figureheight', 'width', '\figurewidth', 'encoding', 'UTF8', 'showInfo', false, 'checkForUpdates', false, ...
    'parseStrings', false, ... % switch off LaTeX parsing by matlab2tikz for titles, axes labels etc. ("greater flexibility", "use straight LaTeX for your labels")
    'floatFormat', '%.4g', ... % limit precision to get smaller .tikz files
    'noSize', false);
%
% axis off
%
% hold off
% box off % Box um die figure herum ausblenden

% mit legendflex hat man mehr Möglichkeiten als mit MATLABs eigener
% legend-Funktion
% legendflex({'LF', 'UF'}, ...
%             'box', 'off', ...
%             'ncol', 1, ...
%             'nrow', 2, ...
%             'anchor', [3, 3], ...
%             'buffer', [-30, -30], ...
%             'xscale', 0.5, ...
%             'padding', [0, 0, 20], ...
%             'Color', [1, 1, 1], ...
%             'Interpreter', 'latex', ...
%             'fontsize', 11)


tightfig; % tightfig entfernt den weißen Rand um die figure herum
% for i = 1:length(ax)
% %     decimal_comma(ax(i), 'XY') % für deutsche Veröffentlichungen kann hiermit der Dezimalpunkt durch ein Dezimal komma ersetzt werden
%     myArrow(ax(i), 'y') % Die Funkion fügt je nach zweitem Argument einen Achsenpfeil hinzu
% end

% FileName_Res = './Figures/PathOptim'; % Dateiname für LaTeX-Export definieren
% Plot2LaTeX(fig_Res, FileName_Res) % Plot2LaTeX erzeugt aus der figure eine SVG-Datei und daraus ein PDF + LaTeX-Datei für den Text