function wesSisotool(C, C2, G, G2, CL, freq, handlesOld, fcnH)
% wesSisoTool
%   wesSisotool(C, C2, G, G2, CL, freq, handlesOld, fcnH) opens up a window
%   to design the controller interactively

%   Project: IMPACT
%   Title of Code: wesSisoTool
%   Relevant additional documentation:
%   Developer: Carlos Gonzalez
%   External Dependencies: NA
%
%   Disclaimer
%   CAUTION: It is strongly recommended that changes to this code
%   should be avoided without first consulting with authors.
%-----------


if isempty(C)
    C = tf(1);
end
if isempty(freq)
    freq = {0.01, 4};
end


nameC = 'OVT Controller';
nameC = sprintf('Controller Design Tool - %s', nameC);


hCLFigure = CLFigure;
handles.hFigCL = hCLFigure.hFigCL;
handles.axesMagCl = hCLFigure.axesMagCl;
handles.axesPhaseCl = hCLFigure.axesPhaseCl;

    
% figure
uiColor = get(0,'defaultUicontrolBackgroundColor');
hFig = findobj('type', 'figure', 'tag', 'mlsDesignView');
if isempty(hFig)
    hFig = figureMLS(...
        'MenuBar',      'none',...
        'Position',     [100, 100, 600, 600], ...
        'Color', uiColor,...
        'Name',         nameC,...
        'NumberTitle',  'off',...
        'DockControls', 'off',...
        'Visible',      'off',...
        'tag',          'mlsDesignView',...
        'Resize',    'off'); % open a new figure
    
    % uimenu
    f = uimenu(hFig, 'Label','Options');
    % Axes
    hAxes = uimenu(f,'Label','Axes');
    hUimenu.Axes.freeze = uimenu(hAxes, 'Label', 'Freeze', 'Checked', 'Off');
    hUimenu.Axes.fit = uimenu(hAxes, 'Label', 'Best fit', 'Checked', 'On');
    % Crossover frequency
    hCrossF = uimenu(f,'Label','Crossover frequency');
    hUimenu.CrossF.freeze = uimenu(hCrossF, 'Label', 'Freeze', 'Checked', 'Off');
    hUimenu.CrossF.move = uimenu(hCrossF, 'Label', 'Moveable', 'Checked', 'On');
    % Callback
    set([hUimenu.Axes.freeze, hUimenu.Axes.fit], 'Callback', {@uimenuAxes, hUimenu});
    set([hUimenu.CrossF.freeze, hUimenu.CrossF.move], 'Callback', {@uimenuCrossF, hUimenu});
else
    handle_tempFig = figureMLS('visible', 'off', 'handleVisibility', 'off');
    guidata(handle_tempFig, 'Cancel');
    hWarn = wrndlgMLS(handle_tempFig, sprintf('A design view tool is already present. Do you want to overwrite it?'), 'invisible');
    uiwait(hWarn);
    button = guidata(handle_tempFig);
    close(handle_tempFig);
    if strcmp(button, 'OK')
        close(hFig);
        wesSisotool(C, C2, G, G2, CL, freq, handlesOld, fcnH)
        return;
    end
end
movegui(hFig, 'center');

hWait = waitbarMLS(0, 'Loading data. Please wait...');
set(0, 'CurrentFigure', hFig);

% toolbar
setptr(hFig, 'arrow');

hTb = uitoolbar(hFig);

toolbarButton = zeros(1, 9);
tbArrow = changeColor('/private/arrow.jpg', uiColor);
tbPole = changeColor('/private/pole.jpg', uiColor);
tbZero = changeColor('/private/zero.jpg', uiColor);
tbComPole = changeColor('/private/comPole.jpg', uiColor);
tbComZero = changeColor('/private/comZero.jpg', uiColor);
tbEraser = changeColor('/private/eraser.jpg', uiColor);
tbZoomIn = changeColor('/private/zoomIn.jpg', uiColor);
tbZoomOut = changeColor('/private/zoomOut.jpg', uiColor);
tbHand = changeColor('/private/hand.jpg', uiColor);
tbInt = changeColor('private/int.jpg', uiColor);
tbDiff = changeColor('private/diff.jpg', uiColor);
tbIntDelete = changeColor('private/intDelete.jpg', uiColor);
tbDiffDelete = changeColor('private/diffDelete.jpg', uiColor);


