immediate(sys,modal,varargin)

function [sys OPTp Et Po Pc output] = immediate(sys,modal,varargin)
%IMMEDIATE	Computes immediate time optimized performance and parameters 
%for a given OCCD system.
%
%	 [OSYS,OPTp,Et,Pctr,Po] = IMMEDIATE(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 and the immediate optimal terminal time.
%		P is the time optimized performance.
%		Et is the terminal energy.
%		Po is the open-loop energy.
%		Pc is the closed-loop energy.

%    [OSYS,P,Et,Pctr,Po] = IMMEDIATE(SYS,modal) returns the performance in
%    each mode rather than total performance.
%
%    IMMEDIATE(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] = IMMEDIATE(...) gives the output structure
%    returned by the call to FMINSEARCH.
%
%    IMMEDIATE uses the values in SYS.CP and SYS.CD as intial guesses.  The
%    first minimum of the uncontrolled performance is used as the initial
%    guess for the terminal time.


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

% Determine first minimum of performance of uncontrolled system.
[Tp,ThtP] = ocnodes(sys);
if ThtP/Tp < 1e-2 % If too soon, take second node
    ThtP = ThtP + Tp;
end
sys.To = ThtP;

Xo = sys.To;
if any(sys.Type == [1 2 4 5])
   % Has P control
   Xo = [Xo;sys.cp];
else    
    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==[1 2 3 7])
    % No open-loop control
    sys.a = zeros(size(sys.xo));
    sys.b = zeros(size(sys.xo));
end

% Compute optimal closed-loop parameters and terminal time
[C fval ignore output] = fminsearch(@(X)OOLPx(X, sys), Xo,opt);

sys.To = C(1);
if any(sys.Type == [1 2 4 5])
    sys.cp = C(2);
end
if any(sys.Type == [2 3 5 6])
    sys.cd = C(end);
end

% Compute corresponding optimal open-loop performance and parameters
if any(sys.Type==[1 2 3 7])
    [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
sys.To = C(1);
if any(sys.Type == [1 2 4 5])
    sys.cp = C(2);
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