function om = add_costs(om, name, cp, varsets)
%ADD_COSTS  Adds a set of user costs to the model
%   om = add_costs(om, name, cp, varsets);
%
%   e.g.
%     cp1 = struct('N', N1, 'Cw', Cw1);
%     cp2 = struct('N', N2, 'Cw', Cw2, 'H', H, 'dd', dd, ...
%                   'rh', rh, 'kk', kk, 'mm', mm);
%     om = add_costs(om, 'usr1', cp1, {'Pg', 'Qg', 'z'});
%     om = add_costs(om, 'usr2', cp2, {'Vm', 'Pg', 'Qg', 'z'});
%
%   x is the vector formed by combining the specified varsets
%   cp is a struct with the following fields:
%   N is sparse, nw x nx
%   Cw, nw x 1
%   H is sparse, nw x nw (optional, all zeros by default)
%   dd, mm are nw x 1 (optional, all ones by default)
%   rh, kk are nw x 1 (optional, all zeros by default)
%
%   R  = N*x - rh
%
%   U(i) =  /  0, -kk(i) <= R(i) <= kk(i)
%           \  1, otherwise
%
%           /  kk(i), R(i) < -kk(i)
%   K(i) = <   0,     -kk(i) <= R(i) <= kk(i)
%           \ -kk(i), R(i) > kk(i)
%
%   RR = R + K
%
%   Dl = diag(mm) * diag(U) * diag(DDL)
%
%   Dq = diag(mm) * diag(U) * diag(DDQ)
%
%   DDL(i) = /  1, dd(i) = 1
%            \  0, otherwise
%
%   DDQ(i) = /  1, dd(i) = 2
%            \  0, otherwise
%
%   w = (Dl + Dq * diag(RR)) * RR
%
%   f = 1/2 * w'*H*w + Cw'*w,
%

%   MATPOWER
%   $Id: add_costs.m,v 1.2 2009/11/04 15:58:44 ray Exp $
%   by Ray Zimmerman, PSERC Cornell
%   Copyright (c) 2008 by Power System Engineering Research Center (PSERC)
%   See http://www.pserc.cornell.edu/matpower/ for more info.

%% prevent duplicate named cost sets
if isfield(om.cost.idx.N, name)
    error('@opf_model/add_costs: cost set named ''%s'' already exists', name);
end

if nargin < 4
    varsets = {};
end
if isempty(varsets)
    varsets = om.var.order;
end
[nw, nx] = size(cp.N);

%% check sizes
nv = 0;
for k = 1:length(varsets)
    nv = nv + om.var.idx.N.(varsets{k});
end
if nx ~= nv
    if nw == 0
        cp.N = sparse(nw, nx);
    else
        error('@opf_model/add_costs: number of columns in N (%d x %d) does not match\nnumber of variables (%d)\n', nw, nx, nv);
    end
end
if size(cp.Cw, 1) ~= nw
    error('@opf_model/add_costs: number of rows of Cw (%d x %d) and N (%d x %d) must match\n', size(cp.Cw), nw, nx);
end
if isfield(cp, 'H') && (size(cp.H, 1) ~= nw || size(cp.H, 2) ~= nw)
    error('@opf_model/add_costs: both dimensions of H (%d x %d) must match the number of rows in N (%d x %d)\n', size(cp.H), nw, nx);
end
if isfield(cp, 'dd') && size(cp.dd, 1) ~= nw
    error('@opf_model/add_costs: number of rows of dd (%d x %d) and N (%d x %d) must match\n', size(cp.dd), nw, nx);
end
if isfield(cp, 'rh') && size(cp.rh, 1) ~= nw
    error('@opf_model/add_costs: number of rows of rh (%d x %d) and N (%d x %d) must match\n', size(cp.rh), nw, nx);
end
if isfield(cp, 'kk') && size(cp.kk, 1) ~= nw
    error('@opf_model/add_costs: number of rows of kk (%d x %d) and N (%d x %d) must match\n', size(cp.kk), nw, nx);
end
if isfield(cp, 'mm') && size(cp.mm, 1) ~= nw
    error('@opf_model/add_costs: number of rows of mm (%d x %d) and N (%d x %d) must match\n', size(cp.mm), nw, nx);
end

%% add info about this user cost set
om.cost.idx.i1.(name)  = om.cost.N + 1;     %% starting index
om.cost.idx.iN.(name)  = om.cost.N + nw;    %% ending index
om.cost.idx.N.(name)   = nw;                %% number of costs (nw)
om.cost.data.N.(name)  = cp.N;
om.cost.data.Cw.(name) = cp.Cw;
om.cost.data.vs.(name) = varsets;
if isfield(cp, 'H')
    om.cost.data.H.(name)  = cp.H;
end
if isfield(cp, 'dd')
    om.cost.data.dd.(name) = cp.dd;
end
if isfield(cp, 'rh')
    om.cost.data.rh.(name) = cp.rh;
end
if isfield(cp, 'kk')
    om.cost.data.kk.(name) = cp.kk;
end
if isfield(cp, 'mm')
    om.cost.data.mm.(name) = cp.mm;
end

%% update number of vars and var sets
om.cost.N  = om.cost.idx.iN.(name);
om.cost.NS = om.cost.NS + 1;

%% put name in ordered list of var sets
om.cost.order{om.cost.NS} = name;
