OCLP(sys, modal, varargin)

function [sys,OPTp,Et,Po,Pc,output] = OCLP(sys, modal, varargin)
%OCLP	Computes the optimum open and closed-loop performance and
%parameters of a given OCCD system.
%
%	 [OSYS,P,Et,Pctr,Po] = OCLP(SYS) where SYS is the structure array (as
%    if created by OCSET), returns:
%		OSYS is the same as SYS, but now with the optimal open and
%        closed-loop parameters.
%		P is the optimal closed loop performance.
%		Et is the terminal energy term.
%		Po is the open-loop energy term.
%		Pc is the closed-loop energy term.
%
%    [OSYS,P,Et,Pctr,Po] = OCLP(SYS,modal) returns the performance in
%    each mode rather than total performance.
%
%    OCLP(SYS,modal,P1,V1,P2,V2,...) uses the given parameter-value pairs
%    in options used in the call to FMINSEARCH.
%
%    [OSYS,P,Et,Pctr,Po,output] = OCLP(...) gives the output structure
%    returned by the call to FMINSEARCH.
%
%    OCLP uses the values in SYS.CP and SYS.CD as intial guesses.


if nargin<2
    modal = false;
end

% Set default options
opt = optimset('TolX',1e-1,'TolFun',1e-5,'disp','none','MaxFunEvals',300);

if nargin>3
    % Apply given parameter/value pairs
    opt = optimset(opt, varargin{:});
end

if ~any(sys.Type == 1:6)
    error('Cannot find OCLP of system without a closed-loop control.')
end

if any(sys.Type == [1 2 4 5])
   % Has P control
   Xo = sys.cp;
else
    Xo = [];
    sys.cp = 0;
end
if any(sys.Type == [2 3 5 6])
   % Has D control
   Xo = [Xo; sys.cd];
else
    sys.cd = 0;   
end

if ~any(sys.Type==[2 3 4 7])
    sys.a = zeros(size(sys.xo));
    sys.b = zeros(size(sys.xo));
end

% Compute optimal closed-loop parameters
[C ignore ignore output] = fminsearch(@(X)OOLPx(X, sys),Xo,opt);
if any(sys.Type == [1 2 4 5])
    sys.cp = C(1);
end
if any(sys.Type == [2 3 5 6])
    sys.cd = C(end);
end

if any(sys.Type==[1 2 3 7])
% Compute corresponding optimal open-loop performance and parameters
    [sys OPTp Et Po Pc] = oolp(sys,modal);
else
    [OPTp Et Po Pc] = performance(sys,modal);
end



function OPTp = OOLPx(C, sys)
% Objective function for optimization
if any(sys.Type == [1 2 4 5])
    sys.cp = C(1);
end
if any(sys.Type == [2 3 5 6])
    sys.cd = C(end);
end
if any(sys.Type==[1 2 3 7])
    [ignore OPTp] = oolp(sys);
else
    OPTp = performance(sys);
end