function [f,df,ddf] = costfmin(x, baseMVA, bus, gen, gencost, branch, areas, ...
                    Ybus, Yf, Yt, mpopt, parms, ccost, N, fparm, H, Cw)
%COSTFMIN  Evaluates objective function, gradient and Hessian for OPF.
%   [f, df, ddf] = costfmin(x, baseMVA, bus, gen, gencost, branch, areas, ...
%                   Ybus, Yf, Yt, mpopt, parms, ccost, N, fparm, H, Cw)

%   MATPOWER
%   $Id: costfmin.m,v 1.8 2006/03/14 21:32:07 ray Exp $
%   by Carlos E. Murillo-Sanchez, PSERC Cornell & Universidad Autonoma de Manizales
%   and Ray Zimmerman, PSERC Cornell
%   Copyright (c) 1996-2006 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;

%% unpack needed parameters
% nb = parms(1);
ng = parms(2);
% nl = parms(3);
ny = parms(4);
% nx = parms(5);
% nvl = parms(6);
nz = parms(7);
% nxyz = parms(8);
% thbas = parms(9);
% thend = parms(10);
% vbas = parms(11);
vend = parms(12);
pgbas = parms(13);
pgend = parms(14);
qgbas = parms(15);
qgend = parms(16);
% ybas = parms(17);
% yend = parms(18);
% zbas = parms(19);
% zend = parms(20);
% pmsmbas = parms(21);
% pmsmend = parms(22);
% qmsmbas = parms(23);
% qmsmend = parms(24);
% sfbas = parms(25);
% sfend = parms(26);
% stbas = parms(27);
% stend = parms(28);

%% grab Pg & Qg
Pg = x(pgbas:pgend);            %% active generation in p.u.
Qg = x(qgbas:qgend);            %% 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
ipwl = find(gencost(:, MODEL) == PW_LINEAR);    %% pw_lin 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
  f = f + ccost * x;
end

%% generalized cost term
if ~isempty(N)
    nw = size(N, 1);
    r = N * x - fparm(:, 2);        %% Nx - rhat
    h = fparm(:, 3);
    iLT = find(r < -h);             %% below dead zone
    iEQ = find(r == 0 & h == 0);    %% dead zone doesn't exist
    iGT = find(r > h);              %% above dead zone
    iND = [iLT; iEQ; iGT];          %% rows that are Not in the Dead region
    D = sparse(iND, iND, 1, nw, nw);
    M = spdiags(fparm(:, 4), 0, nw, nw);
    hh = sparse(iND, iND, [  ones(length(iLT), 1);
                             zeros(length(iEQ), 1);
                            -ones(length(iGT), 1)], nw, nw) * h;
    rr = r + hh;

    %% create diagonal matrix w/ rr on diagonal for quadratic rows (those that
    %% need to be squared), 1 for linear rows
    iL = find(fparm(:, 1) == 1);    %% rows using linear function
    iQ = find(fparm(:, 1) == 2);    %% rows using quadratic function
    iLQ = [iL; iQ];                 %% linear rows first, then quadratic
    SQR = sparse(iLQ, iLQ, [ones(length(iL), 1); rr(iQ, 1)], nw, nw);

    w = M * D * SQR * rr;
    f = f + (w' * H * w) / 2 + Cw' * w;
end

%%----- evaluate cost gradient -----
if nargout > 1
  %% polynomial cost of P and Q
  df_dPgQg = zeros(2*ng, 1);
  for i = ipol'
    df_dPgQg(i) = baseMVA * ... %% w.r.t p.u. Pg
     polyval(polyder(gencost(i, COST:(COST+gencost(i, NCOST)-1))), xx(i));
  end
  df = [  zeros(vend, 1);       %% partial w.r.t. Va & Vm
          df_dPgQg;             %% partial w.r.t. polynomial cost Pg and Qg
          zeros(ny+nz,1);  
  ];

  %% piecewise linear cost of P and Q
  df = df + ccost';  % As in MINOS, the linear cost row is additive wrt
                     % any nonlinear cost.

  %% generalized cost term
  if ~isempty(N)
    SQR2 = (speye(nw) + sparse(iQ, iQ, 1, nw, nw)) * SQR;
    df = df + ((H * w + Cw)' * (M * D * SQR2 * N))';
    
    %% numerical check
    if 0    %% 1 to check, 0 to skip check
      ddff = zeros(size(df));
      step = 1e-7;
      tol  = 1e-3;
      for k = 1:length(x)
        xx = x;
        xx(k) = xx(k) + step;
        ddff(k) = (costfmin(xx, baseMVA, bus, gen, gencost, branch, areas, ...
                      Ybus, Yf, Yt, mpopt, parms, ccost, N, fparm, H, Cw) - f) / step;
      end
      if max(abs(ddff - df)) > tol
        idx = find(abs(ddff - df) == max(abs(ddff - df)));
        fprintf('\nMismatch in gradient\n');
        fprintf('idx             df(num)         df              diff\n');
        fprintf('%4d%16g%16g%16g\n', [ 1:length(df); ddff'; df'; abs(ddff - df)' ]);
        fprintf('MAX\n');
        fprintf('%4d%16g%16g%16g\n', [ idx'; ddff(idx)'; df(idx)'; abs(ddff(idx) - df(idx))' ]);
        fprintf('\n');
      end
    end     %% numeric check
  end
end

return;

% Currently fmincon won't use the Hessian for medium scale problems;
% when it does, or when the LargeScale methods support g, geq, A, Aeq,
% the following must be fixed for mixed poly/pwl costs.

%% ---- evaluate cost Hessian -----
if nargout > 2
  d2f_dPg2 = zeros(ng, 1);
  d2f_dQg2 = zeros(ng, 1);
  for i = 1:ng
    d2f_dPg2(i) = polyval(polyder(polyder(pcost(i,COST:(COST+pcost(i,NCOST)-1)))), ...
                    Pg(i)*baseMVA) * baseMVA^2;     %% w.r.t p.u. Pg
  end
  if ~isempty(qcost)          %% Qg is not free
    for i = 1:ng
      d2f_dQg2(i) = polyval(polyder(polyder(qcost(i,COST:(COST+qcost(i,NCOST)-1)))), ...
                    Qg(i)*baseMVA) * baseMVA^2;     %% w.r.t p.u. Qg
    end
  end
  i = [pgbas:qgend]';
  d2f = sparse(i, i, [d2f_dPg2; d2f_dQg2]);
end

return;