% toggle button
toolbarButton(1) = uitoggletool(hTb,'CData',tbArrow,'TooltipString','Data point','ClickedCallback', @clickedCallback);
toolbarButton(2) = uitoggletool(hTb,'CData',tbPole,'TooltipString','Add pole','ClickedCallback', @clickedCallback);
toolbarButton(3) = uitoggletool(hTb,'CData',tbZero,'TooltipString','Add zero','ClickedCallback', @clickedCallback);
toolbarButton(4) = uitoggletool(hTb,'CData',tbComPole,'TooltipString','Add complex pole','ClickedCallback', @clickedCallback);
toolbarButton(5) = uitoggletool(hTb,'CData',tbComZero,'TooltipString','Add complex zero','ClickedCallback', @clickedCallback);
toolbarButton(6) = uitoggletool(hTb,'CData',tbEraser,'TooltipString','Remove pole/zero','ClickedCallback', @clickedCallback);
toolbarButton(7) = uitoggletool(hTb,'CData',tbZoomIn,'TooltipString','Zoom In','ClickedCallback', @clickedCallback);
toolbarButton(8) = uitoggletool(hTb,'CData',tbZoomOut,'TooltipString','Zoom Out','ClickedCallback', @clickedCallback);
toolbarButton(9) = uitoggletool(hTb,'CData',tbHand,'TooltipString','Drag','ClickedCallback', @clickedCallback);

% push button
tbButton(1) = uipushtool(hTb,'CData',tbInt,'TooltipString','Integrator','ClickedCallback', @buttonClickedCallback);
tbButton(2) = uipushtool(hTb,'CData',tbDiff,'TooltipString','Differentiator','ClickedCallback', @buttonClickedCallback);
tbButton(3) = uipushtool(hTb,'CData',tbIntDelete,'TooltipString','Delete Integrator','ClickedCallback', @buttonClickedCallback);
tbButton(4) = uipushtool(hTb,'CData',tbDiffDelete,'TooltipString','Delete Differentiator','ClickedCallback', @buttonClickedCallback);
set(tbButton(1), 'Separator', 'On');

% axes magnitude
axesMag = subplot(2, 1, 1);
colorGray = [0.4 0.4 0.4];
title('Open Loop Bode Plot', 'Color', colorGray);
ylabel('Magnitude (dB)', 'Color', colorGray);
set(axesMag, 'XScale', 'log', ...
    'XTickLabel', [], ...
    'XColor', colorGray,...
    'YColor', colorGray);
% axis phase
axesPhase = subplot(2, 1, 2);
xlabel('Frequency (rad/s)', 'Color', colorGray)
ylabel('Phase (deg)', 'Color', colorGray)
set(axesPhase, 'XScale', 'log', ...
    'XColor', colorGray,...
    'YColor', colorGray);

%% mag handle
% axes
hMag(1) = axesMag;
grid(hMag(1));
% line
hMag(2) = line(NaN, NaN, 'parent', axesMag);
% real pole
hMag(3) = line(NaN, NaN, 'parent', axesMag, ...
    'MarkerEdgeColor', [1 0 0], ...
    'Marker', 'x', ...
    'MarkerSize', 8,...
    'LineStyle', 'none');
% complex pole
hMag(4) = line(NaN, NaN, 'parent', axesMag, ...
    'MarkerEdgeColor', [1 0 0], ...
    'Marker', 'x',...
    'MarkerSize', 8,...
    'LineWidth', 2,...
    'LineStyle', 'none');
% real zero
hMag(5) = line(NaN, NaN, 'parent', axesMag, ...
    'MarkerEdgeColor', [1 0 0], ...
    'Marker', 'o', ...
    'LineStyle', 'none');
% complex zero
hMag(6) = line(NaN, NaN, 'parent', axesMag, ...
    'MarkerEdgeColor', [1 0 0], ...
    'Marker', 'o',...
    'LineWidth', 2,...
    'LineStyle', 'none');
% 0dB dotted line
hMag(7) = line(NaN, NaN, 'parent', axesMag, ...
    'LineStyle', '-.',...
    'Color', [1 0 1]);
% mag margin text
hMag(8) = uicontrol('style','text',...
    'parent',hFig,...
    'string',{'GM:', 'Freq:', 'Stable loop'},...
    'horiz','left',...
    'backgroundcolor','White',...
    'position',[90 345 120 40]);
% GM dot
hMag(9) = line(NaN, NaN, 'parent', axesMag, ...
	'MarkerEdgeColor',[1 0 1],...
	'MarkerFaceColor',[1 0 1], ...
	'Marker','o','MarkerSize',6);
% GM vertical line
hMag(10) = line(NaN, NaN, 'parent', axesMag, ...
    'Color', [1 0 1]);


% phase
hPhase(1) = axesPhase;
grid(hPhase(1));
hPhase(2) = line(NaN, NaN, 'parent', axesPhase);
hPhase(3) = line(NaN, NaN, 'parent', axesPhase, ...
    'MarkerEdgeColor', [1 0 0], ...
    'Marker', 'x', ...
    'MarkerSize', 8,...
    'LineStyle', 'none');
