function updatePlot(handles)
% updatePlot
%   internal function

%   Project: IMPACT
%   Title of Code: updatePlot
%   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.

%% values in handles
% controller tf
C = handles.C;
C2 = handles.C2;
% plant tf
G = handles.G;
PlantComplex = handles.PlantComplex;
G2 = handles.G2;
% Close loop feedback
CL = handles.CL;
% frequency range
w = handles.Data.w;
% magnitude handles
hMag = handles.hMag;
% phase handles
hPhase = handles.hPhase;
if (gca ~= hMag(1)) && (gca~= hPhase(1))
    set(0, 'CurrentFigure', handles.hFig);
end
% current x limit
xLim = get(gca, 'XLim');

%% freeze or best fit
% mag ylim
yLimMag = get(hMag(1), 'YLim');
% phase ylim
yLimPhase = get(hPhase(1), 'YLim');


%% update lines
arComplexC = complexTF(C,w);
sys = PlantComplex.*arComplexC;


% calculate responses
mag = abs(sys);
if isnan(mag(1)) % no plots
    return;
end
%% freeze or moveable for crossover frequency
if strcmpi(get(handles.hUimenu.CrossF.freeze, 'Checked'), 'on')
    % freeze
    crossF = handles.Data.crossF;
    if crossF<w(1) || crossF>w(end)
        errordlgMLS('Crossover frequency is beyond current frequency range. Please use another gain crossover frequency');
        return;
    end
    crossFMag = interp1(w, mag, crossF);
    crossFMagGain = 1/crossFMag;
    mag = crossFMagGain*mag;
    C = crossFMagGain*C;
    arComplexC = complexTF(C,w);
    sys = PlantComplex.*arComplexC;
end

phase = unwrap(angle(sys));
phase = phase/pi*180;

% solid line
set(hMag(2), 'XData', w, 'YData', 20*log10(mag));
set(hPhase(2), 'XData', w, 'YData', phase);

% set x limit
set(hMag(1), 'XLim',xLim);


%% update gain/phase margin text, Gm Pm dots and vertical lines
[Gm,Pm,Wcg,Wcp] = marginWEC(sys,w);

if Gm == 0
    Gm = inf;
else
    Gm = 20*log10(Gm);
end
if ~isfinite(Gm)
    strGM = 'GM: Inf';
    strGMF = 'Freq: NaN';
else
    strGM = sprintf('GM: %g dB', Gm);
    strGMF = sprintf('Freq: %g rad/s', Wcg);
end
if Gm<0 % Encircle (-1,0)
    str = 'Unstable loop';
else
    str = 'Stable loop';
end

set(hMag(8), 'String', {strGM, strGMF, str});
if ~isfinite(Pm)
    strPM = 'PM: Inf';
    strPMF = 'Freq: NaN';
else
    strPM = sprintf('PM: %g deg', Pm);
    strPMF = sprintf('Freq: %g rad/s', Wcp);
end


% Gm, Pm text
set(hMag(8), 'String', {strGM, strGMF, str});
set(hPhase(8), 'String', {strPM, strPMF});

% dots & vertical lines
set(hMag(9),'Xdata',Wcg,'Ydata',-Gm);
set(hMag(10),'Xdata',[Wcg Wcg],'Ydata',[-Gm 0]);   % vertical line

% Determine the phase line associated with the phase margin (-180 modulo 360)
if isfinite(Pm) && (Wcp>=w(1) && Wcp<=w(end))
    Yp = 180 * round((interp1(w,phase,Wcp)-Pm)/180);
else
    Yp = NaN;
end
if isnan(Yp)
    set(hPhase(9),'Xdata',[],'Ydata',[]);
    set(hPhase(10),'Xdata',[],'Ydata',[]);
else
    set(hPhase(9),'Xdata',Wcp,'Ydata',Yp+Pm);
    set(hPhase(10),'Xdata',[Wcp Wcp],'Ydata',[Yp Yp+Pm]);
end

% reset 0dB, -180C dotted line
yLim = get(hMag(1), 'YLim');
if yLim(1)>0 || yLim(2)<0
    set(hMag(7), 'XData', [], 'YData', []);
else
    set(hMag(7), 'XData', w, 'YData', zeros(1, numel(w)));
end
yLim = get(hPhase(1), 'YLim');
if isnan(Yp)
    set(hPhase(7), 'XData', [], 'YData', []);
else
    set(hPhase(7), 'XData', w, 'YData', Yp*ones(1, numel(w)));
end

%% update zero/pole
[z, p, k] = zpkdata(C);
z = z{1}; p = p{1};

% real/complex
[rZ, cZ] = divideRC(z);
[rP, cP] = divideRC(p);

% get frequencies for given pole/zero
vecX = [];
% real pole
rPX = [];
% real pole for mag and phase
for m = 1:numel(rP)
    vecX = [vecX, abs(rP(m))];
    rPX = [rPX, abs(rP(m))];
end
% real zero
rZX = [];
for m = 1:numel(rZ)
    vecX = [vecX, abs(rZ(m))];
    rZX = [rZX, abs(rZ(m))];
end
% complex pole
cPX = [];
for m = 1:2:numel(cP)
    vecX = [vecX, abs(cP(m))];
    cPX = [cPX, abs(cP(m))];
end
% complex zero
cZX = [];
for m = 1:2:numel(cZ)
    vecX = [vecX, abs(cZ(m))];
    cZX = [cZX, abs(cZ(m))];
end

