%
 function [c, ceq, c_Z, ceq_Z]  = con_otis(Z,data)
%
% This is the constraint function used by the minimization routine
% 
% Z is the array of  control and state values
%
%   |                             |
%   |  u_1   u_2  .... u_Npanels  |
%   |  x_1   x_2  .... x_Npanels  |
%   |                             |
%
% data is a structure that contains information about
% the problem.
%
%
% The equality constraints include the N_states*N_panels
% 'defects' that arise from the collocation procedure,
% followed by (possibly) the end-conditions (Phi_eq)
%
% The inequality constraints include only 
% the end-conditions (Phi_in)
%
   c = []; ceq = []; c_Z=[]; ceq_Z=[];
% 
  i_state   = data.N_controls+1;
  Z_rows    = data.N_controls+data.N_states;
  local.t_r = 0.;
  local.x_r = data.X_initial;
%
% set up some index arrays for sparse matrix manipulations
%
  if nargout > 2
     J_rows     = data.N_panels*data.N_states;
     J_cols     = data.N_panels*(data.N_states+data.N_controls);
     ceq_Z      = spalloc(J_rows,J_cols,...
                  data.N_panels*(2*data.N_states+data.N_controls));
     i_u        = repmat([1:data.N_states]',data.N_controls,1);
     j_u        = repmat([1:data.N_controls],data.N_states,1);
     j_u        = j_u(:);
     i_x        = repmat([1:data.N_states]',data.N_states,1);
     j_x        = repmat([1:data.N_states],data.N_states,1);
     j_x        = j_x(:) + data.N_controls;
     n_sparse_u = data.N_states*data.N_controls;
     n_sparse_x = data.N_states*data.N_states;
  end % if
%
% Loop over the panels
%
  ceq = [];
  for k=1:data.N_panels
    local.t_l = local.t_r;
    if isfield(data,'t_array')
       local.t_r = data.t_array(k);
    else
       local.t_r = local.t_r +  data.T_f/data.N_panels;
    end       
    local.x_l = local.x_r;
    local.x_r = Z((k-1)*Z_rows+i_state:k*Z_rows);
    local.u   = Z((k-1)*Z_rows+1:(k-1)*Z_rows+data.N_controls);
%
%     x_r - x_l - f(t_avg,x_avg,u)*del_t = 0
%
    if nargout < 3
       ceq = [ceq ; defect(local,data)];
    else
       del_t_2 = (local.t_r - local.t_l)/2;
       [resid, rhs_u, rhs_x]  = defect(local,data);
       ceq   = [ ceq; resid ];
       if k > 1
           ceq_Z = ceq_Z   ...
           + sparse(i_x,j_x, ...
               -(eye(data.N_states)+rhs_x*del_t_2),J_rows,J_cols ); % f_x-left
           j_x = j_x + data.N_states+data.N_controls; % col_shift_x;
       end % if k
       ceq_Z = ceq_Z   ...
       + sparse(i_u,j_u,  ...
                -rhs_u*2*del_t_2,J_rows,J_cols )  ...
       + sparse(i_x,j_x,  ...
                (eye(data.N_states)-rhs_x*del_t_2),J_rows,J_cols ); % f_x-right
%
       i_u  = i_u + data.N_states;                 % row_shift_u;
       j_u  = j_u + data.N_states+data.N_controls; % col_shift_u;
       i_x  = i_x + data.N_states;                 % row_shift_x;
%
    end % if nargout block
%
  end % k-loop
%
% augment ceq and c with end-conditions
% also add rows to the corresponding Jacobian for the end-conditions
%
  x_final = Z((data.N_panels-1)*Z_rows + i_state:end);
%
  if isfield(data,'Phi_eq')
     tmp      = feval(data.Phi_eq,x_final);
     N_eq_end = length(tmp);
     ceq      = [ceq ; tmp];
     if nargout > 2
        Phi_eqj = feval(data.Phi_eqj,x_final);
        i_eqj   = repmat([1:N_eq_end]',data.N_states,1);
        j_eqj   = repmat([1:data.N_states],N_eq_end,1);
        j_eqj   = j_eqj(:) + (J_cols - data.N_states );
        ceq_Z   = [ ceq_Z ; 
                  sparse(i_eqj,j_eqj(:),Phi_eqj(:),N_eq_end,J_cols)]; 
     end % nargout
  end   % isfield
%
  if isfield(data,'Phi_in')
     c        = feval(data.Phi_in,x_final);
     if nargout > 2
        N_in_end = length(c);
        i_inj    = repmat([1:N_in_end]',data.N_states,1);
        j_inj    = repmat([1:data.N_states],N_in_end,1);
        j_inj    = j_inj(:) + (J_cols - data.N_states );
        Phi_inj  = [feval(data.Phi_inj,x_final)]';
        c_Z      = sparse(i_inj,j_inj,Phi_inj(:),N_in_end,J_cols) ;
     end % nargout
  end   % isfield
%
  if nargout > 2
     ceq_Z = ceq_Z';
     ceq_Z = full(ceq_Z);
     c_Z   = c_Z'  ;  
     c_Z   = full(c_Z);
  end % if nargout
% return
%
%===========================================================
% sub function for con_otis
%===========================================================
%
  function [resid, rhs_u, rhs_x] = defect(local,data)
% 
% local is a data structure that has 
%
%   loc.t_l  the time value at the 'left' end of this interval
%   loc.t_r  the time value at the 'right' end of this interval
%   loc.x_l  the state value at the 'left' end of this interval
%   loc.t_r  the state value at the 'right' end of this interval
%   loc.u    the control vector on this interval
%
%
% average the left and right values for time and state
%
    t_avg = (local.t_l + local.t_r)/2;
    x_avg = (local.x_l + local.x_r)/2;
%
    del_t = local.t_r - local.t_l;
    resid = local.x_r - local.x_l ...
          - feval(data.Rhs,t_avg,x_avg,[],local.u)*del_t;
%
  if nargout > 1
     [rhs_x, rhs_u] = feval(data.Rhsj,t_avg,x_avg,local.u);
  end % if nargout
%
% return
%
%