hPhase(4) = line(NaN, NaN, 'parent', axesPhase, ...
    'MarkerEdgeColor', [1 0 0], ...
    'Marker', 'x',...
    'MarkerSize', 8, ...
    'LineWidth', 2,...
    'LineStyle', 'none');
hPhase(5) = line(NaN, NaN, 'parent', axesPhase, ...
    'MarkerEdgeColor', [1 0 0], ...
    'Marker', 'o', ...
    'LineStyle', 'none');
hPhase(6) = line(NaN, NaN, 'parent', axesPhase, ...
    'MarkerEdgeColor', [1 0 0], ...
    'Marker', 'o',...
    'LineWidth', 2,...
    'LineStyle', 'none');
% -180C dotted line
hPhase(7) = line(NaN, NaN, 'parent', axesPhase, ...
    'LineStyle', '-.',...
    'Color', [1 0 1]);
% phase margin
hPhase(8) = uicontrol('style','text',...
    'parent',hFig,...
    'string',{'PM:', 'Freq:'},...
    'horiz','left',...
    'backgroundcolor','White',...
    'position',[90 70 120 30]);
% PM dot
hPhase(9) = line(NaN, NaN, 'parent', axesPhase, ...
	'MarkerEdgeColor',[1 0 1],...
	'MarkerFaceColor',[1 0 1], ...
	'Marker','o','MarkerSize',6);
% PM vertical line
hPhase(10) = line(NaN, NaN, 'parent', axesPhase, ...
    'Color', [1 0 1]);

linkaxes([axesMag, axesPhase], 'x');
set([axesMag, axesPhase], 'yLimMode', 'auto');

% store frequency w
warning('off','Control:combination:OutputNameClash');
sys = tf(1); %parallel(series(C, G),series(C2, G2)); CG!!
warning('on','Control:combination:OutputNameClash');

w = freq{:};

set(axesMag, 'xLim', [w(1), w(end)]);
Data.w = w; % currently used frequencies
Data.freq = freq; % original frequency range
Data.zpX = []; % zero/pole x coordinates
Data.crossF = 1; % crossover frequency [rad/s]

waitbarMLS(0.5, hWait);

% toolbar struct
Tb = struct(...
    'selected', '', ...
    'Arrow', [],...
    'Pole', [],...
    'ComPole', [],...
    'ComZero', [],...
    'Eraser', [],...
    'ZoomIn',[],...
    'ZoomOut', [],...
    'Hand',[]);

%% some initial values
Tb.ZoomIn.status = 'off';
hLine = zeros(1, 4);
for m = 1:4
    hLine(m) = line(NaN, NaN, 'parent', gca, ...
        'LineStyle', '--',...
        'Color', [0.3 0.3 0.3]);
end
Tb.ZoomIn.hLine = hLine;

Tb.Hand.zpk = '';
Tb.Hand.status = 'off';

Tb.Arrow.hInfo = uicontrol('style','text',...
    'parent',hFig,...
    'string',{'', ''},...
    'horiz','left',...
    'position',[90 280 120 30]);
strC = 'u1';

%% guidata
handles.toolbarButton = toolbarButton; % toggle button handles
handles.tbButton = tbButton; % push button handles
handles.hFig = hFig; % figure handle
handles.hUimenu = hUimenu; % uimenu
handles.C = C; % controller tf  
handles.G = tf(1); % For compatibility
handles.G2 = G2; % For compatibility
handles.C2 = C2; % For compatibility
handles.CL = CL; % For compatibility
handles.PlantComplex = G;
handles.sys = sys; % system tf
handles.hMag = hMag; % handles related to magnitude plot
handles.hPhase = hPhase; % handles related to phase plot
handles.Data = Data; % some data
handles.Tb = Tb; % toolbar struct, used in callback
handles.fcnH = fcnH; % function handle to store controller value
handles.strC =  strC; % controller string
guidata(hFig, handles); % update handle


% update fcn into controller design
handlesOld.DesignView.updatePlotFcn = @updatePlot;
guidata(handlesOld.edit_ptoCont, handlesOld);

handles.handlesOld = handlesOld; % handle of controller design

% initial axis x limit
set(gca, 'XLim', [w(1), w(end)]);
updatePlot(handles);

waitbarMLS(1, hWait);
close(hWait);

% set fig handlevisibility
set(hFig, 'HandleVisibility', 'Callback', 'Visible', 'On');


function cData = changeColor(pic, uiColor)

pic = strrep(pic, '/', filesep);
cData = imread(pic);
% change color of toolbar icons
for p = 1:3
   mat = cData(:, :, p); 
   [nRows, nCols] = size(mat);
   for m = 1:nRows
      for n = 1:nCols
         if mat(m, n) > 100
             mat(m, n) = uiColor(p)*255;
         end
      end
   end
   cData(:, :, p) = mat;
end
