function [f, g] = fun_copf(x, om, Ybus, Yf, Yt, Afeq, bfeq, Af, bf, mpopt, il)
%CONSFMIN6  Evaluates objective function and constraints for OPF.
%   [f, g] = fun_copf(x, om, Ybus, Yf, Yt, Afeq, bfeq, Af, bf, mpopt)
%   [f, g] = fun_copf(x, om, Ybus, Yf, Yt, Afeq, bfeq, Af, bf, mpopt, il)
%
%   Objective and constraint evaluation function for AC optimal power
%   flow, suitable for use with constr.
%
%   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
%     Afeq : sparse A matrix for linear equality constraints
%     bfeq : right hand side vector for linear equality constraints
%     Af : sparse A matrix for linear inequality constraints
%     bf : right hand side vector for linear inequality constraints
%     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:
%     f   : value of objective function
%     g  : vector of constraint values, in the following order
%             non-linear equality (power balance)
%             linear equality
%             non-linear inequality (flow limits)
%             linear inequality
%          The flow limits (limit^2 - flow^2) can be apparent power
%          real power or current, depending on value of OPF_FLOW_LIM
%          in mpopt (only for constrained lines)

%   MATPOWER
%   $Id: fun_copf.m,v 1.10 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;
[PW_LINEAR, POLYNOMIAL, MODEL, STARTUP, SHUTDOWN, NCOST, COST] = idx_cost;

%% default args
if nargin < 11
    il = [];
end

%% unpack data
mpc = get_mpc(om);
[baseMVA, bus, gen, branch, gencost] = ...
    deal(mpc.baseMVA, mpc.bus, mpc.gen, mpc.branch, mpc.gencost);
cp = get_cost_params(om);
[N, H, Cw] = deal(cp.N, cp.H, cp.Cw);
fparm = [cp.dd cp.rh cp.kk cp.mm];
vv = get_idx(om);

%% problem dimensions
ny = get_var_N(om, 'y');    %% number of piece-wise linear costs
nxyz = length(x);           %% total number of control vars of all types

%% set default constrained lines
if isempty(il)
    nl = size(branch, 1);   %% number of branches
    il = (1:nl);            %% all lines have limits by default
end

%% 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

%%----- evaluate objective function -----
%% polynomial cost of P and Q
% use totcost only on polynomial cost; in the minimization problem
% formulation, pwl cost is the sum of the y variables.
ipol = find(gencost(:, MODEL) == POLYNOMIAL);   %% poly MW and MVAr costs
xx = [ gen(:, PG); gen(:, QG)];
if ~isempty(ipol)
  f = sum( totcost(gencost(ipol, :), xx(ipol)) );  %% cost of poly P or Q
else
  f = 0;
end

%% piecewise linear cost of P and Q
if ny > 0
  ccost = full(sparse(ones(1,ny), vv.i1.y:vv.iN.y, ones(1,ny), 1, nxyz));
  f = f + ccost * x;
end

%% generalized cost term
if ~isempty(N)
    nw = size(N, 1);
    r = N * x - fparm(:, 2);        %% Nx - rhat
    k = fparm(:, 3);
    iLT = find(r < -k);             %% below dead zone
    iEQ = find(r == 0 & k == 0);    %% dead zone doesn't exist
    iGT = find(r > k);              %% above dead zone
    iND = [iLT; iEQ; iGT];          %% rows that are Not in the Dead region
    iL = find(fparm(:, 1) == 1);    %% rows using linear function
    iQ = find(fparm(:, 1) == 2);    %% rows using quadratic function
    LL = sparse(iL, iL, 1, nw, nw);
    QQ = sparse(iQ, iQ, 1, nw, nw);
    kbar = sparse(iND, iND, [   ones(length(iLT), 1);
                                zeros(length(iEQ), 1);
                                -ones(length(iGT), 1)], nw, nw) * k;
    rr = r + kbar;                  %% apply non-dead zone shift
    M = sparse(iND, iND, fparm(iND, 4), nw, nw);    %% dead zone or scale
    diagrr = sparse(1:nw, 1:nw, rr, nw, nw);
    
    %% linear rows multiplied by rr(i), quadratic rows by rr(i)^2
    w = M * (LL + QQ * diagrr) * rr;

    f = f + (w' * H * w) / 2 + Cw' * w;
end

%% ----- 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);

%% rebuild Sbus
Sbus = makeSbus(baseMVA, bus, gen); %% net injected power in p.u.

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

%%----- evaluate constraint function values -----
%% first, the equality constraints (power flow)
geq = [ 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;
  gineq = [ 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)
      gineq = [ 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|
      gineq = [ Sf .* conj(Sf) - flow_max;    %% branch apparent power limits (from bus)
                St .* conj(St) - flow_max ];  %% branch apparent power limits (to bus)
    end
end

g = [geq; Afeq * x - bfeq; gineq; Af * x - bf];