vecX = sort(vecX);
if isempty(vecX)
    mag = [];
    phase = [];
else
    mag = interp1(w, mag, vecX);
    phase = interp1(w, phase, vecX);
end

% real pole
updatePZ(hMag(3), hPhase(3), rPX, vecX, mag, phase);
% real zero
updatePZ(hMag(5), hPhase(5), rZX, vecX, mag, phase);
% complex pole
updatePZ(hMag(4), hPhase(4), cPX, vecX, mag, phase);
% complex zero
updatePZ(hMag(6), hPhase(6), cZX, vecX, mag, phase);

% update zero/pole x values
Data = handles.Data;
Data.zpX = vecX;
Data.zpMag = mag;
Data.zpPhase = phase;
handles.Data = Data;

% update controller gain for crossover frequency
handles.C = C;
guidata(handles.hFig, handles);

%% freeze or best fit
if strcmpi(get(handles.hUimenu.Axes.freeze, 'Checked'), 'on')
    % freeze
    set(hMag(1), 'XLim', xLim, 'YLim', yLimMag);
    set(hPhase(1), 'XLim', xLim, 'YLim', yLimPhase);
end

% % % % update analysis plots
% % % updateAnalysisPlots(handles);
% update controller_design
dataUpdate.GOL = PlantComplex.*arComplexC;
dataUpdate.w = w;
updateCtrlPlot(z, p, k, handles.strC, handles.handlesOld, handles.fcnH, handles.Tb.Hand,dataUpdate,handles);


function updateCtrlPlot(z, p, k, strC, handles, fcnH, Hand, dataUpdate,hnd)
% update controller values in controller design
% update frequency and step responses

% handles: handles of controller design tool
contCell = get(handles.popupmenu_designView,'String');
val = get(handles.popupmenu_designView,'Value');
controller = contCell{val};

% update controllers
% handle of controller edit text

switch controller
    case 'C1'
        hC = handles.edit_ptoCont;
        str = 'ptoCont';
        
    case 'C2'
        hC = handles.edit_ptoContX;
        str = 'ptoContX';
end
strZ = getCStr(z);
strP = getCStr(p);
strK = getCStr(k);
strC = sprintf('zpk(%s, %s, %s)', strZ, strP, strK);
set(hC, 'String', strC);
fcnH(strC, str, handles);

%% update frequency & step plots
% check design view radiobutton

if strcmp(Hand.status, 'off')
    handlesFig = hnd;
    if ~ishandle(handlesFig.hFigCL)
        hFigCL = CLFigure();
        handlesFig.hFigCL = hFigCL.hFigCL;
        handlesFig.axesMagCl = hFigCL.axesMagCl;
        handlesFig.axesPhaseCl = hFigCL.axesPhaseCl;
    end
    colorGray = [0.4 0.4 0.4];
    figure(handlesFig.hFigCL);
    
    axes(handlesFig.axesMagCl);
    GOL = dataUpdate.GOL;
    w = dataUpdate.w;
    GCL = GOL./(1+GOL);
    magCL = abs(GCL);
    phaseCL = angle(GCL)*180/pi;
    semilogx(w,20*log10(magCL));
    grid on;
    set(handlesFig.axesMagCl, 'XScale', 'log', ...
        'XTickLabel', [], ...
        'XColor', colorGray,...
        'YColor', colorGray);
    title('Open Loop Bode Plot', 'Color', colorGray);
    ylabel('Magnitude (dB)', 'Color', colorGray);
    
    axes(handlesFig.axesPhaseCl);
    semilogx(w,phaseCL);
    grid on;
    set(handlesFig.axesPhaseCl, 'XScale', 'log', ...
        'XColor', colorGray,...
        'YColor', colorGray);
    xlabel('Frequency (rad/s)', 'Color', colorGray)
    ylabel('Phase (deg)', 'Color', colorGray)
    guidata(handlesFig.hFig,handlesFig);
end


function str = getCStr(vec)
% return string for given vector

str = '';
for m = 1:numel(vec)
    if isreal(vec(m))
        str = sprintf('%s%g ', str, vec(m));
    else
        str = sprintf('%s%g%+gi ', str, real(vec(m)), imag(vec(m)));
    end
end

str = ['[', str, ']'];




function setYLim(hAxes, vec)
% set the y limit as 110% of max - min

minVal = min(vec);
maxVal = max(vec);
change = (maxVal - minVal)*(1.1-1)/2;
val(1) = minVal - change;
val(2) = maxVal + change;
set(hAxes, 'YLim', val);



function updatePZ(hM, hP, x, w, mag, phase)
% update pole and zero

% hM: mag handle to the pole or zero
% hP: phase handle to the pole or zero
% x: x frequency for one pole or zero
% w: all frequencies

if isempty(x)
    set(hM, 'XData', [], 'YData', []);
    set(hP, 'XData', [], 'YData', []);
    return;
end
y = zeros(1, numel(x));
ph = y; % phase plot
for m = 1:numel(x)
    for n = 1:numel(w)
        if x(m) == w(n)
            y(m) = mag(n);
            ph(m) = phase(n); % phase
            break;
        end
    end
end
set(hM, 'XData', x, 'YData', 20*log10(y));
set(hP, 'XData', x, 'YData', ph);


function [rVec, cVec] = divideRC(vec)
% divide real/complex values
rVec = [];
cVec = [];
for m = 1:numel(vec)
    val = vec(m);
    if isreal(val)
        rVec = [rVec, val];
    else
        cVec = [cVec, val];
    end
end






