function [Fpenalty, Dnew] = satlib_calcPenalty(varargin)
% function [Fpenalty, Dnew] = satlib_calcPenalty(varargin)
%
% Calculates the penalties for a set of points in a triangulated mesh given
% an original distance for each point to its opposite edge. THis is
% intended to stop triangles overlapping.
%
% Inputs: 
%  Dorig - the cell array of original distances from each point to its
%          opposite edges
%  xyedg - the matrix of points in the mesh
%  T  - the Mx3 matrix defining the triangulation of the mesh.
%
% Outputs:
%  Fpenalty - the matrix containing the penalties for each point
%  Dnew - the  cell array of updated distances from each point to its
%          opposite edges
%
% Dr. A. I. Hanna, CMP, 2008.

[Dorig, xyEdg, T] = parseargs(varargin);
Fpenalty = zeros(size(xyEdg));
[Dnew, NormVec] = satlib_triDist2OppEdge('pts', xyEdg, 'T', T);
for i=1:size(xyEdg, 1)
    dnew = Dnew{i};
    dold = Dorig{i};
    N = NormVec{i};
    e = dnew-dold;
    C = ((sign(e)-1)/2).*abs(e);   
     Fpenalty(i,:) =  e*N';
end
return
%%
%
function [Dorig, xyEdg, T] = parseargs(varargin)
varargin = varargin{1};
error(nargchk(0,inf,nargin));
if mod(length(varargin),2) ~= 0
    % input args have not com in pairs, woe is me
    error(['Arguments to ', mfilename, ' must come param/value in pairs.'])
end
Dorig = {}; 
xyEdg = [];
T = [];
for i=1:2:length(varargin)
    switch lower(varargin{i})
        case 'dorig'
            Dorig = varargin{i+1};
        case 'xyedg'
            xyEdg = varargin{i+1};
        case 't'
            T = varargin{i+1};
        otherwise
            error(['Unknown parameter name passed to ', mfilename, '.  Name was ' varargin{i}])
    end
end
if isempty(Dorig)
    error('Parameter Dorig sets must be supplied');
end
if isempty(T)
    error('Parameter T sets must be supplied');
end
if isempty(xyEdg)
    error('Parameter xyEdg sets must be supplied');
end
return