function [xc,good,bad,type] = cornerfinder_saddle_point(xt,I,wintx,winty,wx2,wy2);

%[xc] = cornerfinder_saddle_point(xt,I,wintx,winty);
%
%Finds the sub-pixel corners on the image I with initial guess xt
%xt and xc are 2xN matrices. The first component is the x coordinate
%(horizontal) and the second component is the y coordinate (vertical)
%
%Based on Harris corner finder method
%
%Finds corners to a precision below .1 pixel!
%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!!
%Sept 1998 - UPDATED to handle diverged points: we keep the original points
%good is a binary vector indicating wether a feature point has been properly
%found.
%
%Add a zero zone of size wx2,wy2
%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher
%resolution and larger number of iterations.

% California Institute of Technology
% (c) Jean-Yves Bouguet -- Oct. 14th, 1997

xt = xt';
xt = fliplr(xt);

if nargin < 4,
   winty = 5;
   if nargin < 3,
      wintx = 5;
   end;
end;

if nargin < 6,
   wx2 = -1;
   wy2 = -1;
end;

%mask = ones(2*wintx+1,2*winty+1);
mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2);

if (wx2>0) & (wy2>0),
   if ((wintx - wx2)>=2)&((winty - wy2)>=2),
      mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1);
   end;
end;

offx = [-wintx:wintx]'*ones(1,2*winty+1);
offy = ones(2*wintx+1,1)*[-winty:winty];

resolution = 0.005;

MaxIter = 10;

[nx,ny] = size(I);
N = size(xt,1);

xc = xt; % first guess... they don't move !!!

type = zeros(1,N);

