function [g, h, dg, dh] = consfmin(x, om, Ybus, Yf, Yt, mpopt, il, varargin)
%CONSFMIN  Evaluates nonlinear constraints and their Jacobian for OPF.
%   [g, h] = consfmin(x, om, Ybus, Yf, Yt, mpopt)
%   [g, h, dg, dh] = consfmin(x, om, Ybus, Yf, Yt, mpopt)
%   [g, h, dg, dh] = consfmin(x, om, Ybus, Yf, Yt, mpopt, il)
%
%   Constraint evaluation function for AC optimal power flow, suitable
%   for use with fmincon. Computes constrain vectors and their
%   gradients.
%
%   Inputs:
%     x : optimization vector
%     om : OPF model object
%     Ybus : bus admittance matrix
%     Yf : admittance matrix for "from" end of constrained branches
%     Yt : admittance matrix for "to" end of constrained branches
%     mpopt : MATPOWER options vector
%     il : (optional) vector of branch indices corresponding to
%          branches with flow limits (all others are assumed to be
%          unconstrained). The default is [1:nl] (all branches).
%          Yf and Yt contain only the rows corresponding to il.
%
%   Outputs:
%     g  : vector of inequality constraint values (flow limits)
%          limit^2 - flow^2, where the flow can be apparent power
%          real power or current, depending on value of
%          OPF_FLOW_LIM in mpopt (only for constrained lines)
%     h  : vector of equality constraint values (power balances)
%     dg : (optional) inequality constraint gradients, column j is
%          gradient of g(j)
%     dh : (optional) equality constraint gradients

%   MATPOWER
%   $Id: consfmin.m,v 1.28 2009/12/08 15:45:55 ray Exp $
%   by Carlos E. Murillo-Sanchez, PSERC Cornell & Universidad Autonoma de Manizales
%   and Ray Zimmerman, PSERC Cornell
%   Copyright (c) 1996-2008 by Power System Engineering Research Center (PSERC)
%   See http://www.pserc.cornell.edu/matpower/ for more info.

%%----- initialize -----
%% define named indices into data matrices
[GEN_BUS, PG, QG, QMAX, QMIN, VG, MBASE, GEN_STATUS, PMAX, PMIN, ...
    MU_PMAX, MU_PMIN, MU_QMAX, MU_QMIN, PC1, PC2, QC1MIN, QC1MAX, ...
    QC2MIN, QC2MAX, RAMP_AGC, RAMP_10, RAMP_30, RAMP_Q, APF] = idx_gen;
[F_BUS, T_BUS, BR_R, BR_X, BR_B, RATE_A, RATE_B, RATE_C, ...
    TAP, SHIFT, BR_STATUS, PF, QF, PT, QT, MU_SF, MU_ST, ...
    ANGMIN, ANGMAX, MU_ANGMIN, MU_ANGMAX] = idx_brch;

%% unpack data
mpc = get_mpc(om);
[baseMVA, bus, gen, branch] = deal(mpc.baseMVA, mpc.bus, mpc.gen, mpc.branch);
vv = get_idx(om);

%% problem dimensions
nb = size(bus, 1);          %% number of buses
nl = size(branch, 1);       %% number of branches
ng = size(gen, 1);          %% number of dispatchable injections
nxyz = length(x);           %% total number of control vars of all types

%% set default constrained lines
if nargin < 7
    il = (1:nl);            %% all lines have limits by default
end
nl2 = length(il);           %% number of constrained lines

%% grab Pg & Qg
Pg = x(vv.i1.Pg:vv.iN.Pg);  %% active generation in p.u.
Qg = x(vv.i1.Qg:vv.iN.Qg);  %% reactive generation in p.u.

%% put Pg & Qg back in gen
gen(:, PG) = Pg * baseMVA;  %% active generation in MW
gen(:, QG) = Qg * baseMVA;  %% reactive generation in MVAr
 
%% rebuild Sbus
Sbus = makeSbus(baseMVA, bus, gen); %% net injected power in p.u.

%% ----- evaluate constraints -----
%% reconstruct V
Va = x(vv.i1.Va:vv.iN.Va);
Vm = x(vv.i1.Vm:vv.iN.Vm);
V = Vm .* exp(1j * Va);

%% evaluate power flow equations
mis = V .* conj(Ybus * V) - Sbus;

