function [Gm,Pm,Wcg,Wcp] = marginWEC(arSys,arFreq)
% marginWEC 
%   marginWEC returns the phase and gain margin

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


zModel =  complexToMagPhase(arSys,arFreq);
arMagnitude = zModel.arMagnitude;
arPhase = zModel.arPhase;
arCrossOver = find((arMagnitude(1:end-1)>=1 & arMagnitude(2:end)<=1) | (arMagnitude(1:end-1)<=1 & arMagnitude(2:end)>=1));
if ~isempty(arCrossOver)
    [Pm, minInd] = min(abs(angleTo180(arPhase(arCrossOver))));
    Wcp = arFreq(arCrossOver(minInd));
else
    Pm = inf;
    Wcp = inf;
end

maxPhase = ceil(max(arPhase/360));
minPhase = floor(min(arPhase/360));

arPhaseCross = [];
arPhase360 = arPhase;
for iCont = minPhase:maxPhase
    arPhaseCrossI = find((arPhase360(1:end-1)>=360*(iCont-1)+180 & arPhase360(2:end)<=360*(iCont-1)+180) | (arPhase360(1:end-1)<=360*(iCont-1)+180 & arPhase360(2:end)>=360*(iCont-1)+180));%...
    arPhaseCross = [arPhaseCross,arPhaseCrossI'];
end

if ~isempty(arPhaseCross)
    [Gm, maxInd] = max(arMagnitude(arPhaseCross));
    Gm = 1/Gm;
    Wcg = arFreq(arPhaseCross(maxInd));
else
    Gm = inf;
    Wcg = inf;
end


function arOut = angleTo180(arVal)
arOut = zeros(1,numel(arVal));
for iCont=1:numel(arVal)
    rVal = arVal(iCont);
    if rVal>0
        while(rVal>360)
            rVal = rVal - 360;
        end
        rOut = rVal-180;
    else
        while(rVal<-360)
            rVal = rVal + 360;
        end
        rOut = rVal+180;
    end
    arOut(iCont) = rOut;
end