for i=1:N,

   v_extra = resolution + 1; 		% just larger than resolution

   compt = 0; 				% no iteration yet

   while (norm(v_extra) > resolution) & (compt<MaxIter),

       cIx = xc(i,1); 			%
       cIy = xc(i,2); 			% Coords. of the point
       crIx = round(cIx); 		% on the initial image
       crIy = round(cIy); 		%
       itIx = cIx - crIx; 		% Coefficients
       itIy = cIy - crIy; 		% to compute
       if itIx > 0, 			% the sub pixel
           vIx = [itIx 1-itIx 0]'; 	% accuracy.
       else
           vIx = [0 1+itIx -itIx]';
       end;
       if itIy > 0,
           vIy = [itIy 1-itIy 0];
       else
           vIy = [0 1+itIy -itIy];
       end;

      % What if the sub image is not in?

      if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5;
      elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4;
      else
          xmin = crIx-wintx-2; xmax = crIx+wintx+2;
      end;

      if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5;
      elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4;
      else
          ymin = crIy-winty-2; ymax = crIy+winty+2;
      end;

      SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood
      SI = conv2(conv2(SI,vIx,'same'),vIy,'same');
      SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood

      px = cIx + offx;
      py = cIy + offy;

      if 1, %~saddle,
          [gy,gx] = gradient(SI); 		% The gradient image
          gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only
          gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients
          gxx = gx .* gx .* mask;
          gyy = gy .* gy .* mask;
          gxy = gx .* gy .* mask;

          bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))];

          a = sum(sum(gxx));
          b = sum(sum(gxy));
          c = sum(sum(gyy));

          dt = a*c - b^2;

          xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;
      else

          SI = SI(2:2*wintx+2,2:2*winty+2);
          A =  repmat(mask(:),1,6) .* [px(:).^2 px(:).*py(:) py(:).^2 px(:) py(:) ones((2*wintx+1)*(2*winty+1),1)];
          param = inv(A'*A)*A'*( mask(:).*SI(:));
          xc2 = (-inv([2*param(1) param(2) ; param(2) 2*param(3) ]) * param(4:5))';

      end;

      v_extra = xc(i,:) - xc2;

      xc(i,:) = xc2;

      compt = compt + 1;

  end;

  if 1,

      cIx = xc(i,1); 			%
      cIy = xc(i,2); 			% Coords. of the point
      crIx = round(cIx); 		% on the initial image
      crIy = round(cIy); 		%
      itIx = cIx - crIx; 		% Coefficients
      itIy = cIy - crIy; 		% to compute
      if itIx > 0, 			% the sub pixel
          vIx = [itIx 1-itIx 0]'; 	% accuracy.
      else
          vIx = [0 1+itIx -itIx]';
      end;
      if itIy > 0,
          vIy = [itIy 1-itIy 0];
      else
          vIy = [0 1+itIy -itIy];
      end;

      % What if the sub image is not in?

      if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5;
      elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4;
      else
          xmin = crIx-wintx-2; xmax = crIx+wintx+2;
      end;

      if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5;
      elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4;
      else
          ymin = crIy-winty-2; ymax = crIy+winty+2;
      end;

      SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood
      SI = conv2(conv2(SI,vIx,'same'),vIy,'same');
      SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood
      px = cIx + offx;
      py = cIy + offy;

      SI = SI(2:2*wintx+2,2:2*winty+2);
      A =  repmat(mask(:),1,6) .* [px(:).^2 px(:).*py(:) py(:).^2 px(:) py(:) ones((2*wintx+1)*(2*winty+1),1)];
      param = inv(A'*A)*A'*( mask(:).*SI(:));
      xc2 = (-inv([2*param(1) param(2) ; param(2) 2*param(3) ]) * param(4:5))';

      v_extra = xc(i,:) - xc2;

      xc(i,:) = xc2;
  end;

end;

% check for points that diverge:

delta_x = xc(:,1) - xt(:,1);
delta_y = xc(:,2) - xt(:,2);

%keyboard;

bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty);
good = ~bad;
in_bad = find(bad);

% For the diverged points, keep the original guesses:

xc(in_bad,:) = xt(in_bad,:);

xc = fliplr(xc);
xc = xc';

bad = bad';
good = good';


clear all; close all; clear classes; clc;

%% Set flags.
inspect_only = false;

%% Solve the system.
s = 120;
w = 40;
b = (s + w/2)*2;
dL = 10;
dl = 2;
wvlen = 200;
clear solveropts;
[E, H, obj_array, src_array, J] = maxwell_run(...
	'OSC', 1e-9, wvlen, ...
	'DOM', {'vacuum', 'none', 1.0}, [-610 610; -610 610; 0 dl], [dL dL dl], BC.p, [10*dL 10*dL 0], ...
	'OBJ', {'vacuum', 'none', 1.0}, Box([-w/2 w/2; -w/2 w/2; 0 dl], dl), ...
	'SOBJ', ...  % scatter objects
		{'Johnson/Au', 'y'}, ...
			PolygonalCylinder(Axis.z, dl, dl/2, [w/2 0; w/2+s*sqrt(3)/2 -s/2; w/2+s*sqrt(3)/2 s/2], dl), ...
			PolygonalCylinder(Axis.z, dl, dl/2, [-w/2 0; -w/2-s*sqrt(3)/2 s/2; -w/2-s*sqrt(3)/2 -s/2], dl), ...
	'SRCJ', TFSFPlaneSrc([-b b; -b b; 0 dl], Axis.y, Axis.x), ...
	inspect_only);

%% Visualize the solution.
figure
clear opts
opts.withobjsrc = true;
opts.withabs = true;
% opts.withinterp = false;
% opts.withgrid = true;
% opts.cscale = 1e-1;
% opts.cmax = 1.4;
z_location = 0;
vis2d(E{Axis.x}, Axis.z, z_location, obj_array, src_array, opts)
% vis2d(H{Axis.z}, Axis.z, z_location, obj_array, src_array, opts)

% %% Calculate the power emanating from the source.
% power = powerflux_box(E,H,[-10 10; -10 10; 0 1]);
% fprintf('power = %e\n', power);


 function [xs, info] = pwls_pcg(x, G, W, yi, nder1, R, ...
		M, niter, stepper)
%function [xs, info] = pwls_pcg(x, G, W, yi, nder1, R, ...
%		M, niter, stepper)
%
% weighted least squares with convex non-quadratic penalty
% via preconditioned conjugate gradient algorithm
% cost(x) = (y-Gx)'W(y-Gx)/2 - n'(y-Gx) + R(x)
%
% in
%	x	[np,1]		initial estimate
%	G	[nd,np]		system matrix
%	W	[nd,nd]		data weighting matrix, usually diag_sp(wi)
%	yi	[nd,1]		noisy data
%	nder1	[nn,1]		linear term (obsolete: use "0")
%	R			penalty object (see Reg1.m)
%	M	[np,np]		preconditioner (use "1" if none)
%	niter			# total iterations
%	stepper			method for step-size line search
%				use {} for a good default
% out
%	xs	[np,niter]	estimates each iteration
%	info	[niter, 3]	gamma, step, time
%
% Copyright 1996-7, Jeff Fessler, The University of Michigan

warning 'pwls_pcg is obsolete; use pwls_pcg1 instead'

if nargin < 8, help(mfilename), error args, end

cpu etic

if isempty(nder1), nder1 = 0; end
if isempty(M), M = 1; end
if nder1 ~= 0, error 'nder1 = 0 required', end

if ~isvar('stepper') || isempty(stepper)
	stepper = {'qs', 3};	% quad surr with this # of subiterations
end

xs = zeros(length(x), niter);
xs(:,1) = x;

info = zeros(niter,3);

%
% initialize projections
%
Gx = G * x;

oldinprod = 0;

% iterate
for ii=2:niter
	ticker(mfilename, ii, niter)

	%
	% (negative) gradient
	%
	ngrad = G' * (W * (yi-Gx) - nder1);

	pgrad = R.cgrad(R, x);
	ngrad = ngrad - pgrad;

	%
	% preconditioned gradient
	pregrad = M * ngrad;

	% direction
	newinprod = ngrad' * pregrad;
	if ii == 2
		ddir = pregrad;
		gamma = 0;
	else
		if oldinprod == 0
			warn 'inprod=0. going nowhere!'
			gamma = 0;
		else
			gamma = newinprod / oldinprod;	% Fletcher-Reeves
%			gamma = (newinprod - oldgrad' * pregrad) / oldinprod;
		end
		ddir = pregrad + gamma * ddir;
	end
	oldgrad = ngrad;
	oldinprod = newinprod;

	% check if descent direction
	if real(ddir' * ngrad) < 0
		warning 'wrong direction'
		keyboard
%		ddir = pregrad;	% revert
%		oldinprod = 0;	% reset
	end

	% step size in search direction
	Gdir = G * ddir;
%	Cdir = R.C * ddir;

	% one step based on quadratic surrogate for penalty
	if streq(stepper{1}, 'qs1')
%		pdenom = Cdir' * (R.wpot(R.wt, Cdir) .* Cdir); % cannot be?
		pdenom = (abs(ddir).^2)' * R.denom(R, x);
		denom = Gdir'*(W*Gdir) + pdenom;
		if denom == 0
			warning 'found exact solution???  step=0 now!?'
			step = 0;
		else
			step = real((ddir' * grad) / denom);
		end

	% iteratively minimize \Half || y-G (x+alf*ddir) ||_W^2 + R(x + alf*ddir)
	elseif streq(stepper{1}, 'qs')
		nsub = stepper{2};
		dGWGd = Gdir'*(W*Gdir);
		dGWr = Gdir'*(W*(yi-Gx));
		step = 0;
		for is=1:nsub
%			pdenom = Cdir' * (R.wpot(R.wt, Cdir) .* Cdir);
			pdenom = (abs(ddir).^2)' * R.denom(R, x+step*ddir);
			denom = dGWGd + pdenom;
			pgrad = R.cgrad(R, x + step * ddir);
			step = step - (-dGWr + step * dGWGd + ddir' * pgrad) ...
				/ denom;
%			printf('%d-%d %g', ii, is, step)
		end

	else
		error 'bad stepper'
	end

	if step < 0
		warning('downhill?')
		keyboard
	end

	% update
	Gx	= Gx  + step * Gdir;
%	Cx	= Cx  + step * Cdir;
	x	= x + step * ddir;
	xs(:,ii) = x;

	info(ii,1) = gamma;
	info(ii,2) = step;
	info(ii,3) = cpu('etoc');	% accum. time
end


function M = make_convn_mat(F, sz, shape, pad)
%MAKE_CONVN_MAT   Flexible N-D convolution matrix
%   M = MAKE_CONVN_MAT(F, SZ[, SHAPE[, PAD]]) returns the convolution
%   matrix for the matrix F.  SZ gives the size of the array that the
%   convolution should be applied to.  The returned matrix M is sparse.
%   If X is of size SZ and SHAPE is 'full', then reshape(M * X(:), SZ +
%   size(F) - 1) is the same as convn(X, F).
%
%   The optional parameter SHAPE controls which parts of the convolution
%   result to return in M:
%     * 'full':      (default) returns the full N-D convolution, i.e. the
%                    behavior is identical to convn(X, F, 'full').
%     * 'same':      returns the central part of the convolution that
%                    is the same size as X, i.e. the behavior is
%                    identical to convn(X, F, 'same').
%     * 'sameswap':  identical to 'same' except that rounding needed for
%                    even-sized filters is performed the opposite way.
%     * 'valid':     returns only the part of the result that can be
%                    computed without assuming zero-padded arrays,
%                    i.e. the behavior is identical to convn(X, F,
%                    'valid').
%
%   The optional parameter PAD controls whether to return an M that pads
%   the result of the convolution with zeros (default no padding):
%     * 'full':      returns the result padded to the output size of a
%                    full N-D convolution (works with SHAPE set to 'same'
%                    and 'valid').
%     * 'same':      returns the result padded to the central part of the
%                    convolution that is the same size as X (works with
%                    SHAPE set to 'valid').
%     * 'sameswap':  identical to 'same' except that rounding needed for
%                    even-sized filters is performed the opposite way.
%
%   See also CONVMTXN.
%
%   Author:  Stefan Roth, Department of Computer Science, TU Darmstadt
%   Contact: sroth@cs.tu-darmstadt.de
%   $Date: 2007-03-27 14:09:11 -0400 (Tue, 27 Mar 2007) $
%   $Revision: 252 $

% Copyright 2004-2007, Brown University, Providence, RI. USA
% Copyright 2007-2010 TU Darmstadt, Darmstadt, Germany.
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

  % Default to full matrix
  if (nargin < 3)
    shape = 'full';
  end

  ndims = length(sz);

  % Border sizes for 'same' and 'sameswap'
  Fsize_lo_2 = ceil((size(F) - 1) / 2);
  Fsize_hi_2 = floor((size(F) - 1) / 2);

  % Border sizes for 'valid'
  Fsize = size(F) - 1;

  switch(shape)
    case 'same'
      % Mark valid and invalid pixels (i.e. the ones within and outside
      % of the part to be returned)
      valid = true(sz+size(F)-1);

      for d = 1:ndims
        for e = 1:ndims
          sub{e} = ':';
        end

        sub{d} = 1:Fsize_lo_2(d);
        valid(sub{:}) = false;
        sub{d} = size(valid, d)-Fsize_hi_2(d)+1:size(valid, d);
        valid(sub{:}) = false;
      end

      if (nargin > 3 && strcmp(pad, 'full'))
        % If we're padding to 'full' set the coefficients on the border
        % to zero
        M = convmtxn(F, sz, valid);
      else
        % If we're *not* padding, then suppress the rows of M that
        % correspond to the border
        M = convmtxn(F, sz);
        M = M(valid, :);
      end

   case 'sameswap'
     % Mark valid and invalid pixels (i.e. the ones within and outside
     % of the part to be returned), but round the other way
     valid = true(sz+size(F)-1);

     for d = 1:ndims
       for e = 1:ndims
         sub{e} = ':';
       end

       sub{d} = 1:Fsize_hi_2(d);
       valid(sub{:}) = false;
       sub{d} = size(valid, d)-Fsize_lo_2(d)+1:size(valid, d);
       valid(sub{:}) = false;
     end

     if (nargin > 3 && strcmp(pad, 'full'))
       % If we're padding to 'full' set the coefficients on the border
       % to zero
       M = convmtxn(F, sz, valid);
     else
       % If we're *not* padding, then suppress the rows of M that
       % correspond to the border
       M = convmtxn(F, sz);
       M = M(valid, :);
     end

   case 'valid'
     % Mark valid and invalid pixels (i.e. the ones within and outside
     % of the part to be returned)
     valid = true(sz+size(F)-1);

     for d = 1:ndims
       for e = 1:ndims
         sub{e} = ':';
       end

       sub{d} = 1:Fsize(d);
       valid(sub{:}) = false;
       sub{d} = size(valid, d)-Fsize(d)+1:size(valid, d);
       valid(sub{:}) = false;
     end

     if (nargin > 3)
       % If we're padding, then figure out the area to be padded

       switch (pad)
         case 'same'
           % Mark valid and invalid pixels (i.e. the ones within and outside
           % of the part to be padded)
           pad_valid = true(sz+size(F)-1);

           for d = 1:ndims
             for e = 1:ndims
               sub{e} = ':';
             end

             sub{d} = 1:Fsize_lo_2(d);
             pad_valid(sub{:}) = false;
             sub{d} = size(valid, d)-Fsize_hi_2(d)+1:size(valid, d);
             pad_valid(sub{:}) = false;
           end

           % Set coefficients on the border to zero
           M = convmtxn(F, sz, valid);

           % Suppress rows of M outside of the padded area
           M = M(pad_valid, :);

         case 'sameswap'
           % Mark valid and invalid pixels (i.e. the ones within and outside
           % of the part to be padded), but round the other way
           pad_valid = true(sz+size(F)-1);

           for d = 1:ndims
             for e = 1:ndims
               sub{e} = ':';
             end

             sub{d} = 1:Fsize_hi_2(d);
             pad_valid(sub{:}) = false;
             sub{d} = size(valid, d)-Fsize_lo_2(d)+1:size(valid, d);
             pad_valid(sub{:}) = false;
           end

           % Set coefficients on the border to zero
           M = convmtxn(F, sz, valid);

           % Suppress rows of M outside of the padded area
           M = M(pad_valid, :);

         otherwise
           % Padding to 'full'; only set coefficients on the border to zero
           M = convmtxn(F, sz, valid);
       end
     else
       % No padding; suppress all rows on the border
       M = convmtxn(F, sz);
       M = M(valid, :);
     end

    otherwise
      % Full convolution; return everything
      M = convmtxn(F, sz);

  end


function [ulf, vlf, lf, hf, lfhf, ttlpwr] = CalcLfHfParams(PSD, F, limits,plot_on)
% [ulf, vlf, lf, hf, lfhf, ttlpwr] = CalcLfHfParams(PSD, F, limits,plot_on)
%
%   OVERVIEW: Compute the frequency domain features for a given PSD and
%             frequency bans limits
%
%   INPUT:
%        PSD     - power spectral density
%        F       - frequency vector
%        limits  - frequency domain analysis limits
%        plot_on -
%
%   OUTPUT:
%	- ulf     : (ms^2) Power in the ultra low frequency range (default < 0.003 Hz)
%	- vlf     : (ms^2) Power in very low frequency range (default 0.003 <= vlf < 0.04 Hz)
%	- lf      : (ms^2) Power in low frequency range (default 0.04Hz  <= lf < 0.15 Hz)
%	- hf      : (ms^2) Power in high frequency range (default 0.15 <= hf < 0.4 Hz)
%	- lfhf    : Ratio LF [ms^2]/HF [ms^2]
%	- ttlpwr  : (ms^2) Total spectral power (approximately <0.4 Hz)
%
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Main script written by Adriana N. Vest
%       Dependent scripts written by various authors
%       (see functions for details)
%	COPYRIGHT (C) 2016
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information
%%
if nargin <3
    ULF = [0 .003];
    VLF = [0.003 .04];
    LF = [.04 .15];
    HF = [0.15 0.4];
    limits = [ULF; VLF; LF; HF];
end
if nargin < 4
    plot_on =1;
end

Indx_ULF = find( (limits(1,1) <= F) & (F <= limits(1,2)) );
Indx_VLF = find( (limits(2,1) <= F) & (F <= limits(2,2)) );
Indx_LF = find( (limits(3,1) <= F) & (F <= limits(3,2)) );
Indx_HF = find( (limits(4,1) <= F) & (F <= limits(4,2)) );
space = F(2)-F(1);

ulf = sum(PSD(Indx_ULF)*space) * 1e6; % convert to ms^2
vlf = sum(PSD(Indx_VLF)*space) * 1e6; % convert to ms^2
lf = sum(PSD(Indx_LF)*space) * 1e6;   % convert to ms^2
hf = sum(PSD(Indx_HF)*space) * 1e6;   % convert to ms^2

ttlpwr = sum([ulf vlf lf hf]);

lf_n = lf/ttlpwr; % normalized
hf_n = hf/ttlpwr;
lfhf = round(lf_n/hf_n*100)/100; % lf/hf ratio

if plot_on
    figure
    % plot PSD
    plot(F,10*log10(PSD),'b','linewidth',2)
    hold on
    % plot limits on graph for lf and hf
    plot([F(Indx_LF(1)) F(Indx_LF(1))],[-80 40],'k:')
    hold on
    plot([F(Indx_LF(end)) F(Indx_LF(end))],[-80 40],'k:')
    hold on
    plot([F(Indx_HF(end)) F(Indx_HF(end))],[-80 40],'k:')

    % labelsc
    text(0.07,30,'LF','Fontname','Times New Roman','Fontsize',10)
    text(0.25,30,'HF','Fontname','Times New Roman','Fontsize',10)
    %text(0.15, 35, 'Power Spectral Density','Fontname','Times New Roman','Fontsize',10)
    text(0.3, -60, strcat('LF/HF=',num2str(lfhf)),'Fontname','Times New Roman','Fontsize',10)
    ylabel('Normalized PSD (db/Hz)','Fontname','Times New Roman','fontsize',10)
    xlabel('Frequency (Hz)','Fontname','Times New Roman','fontsize',10)
    axis([0 .45 -80 40]);
    box off
end % end plot

end % end function


function [Vlims, dVlims] = opf_vlim_fcn(x, mpc, idx, mpopt)
%OPF_VLIM_FCN  Evaluates voltage magnitudes and their gradients.
%   [Vlims, dVlims] = OPF_VLIM_FCN(X, MPC, IDX, MPOPT)
%
%   Computes the voltage magnitudes using real and imaginary part of complex voltage for
%   AC optimal power flow. Computes constraint vectors and their gradients.
%
%   Inputs:
%     X : optimization vector
%     MPC : MATPOWER case struct
%     IDX : index of buses whose voltage magnitudes should be fixed
%     MPOPT : MATPOWER options struct
%
%   Outputs:
%     VLIMS  : vector of voltage magnitudes
%     DVLIMS : (optional) magnitude gradients
%
%   Examples:
%       Vlims = opf_vlim_fcn(x, mpc, mpopt);
%       [Vlims, dVlims] = opf_vlim_fcn(x, mpc, idx, mpopt);
%
%   See also OPF_VLIM_HESS

%   MATPOWER
%   Copyright (c) 2018, Power Systems Engineering Research Center (PSERC)
%   by Baljinnyam Sereeter, Delft University of Technology
%   and Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% define named indices into data matrices
[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;

%% unpack data
[Vr, Vi] = deal(x{:});

%% problem dimensions
nb = length(Vi);            %% number of buses
n = length(idx);            %% number of buses with voltage limits

%% compute voltage magnitude
Vm2 = Vr(idx).^2 + Vi(idx).^2;
Vlims = [ mpc.bus(idx, VMIN).^2 - Vm2;
          Vm2 - mpc.bus(idx, VMAX).^2 ];

if nargout > 1
    %% compute partials of voltage magnitude w.r.t Vr and Vi
    dVm_dVr = sparse(1:n, idx, 2 * Vr(idx), n, nb);
    dVm_dVi = sparse(1:n, idx, 2 * Vi(idx), n, nb);
    dVlims = [ -dVm_dVr -dVm_dVi;   %% Vlims w.r.t Vr, Vi
                dVm_dVr  dVm_dVi  ];
end


function [vert,conn,tria,tnum] = tridiv2(varargin)
%TRIDIV2 "quadtree" refinement for 2-simplex triangulations.
%   [VERT,EDGE,TRIA,TNUM] = TRIDIV2(VERT,EDGE,TRIA,TNUM) re-
%   turns a globally refined triangulation, in which all ed-
%   ges are bisected about their midpoints. Such refinement
%   splits each triangle into four new sub-triangles accord-
%   ing to a shape-preserving scheme.
%
%   VERT is a V-by-2 array of XY coordinates in the triangu-
%   lation, EDGE is an array of constrained edges, TRIA is a
%   T-by-3 array of triangles, and TNUM is a T-by-1 array of
%   part indices. Each row of TRIA and EDGE define an eleme-
%   nt. VERT(TRIA(II,1),:), VERT(TRIA(II,2),:) and VERT(TRIA
%   (II,3),:) are the coordinates of the II-TH triangle. The
%   edges in EDGE are defined in a similar manner. NUM is an
%   array of part indexing, such that TNUM(II) is the index
%   of the part in which the II-TH triangle resides.
%
%   [VERT,EDGE,TRIA,TNUM] = TRIDIV2(... ,TDIV) returns a se-
%   lectively refined mesh, where TDIV is a T-by-1 logical
%   array, with TDIV(KK) = TRUE if TRIA(KK,:) is to be refi-
%   ned. Such triangles are refined using the four-way split
%   described above. Additionally, a "halo" of adjacent tri-
%   angles are also refined to preseve compatibility of the
%   mesh. Such triangles are refined using a two-way bisect-
%   ion type scheme.

%   See also REFINE2, SMOOTH2

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 29/01/2017

%---------------------------------------------- extract args
    vert = []; conn = []; tria = []; tnum = [] ;
    tdiv = [];

    if (nargin>=+1), vert = varargin{1}; end
    if (nargin>=+2), conn = varargin{2}; end
    if (nargin>=+3), tria = varargin{3}; end
    if (nargin>=+4), tnum = varargin{4}; end
    if (nargin>=+5), tdiv = varargin{5}; end

%---------------------------------------------- default arg.
    if (isempty(tnum))
        tnum = ones(size(tria,1),1) ;
    end
    if (isempty(tdiv))
        tdiv = true(size(tria,1),1) ;
    end

%---------------------------------------------- basic checks
    if ( ~isnumeric(vert) || ...
         ~isnumeric(conn) || ...
         ~isnumeric(tria) || ...
         ~isnumeric(tnum) || ...
         ~islogical(tdiv) )
        error('tridiv2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    tnum = tnum(:) ; tdiv = tdiv(:) ;
    if (ndims(vert) ~= +2 || ...
        ndims(conn) ~= +2 || ...
        ndims(tria) ~= +2 || ...
        ndims(tnum) ~= +2 || ...
        ndims(tdiv) ~= +2 )
        error('tridiv2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(vert,2)~= +2 || ...
        size(conn,2)~= +2 || ...
        size(tria,2)~= +3 || ...
        size(tnum,2)~= +1 || ...
        size(tria,1)~= size(tnum,1) )
        error('tridiv2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    nvrt = size(vert,1) ;

%---------------------------------------------- basic checks
    if (min(min(conn(:,1:2))) < +1 || ...
            max(max(conn(:,1:2))) > nvrt )
        error('tridiv2:invalidInputs', ...
            'Invalid EDGE input array.') ;
    end

    if (min(min(tria(:,1:3))) < +1 || ...
            max(max(tria(:,1:3))) > nvrt )
        error('tridiv2:invalidInputs', ...
            'Invalid TRIA input array.') ;
    end

%------------------------------ assemble extended adj. info.
   [edge,tria] = tricon2(tria,conn) ;

    ediv = false(size(edge,1),1) ;
    ediv(tria(tdiv,4:6))  = true ;

    snum = length(find(ediv));

    while (true)

    %-------------------------- tria's with >= 2 edge splits
        div3 = sum(double( ...
            ediv(tria(:,4:6))),2)>=2;

    %-------------------------- expand onto adj. edge splits
        ediv(tria(div3,4:6)) = true ;

        snew = length(find(ediv)) ;

        if (snew == snum), break; end

        snum = snew ;

    end

%------------------------------ tria's with == 1 edge splits
    div1 = sum( ...
    double( ediv(tria(:,4:6))),2)==1;

%------------------------------ indexing for mid-point vert.
    ivec = zeros(size(edge,1),1);
    ivec(ediv) = ...
        (1:snum)' + size(vert,1);

%------------------------------------------ update vert. set
    emid = vert(edge(ediv,1),:) ...
         + vert(edge(ediv,2),:) ;
    vert =[vert ; emid * 0.5] ;

%------------------------------------------ update edge. set
    [cvec,eloc] = ...
        setset2(conn,edge(ediv, 1:2)) ;

    epos = ivec(ediv);
    epos = epos(eloc(eloc>0)) ;

    conn = [conn(~cvec,1:2) ; ...
        conn(cvec,1) , epos ; ...
        conn(cvec,2) , epos ] ;

%------------------------------------ push 1-to-4 refinement
    tr41 = ones(length(find(div3)),3) ;
    tn41 = ones(length(find(div3)),1) ;
    tn41(:,1) = tnum(div3,1);
    tr41(:,1) = tria(div3,1);
    tr41(:,2) = ivec(tria(div3,4));
    tr41(:,3) = ivec(tria(div3,6));

    tr42 = ones(length(find(div3)),3) ;
    tn42 = ones(length(find(div3)),1) ;
    tn42(:,1) = tnum(div3,1);
    tr42(:,1) = ivec(tria(div3,4));
    tr42(:,2) = tria(div3,2);
    tr42(:,3) = ivec(tria(div3,5));

    tr43 = ones(length(find(div3)),3) ;
    tn43 = ones(length(find(div3)),1) ;
    tn43(:,1) = tnum(div3,1);
    tr43(:,1) = ivec(tria(div3,6));
    tr43(:,2) = ivec(tria(div3,5));
    tr43(:,3) = tria(div3,3);

    tr44 = ones(length(find(div3)),3) ;
    tn44 = ones(length(find(div3)),1) ;
    tn44(:,1) = tnum(div3,1);
    tr44(:,1) = ivec(tria(div3,6));
    tr44(:,2) = ivec(tria(div3,4));
    tr44(:,3) = ivec(tria(div3,5));

%----------------------- push 1-to-2 refinement about edge 1
    tvec = false(size(tria,1), 1) ;
    tvec(ediv(tria(:,4))&div1) = true ;

    tr21 = ones(length(find(tvec)),3) ;
    tn21 = ones(length(find(tvec)),1) ;
    tn21(:,1) = tnum(tvec,1);
    tr21(:,1) = ivec(tria(tvec,4));
    tr21(:,2) = tria(tvec,3);
    tr21(:,3) = tria(tvec,1);

    tr22 = ones(length(find(tvec)),3) ;
    tn22 = ones(length(find(tvec)),1) ;
    tn22(:,1) = tnum(tvec,1);
    tr22(:,1) = ivec(tria(tvec,4));
    tr22(:,2) = tria(tvec,2);
    tr22(:,3) = tria(tvec,3);

%----------------------- push 1-to-2 refinement about edge 2
    tvec = false(size(tria,1), 1) ;
    tvec(ediv(tria(:,5))&div1) = true ;

    tr23 = ones(length(find(tvec)),3) ;
    tn23 = ones(length(find(tvec)),1) ;
    tn23(:,1) = tnum(tvec,1);
    tr23(:,1) = ivec(tria(tvec,5));
    tr23(:,2) = tria(tvec,1);
    tr23(:,3) = tria(tvec,2);

    tr24 = ones(length(find(tvec)),3) ;
    tn24 = ones(length(find(tvec)),1) ;
    tn24(:,1) = tnum(tvec,1);
    tr24(:,1) = ivec(tria(tvec,5));
    tr24(:,2) = tria(tvec,3);
    tr24(:,3) = tria(tvec,1);

%----------------------- push 1-to-2 refinement about edge 3
    tvec = false(size(tria,1), 1) ;
    tvec(ediv(tria(:,6))&div1) = true ;

    tr25 = ones(length(find(tvec)),3) ;
    tn25 = ones(length(find(tvec)),1) ;
    tn25(:,1) = tnum(tvec,1);
    tr25(:,1) = ivec(tria(tvec,6));
    tr25(:,2) = tria(tvec,2);
    tr25(:,3) = tria(tvec,3);

    tr26 = ones(length(find(tvec)),3) ;
    tn26 = ones(length(find(tvec)),1) ;
    tn26(:,1) = tnum(tvec,1);
    tr26(:,1) = ivec(tria(tvec,6));
    tr26(:,2) = tria(tvec,1);
    tr26(:,3) = tria(tvec,2);

%------------------------------------------ update tria. set
    tria = [tria(~div1&~div3,1:3) ; ...
        tr41 ; tr42 ; tr43 ; tr44 ; ...
        tr21 ; tr22 ;
        tr23 ; tr24 ;
        tr25 ; tr26 ] ;

    tnum = [tnum(~div1&~div3,1:1) ; ...
        tn41 ; tn42 ; tn43 ; tn44 ; ...
        tn21 ; tn22 ;
        tn23 ; tn24 ;
        tn25 ; tn26 ] ;

end


function imb=regPutBorde(im,Nx,Ny,method);
% regPutBorde -  Puts a border to an image:
%	method=1 -> consider the image periodical
%	method=2 -> specular
%	method=3 -> repeat the edge pixel
%
%	imb = regPutBorde(im,Nx,Ny,method);
%
% ON - 10/96 (from putborde)
% Oscar Nestares - 5/99 (renamed to regPutBorde)
%

[sy sx]=size(im);
imb=zeros(sy+2*Ny,sx+2*Nx);
imb(1+Ny:sy+Ny,1+Nx:sx+Nx)=im;

if method == 1
	imb(1:Ny,1+Nx:sx+Nx)=im(sy-Ny+1:sy,:);
	imb(sy+Ny+1:sy+2*Ny,1+Nx:sx+Nx)=im(1:Ny,:);
	imb(1+Ny:sy+Ny,1:Nx)=im(:,sx-Nx+1:sx);
	imb(1+Ny:sy+Ny,sx+Nx+1:sx+2*Nx)=im(:,1:Nx);
	imb(1:Ny,1:Nx)=im(sy-Ny+1:sy,sx-Nx+1:sx);
	imb(1:Ny,sx+Nx+1:sx+2*Nx)=im(sy-Ny+1:sy,1:Nx);
	imb(sy+Ny+1:sy+2*Ny,1:Nx)=im(1:Ny,sx-Nx+1:sx);
	imb(sy+Ny+1:sy+2*Ny,sx+Nx+1:sx+2*Nx)=im(1:Ny,1:Nx);
elseif method == 2
	imb(Ny:-1:1,1+Nx:sx+Nx)=im(2:Ny+1,:);
	imb(sy+2*Ny:-1:sy+Ny+1,1+Nx:sx+Nx)=im(sy-Ny:sy-1,:);
	imb(1+Ny:sy+Ny,Nx:-1:1)=im(:,2:Nx+1);
	imb(1+Ny:sy+Ny,sx+2*Nx:-1:sx+Nx+1)=im(:,sx-Nx:sx-1);
	imb(Ny:-1:1,Nx:-1:1)=im(2:Ny+1,2:Nx+1);
	imb(Ny:-1:1,sx+2*Nx:-1:sx+Nx+1)=im(2:Ny+1,sx-Nx:sx-1);
	imb(sy+2*Ny:-1:sy+Ny+1,Nx:-1:1)=im(sy-Ny:sy-1,2:Nx+1);
	imb(sy+2*Ny:-1:sy+Ny+1,sx+2*Nx:-1:sx+Nx+1)=im(sy-Ny:sy-1,sx-Nx:sx-1);
elseif method==3
	for k=1:Nx
		imb(Ny+1:sy+Ny,k)=im(:,1);
		imb(Ny+1:sy+Ny,k+sx+Nx)=im(:,sx);
	end
	for k=1:Ny
		imb(k,Nx+1:sx+Nx)=im(1,:);
		imb(k+sy+Ny,Nx+1:sx+Nx)=im(sy,:);
	end
else
	error('Not a valid value for method')
end


function colors = distinguishable_colors(n_colors, bg, func)
% DISTINGUISHABLE_COLORS: pick colors that are maximally perceptually distinct
%
% When plotting a set of lines, you may want to distinguish them by color.
% By default, Matlab chooses a small set of colors and cycles among them,
% and so if you have more than a few lines there will be confusion about
% which line is which. To fix this problem, one would want to be able to
% pick a much larger set of distinct colors, where the number of colors
% equals or exceeds the number of lines you want to plot. Because our
% ability to distinguish among colors has limits, one should choose these
% colors to be "maximally perceptually distinguishable."
%
% This function generates a set of colors which are distinguishable
% by reference to the "Lab" color space, which more closely matches
% human color perception than RGB. Given an initial large list of possible
% colors, it iteratively chooses the entry in the list that is farthest (in
% Lab space) from all previously-chosen entries. While this "greedy"
% algorithm does not yield a global maximum, it is simple and efficient.
% Moreover, the sequence of colors is consistent no matter how many you
% request, which facilitates the users' ability to learn the color order
% and avoids major changes in the appearance of plots when adding or
% removing lines.
%
% Syntax:
%   colors = distinguishable_colors(n_colors)
% Specify the number of colors you want as a scalar, n_colors. This will
% generate an n_colors-by-3 matrix, each row representing an RGB
% color triple. If you don't precisely know how many you will need in
% advance, there is no harm (other than execution time) in specifying
% slightly more than you think you will need.
%
%   colors = distinguishable_colors(n_colors,bg)
% This syntax allows you to specify the background color, to make sure that
% your colors are also distinguishable from the background. Default value
% is white. bg may be specified as an RGB triple or as one of the standard
% "ColorSpec" strings. You can even specify multiple colors:
%     bg = {'w','k'}
% or
%     bg = [1 1 1; 0 0 0]
% will only produce colors that are distinguishable from both white and
% black.
%
%   colors = distinguishable_colors(n_colors,bg,rgb2labfunc)
% By default, distinguishable_colors uses the image processing toolbox's
% color conversion functions makecform and applycform. Alternatively, you
% can supply your own color conversion function.
%
% Example:
%   c = distinguishable_colors(25);
%   figure
%   image(reshape(c,[1 size(c)]))
%
% Example using the file exchange's 'colorspace':
%   func = @(x) colorspace('RGB->Lab',x);
%   c = distinguishable_colors(25,'w',func);

% Copyright 2010-2011 by Timothy E. Holy

% Parse the inputs
if (nargin < 2)
    bg = [1 1 1];  % default white background
else
    if iscell(bg)
        % User specified a list of colors as a cell aray
        bgc = bg;
        for i = 1:length(bgc)
            bgc{i} = parsecolor(bgc{i});
        end
        bg = cat(1,bgc{:});
    else
        % User specified a numeric array of colors (n-by-3)
        bg = parsecolor(bg);
    end
end

% Generate a sizable number of RGB triples. This represents our space of
% possible choices. By starting in RGB space, we ensure that all of the
% colors can be generated by the monitor.
n_grid = 30;  % number of grid divisions along each axis in RGB space
x = linspace(0,1,n_grid);
[R,G,B] = ndgrid(x,x,x);
rgb = [R(:) G(:) B(:)];
if (n_colors > size(rgb,1)/3)
    error('You can''t readily distinguish that many colors');
end

% Convert to Lab color space, which more closely represents human
% perception
if (nargin > 2)
    lab = func(rgb);
    bglab = func(bg);
else
    C = makecform('srgb2lab');
    lab = applycform(rgb,C);
    bglab = applycform(bg,C);
end

% If the user specified multiple background colors, compute distances
% from the candidate colors to the background colors
mindist2 = inf(size(rgb,1),1);
for i = 1:size(bglab,1)-1
    dX = bsxfun(@minus,lab,bglab(i,:)); % displacement all colors from bg
    dist2 = sum(dX.^2,2);  % square distance
    mindist2 = min(dist2,mindist2);  % dist2 to closest previously-chosen color
end

% Iteratively pick the color that maximizes the distance to the nearest
% already-picked color
colors = zeros(n_colors,3);
lastlab = bglab(end,:);   % initialize by making the "previous" color equal to background
for i = 1:n_colors
    dX = bsxfun(@minus,lab,lastlab); % displacement of last from all colors on list
    dist2 = sum(dX.^2,2);  % square distance
    mindist2 = min(dist2,mindist2);  % dist2 to closest previously-chosen color
    [~,index] = max(mindist2);  % find the entry farthest from all previously-chosen colors
    colors(i,:) = rgb(index,:);  % save for output
    lastlab = lab(index,:);  % prepare for next iteration
end

%-------------------------------------------------------------------------
function c = parsecolor(s)
if ischar(s)
    c = colorstr2rgb(s);
elseif isnumeric(s) && size(s,2) == 3
    c = s;
else
    error('MATLAB:InvalidColorSpec','Color specification cannot be parsed.');
end

%-------------------------------------------------------------------------
function c = colorstr2rgb(c)
% Convert a color string to an RGB value.
% This is cribbed from Matlab's whitebg function.
% Why don't they make this a stand-alone function?
rgbspec = [1 0 0;0 1 0;0 0 1;1 1 1;0 1 1;1 0 1;1 1 0;0 0 0];
cspec = 'rgbwcmyk';
k = find(cspec==c(1));
if isempty(k)
    error('MATLAB:InvalidColorString','Unknown color string.');
end
if k~=3 || length(c)==1,
    c = rgbspec(k,:);
elseif length(c)>2,
    if strcmpi(c(1:3),'bla')
        c = [0 0 0];
    elseif strcmpi(c(1:3),'blu')
        c = [0 0 1];
    else
        error('MATLAB:UnknownColorString', 'Unknown color string.');
    end
end


function [ element_node, element_att ] = triangle_element_data_example ( ...
  element_num, element_order, element_att_num )

%*****************************************************************************80
%
%% TRIANGLE_ELEMENT_DATA_EXAMPLE returns the element information for the example.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_ORDER, the order of the elements.
%
%    Input, integer ELEMENT_ATT_NUM, the number of element
%    attributes.
%
%    Output, integer ELEMENT_NODE(ELEMENT_ORDER,ELEMENT_NUM),
%    the indices of the nodes that make up each element.
%
%    Output, real ELEMENT_ATT(ELEMENT_ATT_NUM,ELEMENT_NUM),
%    the attributes of each element.
%
  element_att = [];

  element_node = [ ...
    1,  2,  6; ...
    7,  6,  2; ...
    2,  3,  7; ...
    8,  7,  3; ...
    3,  4,  8; ...
    9,  8,  4; ...
    4,  5,  9; ...
   10,  9,  5; ...
    6,  7, 11; ...
   12, 11,  7; ...
    7,  8, 12; ...
   13, 12,  8; ...
    8,  9, 13; ...
   14, 13,  9; ...
    9, 10, 14; ...
   15, 14, 10; ...
   11, 12, 16; ...
   17, 16, 12; ...
   12, 13, 17; ...
   18, 17, 13; ...
   16, 17, 19; ...
   20, 19, 17; ...
   17, 18, 20; ...
   21, 20, 18 ]';

  return
end


% Digital circuit sizing example (GP)
% Boyd, Kim, Patil, and Horowitz, "Digital circuit optimization
% via geometric programming"
% Written for CVX by Almir Mutapcic 02/08/06
%
% Solves the problem of choosing gate scale factors x_i to give
% minimum ckt delay, subject to limits on the total area and power.
% Uses max gate arrival time T formulation that avoids evaluation
% of the delay over all paths in the circuit.
%
%   minimize   T_bar
%       s.t.   T_j <= T_bar      for j an output gate
%              T_j + d_i <= T_i  for j in FI(i)
%              P <= Pmax, A <= Amax
%              x >= 1
%
% where variables are x and T.
%
% We use the circuit topology presented in figure 1 (page 902),
% where we take gates 1, 3 and 6 to be inverters (INV),
% gates 2 and 7 to be three input NANDs (NAND3),
% and gates 4 and 5 to be two input NORs (NOR2).

%********************************************************************
% user specified data (specify problem constant and ckt topology)
%********************************************************************
m = 7;        % number of gates
Vdd = 5;      % supply voltage
Amax = 250;   % maximum area spec

% gate specs
INV   = struct('Cin',3, 'Cint',3, 'Rdrv',0.48, 'A',3,  'Ileak',0.006);
NAND3 = struct('Cin',4, 'Cint',6, 'Rdrv',0.48, 'A',8,  'Ileak',0.007);
NOR2  = struct('Cin',5, 'Cint',6, 'Rdrv',0.48, 'A',10, 'Ileak',0.009);

clear gates;
gates([1 3 6]) = INV;
gates([2 7])   = NAND3;
gates([4 5])   = NOR2;

% primary inputs and primary outputs labels (start with m+1)
primary_inputs = [8 9 10];
primary_outputs = [11 12];
M = m + length( primary_inputs ) + length( primary_outputs );

% fan-in cell array
FI = cell(M,1);
FI{1} = 8;
FI{2} = [8 9 10];
FI{3} = 10;
FI{4} = [1 2];
FI{5} = [2 3];
FI{6} = 4;
FI{7} = [3 4 5];
FI{8} = [];
FI{9} = [];
FI{10} = [];
FI{11} = 6;
FI{12} = 7;

% primary output has Cin capacitance (but has no Cload)
Cin_po = sparse(M,1);
Cin_po(primary_outputs) = [10 10];

% primary input has Cload capacitance (but has no Cin)
Cload_pi = sparse(M,1);
Cload_pi(primary_inputs) = [10 10 10];

% activity frequency of gates and primary inputs
f_gates = 0.001*ones(m,1);
f_pi = sparse(M,1);
f_pi(primary_inputs) = 0.001*[10 10 10];

%********************************************************************
% derived problem data (computed from user inputs)
%********************************************************************
% fan-out cell array (compute it from the fan-in cell array)
FO = cell(M,1);
for gate = [1:m primary_outputs]
  preds = FI{gate};
  for k = 1:length(preds)
    FO{preds(k)}(end+1) = gate;
  end
end

% input and internal capacitance of gates, and driving resistance
Cin_norm  = [gates.Cin]';
Cint_norm = [gates.Cint]';
Rdrv_norm = [gates.Rdrv]';

% area specification for each gate with unit scaling
A_norm = [gates.A]';

% leakage current of gate i with unit scaling
Ileak_norm = [gates.Ileak]';

%********************************************************************
% optimization (with tradeoff curve generation)
%********************************************************************
% objective is the upper bound on the overall delay
% and that is the max of arrival times for output gates
output_gates = [FI{primary_outputs}];

% varying parameters for the tradeoff curve
N = 25;
Pmax = linspace(10,20,N);
min_delay = zeros(N,1);
disp('Generating the optimal tradeoff curve...')
for n = 1:N
  fprintf('Pmax = %6.2f: ',Pmax(n));
  cvx_begin gp quiet
    % optimization variables
    variable x(m)                 % scale factor
    variable T(m)                 % arrival times

    % input capacitance is an affine function of sizes
    Cin  = Cin_norm.*x;
    Cint = Cint_norm.*x;

    % driving resistance is inversily proportional to sizes
    R = Rdrv_norm./x;

    % gate delay is the product of its driving resistance and load cap.
    Cload = cvx( zeros(m,1) );
    for gate = 1:m
      if ~ismember( FO{gate}, primary_outputs )
        Cload(gate) = sum( Cin(FO{gate}) );
      else
        Cload(gate) = Cin_po( FO{gate} );
      end
    end

    % delay
    D = 0.69*ones(m,1).*R.*( Cint + Cload );

    % total area
    area = A_norm'*x;

    % total power calculation
    Pdyn = Vdd^2*sum( f_pi(primary_inputs).*Cload_pi(primary_inputs) ) + ...
           Vdd^2*(f_gates'*(Cint + Cload));
    Pstat = Vdd*Ileak_norm'*x;
    power = Pdyn + Pstat;

    minimize( max( T(output_gates) ) )
    subject to
      % constraints
      x >= 1; %#ok
      area <= Amax; %#ok
      power <= Pmax(n); %#ok

      % create timing constraints
      for gate = 1:m
        if ~ismember( FI{gate}, primary_inputs )
          for j = FI{gate}
            % enforce T_j + D_j <= T_i over all gates j that drive i
            D(gate) + T(j) <= T(gate); %#ok
          end
        else
          % enforce D_i <= T_i for gates i connected to primary inputs
          D(gate) <= T(gate); %#ok
        end
      end
  cvx_end
  fprintf( 'delay = %3.2f\n', cvx_optval );
  min_delay(n) = cvx_optval;
end

% plot the tradeoff curve
figure, clf
plot(Pmax,min_delay);
xlabel('Pmax'); ylabel('Dmin');
title(['Tradeoff curve for Amax = ' num2str(Amax)])
disp('Optimal tradeoff curve plotted.')


function [soln,eqn,info] = DivCurl3PDWG(node,elem,bdFlag,pde,option,varargin)
%% DivCurl3PDWG div-curl system equation:
%        primal-dual weak-Galerkin in 3-D to solve the following system.
%       div(epsilon*u)=f  in \Omega,
%       curl(u) = g       in \Omega,
%       Dirichlet boundary condition epsilon*u \cdot n =g_n on \Gamma_0,
%
% The code is vectorized with no loops. Data structure follows iFEM tradition.
%
% Reference: A New Numerical Method for Div-Curl Systems with Low Regularity Assumptions
% S. Cao, C. Wang, J. Wang, https://arxiv.org/abs/2101.03466
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

if ~exist('option','var'), option = []; end
if ~isfield(option,'inverseh'), option.inverseh = true; end
if ~isfield(option,'dquadorder'), option.dquadorder = 3; end

[lambda,w] = quadpts3(option.dquadorder); % quadrature order is 1 or 2
nQuad = size(lambda,1);

if ~exist('bdFlag','var')
    bdFlag = setboundary3(node,elem,'Dirichlet');
end

%% Mesh related geometric quantities

% local edge to face indices, first two edges' local indices on a face
loc_e2f = [4 5; 2 3; 1 3; 1 2];
% local face to vertices indices, positive oriented
loc_f2v = [2 3 4; 1 4 3; 1 2 4; 1 3 2];

loc_e2v = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];

[elem2face,face] = dof3face(elem);
NT = size(elem,1);
NF = size(face,1);
elem2intdof = reshape(1:3*NT, 3, NT)';

[Dphi,volume] = gradbasis3(node,elem);

hK = (6*volume).^(1/3);

elem2ve = zeros(NT,3,6);
for e = 1:6 % six edges
   elem2ve(:,:,e) = node(elem(:,loc_e2v(e,2)),:)-node(elem(:,loc_e2v(e,1)),:);
end

elem2ve = elem2ve./repmat(sqrt(sum(elem2ve.^2,2)),1,3); % normalize for basis

faceNormal = cross(node(face(:,2),:) - node(face(:,1),:),...
    node(face(:,3),:) - node(face(:,2),:),2);
faceArea = 0.5*sqrt(sum(faceNormal.^2,2));
faceArea2elem = faceArea(elem2face);
faceArea2elemTotal = sum(faceArea2elem,2);

center = (node(elem(:,1),:) + node(elem(:,2),:) + ...
    node(elem(:,3),:) + node(elem(:,4),:))/4;

%% permeability tensor
t = cputime;  % record assembling time

if ~isfield(pde,'Eps'), pde.Eps = []; end

if ~isempty(pde.Eps) && isnumeric(pde.Eps)
    Eps2elem = repmat(pde.Eps, [1,1,NT]);   % Eps is a constant matrix
    Eps2elem = permute(Eps2elem,[3,1,2]); % switch the element idx to the 1st dim
end

if ~isempty(pde.Eps) && ~isnumeric(pde.Eps)
    Eps2elem = zeros(NT,3,3);  %#ok<PREALL>
    % Eps2elem is a NTx3x3 array so that Eps2elem(i,:,:) is the tensor at the
    % center of i-th element
    Epstmp = arrayfun(@(rowidx) pde.Eps(center(rowidx,:)), ...
        1:size(center,1), 'UniformOutput',0);
    Eps2elem = cat(3,Epstmp{:});
    Eps2elem = permute(Eps2elem,[3,1,2]); % switch the element idx to the 1st dim
end

%% compute eps \nabla_w phi
EpsDphi = zeros(NT,3,4);
for i = 1:4
    EpsDphi(:,:,i) = sum(bsxfun(@times, Eps2elem, Dphi(:,:,i)), 2);
end

%% assembling
% DoF order: # of Dofs
% \lambda_h: NT + NF, local 1+4
% u_h: 3*NT
% s_h: s_0 + s_b;  NT + NF (simply-connected case), local 1+4
% q_h: q_0 + q_b; 3*NT + 2*NF (for H(curl) vector field)
% assembling indices following locally within each group

%% equation 1 for u_h and lambda_h

blockUlambda = sparse(4*NT+NF,4*NT+NF);  %#ok<NASGU>
% (u_h, \epsilon\nabla_w phi)
% + \sum h_T^{-1} <\lambda_0 - \lambda_b, \varphi_0 - \varphi_b>_{boundary T}

%% (u_h, \epsilon\nabla_w phi_{face})
% locally the basis for P_0(T)^3 is edge [1, 2], [1, 3], and [1, 4]
% size (NT+NF, 3*NT)
C = sparse(NF,3*NT); %(u_h, \epsilon\nabla_w phi_{face})

for j = 1:3 % constant vector space spanned by 3 edge vectors
    for i = 1:4 % face 1 to 4
        % Cij = (q_j, eps \nabla_w \phi_{F_i})
        % \nabla_w \phi_{F_i} = -3\nabla \phi_i

        Cij = dot(elem2ve(:,:,j),-3*EpsDphi(:,:,i),2).*volume;

        ii = double(elem2face(:,i));
        jj = elem2intdof(:,j);
        C = C + sparse(ii,jj,Cij,NF,3*NT);
    end
end

Ugradphi = [sparse(NT, 3*NT); C];

%% \sum h_T^{-1} <\lambda_0 - \lambda_b, \varphi_0 - \varphi_b>_{boundary T}
% this is essetially a mass matrix stab
Stab_Mh = sparse(NT+NF,NT+NF); %#ok<NASGU>

L0ij = faceArea2elemTotal./hK;
Lambda00 = sparse(1:NT, 1:NT, L0ij, NT, NT); %% <\lambda_0, \lambda_0>

Lambda0b = sparse(NT, NF); %% <\lambda_b, \lambda_0>
Lambdabb = sparse(NF, NF); %% <\lambda_b, \lambda_b>
for j = 1:4 % face 1 to 4
    jj = double(elem2face(:,j));
    Lbj = faceArea2elem(:,j)./hK; % = <\lambda_{F_j}, \lambda_0>
    Lambda0b = Lambda0b + sparse(1:NT, jj, Lbj, NT, NF);
    Lambdabb = Lambdabb + sparse(jj, jj, Lbj, NF, NF);
end

Stab_Mh = [Lambda00 -Lambda0b; -Lambda0b' Lambdabb];
% Stablization on M_h should have null space dimension 1 (global constant)

%% \sum h_T <s_0 - s_b, r_0 - r_b>_{boundary T}

Stab_Sh = sparse(NT+NF,NT+NF); %#ok<NASGU>

S0ij = faceArea2elemTotal.*hK;
S00 = sparse(1:NT, 1:NT, S0ij, NT, NT); %% <s_0, s_0>

Sb0 = sparse(NT, NF); %% <s_b, s_0>
Sbb = sparse(NF, NF); %% <s_b, s_b>
for j = 1:4 % face 1 to 4
    jj = double(elem2face(:,j));
    Sbj = faceArea2elem(:,j).*hK; % = <\lambda_{F_j}, \lambda_0>
    Sb0 = Sb0 + sparse(1:NT, jj, Sbj, NT, NF);
    Sbb = Sbb + sparse(jj, jj, Sbj, NF, NF);
end

Stab_Sh = [S00 -Sb0; -Sb0' Sbb];
% Stablization on s_h should have null space dimension 1 (global constant)

%% Equation 1 for u_h and q_h = {q_0, q_b}

%%  (u_h, \epsilon\nabla_w \times psi_{face})
% locally the basis for P_0(T)^3 is the edge vector representing
% the edge [1, 2], [1, 3], and [1, 4] (local indexing of iFEM tradition)
% locally the basis for the tangential component are
% the edge 1 and 2 vectors for the triangular face (local indexing on this face)
% size: (3*NT+2*NF, 3*NT)

D = sparse(2*NF,3*NT);
%(u_h, \nabla_w \times psi_{face}) with 2 basis vec at each face

for i = 1:4 % face 1 to 4
    for j = 1:3 % constant vector space spanned by 3 edge vectors
        for e = 1:2 % two basis vectors on each face
            % Dije = (q_j, eps \nabla_w \times \psi_{F_i,e})
            % \nabla_w \times [0, \psi_{F_i}] =
            % (rotation of \psi_{F_i} counter-clockwise by pi/2)*|F_i|/|K|
            % = 3 \psi_{F_i} \times \nabla phi_i

            curlpsiFe = 3*mycross(elem2ve(:,:,loc_e2f(i,e)), Dphi(:,:,i));
            Dije = dot(elem2ve(:,:,j),curlpsiFe,2).*volume;

            % k-th (global face indexing) face has 2 DoFs in W_h: 2k-1, and 2k
            ii = 2*double(elem2face(:,i)) - e + 1;
            jj = elem2intdof(:,j);
            D = D + sparse(ii,jj, Dije, 2*NF,3*NT);
        end
    end
end

Ucurlpsi = [sparse(3*NT, 3*NT); D];

%%  (\psi, \epsilon\nabla_w s_h)
Psigrads = sparse(3*NT+2*NF, NT+NF);

E = sparse(3*NT,NF);
for i = 1:3 % constant vector space spanned by 3 edge vectors W_{h,0}
    for j = 1:4 % face 1 to 4
        % Eij = (\psi_{K,i}, eps \nabla_w \phi_{F_j})
        % \nabla_w \phi_{F_j} = -3\nabla \phi_j

        Eij = dot(elem2ve(:,:,i),-3*EpsDphi(:,:,j),2).*volume;

        ii = elem2intdof(:,i);
        jj = double(elem2face(:,j));
        E = E + sparse(ii,jj,Eij,3*NT,NF);
    end
end

%%% E is C's transpose, the code above is for booking-keeping
E = C';
Psigrads(1:3*NT,NT+1:end) = E;

%% \sum h_T^{-1} <(q_0-q_b)\times n, (\psi_0-\psi_b)\times n>_{boundary T}

% \sum h_T^{-1} <q_0\times n, \psi_0 \times n>_{boundary T}
WS00 = sparse(3*NT, 3*NT);
for i = 1:3 % constant vector space spanned by 3 edge vectors W_{h,0}
    for j = 1:3 % constant vector space spanned by 3 edge vectors W_{h,0}
        S00ijface = 0;
        for f = 1:4
            psi0crossn = mycross(elem2ve(:,:,i), Dphi(:,:,f)); % test
            q0crossn = mycross(elem2ve(:,:,j), Dphi(:,:,f)); % trial

            S00ijface = S00ijface + 9*dot(q0crossn,psi0crossn,2)...
                .*volume.^2./faceArea2elem(:,f);
        end
        S00ijface = S00ijface./hK;
        ii = elem2intdof(:,i);
        jj = elem2intdof(:,j);
        WS00 = WS00 + sparse(ii,jj,S00ijface,3*NT, 3*NT);
    end
end

% \sum h_T^{-1} <q_b\times n, \psi_0 \times n>_{boundary T}
WSb0 = sparse(3*NT, 2*NF);
for i = 1:3 % constant vector space spanned by 3 edge vectors W_{h,0}
    for j = 1:4 % 4 face for W_{h,b} each basis has support only on F_j
        for e = 1:2 % each face has 2 DoFs
            psi0crossn = mycross(elem2ve(:,:,i), Dphi(:,:,j)); % test
            qbcrossn = mycross(elem2ve(:,:,loc_e2f(j,e)), Dphi(:,:,j)); % trial

            Sb0ijface = 9*dot(qbcrossn,psi0crossn,2)...
                .*volume.^2./faceArea2elem(:,j)./hK;

            ii = elem2intdof(:,i);
            % k-th (global face indexing) face has 2 DoFs
            jj = 2*double(elem2face(:,j)) - e + 1;
            WSb0 = WSb0 + sparse(ii,jj,Sb0ijface,3*NT,2*NF);
        end
    end
end

% \sum h_T^{-1} <q_b\times n, \psi_b \times n>_{boundary T}
WSbb = sparse(2*NF, 2*NF);
for f = 1:4
    % this term is non-zero only when both trial and test
    % are associated with the same face
    for i = 1:2 % each face has 2 DoFs
        for j = 1:2
            psibcrossn = mycross(elem2ve(:,:,loc_e2f(f,i)), Dphi(:,:,f));
            qbcrossn = mycross(elem2ve(:,:,loc_e2f(f,j)), Dphi(:,:,f));

            Sbbijface = 9*dot(qbcrossn,psibcrossn,2)...
                .*volume.^2./faceArea2elem(:,f)./hK;

            % k-th (global face indexing) face has 2 DoFs
            ii = 2*double(elem2face(:,f)) - i + 1;
            jj = 2*double(elem2face(:,f)) - j + 1;
            WSbb = WSbb + sparse(ii,jj,Sbbijface,2*NF,2*NF);
        end
    end
end

Stab_Wh = [WS00 -WSb0; -WSb0' WSbb];
% Stab_Wh should have null space dimension
% = span{gradient of continuous P_1 Langrange} = # node -1

%% overall stiffness matrix for the system
blockUlambda = [Stab_Mh Ugradphi; Ugradphi' sparse(3*NT,3*NT)];
blockOffDiag = sparse(4*NT+3*NF, 4*NT+NF);
blockOffDiag(NT+NF+1:end, NT+NF+1:end) = Ucurlpsi;
blockQs = [-Stab_Sh Psigrads'; Psigrads Stab_Wh];
bigA = [blockUlambda blockOffDiag'; blockOffDiag blockQs];

%% right hand sides

%% -(f,\varphi_0)
ft = zeros(NT,1);

for p = 1:nQuad
		% quadrature points in the x-y-z coordinate
		pxyz = lambda(p,1)*node(elem(:,1),:) ...
			 + lambda(p,2)*node(elem(:,2),:) ...
			 + lambda(p,3)*node(elem(:,3),:) ...
             + lambda(p,4)*node(elem(:,4),:);
		fp = pde.f(pxyz);
        ft = ft + w(p)*fp;
end

Fphi0 = -accumarray((1:NT)',ft.*volume,[NT 1]);

%% \sum_{T} <eps u\cdot n, \varphi_b>_{\partial T} near \partial \Omega
GNphib = zeros(NF,1); % <eps u\cdot n, \varphi_F>_{\partial T}
% update: face quadorder increased

if ~isfield(option,'gNquadorder')
    option.gNquadorder = 5;   % default order exact for linear gN
end
[lambdagN, weightgN] = quadpts(option.gNquadorder);               % linear bases
nQuadgN = size(lambdagN,1);

for i = 1:4 % 4 faces
    idxBdElem = find(bdFlag(:,i) > 0); % if an element has a boundary face

    if ~isempty(idxBdElem)
        bdNormal = -3*repmat(volume(idxBdElem,:),1,3).*Dphi(idxBdElem,:,:);
        % scaled normal with face area built-in
        loc_face = loc_f2v(i,:);

        EpsUdotN = zeros(size(idxBdElem,1),1);

        for pp = 1:nQuadgN
            % quadrature points in the x-y coordinate
            ppxyz = lambdagN(pp,1)*node(elem(idxBdElem,loc_face(1)),:) ...
                + lambdagN(pp,2)*node(elem(idxBdElem,loc_face(2)),:) ...
                + lambdagN(pp,3)*node(elem(idxBdElem,loc_face(3)),:);

            u_bd = pde.exactu(ppxyz);
            EpsU = sum(bsxfun(@times, Eps2elem(idxBdElem,:,:), u_bd), 2);
            EpsUdotN = EpsUdotN + ...
                weightgN(pp)*dot(squeeze(EpsU), bdNormal(:,:,i), 2);

        end

        GNphib = GNphib + accumarray(elem2face(idxBdElem,i), EpsUdotN, [NF 1]);
    end
end

%% (g, \psi_0)
Gpsi0 = zeros(3*NT,1);

for i = 1:3 % 3 vector basis on each element for \psi_0
    gtpsi0 = zeros(NT,1);
    for p = 1:nQuad
        % quadrature points in the x-y-z coordinate
        pxyz = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:) ...
            + lambda(p,4)*node(elem(:,4),:);
        gp = pde.g(pxyz);

        gtpsi0 = gtpsi0 + w(p)*dot(gp, elem2ve(:,:,i), 2);
    end

    Gpsi0 = Gpsi0 + accumarray(elem2intdof(:,i), gtpsi0.*volume, [3*NT 1]);
end
%% RHS
Rhs = [Fphi0; GNphib]; % test: phi_0, phi_b
Rhs = [Rhs; zeros(3*NT, 1)]; % test: v
Rhs = [Rhs; zeros(NT, 1)]; % test: r_0
Rhs = [Rhs; zeros(NF, 1)]; % test: r_b
Rhs = [Rhs; Gpsi0]; % test: psi_0
Rhs = [Rhs; zeros(2*NF, 1)]; % test: psi_b

%% set up free dof
% DoF order: # of Dofs
% prefix with "idx": global indices
% the DoF variable itself is boolean (fastest on MATLAB)
% \lambda_h: NT + NF, local 1+4 (no BC)
% u_h: 3*NT (no BC)
% s_h: s_0 + s_b;  NT + NF (simply-connected case), s_b = 0 on Gamma_0
% q_h: q_0 + q_b; 3*NT + 2*NF (for H(curl) vector field) q_b\cross n =0
% assembling indices following locally within each group

isBdFace = false(NF,1);
isBdFace(elem2face(bdFlag(:) > 0)) = true;
idxBdFace = find(isBdFace);
idxBdFaceVec = [2*idxBdFace-1 2*idxBdFace]';
idxBdFaceVec = idxBdFaceVec(:);

freeDof = true(8*NT+4*NF,1);
idxBdSb = idxBdFace + 5*NT+NF; % boundary DoF indices for s_b
idxBdQb = idxBdFaceVec + 8*NT+2*NF; % boundary DoF indices for q_b

%%%%% fixing 1 in lambda_0 is enough, the others are optional %%%%%
%%% current config according to the paper: fixing 1 in lambda_0
%%% bd dofs of s and q
freeDof([1; idxBdSb; idxBdQb]) = false;

%%%%% fixing bd dofs for q
% freeDof([1; idxBdQb]) = false;

%%%%% fixing bd dofs for s
% freeDof([1; idxBdSb]) = false;

idxIntBdFace = [];
% old way of handling cavity (second betti number not zero)
if any(elem2face(bdFlag(:) == 2)) && false
    disp('Modifying cavity dofs')
    isIntBdFace = false(NF,1);
    isIntBdFace(elem2face(bdFlag(:) == 2)) = true;
    idxIntBdFace = find(isIntBdFace);
    idxIntBdSb = idxIntBdFace + 5*NT+NF;  % boundary DoF indices for s_b on cavity
    freeDof(idxIntBdSb) = true;
end

%% Record assembling time
assembleTime = cputime - t;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% direct solve
NDof = 8*NT+4*NF;

bigSoln = zeros(NDof,1);

[~,sys] = memory;
if sum(freeDof) > 2e6 && sys.PhysicalMemory.Available < 3e10
    % 2 million DoFs need about 27GB free memory to be solved
    % 3 million DoFs need about 39GB free memory
    fprintf('Number of DoF %d exceeds estimated max dofs allowed in memory, stop. \n',sum(freeDof));
    return
end
bigSoln(freeDof) = bigA(freeDof, freeDof)\Rhs(freeDof);

%% post-processing for cavity
% TO-DO: current code allows only 1 cavity
if any(elem2face(bdFlag(:) == 2))
    disp('Post-processing cavity dofs')
    intBdFaceVec = false(size(freeDof,1), 1);
    isIntBdFace = false(NF,1);
    isIntBdFace(elem2face(bdFlag(:) == 2)) = true;
    idxIntBdFace = find(isIntBdFace);
    idxIntBdSb = idxIntBdFace + 5*NT+NF;  % boundary DoF indices for s_b on cavity
    intBdFaceVec(idxIntBdSb) = true;

    AintBdFaceVec = bigA*intBdFaceVec;
    sbConst = dot((Rhs - bigA*bigSoln),AintBdFaceVec)/dot(AintBdFaceVec,AintBdFaceVec);
    fprintf('s_b on the interior boundary is %6.4g \n',sbConst);
    bigSoln = bigSoln + sbConst*intBdFaceVec;
end

%% computing error
errL2U = zeros(NT,1);

idxUDoF = (NT+NF+1:NT+NF+3*NT)';
uh = bigSoln(idxUDoF);
uhElem = zeros(NT,3);

idxq0DoF = (5*NT+2*NF+1:5*NT+2*NF+3*NT)';
qh0 = bigSoln(idxq0DoF);
qh0Elem = zeros(NT,3);

idxqbDoF = (8*NT+2*NF+1:8*NT+4*NF)';
qhb = bigSoln(idxqbDoF);

idxqDoF = (5*NT+2*NF+1:8*NT+4*NF)';
qh = bigSoln(idxqDoF);

idxlambdaDoF = (1:NT+NF)';
lambdah = bigSoln(idxlambdaDoF);

lambdah0 = bigSoln((1:NT)');
lambdahb = bigSoln((NT+1:NT+NF)');

idxsDoF = (4*NT+NF+1:5*NT+2*NF)';
sh = bigSoln(idxsDoF);
sh0 = bigSoln((4*NT+NF+1:5*NT+NF)');
shb = bigSoln((5*NT+NF+1:5*NT+2*NF)');

for k = 1:3 % three bases in each K
    uhElem = uhElem + repmat(uh(elem2intdof(:,k)),1,3).*elem2ve(:,:,k);
    qh0Elem = qh0Elem + repmat(qh0(elem2intdof(:,k)),1,3).*elem2ve(:,:,k);
end

[lambda,w] = quadpts3(option.dquadorder+2); % quadrature order is 1 or 2
nQuad = size(lambda,1);
for p = 1:nQuad
    pxyz = lambda(p,1)*node(elem(:,1),:) ...
        + lambda(p,2)*node(elem(:,2),:) ...
        + lambda(p,3)*node(elem(:,3),:) ...
        + lambda(p,4)*node(elem(:,4),:);

    up = pde.exactu(pxyz);
    EpsUminusUh = sum(bsxfun(@times, Eps2elem, up - uhElem), 2);
    errL2U = errL2U + w(p)*dot(squeeze(EpsUminusUh), up - uhElem, 2);
end

errL2U = sqrt(errL2U.*volume);
errL2UTotal = norm(errL2U);

% idxfreeFace = find(~isBdFace);
% idxFreeFaceVec = [2*idxfreeFace-1 2*idxfreeFace]';
% idxFreeFaceVec = idxFreeFaceVec(:);
% errorStabTotal = sqrt(qh(idxFreeFaceVec)'*...
%     Stab_Wh(idxFreeFaceVec,idxFreeFaceVec)*qh(idxFreeFaceVec)...
%     +lambdah'*Stab_Mh*lambdah);
errorStabTotal = sqrt(qh'*Stab_Wh*qh+lambdah'*Stab_Mh*lambdah);
errorsTotal = sqrt(sh'*Stab_Sh*sh);

%% Output information
if nargout == 1
    soln = bigSoln;
else
    soln = struct('u',uh,...
                  'uh2elem', uhElem,...
                  'q0',qh0,...
                  'qb',qhb,...
                  'lambda0',lambdah0,...
                  'lambdab',lambdahb,...
                  's0',sh0,...
                  'sb',shb);
    eqn = struct('A',bigA,'b',Rhs,...
                'face',face,...
                'freeDof',freeDof,...
                'bdDofScalar',idxBdFace,...
                'bdDofVec',idxBdFaceVec,...
                'bdDofCavity',idxIntBdFace,...
                'errorU',errL2UTotal, ...
                'errorqlambda', errorStabTotal,...
                'errorS', errorsTotal,...
                'errorUelem',errL2U,...
                'NFace',NF);
    info.assembleTime = assembleTime;
end

%% end of function
end


%
%  T = build_toep( c, d, n );
%
%  Given:
%    c - the nonzero part of a central column of a banded Toeplitz
%        matrix
%    d - index of c containing the diagonal element of T
%    n - dimension of the banded Toeplitz matrix
%
%  The banded Toeplitz matrix is constructed explicitly.
%
function T = build_toep( c, d, n )

m = length( c );

col = zeros(n,1);
row = col';
col(1:m-d+1,1) = c(d:m);
row(1,1:d) = c(d:-1:1)';
T = toeplitz( col, row );


function [T_request_Nm, OverallRatio] = control_ForceRequestConverter(F_x_request_N, ...
                        tire_radius_m, i_c, i_f, i_r, i_gearset, ...
                        gear_in)
%__________________________________________________________________________
%% Documentation
%
% Author:       Dominik St�rk (dominik.staerk@tum.de)
%
% Start Date:   21.12.2020
%
% Description:
% This function converts the requested overall longitudinal force
% in a torque request for the engine. The effects of gear-ratios and
% -efficiencies as well as differentials and acceleration-losses are taken
% into account.
%
% Inputs:
%   F_x_request_N   - requested overall longitudinal force [N]
%   tire_radius_m   - tire`s radius [m]
%   i_c [2x1]       - central differential ratio and efficiency [-, -]
%   i_f [2x1]       - front differential ratio and efficiency [-, -]
%   i_r [2x1]       - rear differential ratio and efficiency [-, -]
%   i_gearset [4x7] - gear information
%   gear_in         - currently selected gear
%
% Outputs:
%   T_request_Nm    - requested engine torque [Nm]

p_torque_rear = 1;

%% request engine torque
T_Nm = F_x_request_N * tire_radius_m;

if gear_in ~= 0
    T_request_Nm = T_Nm / (i_c(1)*i_c(2) * ...
      ((1-p_torque_rear)*i_f(1)*i_f(2) + p_torque_rear*i_r(1)*i_r(2)) * ...
      i_gearset(2, gear_in+1)*i_gearset(4, gear_in+1));
    OverallRatio = (i_c(1)*i_c(2) * ...
      ((1-p_torque_rear)*i_f(1)*i_f(2) + p_torque_rear*i_r(1)*i_r(2)) * ...
      i_gearset(2, gear_in+1)*i_gearset(4, gear_in+1))/tire_radius_m;
else
    % use first gear even if in neutral
    T_request_Nm = T_Nm / (i_c(1)*i_c(2) * ...
      ((1-p_torque_rear)*i_f(1)*i_f(2) + p_torque_rear*i_r(1)*i_r(2)) * ...
      i_gearset(2, gear_in+2)*i_gearset(4, gear_in+1));
    % no brake torque when in neutral
    OverallRatio = 0;
end


function c = ttv(a,v,dims)
%TTV Tensor times vector for ktensor.
%
%   Y = TTV(X,A,N) computes the product of Kruskal tensor X with a
%   (column) vector A.  The integer N specifies the dimension in X
%   along which A is multiplied.  If size(A) = [I,1], then X must have
%   size(X,N) = I.  Note that ndims(Y) = ndims(X) - 1 because the N-th
%   dimension is removed.
%
%   Y = TTV(X,{A1,A2,...}) computes the product of tensor X with a
%   sequence of vectors in the cell array.  The products are computed
%   sequentially along all dimensions (or modes) of X. The cell array
%   contains ndims(X) vectors.
%
%   Y = TTV(X,{A1,A2,...},DIMS) computes the sequence tensor-vector
%   products along the dimensions specified by DIMS.
%
%   See also TENSOR/TTV, KTENSOR, KTENSOR/TTM.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%%%%%%%%%%%%%%%%%%%%%%
%%% ERROR CHECKING %%%
%%%%%%%%%%%%%%%%%%%%%%

% Check the number of arguments
if (nargin < 2)
    error('TTV requires at least two arguments.');
end

% Check for 3rd argument
if ~exist('dims','var')
    dims = [];
end

% Check that 2nd argument is cell array. If not, recall with v as a
% cell array with one element.
if ~iscell(v)
    c = ttv(a,{v},dims);
    return;
end

% Get sorted dims and index for multiplicands
[dims,vidx] = tt_dimscheck(dims,ndims(a),numel(v));

% Check that each multiplicand is the right size.
for i = 1:numel(dims)
    if ~isequal(size(v{vidx(i)}),[size(a,dims(i)) 1])
        error('Multiplicand is wrong size');
    end
end

% Figure out which dimensions will be left when we're done
remdims = setdiff(1:ndims(a),dims);

% Collapse dimensions that are being multiplied out
newlambda = a.lambda;
for i = 1:numel(dims)
    newlambda = newlambda .* ( a.u{dims(i)}' * v{vidx(i)} );
end

% Create final result
if isempty(remdims)
    c = sum(newlambda);
else
    c = ktensor(newlambda,a.u{remdims});
end


function visible = visiblevertices(FV, R)

V2 = (R*FV.vertices')';

% Store the vertex depths for z-buffering
Z = V2(:, 3);

% Compute the projected vertices in the image plane
UV(:, 1)    = V2(:, 1) ;	% orthographic projection for x
UV(:, 2)    = V2(:, 2) ;	% orthographic projection for y

% Transform to the pixel plane (the axes remain switched)
UV(:, 1)    = UV(:, 1)-min(UV(:, 1));
UV(:, 2)    = UV(:, 2)-min(UV(:, 2));
UV          = UV + 1;

UV=(UV./max(UV(:))).*1000;
width=1000;
height=1000;

% Get the triangle vertices
v1      = FV.faces(:, 1);
v2      = FV.faces(:, 2);
v3      = FV.faces(:, 3);
Nfaces  = size(FV.faces, 1);

% Compute bounding boxes for the projected triangles
x       = [UV(v1, 1), UV(v2, 1), UV(v3, 1)];
y       = [UV(v1, 2), UV(v2, 2), UV(v3, 2)];
minx    = ceil (min(x, [], 2));
maxx    = floor(max(x, [], 2));
miny    = ceil (min(y, [], 2));
maxy    = floor(max(y, [], 2));

clear x y

% Frustum culling
minx    = max(1,        minx);
maxx    = min(width,    maxx);
miny    = max(1,        miny);
maxy    = min(height,   maxy);

% Construct the pixel grid (can speed up by precomputing if shared among the images)
[rows, cols] = meshgrid(1: width, 1: height);

% Initialize the depth-, face- and weight-buffers
zbuffer     = -inf(height, width);
fbuffer     = zeros(height, width);
wbuffer1    = NaN(height, width);
wbuffer2    = NaN(height, width);
wbuffer3    = NaN(height, width);

% For each triangle (can speed up by comparing the triangle depths to the z-buffer and priorly sorting the triangles by increasing depth)
for i = 1: Nfaces

    % If some pixels lie in the bounding box
    if minx(i) <= maxx(i) && miny(i) <= maxy(i)

        % Get the pixels lying in the bounding box
        px = rows(miny(i): maxy(i), minx(i): maxx(i));
        py = cols(miny(i): maxy(i), minx(i): maxx(i));
        px = px(:);
        py = py(:);

        % Compute the edge vectors
        e0 = UV(v1(i), :);
        e1 = UV(v2(i), :) - e0;
        e2 = UV(v3(i), :) - e0;

        % Compute the barycentric coordinates (can speed up by first computing and testing a solely)
        det     = e1(1) * e2(2) - e1(2) * e2(1);
        tmpx    = px - e0(1);
        tmpy    = py - e0(2);
        a       = (tmpx * e2(2) - tmpy * e2(1)) / det;
        b       = (tmpy * e1(1) - tmpx * e1(2)) / det;

        % Test whether the pixels lie in the triangle
        test = a >= 0 & b >= 0 & a + b <= 1;

        % If some pixels lie in the triangle
        if any(test)

            % Get the pixels lying in the triangle
            px = px(test);
            py = py(test);

            % Interpolate the triangle depth for each pixel
            w2 = a(test);
            w3 = b(test);
            w1 = 1 - w2 - w3;
            pz = Z(v1(i)) * w1 + Z(v2(i)) * w2 + Z(v3(i)) * w3;

            % For each pixel lying in the triangle
            for j = 1: length(pz)

                % Frustum culling
%                if pz(j) <= -near && pz(j) >= -far

                    % Update the depth-, face- and weight-buffers
                    if pz(j) > zbuffer(py(j), px(j))
                        zbuffer(py(j), px(j))   = pz(j);
                        fbuffer(py(j), px(j))   = i;
                        wbuffer1(py(j), px(j))  = w1(j);
                        wbuffer2(py(j), px(j))  = w2(j);
                        wbuffer3(py(j), px(j))  = w3(j);
                    end

%                end

            end

        end

    end

end

%figure; imshow(zbuffer,[])

clear UV Z zbuffer px py pz minx maxx miny maxy

% Get the vertices to render
test    = fbuffer ~= 0;
f       = unique(fbuffer(test));
v       = unique([v1(f); v2(f); v3(f)]);
f       = find(any(ismember(FV.faces, v), 2));
Nfaces  = length(f);

visible = v;

return

% Compute the edge vectors
e1s = V2(v2(f), :) - V2(v1(f), :);
e2s = V2(v3(f), :) - V2(v1(f), :);
e3s = V2(v2(f), :) - V2(v3(f), :);

clear V2

% Normalize the edge vectors
e1s_norm = e1s ./ repmat(sqrt(sum(e1s.^2, 2)), 1, 3);
e2s_norm = e2s ./ repmat(sqrt(sum(e2s.^2, 2)), 1, 3);
e3s_norm = e3s ./ repmat(sqrt(sum(e3s.^2, 2)), 1, 3);

% Compute the angles
angles(:, 1) = acos(sum(e1s_norm .* e2s_norm, 2));
angles(:, 2) = acos(sum(e3s_norm .* e1s_norm, 2));
angles(:, 3) = pi - (angles(:, 1) + angles(:, 2));

% Compute the triangle weighted normals
triangle_normals    = cross(e1s, e3s, 2);
w1_triangle_normals = triangle_normals .* repmat(angles(:, 1), 1, 3);
w2_triangle_normals = triangle_normals .* repmat(angles(:, 2), 1, 3);
w3_triangle_normals = triangle_normals .* repmat(angles(:, 3), 1, 3);

clear e1s e2s e3s e1s_norm e2s_norm e3s_norm angles triangle_normals

% Initialize the vertex normals
normals = zeros(Nvertices, 3);

% Update the vertex normals
for i = 1: Nfaces
    normals(v1(f(i)), :) = normals(v1(f(i)), :) + w1_triangle_normals(i, :);
    normals(v2(f(i)), :) = normals(v2(f(i)), :) + w2_triangle_normals(i, :);
    normals(v3(f(i)), :) = normals(v3(f(i)), :) + w3_triangle_normals(i, :);
end

clear w1_triangle_normals w2_triangle_normals w3_triangle_normals

% Normalize the vertex normals
normals = normals(v, :);
normals = normals ./ repmat(sqrt(sum(normals.^2, 2)), 1, 3);

% Self-occlusions
test = normals(:, 3) >= 0;

% Interpolate the z-buffer at the projected vertices
vZ          = inf(size(FV.vertices, 1), 1);
vZ(test)    = interp2(rows, cols, zbuffer, UV(test, 1), UV(test, 2), '*linear');

% Fix the border
tmp_test            = vZ(test) == -inf;
vZ(test(tmp_test))	= interp2(rows, cols, zbuffer, UV(test(tmp_test), 1), UV(test(tmp_test), 2), '*nearest');

% Compute the vertex visibility index in terms of relative depth compared to the z-buffer
test(test) = abs(vZ(test) - Z(test)) <= 0.05 * (max(Z) - min(Z));

clear ambient1 diffuse1 specular1 ambient2 diffuse2 specular2 ambient3 diffuse3 specular3 v

% Initialize the image
im1 = NaN(height, width);
im2 = NaN(height, width);
im3 = NaN(height, width);

% Rasterize the image
v1          = v1(fbuffer(test));
v2          = v2(fbuffer(test));
v3          = v3(fbuffer(test));
w1          = wbuffer1(test);
w2          = wbuffer2(test);
w3          = wbuffer3(test);
im1(test)   = w1 .* texture1(v1) + w2 .* texture1(v2) + w3 .* texture1(v3);
im2(test)   = w1 .* texture2(v1) + w2 .* texture2(v2) + w3 .* texture2(v3);
im3(test)   = w1 .* texture3(v1) + w2 .* texture3(v2) + w3 .* texture3(v3);

clear v1 v2 v3 w1 w2 w3

% Reshape the image
im = cat(3, im1, im2, im3);

clear im1 im2 im3 test

im = flipud(fliplr(im));

end


function [bcx,bcy] = specific_flow(xbd,ybd)
%backwardstep_flow   Reference problem 5.2 inflow condition
%   [bcx,bcy] = specific_flow(xbd,ybd);
%   input
%          xbd          x coordinate vector
%          ybd          y coordinate vector
%
%   specifies backward step flow boundary condition
%   IFISS function: DJS; 6 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
bcx=0*xbd; bcy=0*xbd;
k=find(xbd==-1); bcx(k)=4*ybd(k).*(1-ybd(k));
return


function [M,P] = ckf_predict(M,P,f,Q,f_param)
% CKF_PREDICT - Cubature Kalman filter prediction step
%
% Syntax:
%   [M,P] = CKF_PREDICT(M,P,[f,Q,f_param])
%
% In:
%   M - Nx1 mean state estimate of previous step
%   P - NxN state covariance of previous step
%   f - Dynamic model function as a matrix A defining
%       linear function f(x) = A*x, inline function,
%       function handle or name of function in
%       form f(x,param)                   (optional, default eye())
%   Q - Process noise of discrete model   (optional, default zero)
%   f_param - Parameters of f               (optional, default empty)
%
% Out:
%   M - Updated state mean
%   P - Updated state covariance
%
% Description:
%   Perform additive form spherical-radial cubature Kalman filter (CKF)
%   prediction step.
%
%   Function f(.) should be such that it can be given a
%   DxN matrix of N sigma Dx1 points and it returns
%   the corresponding predictions for each sigma
%   point.
%
% See also:
%   CKF_UPDATE, CRTS_SMOOTH, CKF_TRANSFORM, SPHERICALRADIAL
%
% References:
%   Arasaratnam and Haykin (2009). Cubature Kalman Filters.
%    IEEE Transactions on Automatic Control, vol. 54, no. 5, pp.1254-1269

% Copyright (c) 2010 Arno Solin
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.
%%

  %
  % Check which arguments are there
  %
  if nargin < 2
    error('Too few arguments');
  end
  if nargin < 3
    f = [];
  end
  if nargin < 4
    Q = [];
  end

  %
  % Apply defaults
  %
  if isempty(f)
    f = eye(size(M,1));
  end
  if isempty(Q)
    Q = zeros(size(M,1));
  end

  %
  % Do transform and add process noise
  %
  if nargin < 5
    [M,P] = ckf_transform(M,P,f);
  else
    [M,P] = ckf_transform(M,P,f,f_param);
  end
  P = P + Q;


function [Vx,Vy] = computeFlowBkgSup(im1curr,im2curr,params,gparams)
% function [Vx,Vy] = computeFlowBkgSup(im1curr,im2curr,params)
% Computes flow and then suppresses flow in the background.

%There is some flow towards the center that can't be estimated
% properly. The flow increases as we go away from the center.
% d_err is to account for that.

sz = round(size(im1curr));
bwimg = zeros(sz(1),sz(2));
ctr = [ceil( (sz(1)+1)/2),ceil( (sz(2)+1)/2)];
bwimg(ctr(1),ctr(2))=1;
dimg = bwdist(bwimg,'euclidean');
[xx,yy]= meshgrid(1:sz(2),1:sz(1));
aimg = atan2(-(yy-ctr(1)),-(xx-ctr(2)));

dd_err = dimg/size(im1curr,1);
flow_thres = gparams.flow_thres;

uv = estimate_flow_interface(im1curr,im2curr,...
  'hs-brightness',{'max_warping_iters',gparams.warping_iters});

if ~params.stationary,
  Vx = uv(:,:,1);
  Vy = uv(:,:,2);
  return;
end

cdx = params.dx;
cdy = params.dy;
curt = params.theta;
rotd = [cos(curt) sin(curt); -sin(curt) cos(curt)]*[cdx;cdy];
cdx = -rotd(1); cdy = -rotd(2);

ctheta = params.dtheta;
rotflowu = dimg.*(cos(aimg+ctheta)-cos(aimg));
rotflowv = dimg.*(sin(aimg+ctheta)-sin(aimg));

dd1 = sqrt( (uv(:,:,1)-cdx-rotflowu).^2 + (uv(:,:,2)-cdy-rotflowv).^2);

crdx = params.rdx;  crdy = params.rdy;
rotd = [cos(curt) sin(curt); -sin(curt) cos(curt)]*[crdx;crdy];
fly_flow = rotd;

dd2 = sqrt( (uv(:,:,1)-fly_flow(1)).^2 + (uv(:,:,2)-fly_flow(2)).^2);
dd = min(dd1,dd2);
for ndx = 1:2
  tt = uv(:,:,ndx);
  tt( dd<(dd_err+flow_thres)) = 0;
  uv(:,:,ndx) = tt;
end
Vx = uv(:,:,1);
Vy = uv(:,:,2);


% Copyright 2016 Google Inc.
%
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
% http ://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and
% limitations under the License.

function A = buildSecondDerivZMatrix(grid_size)

% d/dz for every entry in the cube except the first and last slice.
m = grid_size(1) * grid_size(2) * (grid_size(3) - 2);
n = grid_size(1) * grid_size(2) * grid_size(3);
e = ones(m, 1);
interior = spdiags([e, -2 * e, e], ...
    [0, grid_size(1) * grid_size(2), 2 * grid_size(1) * grid_size(2)], ...
    m, n);

boundary_z1 = makeBoundaryZ1(n);
boundary_zend = makeBoundaryZEnd(n);

% The matrix for a full cube.
cube = [boundary_z1; interior; boundary_zend];

% Repeat the cube for the last 2 dimensions.
A = sparse(0, 0);
for v = 1:grid_size(5)
    for u = 1:grid_size(4)
        A = blkdiag(A, cube);
    end
end

% Boundary conditions for the first slice.
% n is the number of columns for the full matrix (the number of variables
% in the grid).
function A = makeBoundaryZ1(n)
    mm = grid_size(1) * grid_size(2);
    nn = grid_size(1) * grid_size(2) * 2;
    e = ones(mm, 1);
    B = spdiags([-e, e], [0, grid_size(1) * grid_size(2)], mm, nn);
    % Concat zero columns to the right so we can vertical concat with the full
    % matrix later.
    A = [B, sparse(mm, n - nn)];
end

% Boundary conditions for the last slice.
function A = makeBoundaryZEnd(n)
    mm = grid_size(1) * grid_size(2);
    nn = grid_size(1) * grid_size(2) * 2;
    e = ones(mm, 1);
    B = spdiags([e, -e], [0, grid_size(1) * grid_size(2)], mm, nn);
    % Concat zero columns to the right so we can vertical concat with the full
    % matrix later.
    A = [sparse(mm, n - nn), B];
end

end


function [vars,eval] = km_akcca(pars,data)
% KM_AKCCA performs Alternating Kernel Canonical Correlation Analysis to
% blindly identify and equalize a single-input multiple-output Wiener.
%
% Input:	- pars: structure containing parameters of the alternating KCCA
%             algorithm
%			- data: structure containing the available data x, and the
%             unavailable data (source signal s, internal signals y, and
%             linear channels B)
% Output:	- vars: estimated variables
%           - eval: structure containing the resuls of the algorithm
%             evaluation
% USAGE: [vars,eval] = km_akcca(pars,data)
%
% Author: Steven Van Vaerenbergh (steven *at* gtas.dicom.unican.es), 2011.
%
% The algorithm in this file is based on the following publication:
% S. Van Vaerenbergh, J. Via and I. Santamaria, "Blind Identification of
% SIMO Wiener Systems based on Kernel Canonical Correlation Analysis",
% accepted for publication in IEEE Transactions on Signal Processing, 2013.
%
% This file is part of the Kernel Methods Toolbox for MATLAB.
% https://github.com/steven2358/kmbox

% AKCCA CORE
[vars, eval] = AKCCA_CORE_INIT(pars,data.x); % initialize
vars.it = 0;
eval.converged = false;
while ((vars.it < pars.it_max) && ~eval.converged)
    fprintf('.');
    vars.it = vars.it+1;
    vars = AKCCA_CORE_LINID(pars,vars); % CCA 1: estimate h and obtain z
    vars = AKCCA_CORE_NLINID(pars,vars); % CCA 2: estimate alpha and obtain y
    vars = AKCCA_CORE_EQ(pars,vars); % EQUALIZATION
    eval = AKCCA_CORE_EVAL(pars,vars,data,eval); % Error calculation and check convergence
end
fprintf('\n');

function [vars,eval] = AKCCA_CORE_INIT(pars,x)

% READ PARAMETERS AND VARIABLES
p = pars.p;
m = pars.m;
N = pars.data_N;
num_it = pars.it_max;
ktype = pars.kernel.type;
% ktype = pars.kernel{1};
kpar = pars.kernel.par;
diff_nlin = ~pars.identical_nonlin;	% boolean indicating different nonlinearities

% CONSTRUCT KERNEL MATRIX DECOMPOSITIONS
K = cell(p,1);
c = cell(p,1);
if diff_nlin,
    switch pars.decomp
        case 'KPCA'
            for i=1:p,
                if isa(kpar,'function_handle')	% determine kernel parameter
                    parfun = kpar;
                    c{i} = parfun(x{i});
                else c{i} = kpar(i);
                end
                Kfull = km_kernel_center(x{i},x{i},x{i},ktype,c{i});
                Kfull = 0.5*(Kfull+Kfull');	% avoid matlab rounding errors
                [V,D] = eig(Kfull);
                [D_sort,ind] = sort(real(diag(D)),'descend');
                if m<1,	% m represents the fraction of energy to be witheld
                    energy = sum(D_sort);
                    cumul_energy = cumsum(D_sort)/energy;
                    m = sum(cumul_energy < 1-m)+1;	% new m is number of required eigenvectors
                end
                K{i} = V(:,ind(1:m));
            end
        case 'ICD'

            for i=1:p,
                if isa(kpar,'function_handle')	% determine kernel parameter
                    parfun = kpar;
                    c{i} = parfun(x{i});
                else
                    c{i} = kpar(i);
                end
                if m<1,	% m represents the precision in ICD
                    precision = m;
                    mm = N;
                else
                    precision = 1E-10;
                    mm = m;
                end
                G = km_kernel_icd(x{i},ktype,c{i},mm,precision);
                G = G-repmat(mean(G),N,1);
                K{i} = G;
            end
        otherwise
            error 'wrong decomposition method';
    end

    y_est = x;

else
    Xf = [];
    for i=1:p,
        Xf = [Xf;x{i}]; %#ok<AGROW>
    end

    if isa(kpar,'function_handle')	% determine kernel parameter
        parfun = kpar;
        c = parfun(Xf);
    else c = kpar(i);
    end

    switch pars.decomp
        case 'KPCA'
            Kfull = km_kernel_center(Xf,Xf,Xf,ktype,c);
            Kfull = 0.5*(Kfull+Kfull');
            [V,D] = eig(Kfull);
            [D_sort,ind] = sort(real(diag(D)),'descend');

            if m<1,	% m represents the fraction of energy to be witheld
                energy = sum(D_sort);
                cumul_energy = cumsum(D_sort)/energy;
                m = sum(cumul_energy < 1-m)+1;	% new m is number of required eigenvectors
            else
                m=m*p;
            end
            for i=1:p,
                stind = (i-1)*N + 1;
                ndind = stind+N - 1;
                K{i} = V(stind:ndind,ind(1:m));
            end

        case 'ICD'

            if m<1,	% m represents the precision in ICD
                precision = m;
                mm = N;
            else
                precision = 1E-10;
                mm=m*p;
            end
            G = km_kernel_icd(Xf,ktype,c,mm,precision);
            G = G-repmat(mean(G),p*N,1);

            for i=1:p,
                stind = (i-1)*N + 1;
                ndind = stind+N - 1;
                K{i} = G(stind:ndind,:);
            end
        otherwise
            error 'wrong decomposition method';
    end

    y_est = x;
end

% WRITE EVAL AND VARIABLES
eval.MSE_y = zeros(1,num_it);
eval.MSE_z = zeros(2,num_it);
eval.MSE_h = zeros(1,num_it);
eval.result = zeros(1,num_it);

vars.y_est = y_est;		% initialize without taking into account the nonlinearity
vars.K = K;		% reduced kernel matrices
vars.x = x;		% original output data
vars.c = c;     % kernel parameter

function vars = AKCCA_CORE_LINID(pars,vars)
% estimate linear filters

% COPY PARAMETERS
p = pars.p;
L = pars.model_L;
N = pars.data_N;
K = vars.K;

y_est = vars.y_est;

% PROGRAM
n = N-L+1;

% get time-embedded data matrices
Ka = cell(p,1);
for i=1:p,
	Ka{i} = zeros(n,L);
% 	for n_ind = 1:n,    % too slow, better fill differently
% 		Ka{i}(n_ind,:) = y_est{i}(n_ind+L-1:-1:n_ind)';
% 	end
    for l_ind = 1:L,
        Ka{i}(:,l_ind) = y_est{i}(L-l_ind+1:L-l_ind+n)';
    end
end

h = SIMO_id_CCA(Ka);

zh_est = cell(p);
W = cell(p);
% rescale solution to fulfill restriction sumsqr = 1
sumnrg_h = 0;
for i=1:p,
	for j=1:p,
		if(i~=j)
			sumnrg_h = sumnrg_h + sum((Ka{i}*h{j}).^2);
		end
	end
end
for i=1:p,
	h{i} = h{i}/sqrt(sumnrg_h);
end

for i=1:p,
	for j=1:p,
		if (i~=j)
            mi = size(K{i},2);
            W{i,j} = zeros(n,mi);
			zh_est{i,j} = Ka{i}*h{j};	% system output
%             for n_ind=1:n,  % too slow, better fill differently
%                 kimat = K{i}(n_ind+L-1:-1:n_ind,:);
%                 W{i,j}(n_ind,:) = h{j}'*kimat;	% auxiliary variable
%             end
            for l_ind = 1:L,    % superfast
                W{i,j} = W{i,j} + h{j}(l_ind)*K{i}(L-l_ind+1:L-l_ind+n,:);
            end
		end
	end
end

% COPY VARIABLES
vars.h = h;		% filter estimation
vars.zh_est = zh_est;	% new output estimate
vars.W = W;		% auxiliary variable (filtered kernel matrices)

function vars = AKCCA_CORE_NLINID(pars,vars)
% estimate inverse nonlinearities

% READ PARAMETERS AND VARIABLES
p = pars.p;
diff_nlin = ~pars.identical_nonlin;	% boolean indicating different nonlinearities

K = vars.K;
W = vars.W;
reg = pars.reg;

% PROGRAM: 1 iteration
za_est = cell(p);
y_est = cell(p,1);
if diff_nlin
	% standard case: no identical nonlinearity
	a = SIMO_id_CCA_dual(W,reg);

	% rescale solution to fulfill restriction sumsqr = 1
	sumnrg_a = 0;
	for i=1:p,
		for j=1:p,
			if(i~=j)
				sumnrg_a = sumnrg_a + sum((W{i,j}*a{i}).^2);
			end
		end
	end

	for i=1:p,
		a{i} = a{i}/sqrt(sumnrg_a);
		y_est{i} = K{i}*a{i};	% get y estimates
		y_est{i} = y_est{i}/sqrt(norm(y_est{i}))*sqrt(norm(vars.x{i}));	% normalize
		for j=1:p,
			if (i~=j)
			za_est{i,j} = W{i,j}*a{i};	% system output
			end
		end
	end
else
	% identical nonlinearity
	a = SIMO_id_CCA_dual_identical(W,reg);
	for i=1:p,
		y_est{i} = K{i}*a;	% get y estimates
		for j=1:p,
			if (i~=j)
			za_est{i,j} = W{i,j}*a;	% system output
			end
		end
	end
end

% WRITE VARIABLES
vars.alpha = a;
vars.y_est = y_est;
vars.za_est = za_est;

function vars = AKCCA_CORE_EQ(pars,vars)
% equalize linear channels (zero-forcing)

% COPY PARAMETERS
p = pars.p;
L = pars.model_L;
N = pars.data_N;
k = pars.zf_k;

y_est = vars.y_est;
h = vars.h;

% PROGRAM
% use k observations to generate filter matrix and estimate equalizers
Hc = cell(p,1); Hr = cell(p,1);
TH = zeros(p*k,k+L-1);
for i=1:p,
	Hc{i} = [h{i}(1);zeros(k-1,1)];
	Hr{i} = [h{i}' zeros(1,k-1)];
	TH(i:p:end,:) = toeplitz(Hc{i},Hr{i});
end

% zero-forced equalizing
ZF = pinv(TH);

% S_est = zeros(N-k+1,k);
% Yi = zeros(p*k,1);
% for i=1:N-k+1,  % too slow, better fill differently
% 	for j=1:p,
% 		yi = y_est{j}(i+k-1:-1:i);
% 		Yi(j:p:end) = yi;
% 	end
% 	s_esti = ZF*Yi;
% 	S_est(i,:) = s_esti(1:k).';
% end
YY = zeros(N-k+1,p*k);
for j = 1:p,
    for k_ind=1:k,
        col = (k_ind-1)*p + j;
        YY(:,col) = y_est{j}(k-k_ind+1:N-k_ind+1);
    end
end
S_est = YY*ZF(1:k,:)';

cnorms = zeros(k,1);
for i=1:k,
	cnorms(i) = norm(ZF(:,i));
end
[mm,zfi] = min(cnorms); %#ok<ASGLU>
s_est = S_est(:,zfi);

inds = k-zfi+1:N-zfi+1;

% COPY VARIABLES
vars.s_est = s_est;
vars.ind_s = inds;
vars.ZF = ZF;

function eval = AKCCA_CORE_EVAL(pars,vars,data,eval)
% evaluate solutions: calculate errors

% COPY PARAMETERS
p = pars.p;
vb = pars.verbose;

y_est = vars.y_est;
s_est = vars.s_est;
h = vars.h;
it = vars.it;
inds = vars.ind_s;

s = data.s; % true source signal
B = data.B; % true channels
y = data.y; % true internal channels

% PROGRAM
MSE_y = 0; MSE_h = 0; MSE_za = 0; MSE_zh = 0;
for i=1:p,
	% check if system internals are known
	if (iscell(y))
		% MSE Y
		MSE_y = MSE_y + norm_compare(y{i},y_est{i})/p;
		% MSE H
		numz = length(h{i})-length(B{i});
		if (numz>0)
			% channel overestimation case
			B{i} = [B{i};zeros(numz,1)];
		end
		MSE_h = MSE_h + norm_compare(B{i},h{i})/p;
	end

	% MSE Z
	if isfield(vars,'za_est')
		za_est = vars.za_est;
	end
	zh_est = vars.zh_est;

	for j=i+1:p,
		if (i~=j)
			MSE_zh = MSE_zh + sum((zh_est{i,j}-zh_est{j,i}).^2);
			if isfield(vars,'za_est')
				MSE_za = MSE_za + sum((za_est{i,j}-za_est{j,i}).^2);
			end
		end
	end
end

signal_test = s(inds);
signal_est = s_est;

switch lower(pars.data_type)
	case {'gaussian'}
		% result = MSE between s and s_est
		result = norm_compare(signal_test,signal_est);
	case 'bits'
		% result = BER
		[MSEsb,sci] = scale_compare(signal_test,signal_est); %#ok<ASGLU>
		result = sum(sign(signal_test)~=sign(signal_est/sci))/length(signal_test);
        s_est = sign(signal_est/sci);
	otherwise
		disp('Unknown signal type.')
end

% OUTPUT
if vb, fprintf(1,'MSE z_a: %f\n',MSE_za); end;
if vb, fprintf(1,'MSE z_h: %f\n',MSE_zh); end;
if vb, fprintf(1,'MSE y: %f\n',MSE_y); end;
if vb, fprintf(1,'Result: %f\n',result); end;

% COPY VARIABLES
eval.MSE_h(it) = MSE_h;
eval.MSE_y(it) = MSE_y;
eval.MSE_z(1,it) = MSE_zh;
eval.MSE_z(2,it) = MSE_za;
eval.result(it) = result;
eval.s_est = s_est;

% CHECK STOP
if (it>2)
	% stop when both sub-iterations obtain consecutive equal costs
	change1 = abs(eval.MSE_z(2*it)-eval.MSE_z(2*it-2));
	change2 = abs(eval.MSE_z(2*it-1)-eval.MSE_z(2*it-3));
	if ((change1 < pars.it_stop) && (change2 < pars.it_stop))
		eval.converged = true;
		num = pars.it_max - it;
		eval.MSE_h(it+1:pars.it_max) = repmat(MSE_h,num,1);
		eval.MSE_y(it+1:pars.it_max) = repmat(MSE_y,num,1);
		eval.MSE_z(2*it+1:2*pars.it_max) = repmat(MSE_zh,2*num,1);
		eval.result(it+1:pars.it_max) = repmat(result,num,1);
	end
end

eval.finalresult = result;

function [h,alphas,betas] = SIMO_id_CCA(X)
% SIMO_ID_CCA estimates various channels of a SIMO system using CCA.
% Finds the optimal h_i such that X_i h_j = X_j H_i s.t. sum (i = j)
% ||X_i h_j||^2 = 1.
% Input:	- X: cell containing p sets of length N and dimension L (N x L)
% Output:	- h: identified channels (cell)
%			- alphas: sorted eigenvectors
%			- betas: sorted eigenvalues
%
% Steven Van Vaerenbergh 2008

p = length(X);
L = size(X{1},2);

Rf = zeros(p*L); Df = zeros(p*L);
for i=1:p,
	D = zeros(L);
	for j=1:p,
		if i~=j
			sti = (i-1)*L+1;
			ndi = i*L;
			stj = (j-1)*L+1;
			ndj = j*L;
			Rf(sti:ndi,stj:ndj) = X{j}'*X{i};
			D = D + X{j}'*X{j};
		end
	end
	Df(sti:ndi,sti:ndi) = D;
end

[alphas,betas] = eig(Rf,Df);
[betas,ind] = sort(real(diag(betas)),'ascend');
alpha = alphas(:,ind(end));
% alpha = alpha/norm(alpha);

h = cell(p,1);
for i=1:p,
	sti = (i-1)*L+1;
	ndi = i*L;
	h{i} = alpha(sti:ndi);
end

function [h,alphas,betas] = SIMO_id_CCA_dual_identical(X,reg)
% SIMO_ID_CCA estimates various channels of a SIMO system using CCA.
% Finds the optimal h_i such that X_ij h = X_ji h
% Input:
%	X: cell containing pxp data sets of length N and dimension m (N by m)
%   reg: regularization
%
% outputs:
%	h: identified channels
%	alphas: sorted eigenvectors
%	betas: sorted eigenvalues
%
% Reference: Via, Santamaria [...]
%
% Steven Van Vaerenbergh 2008

if nargin<2
    reg = 1E-8;
end

p = size(X,1);
m = size(X{1,2},2);

Rf = zeros(m); Df = zeros(m);
for i=1:p,
	for j=1:p,
		if i~=j
			Rf = Rf + X{i,j}'*X{j,i};
			Df = Df + X{i,j}'*X{i,j};
		end
	end
end
Df = Df + reg*eye(size(Df));

% solve eigenvalue problem
[alphas,betas] = eig(Rf,Df);
[betas_a,ind] = sort(real(diag(betas))); %#ok<ASGLU>
h = alphas(:,ind(end));

function [h,alphas,betas] = SIMO_id_CCA_dual(X,reg)
% SIMO_ID_CCA estimates various channels of a SIMO system using CCA.
% Finds the optimal h_i such that X_ij h_i = X_ji h_j s.t. sum (i~=j)
% ||X_ij h_i||^2 = 1.
% Input:	- X: cell containing pxp sets of length N and dimension mi (N x mi).
%           - reg: regularization
% Outputs:	- h: identified channels (cell)
%			- alphas: sorted eigenvectors
%			- betas: sorted eigenvalues
%
% Steven Van Vaerenbergh 2008

if nargin<2
    reg = 1E-8;
end

p = size(X,1);
m = zeros(p,1);
for i=1:p,
	j = mod(i,p)+1;
	m(i) = size(X{i,j},2);
end
mcum = [0;cumsum(m)];

Rf = zeros(mcum(end)); Df = zeros(mcum(end));
for i=1:p,
	D = zeros(m(i));
	for j=1:p,
		if i~=j
			sti = mcum(i)+1;
			ndi = sti+m(i)-1;
			stj = mcum(j)+1;
			ndj = stj+m(j)-1;
			Rf(sti:ndi,stj:ndj) = X{i,j}'*X{j,i};
			D = D + X{i,j}'*X{i,j};
		end
	end
	Df(sti:ndi,sti:ndi) = D + reg*eye(size(D));
end

[alphas,betas] = eig(Rf,Df);
[betas,ind] = sort(real(diag(betas)),'ascend');
alpha = alphas(:,ind(end));
% alpha = alpha/norm(alpha);

h = cell(p,1);
for i=1:p
	sti = mcum(i)+1;
	ndi = mcum(i+1);
	h{i} = alpha(sti:ndi);
end

function MSE = norm_compare(data_1,data_2)
% NORM_COMPARE calculates the squared error between 2 normalized signals

data_1_norm = data_1/norm(data_1);
data_2_norm = data_2/norm(data_2);

% take into account sign ambiguity
err_vecA = data_1_norm - data_2_norm;
err_vecB = data_1_norm + data_2_norm;

MSEA = err_vecA'*err_vecA;
MSEB = err_vecB'*err_vecB;

MSE = min(MSEA,MSEB);

function [MSE,sc] = scale_compare(data_1,data_2)

data_1_norm = data_1/norm(data_1);
sc = data_1_norm'*data_2/(data_2'*data_2);

err_vec = data_1_norm - sc*data_2;

MSE = err_vec'*err_vec;
sc = sc*norm(data_1);


function [p] = ft_connectivity_plm(inputdata, varargin)

% FT_CONNECTIVITY_PLM computes the phase linearity measurement from a cell array of
% time-domain data, where each cell is an epoch. This implements the metric described
% in Baselice et al. "Phase Linearity Measurement: a novel index for brain functional
% connectivity", IEEE Transactions on Medical Imaging, 2018.
%
% Use as
%   [p] = ft_connectivity_plm(inputdata, ...)
%
% The input data input should be organized as a cell-array, one element for each
% epoch/repetition. Each cell should be a matrix of of nchan x nsamples values.
%
% Additional optional input arguments come as key-value pairs:
%   'bandwidth'	=	scalar, half-bandwidth parameter: the frequency range across which to integrate
%   'fsample'   = sampling frequency, needed to convert bandwidth to number of bins
%
% The output p contains the phase linearity measurement in the [0, 1] interval. It is
% organized as a 3D matrix of Nrpt x Nchan x Nchan dimensions.
%
% See also CONNECTIVITY, FT_CONNECTIVITYANALYSIS

% Copyright (C) 2018, Fabio Baselice, Pierpaolo Sorrentino, Jan-Mathijs Schoffelen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% the sequence of steps is as follows:
%  - Hilbert transformation
%  - multiply with complex conjugate
%  - fft
%  - remove volume conduction component
%  - integrate over bandwidth

% NOTE BY JM: if the user inputs data with different length trials, the fft per trial is going
% to have different frequency resolutions, which is not good. Better to throw an error in that
% case.
fs = ft_getopt(varargin, 'fsample');
B = ft_getopt(varargin, 'bandwidth');
if isempty(fs)
  error('sampling rate is not defined');
end
if isempty(B)
  warning('bandwidth parameter is not defined, assumed 1Hz');
  B=1;
end

nsmp = cellfun('size', inputdata, 2);
assert(all(nsmp==nsmp(1)), 'currently there is no support for input, where the trials are of different length');

nrpt=numel(inputdata);
for k = 1:numel(inputdata)
  inputdata{k} = hilbert(inputdata{k}')';
end
% NOTE by JM: Is it expected that the data has been bandpassfiltered at
% this point? How would this be checked?

nchan=size(inputdata{1},1);
trial_length=size(inputdata{1},2);
ph_min=0.1;        % Eps of Eq.(17) of the manuscript
f=(fs/trial_length)*(0:(trial_length-1));
f_integr=(abs(f)<B) | (abs(f-fs)<B);
p=zeros(nchan, nchan, nrpt);

for ktime=1:nrpt
  for kchan1=1:(nchan-1)
    for kchan2=(kchan1+1):nchan
      temp=fft(inputdata{ktime}(kchan1,:).*conj(inputdata{ktime}(kchan2,:)));    % NOTE BY FB: The inner cycle can be vectorized
      temp(1)=temp(1).*(abs(angle(temp(1)))>ph_min);  % Volume conduction suppression
      temp=(abs(temp)).^2;
      p_temp=sum(temp(f_integr))./sum(temp);
      p(kchan1, kchan2, ktime)=p_temp;
      p(kchan2, kchan1, ktime)=p_temp;
    end
  end
end

p = permute(p, [3 1 2]); % permute to adhere to the conventional matrix shape of the ft_connectivity_* codebase


function [L,L_u,L_k] = pix2PluckerRay(k,u)

% PIX2PLUCKERRAY  Plucker ray from optical center through pixel.
%   PIX2PLUCKERRAY(K,U) is a Plucker line passing over pixel U and the
%   optical center, in a pin-Hole camera of intrinsic vector K.
%
%   [R, R_k, R_u] = PIX2PLUCKERRAY(...) returns the Jacobians wrt K and U.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

if nargout == 1

    n = [0;0;0];
    v = invPinHole(u,1,k);

    L = [n;v];

else

    n = [0;0;0];
    [v,V_u,~,V_k] = invPinHole(u,1,k);

    L = [n;v];
    L_u = [zeros(3,2);V_u];
    L_k = [zeros(3,4);V_k];

end

return

%%
syms u1 u2 u0 v0 au av real
k = [u0;v0;au;av];
u = [u1;u2];

[L,L_u,L_k] = pix2PluckerRay(k,u);

simplify(L_u - jacobian(L,u))
simplify(L_k - jacobian(L,k))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE