[ u, x, p, cnv, ztrue]=disope( u0, A, B, Q, R, PHI, r1, r2, kr, options, tol )

function  [ u,  x, p, cnv, ztrue]=disope( u0, A, B, Q, R, PHI, r1, r2, kr, options, tol )

% Main DISOPE algorithm in Continuous Time
% Implemented by Dr. V.M. Becerra
% September 1999

  iter_max  = options(1);

  nstep     = options(2);

  debug     = options(3);
 
  x0opt     = options(4);

  deltat    = options(5);


  [ n,  m ] = size( B );


  x0 = fstar( [], [], 'xinit' );


% Compute Qbar Rbar:

  Qbar = Q + r2*eye(size( Q ) );

  Rbar = R + r1*eye(size( R ) );

% initialize some matrices:
 
  lambda = zeros(m, nstep+1  );

  beta   = zeros(n, nstep+1  );

  alpha  = zeros(n, nstep+1  );

% Set nominal solution

  v = u0; 

  ph = zeros( n, nstep+1 );

% Compute performance index

  [pin z] = pindex( v, x0, options );

  cnv = [ pin  0 ];

% iterative loop:

  for iter=1:iter_max,


       for i=1:nstep+1,

       % Compute alpha

         alpha(:,i) = fstar( z(:,i), v(:,i), 'value' ) - A*z(:,i) - B*v(:,i);

       % Compute beta

         beta(:,i) = ( A - fstar( z(:,i), v(:,i), 'gradz') )'*ph(:,i);

         beta(:,i) = beta(:,i) + Qbar*z(:,i) - lstar(z(:,i),v(:,i),'gradz');  
  
       % Compute lambda

         lambda(:,i) = ( B - fstar( z(:,i), v(:,i), 'gradv') )'*ph(:,i);

         lambda(:,i) = lambda(:,i) + Rbar*v(:,i) - lstar(z(:,i),v(:,i),'gradv');  
       end;

       % Compute GAMMA1

       GAMMA1 = PHI*z(:,nstep+1) - phistar( z(:,nstep+1), 'gradz');


       if ( x0opt == 1 & iter>1 )

           x0 = Qbar\(-A'*ph(:,2) + beta(:,1) );

       end;
           
       % Solve MMOP

       [u,x,p] = mmopsol(A,B,Qbar,Rbar,PHI,x0,lambda,beta,alpha,GAMMA1,nstep,deltat);

       % Calculate control variation norm

       nrm = norm( u - v );

       % Update variables

       v = v + kr(1)*( u - v );

       z = z + kr(2)*( x - z );

       ph= ph+ kr(3)*( p - ph);

       % Compute performace index

       [pin ztrue] = pindex( v,  x0, options );

       cnv =  [ cnv; [pin, nrm] ];

       if debug == 1 

        disp(['Iter: ', num2str(iter), ' J: ', num2str(pin), ' norm: ' num2str(nrm)]);
       end;

       % Check convergence

       if ( nrm <= tol )
 
             break;

       end ;

  end;


return;