%%----- evaluate constraint function values -----
%% first, the equality constraints (power flow)
h = [ real(mis);            %% active power mismatch for all buses
      imag(mis) ];          %% reactive power mismatch for all buses

%% then, the inequality constraints (branch flow limits)
flow_max = (branch(il, RATE_A)/baseMVA).^2;
flow_max(flow_max == 0) = Inf;
if mpopt(24) == 2       %% current magnitude limit, |I|
  If = Yf * V;
  It = Yt * V;
  g = [ If .* conj(If) - flow_max;      %% branch current limits (from bus)
        It .* conj(It) - flow_max ];    %% branch current limits (to bus)
else
    %% compute branch power flows
    Sf = V(branch(il, F_BUS)) .* conj(Yf * V);  %% complex power injected at "from" bus (p.u.)
    St = V(branch(il, T_BUS)) .* conj(Yt * V);  %% complex power injected at "to" bus (p.u.)
    if mpopt(24) == 1   %% active power limit, P (Pan Wei)
      g = [ real(Sf).^2 - flow_max;         %% branch real power limits (from bus)
            real(St).^2 - flow_max ];       %% branch real power limits (to bus)
    else                %% apparent power limit, |S|
      g = [ Sf .* conj(Sf) - flow_max;      %% branch apparent power limits (from bus)
            St .* conj(St) - flow_max ];    %% branch apparent power limits (to bus)
    end
end

%%----- evaluate partials of constraints -----
if nargout > 2
  %% index ranges
  iVa = vv.i1.Va:vv.iN.Va;
  iVm = vv.i1.Vm:vv.iN.Vm;
  iPg = vv.i1.Pg:vv.iN.Pg;
  iQg = vv.i1.Qg:vv.iN.Qg;

  %% compute partials of injected bus powers
  [dSbus_dVm, dSbus_dVa] = dSbus_dV(Ybus, V);           %% w.r.t. V
  neg_Cg = sparse(gen(:, GEN_BUS), 1:ng, -1, nb, ng);   %% Pbus w.r.t. Pg
                                                        %% Qbus w.r.t. Qg
  
  %% construct Jacobian of equality constraints (power flow) and transpose it
  dh = sparse(nxyz, 2*nb);
  dh([iVa iVm iPg iQg], :) = [
    real([dSbus_dVa dSbus_dVm]) neg_Cg sparse(nb, ng);  %% P mismatch w.r.t Va, Vm, Pg, Qg
    imag([dSbus_dVa dSbus_dVm]) sparse(nb, ng) neg_Cg;  %% Q mismatch w.r.t Va, Vm, Pg, Qg
   ]';

  %% compute partials of Flows w.r.t. V
  if mpopt(24) == 2     %% current
    [dFf_dVa, dFf_dVm, dFt_dVa, dFt_dVm, Ff, Ft] = dIbr_dV(branch(il,:), Yf, Yt, V);
  else                  %% power
    [dFf_dVa, dFf_dVm, dFt_dVa, dFt_dVm, Ff, Ft] = dSbr_dV(branch(il,:), Yf, Yt, V);
  end
  if mpopt(24) == 1     %% real part of flow (active power)
    dFf_dVa = real(dFf_dVa);
    dFf_dVm = real(dFf_dVm);
    dFt_dVa = real(dFt_dVa);
    dFt_dVm = real(dFt_dVm);
    Ff = real(Ff);
    Ft = real(Ft);
  end

  %% squared magnitude of flow (of complex power or current, or real power)
  [df_dVa, df_dVm, dt_dVa, dt_dVm] = ...
          dAbr_dV(dFf_dVa, dFf_dVm, dFt_dVa, dFt_dVm, Ff, Ft);

  %% construct Jacobian of inequality constraints (branch limits)
  %% and transpose it so fmincon likes it
  dg = sparse(nxyz, 2*nl2);
  dg([iVa iVm], :) = [
    df_dVa, df_dVm;                     %% "from" flow limit
    dt_dVa, dt_dVm;                     %% "to" flow limit
  ]';

  %% use non-sparse matrices
  if mpopt(51) == 0     %% hijacked SPARSE_QP to indicate Matlab 6 => full matrices
    dh = full(dh);
    dg = full(dg);
  end
end
