function generateOpenfieldPB(fieldSizeX, fieldSizeY, beamletSize);

xP = -(fieldSizeX-beamletSize)/2 : beamletSize : (fieldSizeX-beamletSize)/2;
yP = -(fieldSizeY-beamletSize)/2 : beamletSize : (fieldSizeY-beamletSize)/2;
[xPosV, yPosV] = meshgrid(xP, yP);
xPosV = xPosV(:);
yPosV = yPosV(:);
w_field = ones(size(xPosV));
beamlet_delta_x = beamletSize*ones(size(xPosV));
beamlet_delta_y = beamlet_delta_x;

filename = ['PB_', num2str(fieldSizeX), 'x', num2str(fieldSizeY), '_PBsize', num2str(beamletSize), 'cm.mat']
disp ('save PB info to above filename.')
save (filename, 'xPosV', 'yPosV', 'beamlet_delta_x', 'beamlet_delta_y', 'w_field');

% Display generated PB information.
figure;hAxis2 = axes;hold on;
%axis(hAxis2, 'manual');
           %     w_colors = floor((w_field ./ max(w_field))*255)+1;
                %set(gcf, 'doublebuffer', 'on');
                for i=1:length(xPosV)
                    patch([xPosV(i) - beamlet_delta_x(i)/2 xPosV(i) - beamlet_delta_x(i)/2 xPosV(i) + beamlet_delta_x(i)/2 xPosV(i) + beamlet_delta_x(i)/2 xPosV(i) - beamlet_delta_x(i)/2], [yPosV(i) - beamlet_delta_y(i)/2 yPosV(i) + beamlet_delta_y(i)/2 yPosV(i) + beamlet_delta_y(i)/2 yPosV(i) - beamlet_delta_y(i)/2 yPosV(i) - beamlet_delta_y(i)/2], w_field(i));
                end
            %    axis([hAxis1 hAxis2], 'ij');
            %    kids = get(hAxis2, 'children');
            %     set(kids, 'edgecolor', 'none');
            %    cMap = colormap('jet');
            %   set(hAxis2, 'color', cMap(1,:));

end


function p = presolve_upforce(p)

p.upForce = {};
if p.K.l == 0
    return
end
% Extract linear inequalities
ff = p.F_struc(startofLPCone(p.K):startofLPCone(p.K)+p.K.l-1,:);
% Search for y <= x1 + x2 +...
candidates = find(ff(:,1)==0);
for i = 1:length(candidates)
    row = p.F_struc(candidates(i),2:end);
    xx = find(row==1);
    yy = find(row==-1);
    if length(yy)==1 && length(xx)+1==nnz(row)
        % Yes, this matches y>=x1+x2+x3+...
        if all(ismember(find(row),p.binary_variables))
            p.upForce{end+1}.forcing = yy;
            p.upForce{end}.forced = xx;
        end
    end
end


function [b0,dt6] = dtiInterpDt6(mmPerVoxOut, dt6PathName, outPathName)
%
% [b0,dt6] = dtiInterpDt6(mmPerVoxOut, b0PathName, outPathName)
%
% Takes dt6 file and upsamples b0 and dt6 data to desired resolution
% Uses C versions of pajevic and interp3 algorithms.
% The input file should be already preped using dtiMakeDt6 to
% some resolution
%
% HISTORY:
% 2004.02.27 GSM (gmulye@white.stanford.edu) wrote it (based on
% dtiMakeDt6)

if ~exist('mmPerVoxOut','var') | isempty(mmPerVoxOut)
    mmPerVoxOut = [1 1 1];
end

if ~exist('dt6PathName','var') | isempty(dt6PathName)
   [f, p] = uigetfile({'*.mat','dt6 files (*.mat)';'*.*','All files'}, 'Select one of the dt6 files...');
   if(isnumeric(f)) error('Need a dt6 file to continue...'); end
   dt6PathName = fullfile(p, f);
   disp(dt6PathName);
end

% Upsampling tensors
disp('Interpolating tensors...');
oldDt6File = load(dt6PathName);
dt6 = oldDt6File.dt6;
xform = oldDt6File.xformToAcPc;
mmPerVoxIn = oldDt6File.mmPerVox;
sz = size(dt6);
boundingBox = [0 0 0; sz(1:3).*mmPerVoxIn-1]; %with this config, A(i) = A(2i-2)
%===================================
%Ripped from dtiResliceTensorAffine
%===================================
%We leave out all the transforms in dtiTensorAffine
%Tensors do not have to be rotated or brought to image space
x = (boundingBox(1,1):mmPerVoxOut(1):boundingBox(2,1));
y = (boundingBox(1,2):mmPerVoxOut(2):boundingBox(2,2));
z = (boundingBox(1,3):mmPerVoxOut(3):boundingBox(2,3));

[X,Y,Z] = meshgrid(x, y, z);
clear x y z;
coords = [X(:) Y(:) Z(:)]; %coords is Nx3
newSize = size(X);
clear X Y Z;
% HACK TO FIND CORRECT PAJEVIC LIBRARY
old = pwd;
cd(fileparts(which('dtiTensorInterp_Pajevic')));
dt6 = dtiTensorInterp_Pajevic(dt6, coords, mmPerVoxIn, 1, mmPerVoxIn./2);
cd(old);
dt6 = reshape(dt6, [newSize, 6]);

% Upsampling B0 grid
disp('Interpolating B0...');
% mmPerVox = b0.cannonical_mmPerVox;
% notes = b0.notes;
% xformToAnat = b0.anatXform;
% xformToTal = b0.talXform;
% xformToAcPc = b0.acpcXform;
% b0 = int16(round(b0.cannonical_img));
% anat.img = int16(t1.cannonical_img);
% anat.mmPerVox = t1.cannonical_mmPerVox;
% anat.xformToTal = t1.talXform;
% anat.xformToAcPc = t1.acpcXform;
% save(fullfile(outPathName,'dt6'), 'dt6', 'mmPerVox', 'notes', 'xformToAnat', 'xformToTal', 'xformToAcPc', 'b0', 'anat');

%============================
%Ripped from dtiResliceVolume
%============================
% myCinterp3 likes [rows,columns,slices], so we need a permute here
% myCinterp3 also wants pure image space, so we remove the mmPerVox.
coords(:,1) = coords(:,1)./mmPerVoxIn(1);
coords(:,2) = coords(:,2)./mmPerVoxIn(2);
coords(:,3) = coords(:,3)./mmPerVoxIn(3);
b0 = oldDt6File.b0;
b0 = double(permute(b0,[2,1,3]));
interpedb0 = myCinterp3(b0, [size(b0,1) size(b0,2)], size(b0,3), coords, 0.0);
b0 = squeeze(reshape(interpedb0,newSize));
clear interpedb0;

if(~exist('outPathName','var') | isempty(outPathName))
    [fullpath fName] = fileparts(dt6PathName);
    outPathName = fullfile(fullpath, [fName sprintf('%dx%dx%dmm',mmPerVoxIn) '_upto_' sprintf('%dx%dx%dmm',mmPerVoxOut)]);
end

%Find new transform to anatomy image
oldXformToAnat = oldDt6File.xformToAnat;
anat = oldDt6File.anat;
scale = mmPerVoxOut./anat.mmPerVox;
scale = diag([scale 1]);
xformToAnat = scale*oldXformToAnat;

mmPerVox = mmPerVoxOut;
notes = oldDt6File.notes;
b0 = int16(round(b0));
b0 = permute(b0,[2,1,3]); %Undo earlier permute
dt6 = permute(dt6,[2 1 3 4]); %Undo permute from earlier

disp(['Saving to ' outPathName '...']);
save(outPathName, 'b0', 'xformToAnat','anat','notes','mmPerVox','dt6');
save([outPathName '_anon'], 'b0', 'xformToAnat','anat','notes','mmPerVox','dt6');

return


function [x,f,funEvals] = minConF_BC(funObj,x,LB,UB,options)
% function [x,f] = minConF_BC(funObj,x,LB,UB,options)
%
% Function for using Two-Metric Projection to solve problems of the form:
%   min funObj(x)
%   s.t. LB_i <= x_i <= UB_i
%
%   @funObj(x): function to minimize (returns gradient as second argument)
%
%   options:
%       verbose: level of verbosity (0: no output, 1: final, 2: iter (default), 3:
%       debug)
%       optTol: tolerance used to check for progress (default: 1e-7)
%       maxIter: maximum number of calls to funObj (default: 250)
%       numDiff: compute derivatives numerically (0: use user-supplied
%       derivatives (default), 1: use finite differences, 2: use complex
%       differentials)
%       method: 'sd', 'lbfgs', 'newton'

nVars = length(x);

% Set Parameters
if nargin < 5
    options = [];
end
[verbose,numDiff,optTol,maxIter,suffDec,interp,method,corrections,damped] = ...
    myProcessOptions(...
    options,'verbose',3,'numDiff',0,'optTol',1e-6,'maxIter',500,'suffDec',1e-4,...
    'interp',1,'method','lbfgs','corrections',100,'damped',0);

% Output Log
if verbose >= 3
    fprintf('%10s %10s %15s %15s %15s\n','Iteration','FunEvals','Step Length','Function Val','Opt Cond');
end

% Make objective function (if using numerical derivatives)
funEvalMultiplier = 1;
if numDiff
    if numDiff == 2
        useComplex = 1;
    else
        useComplex = 0;
    end
    funObj = @(x)autoGrad(x,useComplex,funObj);
    funEvalMultiplier = nVars+1-useComplex;
end

% Evaluate Initial Point
x = projectBounds(x,LB,UB);
if strcmp(method,'newton')
    [f,g,H] = funObj(x);
    secondOrder = 1;
else
    [f,g] = funObj(x);
    secondOrder = 0;
end
funEvals = 1;

% Compute Working Set
working = ones(nVars,1);
working((x < LB+optTol*2) & g >= 0) = 0;
working((x > UB-optTol*2) & g <= 0) = 0;
working = find(working);

% Check Optimality
if isempty(working)
    if verbose >= 1
        fprintf('All variables are at their bound and no further progress is possible at initial point\n');
    end
    return;
elseif norm(g(working)) <= optTol
    if verbose >=1
        fprintf('All working variables satisfy optimality condition at initial point\n');
    end
    return;
end

if verbose >= 3
    switch method
        case 'sd'
            fprintf('Steepest Descent\n');
        case 'lbfgs'
            fprintf('L-BFGS\n');
        case 'bfgs'
            fprintf('BFGS\n');
        case 'newton'
            fprintf('Newton\n');
    end
end

i = 1;
while funEvals <= maxIter

    % Compute Step Direction
    d = zeros(nVars,1);
    switch(method)
        case 'sd'
            d(working) = -g(working);
        case 'lbfgs'
            if i == 1
                d(working) = -g(working);
                old_dirs = zeros(nVars,0);
                old_stps = zeros(nVars,0);
                Hdiag = 1;
            else
                if damped
                    [old_dirs,old_stps,Hdiag] = dampedUpdate(g-g_old,x-x_old,corrections,verbose==3,old_dirs,old_stps,Hdiag);
                else
                    [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,x-x_old,corrections,verbose==3,old_dirs,old_stps,Hdiag);
                end
                    curvSat = sum(old_dirs(working,:).*old_stps(working,:)) > 1e-10;
               d(working) = lbfgs(-g(working),old_dirs(working,curvSat),old_stps(working,curvSat),Hdiag);
            end
            g_old = g;
            x_old = x;
        case 'bfgs'
            if i == 1
                d(working) = -g(working);
                B = eye(nVars);
            else
                y = g-g_old;
                s = x-x_old;

                ys = y'*s;

                if i == 2
                    if ys > 1e-10
                        B = ((y'*y)/(y'*s))*eye(nVars);
                    end
                end
                if ys > 1e-10
                    B = B + (y*y')/(y'*s) - (B*s*s'*B)/(s'*B*s);
                else
                    if verbose == 2
                        fprintf('Skipping Update\n');
                    end
                end
                d(working) = -B(working,working)\g(working);
            end
            g_old = g;
            x_old = x;

        case 'newton'
            [R,posDef] = chol(H(working,working));

            if posDef == 0
                d(working) = -R\(R'\g(working));
            else
                if verbose == 3
                    fprintf('Adjusting Hessian\n');
                end
                H(working,working) = H(working,working) + eye(length(working)) * max(0,1e-12 - min(real(eig(H(working,working)))));
                d(working) = -H(working,working)\g(working);
            end
        otherwise
            fprintf('Unrecognized Method: %s\n',method);
            break;
    end

    % Check that Progress can be made along the direction
    f_old = f;
    gtd = g'*d;
    if gtd > -optTol
        if verbose >= 2
            fprintf('Directional Derivative below optTol\n');
        end
        break;
    end

    % Select Initial Guess to step length
    if i == 1 && ~secondOrder
        t = min(1,1/sum(abs(g(working))));
    else
        t = 1;
    end

    % Evaluate the Objective and Projected Gradient at the Initial Step
    x_new = projectBounds(x+t*d,LB,UB);
    if secondOrder
        [f_new,g_new,H] = funObj(x_new);
    else
        [f_new,g_new] = funObj(x_new);
    end
    funEvals = funEvals+1;

    % Backtracking Line Search
    lineSearchIters = 1;
    while f_new > f + suffDec*g'*(x_new-x) || ~isLegal(f_new)
        temp = t;
        if interp == 0 || ~isLegal(f_new) || ~isLegal(g_new)
            if verbose == 3
                fprintf('Halving Step Size\n');
            end
            t = .5*t;
        else
            if verbose == 3
                fprintf('Cubic Backtracking\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d]);
        end

        % Adjust if change is too small
        if t < temp*1e-3
            if verbose == 3
                fprintf('Interpolated value too small, Adjusting\n');
            end
            t = temp*1e-3;
        elseif t > temp*0.6
            if verbose == 3
                fprintf('Interpolated value too large, Adjusting\n');
            end
            t = temp*0.6;
        end

        % Check whether step has become too small
        if sum(abs(t*d)) < optTol
            if verbose == 3
                fprintf('Line Search failed\n');
            end
            t = 0;
            f_new = f;
            g_new = g;
            break;
        end

        % Evaluate New Point
        x_new = projectBounds(x+t*d,LB,UB);
        [f_new,g_new] = funObj(x_new);
        funEvals = funEvals+1;
        lineSearchIters = lineSearchIters+1;

    end

    % Take Step
    x = x_new;
    f = f_new;
    g = g_new;

    % Compute Working Set
    working = ones(nVars,1);
    working((x < LB+optTol*2) & g >= 0) = 0;
    working((x > UB-optTol*2) & g <= 0) = 0;
    working = find(working);

    % Output Log
    if verbose >= 2
        fprintf('%10d %10d %15.5e %15.5e %15.5e\n',i,funEvals*funEvalMultiplier,t,f,sum(abs(g(working))));
    end

    % Check Optimality
    if isempty(working)
        if verbose >= 1
            fprintf('All variables are at their bound and no further progress is possible\n');
        end
        break;
    elseif norm(g(working)) <= optTol
        if verbose >=1
            fprintf('All working variables satisfy optimality condition\n');
        end
        break;
    end

    % Check for lack of progress
    if sum(abs(t*d)) < optTol
        if verbose >= 1
            fprintf('Step size below optTol\n');
        end
        break;
    end

    if abs(f-f_old) < optTol
        if verbose >= 1
            fprintf('Function value changing by less than optTol\n');
        end
        break;
    end

    if funEvals*funEvalMultiplier > maxIter
        if verbose >= 1
            fprintf('Function Evaluations exceeds maxIter\n');
        end
        break;
    end

    % If necessary, compute Hessian
    if secondOrder && lineSearchIters > 1
        [f_new,g_new,H] = funObj(x);
    end

    i = i + 1;
end
end

function [x] = projectBounds(x,LB,UB)
x(x < LB) = LB(x < LB);
x(x > UB) = UB(x > UB);
end


% THis function will search...
% INputs:
% lines: cell array of entries to be searched on
% pattern: query
% start:

function found = searchInCellBinary(lines, pattern, start, stop)

found = 0;

if stop-start <= 10
    for i=start:stop
        if strcmp(lines{i}, pattern)
            found = i;
            return;
        end
    end

else

    middle = floor( (start+stop)/2 );
    notEqual = 0;
    middleWord = lines{middle};
    min_length = min( length(pattern), length(middleWord) );
    for i=1:min_length
        if int16(pattern(i)) > int16(middleWord(i))
            notEqual = 1;
            found = searchInCellBinary(lines, pattern, middle, stop);
            break;
        elseif int16(pattern(i)) < int16(middleWord(i))
            notEqual = 1;
            found = searchInCellBinary(lines, pattern, start, middle);
            break;
        end
    end

    if notEqual == 0
        if length(pattern) == length(middleWord)
            found = middle;
        elseif length(pattern) > length(middleWord)
            found = searchInCellBinary(lines, pattern, middle+1, stop);
        elseif length(pattern) < length(middleWord)
            found = searchInCellBinary(lines, pattern, start, middle-1);
        end
    end
end


%%%  e8.4_FLY
clear
%The maximum control speed and maximum navigation angle can be modified as appropriate
path( './icon/',path);
load vehicle_local_position.mat;

%Constant value
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
%throttle when UAV is hovering
THR_HOVER = 0.609;

%% control parameter
%Attitude PID parameters
Kp_PITCH_ANGLE =6.5;
Kp_PITCH_AngleRate = 0.1;
Ki_PITCH_AngleRate = 0.02;
Kd_PITCH_AngleRate = 0.001;
Kp_ROLL_ANGLE =6.5;
Kp_ROLL_AngleRate = 0.1;
Ki_ROLL_AngleRate = 0.02;
Kd_ROLL_AngleRate = 0.001;

Kp_YAW_ANGLE = 3;
Kp_YAW_AngleRate = 0.5;
Ki_YAW_AngleRate = 0.01;
Kd_YAW_AngleRate = 0.00;

%Position PID parameters
Kpxp = 1.0;
Kpyp = 1.0;
Kpzp = 4.0;
Kvxp = 3.0; Kvxi = 0.1; Kvxd = 0.01;
Kvyp = 3.0; Kvyi = 0.1; Kvyd = 0.01;
Kvzp = 0.45; Kvzi = 0.01; Kvzd = 0.005;

Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;
Saturation_I_ah = 3.43;
Saturation_I_az = 5;

%max control angle,default 35deg
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH  = 35;
%Maximum navigation angle, used in position control to avoid large attitude angles
MAX_CONTROL_ANGLE_NAV_ROLL = 15;
MAX_CONTROL_ANGLE_NAV_PITCH  = 15;
%max control angle rate,rad/s
MAX_CONTROL_ANGLE_RATE_PITCH = 220;
MAX_CONTROL_ANGLE_RATE_ROLL = 220;
MAX_CONTROL_ANGLE_RATE_Y = 200;
%Maximum control speed, m/s
MAX_CONTROL_VELOCITY_XY = 0.5;
MAX_CONTROL_VELOCITY_Z = 1;
%Throttle amplitude
MAX_MAN_THR = 0.9;
MIN_MAN_THR = 0.05;
%% run simulink model
e8_4_FLY


% goplotpso.m
% default plotting script used in PSO functions
%
% this script is not a function,
% it is a plugin for the main PSO routine (pso_Trelea_vectorized)
% so it shares all the same variables, be careful with variable names
% when making your own plugin

% Brian Birge
% Rev 2.0
% 3/1/06

% setup figure, change this for your own machine

 clf
 set(gcf,'Position',[651    31   626   474]); % this is the computer dependent part
 %set(gcf,'Position',[743    33   853   492]);
 set(gcf,'Doublebuffer','on');

% particle plot, upper right
 subplot('position',[.7,.6,.27,.32]);
 set(gcf,'color','k')

 plot3(pos(:,1),pos(:,D),out,'b.','Markersize',7)

 hold on
 plot3(pbest(:,1),pbest(:,D),pbestval,'g.','Markersize',7);
 plot3(gbest(1),gbest(D),gbestval,'r.','Markersize',25);

 % crosshairs
 offx = max(abs(min(min(pbest(:,1)),min(pos(:,1)))),...
            abs(max(max(pbest(:,1)),max(pos(:,1)))));

 offy = max(abs(min(min(pbest(:,D)),min(pos(:,D)))),...
            abs(min(max(pbest(:,D)),max(pos(:,D)))));
 plot3([gbest(1)-offx;gbest(1)+offx],...
       [gbest(D);gbest(D)],...
       [gbestval;gbestval],...
       'r-.');
 plot3([gbest(1);gbest(1)],...
       [gbest(D)-offy;gbest(D)+offy],...
       [gbestval;gbestval],...
       'r-.');

 hold off

 xlabel('Dimension 1','color','y')
 ylabel(['Dimension ',num2str(D)],'color','y')
 zlabel('Cost','color','y')

 title('Particle Dynamics','color','w','fontweight','bold')

 set(gca,'Xcolor','y')
 set(gca,'Ycolor','y')
 set(gca,'Zcolor','y')
 set(gca,'color','k')

 % camera control
 view(2)
 try
   axis([gbest(1)-offx,gbest(1)+offx,gbest(D)-offy,gbest(D)+offy]);
 catch
   axis([VR(1,1),VR(1,2),VR(D,1),VR(D,2)]);
 end

% error plot, left side
 subplot('position',[0.1,0.1,.475,.825]);
  semilogy(tr(find(~isnan(tr))),'color','m','linewidth',2)
  %plot(tr(find(~isnan(tr))),'color','m','linewidth',2)
  xlabel('epoch','color','y')
  ylabel('gbest val.','color','y')

  if D==1
     titstr1=sprintf(['%11.6g = %s( [ %9.6g ] )'],...
                gbestval,strrep(functname,'_','\_'),gbest(1));
  elseif D==2
     titstr1=sprintf(['%11.6g = %s( [ %9.6g, %9.6g ] )'],...
                gbestval,strrep(functname,'_','\_'),gbest(1),gbest(2));
  elseif D==3
     titstr1=sprintf(['%11.6g = %s( [ %9.6g, %9.6g, %9.6g ] )'],...
                gbestval,strrep(functname,'_','\_'),gbest(1),gbest(2),gbest(3));
  else
     titstr1=sprintf(['%11.6g = %s( [ %g inputs ] )'],...
                gbestval,strrep(functname,'_','\_'),D);
  end
  title(titstr1,'color','m','fontweight','bold');

  grid on
%  axis tight

  set(gca,'Xcolor','y')
  set(gca,'Ycolor','y')
  set(gca,'Zcolor','y')
  set(gca,'color','k')

  set(gca,'YMinorGrid','off')

% text box in lower right
% doing it this way so I can format each line any way I want
subplot('position',[.62,.1,.29,.4]);
  clear titstr
  if trelea==0
       PSOtype  = 'Common PSO';
       xtraname = 'Inertia Weight : ';
       xtraval  = num2str(iwt(length(iwt)));

     elseif trelea==2 | trelea==1

       PSOtype  = (['Trelea Type ',num2str(trelea)]);
       xtraname = ' ';
       xtraval  = ' ';

     elseif trelea==3
       PSOtype  = (['Clerc Type 1"']);
       xtraname = '\chi value : ';
       xtraval  = num2str(chi);

  end
  if isnan(errgoal)
    errgoalstr='Unconstrained';
  else
    errgoalstr=num2str(errgoal);
  end
  if minmax==1
     minmaxstr = ['Maximize to : '];
  elseif minmax==0
     minmaxstr = ['Minimize to : '];
  else
     minmaxstr = ['Target   to : '];
  end

  if rstflg==1
     rststat1 = 'Environment Change';
     rststat2 = ' ';
  else
     rststat1 = ' ';
     rststat2 = ' ';
  end

  titstr={'PSO Model: '      ,PSOtype;...
          'Dimensions : '    ,num2str(D);...
          '# of particles : ',num2str(ps);...
          minmaxstr          ,errgoalstr;...
          'Function : '      ,strrep(functname,'_','\_');...
          xtraname           ,xtraval;...
          rststat1           ,rststat2};

  text(.1,1,[titstr{1,1},titstr{1,2}],'color','g','fontweight','bold');
  hold on
  text(.1,.9,[titstr{2,1},titstr{2,2}],'color','m');
  text(.1,.8,[titstr{3,1},titstr{3,2}],'color','m');
  text(.1,.7,[titstr{4,1}],'color','w');
  text(.55,.7,[titstr{4,2}],'color','m');
  text(.1,.6,[titstr{5,1},titstr{5,2}],'color','m');
  text(.1,.5,[titstr{6,1},titstr{6,2}],'color','w','fontweight','bold');
  text(.1,.4,[titstr{7,1},titstr{7,2}],'color','r','fontweight','bold');

  % if we are training a neural net, show a few more parameters
  if strcmp('pso_neteval',functname)
    % net is passed from trainpso to pso_Trelea_vectorized in case you are
    % wondering where that structure comes from
    hiddlyrstr = [];
    for lyrcnt=1:length(net.layers)
       TF{lyrcnt} = net.layers{lyrcnt}.transferFcn;
       Sn(lyrcnt) = net.layers{lyrcnt}.dimensions;
       hiddlyrstr = [hiddlyrstr,', ',TF{lyrcnt}];
    end
    hiddlyrstr = hiddlyrstr(3:end);

    text(0.1,.35,['#neur/lyr = [ ',num2str(net.inputs{1}.size),'  ',...
               num2str(Sn),' ]'],'color','c','fontweight','normal',...
               'fontsize',10);
    text(0.1,.275,['Lyr Fcn: ',hiddlyrstr],...
       'color','c','fontweight','normal','fontsize',9);

  end

  legstr = {'Green = Personal Bests';...
            'Blue  = Current Positions';...
            'Red   = Global Best'};
  text(.1,0.025,legstr{1},'color','g');
  text(.1,-.05,legstr{2},'color','b');
  text(.1,-.125,legstr{3},'color','r');

  hold off

  set(gca,'color','k');
  set(gca,'visible','off');

  drawnow


function [bel, niter] = parallel_protocol(engine, evidence, pot_type, local_kernel, msg)

bnet = bnet_from_engine(engine);
ns = bnet.node_sizes;
onodes = find(~isemptycell(evidence));

ndoms = length(engine.gdl.doms);
prod_of_msg = cell(1, ndoms);
bel = cell(1, ndoms);
old_bel = cell(1, ndoms);

converged = 0;
iter = 1;
while ~converged & (iter <= engine.max_iter)

  % each node multiplies all its incoming msgs and computes its local belief
  old_bel = bel;
  for i=1:ndoms
    prod_of_msg{i} = mk_initial_pot(pot_type, engine.gdl.doms{i}, ns, bnet.cnodes, onodes);
    nbrs = engine.gdl.nbrs{i};
    for j=nbrs(:)'
      prod_of_msg{i} = multiply_by_pot(prod_of_msg{i}, msg{j,i});
    end
    bel{i} = normalize_pot(multiply_by_pot(local_kernel{i}, prod_of_msg{i}));
  end

  if ~isempty(engine.fid)
    for i=1:ndoms
      tmp = pot_to_marginal(bel{i});
      %fprintf(engine.fid, '%9.7f ', tmp.T(1));
      fprintf(engine.fid, '%9.7f ', tmp.U(1));
    end
    %fprintf(engine.fid, '  U ');
    %for i=1:ndoms
    %  tmp = pot_to_marginal(bel{i});
    %  fprintf(engine.fid, '%9.7f ', tmp.U(1));
    %end
    fprintf(engine.fid, '\n');
  end

  % converged?
  if iter==1
    converged = 0;
  else
    converged = 1;
    for i=1:ndoms
      if ~approxeq_pot(bel{i}, old_bel{i}, engine.tol)
	converged = 0;
	break;
      end
    end
  end

  if ~converged
    old_msg = msg;
    % each node sends a msg to each of its neighbors
    for i=1:ndoms
      nbrs = engine.gdl.nbrs{i};
      for j=nbrs(:)'
	% multiply all incoming msgs except from j
	temp = prod_of_msg{i};
	temp = divide_by_pot(temp, old_msg{j,i});
	% send msg from i to j
	temp = multiply_by_pot(temp, local_kernel{i});
	temp2 = marginalize_pot(temp, engine.gdl.sepset{i,j}, engine.maximize);
	msg{i,j} = normalize_pot(temp2);
      end
    end
  end

  iter = iter + 1;
end

niter = iter-1;

if 0
for i=1:ndoms
  prod_of_msg{i} = mk_initial_pot(pot_type, engine.gdl.doms{i}, ns, bnet.cnodes, onodes);
  nbrs = engine.gdl.nbrs{i};
  for j=nbrs(:)'
    prod_of_msg{i} = multiply_by_pot(prod_of_msg{i}, msg{j,i});
  end
  bel{i} = normalize_pot(multiply_by_pot(local_kernel{i}, prod_of_msg{i}));
end
end


classdef StressAndMaxNormWithExponentPlotter < StressNormPlotter

    methods (Access = public)

        function obj = StressAndMaxNormWithExponentPlotter(d)
            obj.init(d);
        end

        function plot(obj)
            obj.fig = figure(1);
            obj.plotSmoothInfo();
            obj.plotNonSmoothInfo();
            obj.addLegend();
            obj.print();
        end

    end

    methods (Access = private)

        function plotSmoothInfo(obj)
            dB = obj.smoothDB;
            obj.plotStressNorm(dB);
            obj.plotStressMax(dB);
        end

        function plotNonSmoothInfo(obj)
            dB = obj.nonSmoothDB;
            obj.plotStressNorm(dB);
            obj.plotStressMax(dB);
        end

        function plotStressNorm(obj,dB)
            obj.x        = dB.pNorm;
            obj.y        = dB.invStressNorm;
            obj.lineInfo = [dB.color,'-+'];
            obj.appendPlot()
        end

        function plotStressMax(obj,dB)
            obj.x            = dB.pNorm;
            obj.y            = dB.invStressMax*ones(1,length(obj.x));
            obj.lineInfo     = [dB.color,'--'];
            obj.appendPlot()
        end

        function addLegend(obj)
            legStr = {'Smooth','NonSmooth','SmoothMaxNorm','NonSmoothMaxNorm'};
            lines = obj.plotLines;
            nlinesType = length(legStr);
            ilines = 1:obj.nExp:obj.nExp*nlinesType;
            legLines = [lines{ilines(1)} lines{ilines(2)} lines{ilines(3)} lines{ilines(4)}];
            legend(legLines,legStr);
        end

        function print(obj)
            sFig = obj.fig;
            ca = get(sFig,'CurrentAxes');
            xl = get(ca,'xlabel');
            set(xl,'string','Pnorm');
            figureName = 'StressNormWithPnorm';
            outPutFigName = fullfile(obj.outPutPath,figureName);
            pL   = obj.plotLines;
            fp   = plotPrinter(sFig,pL);
            fp.print(outPutFigName)
        end

    end

end


clear;
data = load('data_completeness.mat');
data = data.data;

orange = [ 0.91,0.41,0.17];
blue = [0,0.4470,0.7410];
figure('DefaultAxesFontSize',14);
ax = gca;
xlabel('Month');

yyaxis left;
h1 = plot(data(:,1),'-','color',blue,'linewidth',2,'DisplayName','rmse');
ylabel('Data amount (\times 10^6)');
ylim([0,1.6e+6]);
ax.YTick = 0:0.4e+6:1.6e+6;
ax.YTickLabel = ['0  ';'0.4';'0.8';'1.2';'1.6'];
yyaxis right;
h2 = plot(data(:,2),'-','color',orange,'linewidth',2,'DisplayName','lb');
ylabel('Completeness');
ax.YTick = 0:0.25:1;
ax.YTickLabel = 0:0.25:1;

box on;
grid on;
ax.XTick = 1:4:48;
ax.XTickLabel = ['Apr.';'Aug.';'Dec.';'Apr.';'Aug.';'Dec.';'Apr.';'Aug.';'Dec.';'Apr.';'Aug.';'Dec.';];
ax.GridLineStyle = '-.';
set(gcf, 'PaperSize', [8 3]);
set(gcf, 'PaperPositionMode', 'manual');
set(gcf, 'PaperPosition', [0 0 8 3]);
saveas(gcf,'nyc_data_completeness','pdf');


function y = permute( x, order )

%   Disciplined convex/geometric programming information for PERMUTE:
%      PERMUTE(A,ORDER) imposes no convexity restrictions on A. ORDER
%      must be constant.

%
% Determine the permutation
%

s = x.size_;
ndxs = 1 : prod( s );
ndx2 = permute( reshape( ndxs, s ), order );

%
% Permute the data
%

b = x.basis_;
try
    b = x.basis_( :, ndx2 );
catch %#ok
    ndxs( ndx2( : ).' ) = ndxs;
    [ r, c, v ] = find( b );
    b = sparse( r, ndxs( c ), v, size( b, 1 ), size( b, 2 ) );
    clear r c v
end
y = cvx( size( ndx2 ), b );

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [r, itr, status, box_num, seg_result] = fooling_seg_net(x, seg_mask_target, seg_mask_ori, net, config)
% process to generate adversarial examples in segmentation network
% -------------------------------------------------------------

try
    eval(config);
catch
    keyboard;
end

% initilization of fooling process
r = x * 0;
itr = 0;

% intilization of fooling target
[pred_target, dr, seg_result] = forward_and_back_propogation_seg(x+r, seg_mask_target, net);
box_num(itr+1) = pred_target;

pred_target_ori = sum(sum(seg_mask_ori>0));

while (pred_target > 0.01*pred_target_ori) && itr<MAX_ITER
    itr = itr + 1;
    sprintf('iteration number %d\n', itr)

    % process of noise
    dr_temp = reshape(dr, numel(dr), 1);
    r_gain = step_length/max(abs(dr_temp));
    r = r + dr*r_gain;
    r_max = max(reshape(abs(r), numel(r), 1));
    fprintf('max value in the perturbation is %.2f\n', r_max);

    % calculate the candidate for the next interation
    [pred_target, dr, seg_result] = forward_and_back_propogation_seg(x+r, seg_mask_target, net);
    fprintf('%d pixels remained\n', pred_target);
    box_num(itr+1) = pred_target;
end

if pred_target >= 0.01*pred_target_ori
    status = 0;
else
    status = 1;
end

end


function rts_retrieveDataAndStore(tcpipClient, numData, dataType)
%rts_retrieveDataAndStore Summary of this function goes here
%   Detailed explanation goes here
    readasync(tcpipClient);

    bytesDbl = 8;
    dataPts = [];
    frameName = [];
    frameNum = [];
    transmitDateTime = [];

    try
        if(tcpipClient.BytesAvailable >= 88) %88 bytes is the size of the frame header
            while(tcpipClient.BytesAvailable > 88)
                [frameName, frameNum, transmitDateTime] = rts_readMinorFrameHeader(tcpipClient);
                if(strcmpi(dataType,'d'))
                    if(numData == -1)
                        dataPts = fread(tcpipClient,floor(tcpipClient.BytesAvailable/bytesDbl),'double');
                    else
                        numDataToRet = min(numData, floor(tcpipClient.BytesAvailable/bytesDbl));
                        if(numDataToRet > 0)
                            dataPts = fread(tcpipClient,numDataToRet,'double');
                        else
                            dataPts = [];
                        end
                    end
                elseif(strcmpi(dataType,'s'))
                    dataPts = deblank(fscanf(tcpipClient,'%c',numData));
                end
            end

            retDateTime = datevec(now);
            data = {dataPts, frameName, frameNum, transmitDateTime', retDateTime};
            set(tcpipClient,'UserData',data);
        end
    catch ME
        disp(ME.message);
    end

end


function targets = sledgeset2adjlist(n, edges, sch)
%SLEDGESET2ADJLIST Converts edge set to adjacency list
%
% $ Syntax $
%   - targets = sledgeset2adjlist(n, edges, sch)
%
% $ Arguments $
%   - n:        The number of (source) nodes
%   - edges:    The set of edges (nedges x 2 or nedges x 3)
%   - sch:      The id of scheme of conversion to take
%               - 0:    no value -> no value
%               - 1:    no value -> has value
%               - 2:    has value -> no value
%               - 3:    has value -> has value
%   - targets:  The cell array of targets in adj list
%
% $ Remarks $
%   - an internal function for graph representation conversion.
%     no checking of input arguments would be performed.
%
% $ History $
%   - Created by Dahua Lin, on Sep 9, 2006
%

%% prepare storage

targets = cell(n, 1);

%% sort edges

I = edges(:, 1);
J = edges(:, 2);

[I, si] = sort(I);
J = J(si);

if sch == 3
    V = edges(si, 3);
elseif sch == 1
    nedges = length(I);
    V = ones(nedges, 1);
end

%% group targets

nums = slcount(I);
ni = length(nums);
[spos, epos] = slnums2bounds(nums);

if sch == 1 || sch == 3
    for i = 1 : ni
        s = spos(i); e = epos(i);
        if s <= e
            targets{I(s)} = [J(s:e), V(s:e)];
        end
    end
else
    for i = 1 : ni
        s = spos(i); e = epos(i);
        if s <= e
            targets{I(s)} = J(s:e);
        end
    end
end


%% test2.m - exp03
% This script shows histograms for errors computed in test1.m
%
% 02-08-10 Michal Uricar
% 07-03-11 Michal Uricar

% clc;
close all; clearvars;

%% Timestamp

fprintf(1,'Started on %s\n\n', datestr(now));

%% Add path

addpath('./Functions/');

%% Load errors

load('./results/errors.mat');

%% Options

% show images
% opt.verbose = true;
opt.verbose = false;
% save images
opt.save = true;
% opt.save = false;

%% Show histograms

if (~exist('./img/', 'dir'))
    mkdir('./img/');
end;

% get screen size
scrsz = get(0,'ScreenSize');

% histogram of mean errors
x = 0:options.bw(1); y = round(L);
hist_mean = hist(y, x);
% cumulative histogram of mean errors
n_elements = histc(y, x); c_elements = cumsum(n_elements);
if (opt.verbose)
    figure; hist(y, x);
    set(gcf, 'OuterPosition', [scrsz(1) scrsz(2) scrsz(3) scrsz(4)]);
    set(gca, 'XTick', x); set(gca, 'xlim', [-0.5 options.bw(1)+0.5]);
    xlabel('Distance in pixels'); ylabel('Count of occurrences');
    set(gca, 'YTick', 0:50:2*c_elements(end));
    title('Histogram of mean errors');
    if (opt.save)
        saveas(gcf, './img/histogram_mean.png');
        saveas(gcf, './img/histogram_mean.fig');
    end;

    figure; bar(x, c_elements / max(c_elements));
    xlabel('RMS [px]'); ylabel('Count of occurrences [%]');
    set(gcf, 'OuterPosition', [scrsz(1) scrsz(2) scrsz(3) scrsz(4)]);
    set(gca, 'XTick', x); set(gca, 'xlim', [-0.5 options.bw(1)+0.5]);
    set(gca, 'YTick', 0:0.05:1);
    title('Cumulative historgam of mean errors');
    if (opt.save)
        saveas(gcf, './img/cumhist_mean.png');
        saveas(gcf, './img/cumhist_mean.fig');
    end;
end;

% histogram of max errors
x = 0:options.bw(1); y = round(err_maxdist);
hist_max = hist(y, x);
% cumulative histogram of max errors
n_elements = histc(y, x); c_elements = cumsum(n_elements);
if (opt.verbose)
    figure; hist(y, x);
    set(gcf, 'OuterPosition', [scrsz(1) scrsz(2) scrsz(3) scrsz(4)]);
    set(gca, 'XTick', x); set(gca, 'xlim', [-0.5 options.bw(1)+0.5]);
    xlabel('Distance in pixels'); ylabel('Count of occurrences');
    set(gca, 'YTick', 0:50:2*c_elements(end));
    title('Histogram of max errors');
    if (opt.save)
        saveas(gcf, './img/histogram_max.png');
        saveas(gcf, './img/histogram_max.fig');
    end;

    figure; bar(x, c_elements / max(c_elements));
    xlabel('RMS [px]'); ylabel('Count of occurrences [%]');
    set(gcf, 'OuterPosition', [scrsz(1) scrsz(2) scrsz(3) scrsz(4)]);
    set(gca, 'XTick', x); set(gca, 'xlim', [-0.5 options.bw(1)+0.5]);
    set(gca, 'YTick', 0:0.05:1);
    title('Cumulative historgam of max errors');
    if (opt.save)
        saveas(gcf, './img/cumhist_max.png');
        saveas(gcf, './img/cumhist_max.fig');
    end;
end;
% save histograms
save('./results/histograms.mat', 'hist_mean', 'hist_max');

%% Show mean and max error for each point from ground truth

mean_nose = mean(err_nose);
mean_canthus_rl = mean(err_canthus_rl);
mean_canthus_lr = mean(err_canthus_lr);
mean_mouth_corner_r = mean(err_mouth_corner_r);
mean_mouth_corner_l = mean(err_mouth_corner_l);
mean_canthus_rr = mean(err_canthus_rr);
mean_canthus_ll = mean(err_canthus_ll);

max_nose = max(err_nose);
max_canthus_rl = max(err_canthus_rl);
max_canthus_lr = max(err_canthus_lr);
max_mouth_corner_r = max(err_mouth_corner_r);
max_mouth_corner_l = max(err_mouth_corner_l);
max_canthus_rr = max(err_canthus_rr);
max_canthus_ll = max(err_canthus_ll);

fprintf('\n\n__________________________________________________________\n');
fprintf('\nResults:\n');
fprintf('Mean error on nose: \t\t\t\t%.4f\t\n', mean_nose);
fprintf('Mean error on mean_canthus_rl: \t\t%.4f\t\n', mean_canthus_rl);
fprintf('Mean error on mean_canthus_lr: \t\t%.4f\t\n', mean_canthus_lr);
fprintf('Mean error on mean_mouth_corner_r: \t%.4f\t\n', mean_mouth_corner_r);
fprintf('Mean error on mean_mouth_corner_l: \t%.4f\t\n', mean_mouth_corner_l);
fprintf('Mean error on mean_canthus_rr: \t\t%.4f\t\n', mean_canthus_rr);
fprintf('Mean error on mean_canthus_ll: \t\t%.4f\t\n', mean_canthus_ll);
fprintf('__________________________________________________________\n');

fprintf('\nR_mean_tst: \t\t\t\t\t%.5f\n', Rtst);
fprintf('R_max_tst: \t\t\t\t\t\t%.5f\n', sum(err_maxdist)/length(err_maxdist));

fprintf('__________________________________________________________\n');

%%

xSC = 0:ceil(max(L));
ySC = round(L);
hist_meanSC = hist(ySC, xSC);
% cumulative histogram of mean errors
n_elementsSC = histc(ySC, xSC);
c_elementsSC = cumsum(n_elementsSC);
% max
xSCmax = 0:ceil(max(L));
ySCmax = round(err_maxdist);
hist_maxSC = hist(ySCmax, xSCmax);
% cumulative histogram of max errors
n_elementsSCmax = histc(ySCmax, xSCmax);
c_elementsSCmax = cumsum(n_elementsSCmax);

fig = figure;
y1 = c_elementsSC / max(c_elementsSC) * 100;
plot(xSC(1:end), y1(1:end), 'b', 'LineWidth', 3); hold on; grid on;
set(gcf, 'OuterPosition', [scrsz(1) scrsz(2) scrsz(3) scrsz(4)]);
set(gca, 'XTick', xSC); set(gca, 'YTick', 0:5:100, 'FontSize', 15); set(gca, 'ylim', [0 105]);
l1 = legend('corners dataset, normalized trn loss', 'Location', 'SouthEast');
xlabel(['relative error [%]'], 'FontSize', 20);
ylabel('Count of occurrences [%]', 'FontSize', 20);
title('Cumulative historgam of mean errors', 'FontSize', 20);

% r = 781; c = 731;
r = 801; c = 801;
% Sets position and size of figure on the screen
set(fig, 'Units', 'pixels', 'position', [0 0 c r] );
% Sets axes to fill the figure space
% set(gca, 'Units', 'pixels', 'position', [0 0 c+1 r+1 ]);
% Sets print properties; Looks like 1 pixel = (3/4)th of a point
set(fig, 'paperunits', 'points', 'papersize', [fix((c-1)*(3/4))+1 fix((r-1)*(3/4))+1]);
set(fig, 'paperunits', 'normalized', 'paperposition', [0 0 1 1]);
print( fig, sprintf('-r%d', ceil(72*(4/3))), '-dpng', './img/cumhist-mean.png');

fig = figure;
y2 = c_elementsSCmax / max(c_elementsSCmax) * 100;
plot(xSCmax(1:end), y2(1:end), 'b', 'LineWidth', 3); hold on; grid on;
set(gcf, 'OuterPosition', [scrsz(1) scrsz(2) scrsz(3) scrsz(4)]);
set(gca, 'XTick', xSCmax); set(gca, 'YTick', 0:5:100, 'FontSize', 15);
set(gca, 'ylim', [0 105]);
l1 = legend('corners dataset, normalized trn loss', 'Location', 'SouthEast');
xlabel(['relative error [%]'], 'FontSize', 20);
ylabel('Count of occurrences [%]', 'FontSize', 20);
title('Cumulative historgam of max errors', 'FontSize', 20);

r = 801; c = 801;
% Sets position and size of figure on the screen
set(fig, 'Units', 'pixels', 'position', [0 0 c r] );
% Sets axes to fill the figure space
% set(gca, 'Units', 'pixels', 'position', [0 0 c+1 r+1 ]);
% Sets print properties; Looks like 1 pixel = (3/4)th of a point
set(fig, 'paperunits', 'points', 'papersize', [fix((c-1)*(3/4))+1 fix((r-1)*(3/4))+1]);
set(fig, 'paperunits', 'normalized', 'paperposition', [0 0 1 1]);
print( fig, sprintf('-r%d', ceil(72*(4/3))), '-dpng', './img/cumhist-max.png');

%% Timestamp

fprintf(1,'Finished on %s\n\n', datestr(now));


function colorspace_demo(Cmd)
% Demo for colorspace.m - 3D visualizations of various color spaces

% Pascal Getreuer 2006

if nargin == 0
   % Create a figure with a drop-down menu
   figure('Color',[1,1,1]);
   h = uicontrol('Style','popup','Position',[15,10,90,21],...
      'BackgroundColor',[1,1,1],'Value',2,...
      'string',{'sRGB','Y''PbPr','HSV','HSL','L*a*b*','L*u*v*','L*ch'});
   set(h,'Callback',sprintf('%s(get(%.20g,''Value''))',mfilename,h));
   Cmd = 2;
end

% Plot a figure
switch Cmd
case 1
   [x,y,z,Tri] = makeshape('Cube');
   CData = [x,y,z];
   myplot((x-0.5)*0.8,(y-0.5)*0.8,(z)*0.8,Tri,CData);
   coloraxis('R''',5,0.5*0.8);
   coloraxis('G''',6,0.5*0.8);
   coloraxis('B''',3);
case 2
   [x,y,z,Tri] = makeshape('Cylinder');
   CData = colorspace('rgb<-ypbpr',[z,x/2,y/2]);
   myplot(x,y,0.8*z,Tri,CData);
   coloraxis('Y''',3);
   coloraxis('P_b',5,0.8);
   coloraxis('P_r',6,0.8);
case 3
   [x,y,z,Tri] = makeshape('Hexacone');
   CData = colorspace('rgb<-hsv',[(pi+atan2(-y,-x))*180/pi,sqrt(x.^2+y.^2),z]);
   myplot(x,y,z,Tri,CData);
   coloraxis('H',1);
   coloraxis('S',2);
   coloraxis('V',3);
case 4
   [x,y,z,Tri] = makeshape('Double Hexacone');
   CData = colorspace('rgb<-hsl',[(pi+atan2(-y,-x))*180/pi,sqrt(x.^2+y.^2),z]);
   myplot(x,y,2*z,Tri,CData);
   coloraxis('H',1);
   coloraxis('S',2);
   coloraxis('L',4);
case 5
   [x,y,z,Tri] = makeshape('Blobs');
   CData = colorspace('rgb<-lab',[z*100,x*100,y*100]);
   myplot(x,y,2*z,Tri,CData);
   coloraxis('L*',4);
   coloraxis('a*',5,2);
   coloraxis('b*',6,2);
case 6
   [x,y,z,Tri] = makeshape('Blobs');
   CData = colorspace('rgb<-luv',[z*100,x*125,y*125]);
   myplot(x,y,2*z,Tri,CData);
   coloraxis('L*',4);
   coloraxis('u*',5,2);
   coloraxis('v*',6,2);
case 7
   [x,y,z,Tri] = makeshape('Blobs');
   CData = colorspace('rgb<-lab',[z*100,x*100,y*100]);
   myplot(x,y,2*z,Tri,CData);
   coloraxis('L*',4);
   coloraxis('c',2);
   coloraxis('h',1);
end

axis equal;
axis off;
pbaspect([1,1,1]);
view(70,27);
rotate3d on;

return;

function myplot(x,y,z,Tri,CData)
% Plot a triangular mesh with color data
cla;
CData = min(max(CData,0),1);
patch('Faces',Tri,'Vertices',[x,y,z],'FaceVertexCData',CData,...
   'FaceColor','interp','EdgeColor','none');
hold on;
return;

function coloraxis(Name,Type,h)
% Draw color axes as 3D objects
FontSize = 14;

switch Type
case 1
   set(text(-0.25,-1.3,1.1,Name),'FontWeight','bold',...
      'FontSize',FontSize,'Color',[0,0,0]);
   t = linspace(0,pi*3/2,60);
   x = cos(t)*1.1;
   y = sin(t)*1.1;
   set(plot3(x,y,zeros(size(x)),'k-'),'LineWidth',2.5,'Color',[1,1,1]*0.8);
   x = [x,-0.1,0,-0.1];
   y = [y,-1.05,-1.1,-1.15];
   set(plot3(x,y,ones(size(x)),'k-'),'LineWidth',2.5);
case 2
   set(text(0,-0.6,0.15,Name),'FontWeight','bold','FontSize',FontSize,'Color',[0,0,0]);
   set(plot3([-0.05,0,0.05,0,0],[-0.9,-1,-0.9,-1,0],[0,0,0,0,0],'k-'),'LineWidth',2.5);
case 3
   set(text(0,0.15,1.3,Name),'FontWeight','bold','FontSize',FontSize,'Color',[0,0,0]);
   set(plot3([0,0,0,0,0],[-0.05,0,0.05,0,0],[1.6,1.7,1.6,1.7,0],'k-'),'LineWidth',2.5);
case 4
   set(text(0,0.15,2.4,Name),'FontWeight','bold','FontSize',FontSize,'Color',[0,0,0]);
   set(plot3([0,0,0,0,0],[-0.05,0,0.05,0,0],[2.4,2.5,2.4,2.5,0],'k-'),'LineWidth',2.5);
case 5
   set(text(0.6,0,h+0.15,Name),'FontWeight','bold','FontSize',FontSize,'Color',[0,0,0]);
   set(plot3([0.9,1,0.9,1,-1,-0.9,-1,-0.9],[-0.05,0,0.05,0,0,-0.05,0,0.05],...
      zeros(1,8)+h,'k-'),'LineWidth',2.5);
case 6
   set(text(0,0.6,h+0.15,Name),'FontWeight','bold','FontSize',FontSize,'Color',[0,0,0]);
   set(plot3([-0.05,0,0.05,0,0,-0.05,0,0.05],[0.9,1,0.9,1,-1,-0.9,-1,-0.9],...
      zeros(1,8)+h,'k-'),'LineWidth',2.5);
end
return;

function [x,y,z,Tri] = makeshape(Shape)
% Make a 3D shape: Shape = 'Cube', 'Cylinder', 'Cone', 'Double Cone', 'Blobs'

% Cube
N = 12;    % Vertices per edge
% Cylinder, Cone, Double Cone
Nth = 25;  % Vertices over angles, (Nth - 1) should be a multiple of 12
Nr = 4;    % Vertices over radius
Nz = 8;    % Vertices over z-dimension

switch lower(Shape)
case 'cube'
   [u,v] = meshgrid(linspace(0,1,N),linspace(0,1,N));
   u = u(:);
   v = v(:);
   x = [u;u;u;u;zeros(N^2,1);ones(N^2,1)];
   y = [v;v;zeros(N^2,1);ones(N^2,1);v;v];
   z = [zeros(N^2,1);ones(N^2,1);v;v;u;u];
   Tri = trigrid(N,N);
   Tri = [Tri;N^2+Tri;2*N^2+Tri;3*N^2+Tri;4*N^2+Tri;5*N^2+Tri];
case 'cylinder'
   Nth = ceil(Nth*0.75);
   [u,v] = meshgrid(linspace(0,pi*3/2,Nth),linspace(0,1,Nz));
   Tri = trigrid(Nth,Nz);
   x = cos(u(:));
   y = sin(u(:));
   z = v(:);
   [u,v] = meshgrid(linspace(0,pi*3/2,Nth),linspace(0,1,Nr));
   Tri = [Tri;Nth*Nz+trigrid(Nth,Nr);Nth*Nz+Nth*Nr+trigrid(Nth,Nr)];
   x = [x;v(:).*cos(u(:));v(:).*cos(u(:))];
   y = [y;v(:).*sin(u(:));v(:).*sin(u(:))];
   z = [z;zeros(Nth*Nr,1);ones(Nth*Nr,1)];
   [u,v] = meshgrid(linspace(0,1,Nr),linspace(0,1,Nz));
   Tri = [Tri;Nth*Nz+2*Nth*Nr+trigrid(Nr,Nz);Nth*Nz+2*Nth*Nr+Nr*Nz+trigrid(Nr,Nz)];
   x = [x;u(:);zeros(Nr*Nz,1)];
   y = [y;zeros(Nr*Nz,1);-u(:)];
   z = [z;v(:);v(:)];
case 'cone'
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nz));
   Tri = trigrid(Nth,Nz);
   x = v(:).*cos(u(:));
   y = v(:).*sin(u(:));
   z = v(:);
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nr));
   Tri = [Tri;Nth*Nz+trigrid(Nth,Nr);];
   x = [x;v(:).*cos(u(:));];
   y = [y;v(:).*sin(u(:));];
   z = [z;ones(Nth*Nr,1)];
case 'double cone'
   Nz = floor(Nz/2)*2+1;
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nz));
   Tri = trigrid(Nth,Nz);
   r = 1 - abs(2*v(:) - 1);
   x = r.*cos(u(:));
   y = r.*sin(u(:));
   z = v(:);
case 'hexacone'
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nz));
   Tri = trigrid(Nth,Nz);
   r = 0.92./max(max(abs(cos(u)),abs(cos(u - pi/3))),abs(cos(u + pi/3)));
   x = v(:).*cos(u(:)).*r(:);
   y = v(:).*sin(u(:)).*r(:);
   z = v(:);
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nr));
   Tri = [Tri;Nth*Nz+trigrid(Nth,Nr);];
   v = 0.92*v./max(max(abs(cos(u)),abs(cos(u - pi/3))),abs(cos(u + pi/3)));
   x = [x;v(:).*cos(u(:));];
   y = [y;v(:).*sin(u(:));];
   z = [z;ones(Nth*Nr,1)];
case 'double hexacone'
   Nz = floor(Nz/2)*2+1;
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nz));
   Tri = trigrid(Nth,Nz);
   r = 1 - abs(2*v - 1);
   r = 0.92*r./max(max(abs(cos(u)),abs(cos(u - pi/3))),abs(cos(u + pi/3)));
   x = r(:).*cos(u(:));
   y = r(:).*sin(u(:));
   z = v(:);
case 'blobs'
   Nz = 47;
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nz));
   Tri = trigrid(Nth,Nz);
   r = sin(v(:)*pi*3).^2.*(1 - 0.6*abs(2*v(:) - 1));
   x = r.*cos(u(:));
   y = r.*sin(u(:));
   z = v(:);
end
return;

function Tri = trigrid(Nu,Nv)
% Construct the connectivity data for a grid of triangular patches
i = (1:(Nu-1)*Nv-1).';
i(Nv:Nv:end) = [];
Tri = [i,i+1,i+Nv;i+1,i+Nv+1,i+Nv];
return;


function [ y, j ] = j_borrow_gregorian ( y, j )

%*****************************************************************************80
%
%% J_BORROW_GREGORIAN borrows year-days from years in a Gregorian date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, J, a YJ date.
%
  while ( j <= 0 )

    y = y - 1;

    days = year_length_gregorian ( y );

    j = j + days;

  end

  return
end


clear all;
% -------------------------------------------------------------------------
%   Description:
%       Script to demo MS-LapSRN for one image
%
%   Citation:
%       Fast and Accurate Image Super-Resolution with Deep Laplacian Pyramid Networks
%       Wei-Sheng Lai, Jia-Bin Huang, Narendra Ahuja, and Ming-Hsuan Yang
%       arXiv, 2017
%
%   Contact:
%       Wei-Sheng Lai
%       wlai24@ucmerced.edu
%       University of California, Merced
% -------------------------------------------------------------------------

img_filename = 'emma.jpg';

%% parameters
%model_name  = 'MSLapSRN_D5R2';
%model_name  = 'MSLapSRN_D5R5';
model_name  = 'MSLapSRN_D5R8';

model_scale = 4; % model SR scale
test_scale  = 4; % testing SR scale
gpu         = 1; % GPU ID

%% setup paths
addpath(genpath('utils'));
addpath(fullfile(pwd, 'matconvnet/matlab'));
vl_setupnn;

%% Load pretrained multi-scale model
model_filename = fullfile('pretrained_models', sprintf('%s.mat', model_name));
fprintf('Load %s\n', model_filename);

net = load(model_filename);
net_trained = dagnn.DagNN.loadobj(net.net);

opts_filename = fullfile('pretrained_models', sprintf('%s_opts.mat', model_name));
fprintf('Load %s\n', opts_filename);

opts = load(opts_filename);
opts = opts.opts;

opts.scales = [model_scale];

%% create single-scale model
net = init_MSLapSRN_model(opts, 'test');

%% copy pretrained weights
fprintf('Copy weights to single scale model...\n');
net = copy_model_weights(net, net_trained);

if( gpu ~= 0 )
    gpuDevice(gpu)
    net.move('gpu');
end

%% Load GT image
fprintf('Load %s\n', img_filename);
img_GT = im2double(imread(img_filename));
img_GT = mod_crop(img_GT, test_scale);

%% Generate LR image
img_LR = imresize(img_GT, 1/test_scale);

%% apply LapSRN
fprintf('Apply MS-LapSRN for %dx SR\n', test_scale);
img_HR = SR_MSLapSRN(img_LR, net, model_scale, test_scale, gpu);

%% show results
img_LR = imresize(img_LR, test_scale);
figure, imshow(cat(2, img_LR, img_HR, img_GT));
title(sprintf('Bicubic %dx    |    MS-LapSRN %dx    |    Ground Truth', test_scale, test_scale));


function T = findFutileCycle(model, cut,closedModel)
% This function attempts to find reactions involved in the atp-driving
% futile cycle
%
% INPUT
% model         model structure
% cut           cutoff value for reactions to be displayed
% closedModel   if 1 the model will be closed otw the applied medium
%               constraints count (DEFAULT:1);
% OUTPUT
% T         Table of reactions potentially involved
%
% Ines Thiele 03/2022

if ~exist('cut','var')
    cut = 250;
end

if ~exist('closedModel','var')
    closedModel = 1;
end

modelClosed = model;
if closedModel
    modelexchanges1 = strmatch('Ex_',modelClosed.rxns);
    modelexchanges4 = strmatch('EX_',modelClosed.rxns);
    modelexchanges2 = strmatch('DM_',modelClosed.rxns);
    modelexchanges3 = strmatch('sink_',modelClosed.rxns);
    selExc = (find( full((sum(abs(modelClosed.S)==1,1) ==1) & (sum(modelClosed.S~=0) == 1))))';

    modelexchanges = unique([modelexchanges1;modelexchanges2;modelexchanges3;modelexchanges4;selExc]);
    modelClosed.lb(ismember(modelClosed.rxns,modelClosed.rxns(modelexchanges)))=0;
end
modelClosed = changeObjective(modelClosed,'DM_atp_c_');

fba = optimizeCbModel(modelClosed,'max','zero')
fba.v(find(abs(fba.v)<=1e-6))=0;
tab = [modelClosed.lb modelClosed.ub fba.v];
a = printRxnFormula(model);

R = modelClosed.rxns(find(abs(fba.v)>cut));
L = modelClosed.lb(find(abs(fba.v)>cut));
U = modelClosed.ub(find(abs(fba.v)>cut));
F = fba.v(find(abs(fba.v)>cut));
A = a(find(abs(fba.v)>cut));
if ~isempty(A)
T = table(R, num2str(L),num2str(U), num2str(F),A);
else
    T = {};
end


function ret = spm_ov_goto_max(varargin)
% Goto maximum intensity tool - plugin for spm_orthviews
%
% This tool provides capabilities similar to the "Goto ... maximum"
% functionality in spm_mip_ui.m. When the tool is called for the first
% time, it has to read the whole image data file. This might result in a
% slow response depending on the image dimensions.
%
% This routine is a plugin to spm_orthviews. For general help about
% spm_orthviews and plugins type
%             help spm_orthviews
% at the MATLAB prompt.
%__________________________________________________________________________
% Copyright (C) 2012 Wellcome Trust Centre for Neuroimaging

% Volkmar Glauche
% $Id: spm_ov_goto_max.m 4996 2012-10-11 18:28:37Z guillaume $

global st;
if isempty(st)
    error('goto_max: This routine can only be called as a plugin for spm_orthviews!');
end

if nargin < 2
    error('goto_max: Wrong number of arguments. Usage: spm_orthviews(''goto_max'', cmd, volhandle, varargin)');
end

cmd = lower(varargin{1});
volhandle = varargin{2};

switch cmd

    % Context menu and callbacks
    case 'context_menu'
        item0 = uimenu(varargin{3}, 'Label', 'Goto maximum');
        item1 = uimenu(item0, 'Label', 'Goto global maximum',...
            'Callback', ['spm_orthviews(''goto_max'', ''global'',' num2str(volhandle) ');']);
        item2 = uimenu(item0, 'Label', 'Goto nearest local maximum',...
            'Callback', ['spm_orthviews(''goto_max'', ''local'',' num2str(volhandle) ');']);
        ret = item0;

    case 'global'
        if ~isfield(st.vols{volhandle}, 'goto_max')
            [dat, xyz] = spm_read_vols(st.vols{volhandle});
            [unused, mxind] = max(dat(:));
            st.vols{volhandle}.goto_max.globalmm = xyz(:, mxind);
        end
        posmm = st.vols{volhandle}.premul*[st.vols{volhandle}.goto_max.globalmm;1];
        spm_orthviews('reposition', posmm(1:3));

    case 'local'
        % Read volume, goto local maxima higher or equal current voxel
        % intensity
        dat       = spm_read_vols(st.vols{volhandle});
        [x, y, z] = ndgrid(1:st.vols{volhandle}.dim(1), 1:st.vols{volhandle}.dim(2), 1:st.vols{volhandle}.dim(3));
        xyz       = [x(:) y(:) z(:)]';
        posvx     = round((st.vols{volhandle}.premul*st.vols{volhandle}.mat)\[spm_orthviews('pos');1]);
        try
            if isfinite(dat(posvx(1), posvx(2), posvx(3)))
                sel = isfinite(dat(:)) & dat(:) >= dat(posvx(1), posvx(2), posvx(3)) - eps;
            else
                sel = isfinite(dat(:));
            end
        catch
            sel = isfinite(dat(:));
        end
        [unused, unused, XYZ]  = spm_max(dat(sel), xyz(:,sel));
        XYZdist      = bsxfun(@minus,XYZ,posvx(1:3));
        [unused, nmaxind] = min(sum(XYZdist.^2));
        posmm        = st.vols{volhandle}.premul*st.vols{volhandle}.mat*[XYZ(:, nmaxind); 1];
        spm_orthviews('reposition', posmm(1:3));
end


%

% @
% Copyright (C) 2017 Jonas Ruesch
%
% This program 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.
%
% This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
% @

% The tornado installation directory.
% Must contain the aircraft geometry file and the status used below.
tornado_root_directory = 'F:\svn\dev\matlab\tornado\T135_export';

% The state:
% Velocity 12.5 m/s , ~45km/h (average velocity as measured in real flight)
% Velocity doesn't matter much for coefficient computation.
% Coefficients are largely velocity independent (tested)).
state.AS=      12.5;      %Airspeed m/s
state.alpha=   0.0;       %Angle of attack, radians
state.betha=   0;         %Angle of sideslip, radians
state.P=       0;         %Rollrate, rad/s
state.Q=       0;         %pitchrate, rad/s
state.R=       0;         %yawrate, rad/s
state.adot=    0;         %Alpha time derivative rad/s
state.bdot=    0;         %Betha time derivative rad/s
state.ALT=     0;         %Altitude, m
state.rho=     1.225;     %Desity, kg/m^3
state.pgcorr=  0;         %Apply prandtl glauert compressibility correction

% The ExperimentalCarrier geometry.
aircraft_geometry_name = 'ExperimentalCarrier';

% Center of gravity
centerOfGravity = 0.092; % Tornado x-axis extends aft (!).

% The sweep ranges and sampling resolution for Simulink modelling
alphaStart = -10;
alphaEnd = 20;
numAlphas = 31;
betaStart = -10;
betaEnd = 10;
numBetas = 21;

% % Quick Test
% alphaStart = -10;
% alphaEnd = 20;
% numAlphas = 2;
% betaStart = -10;
% betaEnd = 10;
% numBetas = 2;

% Coefficient computation
[alpha, beta, CX, CY, CZ, Cl, Cm, Cn, CX_d, CY_d, CZ_d, Cl_d, Cm_d, Cn_d, ...
     CX_P, CY_P, CZ_P, Cl_P, Cm_P, Cn_P, CX_Q, CY_Q, CZ_Q, Cl_Q, Cm_Q, Cn_Q, CX_R, CY_R, CZ_R, Cl_R, Cm_R, Cn_R]=computeAerodynamicCoefficients(...
     aircraft_geometry_name, state, tornado_root_directory, alphaStart, ...
     alphaEnd, numAlphas, betaStart, betaEnd, numBetas, centerOfGravity);

plotCoefficients(alpha, beta, CX, CY, CZ, Cl, Cm, Cn, CX_d, CY_d, CZ_d, Cl_d, Cm_d, Cn_d, ...
    CX_P, CY_P, CZ_P, Cl_P, Cm_P, Cn_P, CX_Q, CY_Q, CZ_Q, Cl_Q, Cm_Q, Cn_Q, CX_R, CY_R, CZ_R, Cl_R, Cm_R, Cn_R);

plotAircraftGeometry(aircraft_geometry_name, state, tornado_root_directory);

save;


function sudoku_test ( )

%*****************************************************************************80
%
%% SUDOKU_TEST tests the SUDOKU library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2007
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SUDOKU_TEST:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the SUDOKU library.\n' );

  sudoku_test01 ( );
  sudoku_test02 ( );
  sudoku_test03 ( );
  sudoku_test04 ( );
  sudoku_test05 ( );
  sudoku_test06 ( );
  sudoku_test07 ( );
  sudoku_test08 ( );
  sudoku_test09 ( );
  sudoku_test10 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SUDOKU_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function asa066_test ( )

%*****************************************************************************80
%
%% ASA066_TEST tests ASA066.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ASA066_TEST:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the ASA066 library.\n' );

  asa066_test01 ( );
  asa066_test02 ( );
  asa066_test03 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ASA066_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function [y,w,s,g] = spm_csd_mtf(P,M,U)
% Spectral response of a NMM (transfer function x noise spectrum)
% FORMAT [y,w,s,g] = spm_csd_mtf(P,M,U)
% FORMAT [y,w,s,g] = spm_csd_mtf(P,M)
%
% P - parameters
% M - neural mass model structure
% U - trial-specific effects (induces expansion around steady state)
%
% y - {y(N,nc,nc}} - cross-spectral density for nc channels {trials}
%                  - for N frequencies in M.Hz [default 1:64Hz]
% w - frequencies
% s - modulation transfer functions (complex)
% g - normalised modulation transfer function (true Granger causality)
%
% When called with U this function will return a cross-spectral response
% for each of the condition-specific parameters specified in U.X; otherwise
% it returns the complex CSD for the parameters in P (using the expansion
% point supplied in M.x)
%
% When the observer function M.g is specified the CSD response is
% supplemented with channel noise in sensor space; otherwise the CSD
% pertains to hidden states.
%
% NB: requires M.u to specify the number of endogenous inputs
% This routine and will solve for the (hidden) steady state and use it as
% the expansion point for subsequent linear systems analysis (if trial
% specific effects are specified).
%
% See also:
%  spm_ccf2csd.m, spm_ccf2mar, spm_csd2ccf.m, spm_csd2mar.m, spm_mar2csd.m,
%  spm_csd2coh.m, spm_dcm_mtf.m, spm_Q.m, spm_mar.m and spm_mar_spectral.m
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_csd_mtf.m 7529 2019-02-06 19:21:38Z karl $

% between-trial (experimental) inputs
%==========================================================================
try
    X = U.X;
    if ~size(X,1)
        X = sparse(1,0);
    end
catch

    % default inputs - one trial (no trial-specific effects)
    %----------------------------------------------------------------------
    X = sparse(1,0);

end

% compute log-spectral density
%==========================================================================

% frequencies of interest
%--------------------------------------------------------------------------
if isfield(M,'Hz')
    w    = M.Hz;
else
    w    = 1:64;
    M.Hz = w;
end

% number of channels and exogenous (neuronal) inputs or sources
%--------------------------------------------------------------------------
nc   = M.l;
nw   = length(M.Hz);

% spectrum of innovations (Gu) and noise (Gs and Gn)
%--------------------------------------------------------------------------
if isfield(M,'g')
    [Gu,Gs,Gn] = spm_csd_mtf_gu(P,M.Hz);
else
    Gu         = spm_csd_mtf_gu(P,M.Hz);
    nc         = size(Gu,2);
end

% cycle over trials (experimental conditions)
%==========================================================================
for  c = 1:size(X,1)

    % condition-specific parameters
    %----------------------------------------------------------------------
    Q   = spm_gen_Q(P,X(c,:));

    % solve for steady-state - if exogenous inputs are specified
    %----------------------------------------------------------------------
    if nargin > 2
        M.x = spm_dcm_neural_x(Q,M);
    end

    % transfer functions (FFT of kernel)
    %----------------------------------------------------------------------
    S     = spm_dcm_mtf(Q,M);

    % predicted cross-spectral density
    %----------------------------------------------------------------------
    G     = zeros(nw,nc,nc);
    for i = 1:nw
        Si       = shiftdim(S(i,:,:),1);
        G(i,:,:) = Si*diag(Gu(i,:))*Si';
    end

    % save trial-specific frequencies of interest
    %----------------------------------------------------------------------
    g{c}  = G;
    s{c}  = S;

end

% and add channel noise
%==========================================================================
if isfield(M,'g')

    for c = 1:length(g)
        G = g{c};
        for i = 1:nc

            % channel specific noise
            %--------------------------------------------------------------
            try
                G(:,i,i) = G(:,i,i) + Gs(:,i);
            catch
                G(:,i,i) = G(:,i,i) + Gs(:,1);
            end

            % and cross-spectral density from common channel noise
            %--------------------------------------------------------------
            for j = 1:nc
                G(:,i,j) = G(:,i,j) + Gn;
            end
        end
        y{c} = G;

    end
else
    y = g;
end

% model data features summarised with a MAR process
%==========================================================================
if isfield(M,'p')
    p = M.p - 1;
    if isfield(M,'dt')
        dt = M.dt;
    else
        dt = 1/(2*w(end));
    end
    for c  = 1:length(y)
        mar  = spm_csd2mar(y{c},w,p,dt);
        y{c} = spm_mar2csd(mar,w,1/dt);
    end
end

% model the effect of filtering during preprocessing
%==========================================================================
if isfield(P,'f')
    for c  = 1:length(y)
        f     = (1:nw)';
        f     = exp(P.f(1) + P.f(2)*f/nw);
        for i = 1:nc
            for j = 1:nc
                y{c}(:,i,j) = y{c}(:,i,j).*f;
            end
        end
    end
end

% Granger causality (normalised transfer functions) if requested
%==========================================================================
if nargout > 3
    for c = 1:length(s)
        g{c} = spm_dtf2gew(s{c},Gu);
    end
end


function [Ep,Cp,Eh,F,L,dFdp,dFdpp] = spm_nlsi_GN(M,U,Y)
% Bayesian inversion of nonlinear models - Gauss-Newton/Variational Laplace
% FORMAT [Ep,Cp,Eh,F] = spm_nlsi_GN(M,U,Y)
%
% [Dynamic] MIMO models
%__________________________________________________________________________
%
% M.IS - function name f(P,M,U) - generative model
%        This function specifies the nonlinear model:
%          y = Y.y = IS(P,M,U) + X0*P0 + e
%        where e ~ N(0,C). For dynamic systems this would be an integration
%        scheme (e.g. spm_int). spm_int expects the following:
%
%     M.f  - f(x,u,P,M)
%     M.g  - g(x,u,P,M)
%     M.h  - h(x,u,P,M)
%       x  - state variables
%       u  - inputs or causes
%       P  - free parameters
%       M  - fixed functional forms and parameters in M
%
% M.FS - function name f(y,M)   - feature selection
%        This [optional] function performs feature selection assuming the
%        generalized model y = FS(y,M) = FS(IS(P,M,U),M) + X0*P0 + e
%
% M.P  - starting estimates for model parameters [optional]
%
% M.pE - prior expectation      - E{P}   of model parameters
% M.pC - prior covariance       - Cov{P} of model parameters
%
% M.hE - prior expectation      - E{h}   of log-precision parameters
% M.hC - prior covariance       - Cov{h} of log-precision parameters
%
% U.u  - inputs (or just U)
% U.dt - sampling interval
%
% Y.y  - outputs (samples x observations x ...)
% Y.dt - sampling interval for outputs
% Y.X0 - confounds or null space      (over size(y,1) samples or all vec(y))
% Y.Q  - q error precision components (over size(y,1) samples or all vec(y))
%
%
% Parameter estimates
%--------------------------------------------------------------------------
% Ep  - (p x 1)         conditional expectation    E{P|y}
% Cp  - (p x p)         conditional covariance     Cov{P|y}
% Eh  - (q x 1)         conditional log-precisions E{h|y}
%
% log evidence
%--------------------------------------------------------------------------
% F   - [-ve] free energy F = log evidence = p(y|f,g,pE,pC) = p(y|m)
%
%__________________________________________________________________________
% Returns the moments of the posterior p.d.f. of the parameters of a
% nonlinear model specified by IS(P,M,U) under Gaussian assumptions.
% Usually, IS is an integrator of a dynamic MIMO input-state-output model
%
%              dx/dt = f(x,u,P)
%              y     = g(x,u,P)  + X0*P0 + e
%
% A static nonlinear observation model with fixed input or causes u
% obtains when x = []. i.e.
%
%              y     = g([],u,P) + X0*P0e + e
%
% but static nonlinear models are specified more simply using
%
%              y     = IS(P,M,U) + X0*P0 + e
%
% Priors on the free parameters P are specified in terms of expectation pE
% and covariance pC. The E-Step uses a Fisher-Scoring scheme and a Laplace
% approximation to estimate the conditional expectation and covariance of P
% If the free-energy starts to increase,  an abbreviated descent is
% invoked.  The M-Step estimates the precision components of e, in terms
% of log-precisions.  Although these two steps can be thought of in
% terms of E and N steps they are in fact variational steps of a full
% variational Laplace scheme that accommodates conditional uncertainty
% over both parameters and log precisions (c.f. hyperparameters with hyper
% priors)
%
% An optional feature selection can be specified with parameters M.FS.
%
% For generic aspects of the scheme see:
%
% Friston K, Mattout J, Trujillo-Barreto N, Ashburner J, Penny W.
% Variational free energy and the Laplace approximation.
% NeuroImage. 2007 Jan 1;34(1):220-34.
%
% This scheme handels complex data along the lines originally described in:
%
% Sehpard RJ, Lordan BP, and Grant EH.
% Least squares analysis of complex data with applications to permittivity
% measurements.
% J. Phys. D. Appl. Phys 1970 3:1759-1764.
%
%__________________________________________________________________________
% Copyright (C) 2001-2015 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_nlsi_GN.m 7279 2018-03-10 21:22:44Z karl $

% options
%--------------------------------------------------------------------------
try, M.nograph; catch, M.nograph = 0;   end
try, M.noprint; catch, M.noprint = 0;   end
try, M.Nmax;    catch, M.Nmax    = 128; end

% figure (unless disabled)
%--------------------------------------------------------------------------
if ~M.nograph
    Fsi = spm_figure('GetWin','SI');
end

% check integrator
%--------------------------------------------------------------------------
try
    M.IS;
catch
    M.IS = 'spm_int';
end

% composition of feature selection and prediction (usually an integrator)
%--------------------------------------------------------------------------
try
    y  = Y.y;
catch
    y  = Y;
end

try

    % try FS(y,M)
    %----------------------------------------------------------------------
    try
        y  = feval(M.FS,y,M);
        IS = inline([M.FS '(' M.IS '(P,M,U),M)'],'P','M','U');

        % try FS(y)
        %------------------------------------------------------------------
    catch
        y  = feval(M.FS,y);
        IS = inline([M.FS '(' M.IS '(P,M,U))'],'P','M','U');
    end

catch

    % otherwise FS(y) = y
    %----------------------------------------------------------------------
    try
        IS = inline([M.IS '(P,M,U)'],'P','M','U');
    catch
        IS = M.IS;
    end
end

% converted to function handle
%--------------------------------------------------------------------------
IS  = spm_funcheck(IS);

% paramter update eqation
%--------------------------------------------------------------------------
if isfield(M,'f'), M.f = spm_funcheck(M.f);  end
if isfield(M,'g'), M.g = spm_funcheck(M.g);  end
if isfield(M,'h'), M.h = spm_funcheck(M.h);  end

% size of data (samples x response component x response component ...)
%--------------------------------------------------------------------------
if iscell(y)
    ns = size(y{1},1);
else
    ns = size(y,1);
end
ny   = length(spm_vec(y));          % total number of response variables
nr   = ny/ns;                       % number response components
M.ns = ns;                          % number of samples M.ns

% initial states
%--------------------------------------------------------------------------
try
    M.x;
catch
    if ~isfield(M,'n'), M.n = 0;    end
    M.x = sparse(M.n,1);
end

% input
%--------------------------------------------------------------------------
try
    U;
catch
    U = [];
end

% initial parameters
%--------------------------------------------------------------------------
try
    spm_vec(M.P) - spm_vec(M.pE);
    fprintf('\nParameter initialisation successful\n')
catch
    M.P = M.pE;
end

% time-step
%--------------------------------------------------------------------------
try
    dt = Y.dt;
catch
    dt = 1;
end

% precision components Q
%--------------------------------------------------------------------------
try
    Q = Y.Q;
    if isnumeric(Q), Q = {Q}; end
catch
    Q = spm_Ce(ns*ones(1,nr));
end
nh    = length(Q);                  % number of precision components
nq    = ny/length(Q{1});            % for compact Kronecker form of M-step

% prior moments (assume uninformative priors if not specifed)
%--------------------------------------------------------------------------
pE       = M.pE;
try
    pC   = M.pC;
catch
    np   = spm_length(M.pE);
    pC   = speye(np,np)*exp(16);
end

% confounds (if specified)
%--------------------------------------------------------------------------
try
    nb   = size(Y.X0,1);            % number of bins
    nx   = ny/nb;                   % number of blocks
    dfdu = kron(speye(nx,nx),Y.X0);
catch
    dfdu = sparse(ny,0);
end
if isempty(dfdu), dfdu = sparse(ny,0); end

% hyperpriors - expectation (and initialize hyperparameters)
%--------------------------------------------------------------------------
try
    hE  = M.hE;
    if length(hE) ~= nh
        hE = hE + sparse(nh,1);
    end
catch
    hE  = sparse(nh,1) - log(var(spm_vec(y))) + 4;
end
h       = hE;

% hyperpriors - covariance
%--------------------------------------------------------------------------
try
    ihC = spm_inv(M.hC);
    if length(ihC) ~= nh
        ihC = ihC*speye(nh,nh);
    end
catch
    ihC = speye(nh,nh)*exp(4);
end

% unpack covariance
%--------------------------------------------------------------------------
if isstruct(pC);
    pC = spm_diag(spm_vec(pC));
end

% dimension reduction of parameter space
%--------------------------------------------------------------------------
V     = spm_svd(pC,0);
nu    = size(dfdu,2);                 % number of parameters (confounds)
np    = size(V,2);                    % number of parameters (effective)
ip    = (1:np)';
iu    = (1:nu)' + np;

% second-order moments (in reduced space)
%--------------------------------------------------------------------------
pC    = V'*pC*V;
uC    = speye(nu,nu)/1e-8;
ipC   = inv(spm_cat(spm_diag({pC,uC})));

% initialize conditional density
%--------------------------------------------------------------------------
Eu    = spm_pinv(dfdu)*spm_vec(y);
p     = [V'*(spm_vec(M.P) - spm_vec(M.pE)); Eu];
Ep    = spm_unvec(spm_vec(pE) + V*p(ip),pE);

% EM
%==========================================================================
criterion = [0 0 0 0];

C.F   = -Inf;                                   % free energy
v     = -4;                                     % log ascent rate
dFdh  = zeros(nh,1);
dFdhh = zeros(nh,nh);
for k = 1:M.Nmax

    % time
    %----------------------------------------------------------------------
    tStart = tic;

    % E-Step: prediction f, and gradients; dfdp
    %======================================================================
    try

        % gradients
        %------------------------------------------------------------------
        [dfdp,f] = spm_diff(IS,Ep,M,U,1,{V});
        dfdp     = reshape(spm_vec(dfdp),ny,np);

        % check for stability
        %------------------------------------------------------------------
        normdfdp = norm(dfdp,'inf');
        revert   = isnan(normdfdp) || normdfdp > exp(32);

    catch
        revert   = true;
    end

    if revert && k > 1
        for i = 1:4

            % reset expansion point and increase regularization
            %--------------------------------------------------------------
            v        = min(v - 2,-4);

            % E-Step: update
            %--------------------------------------------------------------
            p        = C.p + spm_dx(dFdpp,dFdp,{v});
            Ep       = spm_unvec(spm_vec(pE) + V*p(ip),pE);

            % try again
            %--------------------------------------------------------------
            try
                [dfdp,f] = spm_diff(IS,Ep,M,U,1,{V});
                dfdp     = reshape(spm_vec(dfdp),ny,np);

                % check for stability
                %----------------------------------------------------------
                normdfdp = norm(dfdp,'inf');
                revert   = isnan(normdfdp) || normdfdp > exp(32);

            catch
                revert   = true;
            end

            % break
            %--------------------------------------------------------------
            if ~revert, break, end

        end
    end

    % convergence failure
    %----------------------------------------------------------------------
    if revert
        error('SPM:spm_nlsi_GN','Convergence failure.');
    end

    % prediction error and full gradients
    %----------------------------------------------------------------------
    e     =  spm_vec(y) - spm_vec(f) - dfdu*p(iu);
    J     = -[dfdp dfdu];

    % M-step: Fisher scoring scheme to find h = max{F(p,h)}
    %======================================================================
    for m = 1:8

        % precision and conditional covariance
        %------------------------------------------------------------------
        iS    = sparse(0);
        for i = 1:nh
            iS = iS + Q{i}*(exp(-32) + exp(h(i)));
        end
        S     = spm_inv(iS);
        iS    = kron(speye(nq),iS);
        Pp    = real(J'*iS*J);
        Cp    = spm_inv(Pp + ipC);

        % precision operators for M-Step
        %------------------------------------------------------------------
        for i = 1:nh
            P{i}   = Q{i}*exp(h(i));
            PS{i}  = P{i}*S;
            P{i}   = kron(speye(nq),P{i});
            JPJ{i} = real(J'*P{i}*J);
        end

        % derivatives: dLdh = dL/dh,...
        %------------------------------------------------------------------
        for i = 1:nh
            dFdh(i,1)      =   trace(PS{i})*nq/2 ...
                - real(e'*P{i}*e)/2 ...
                - spm_trace(Cp,JPJ{i})/2;
            for j = i:nh
                dFdhh(i,j) = - spm_trace(PS{i},PS{j})*nq/2;
                dFdhh(j,i) =   dFdhh(i,j);
            end
        end

        % add hyperpriors
        %------------------------------------------------------------------
        d     = h     - hE;
        dFdh  = dFdh  - ihC*d;
        dFdhh = dFdhh - ihC;
        Ch    = spm_inv(real(-dFdhh));

        % update ReML estimate
        %------------------------------------------------------------------
        dh    = spm_dx(dFdhh,dFdh,{4});
        dh    = min(max(dh,-1),1);
        h     = h  + dh;

        % convergence
        %------------------------------------------------------------------
        dF    = dFdh'*dh;
        if dF < 1e-2, break, end

    end

    % E-Step with Levenberg-Marquardt regularization
    %======================================================================

    % objective function: F(p) = log evidence - divergence
    %----------------------------------------------------------------------
    L(1) = spm_logdet(iS)*nq/2  - real(e'*iS*e)/2 - ny*log(8*atan(1))/2;            ...
    L(2) = spm_logdet(ipC*Cp)/2 - p'*ipC*p/2;
    L(3) = spm_logdet(ihC*Ch)/2 - d'*ihC*d/2;
    F    = sum(L);

    % record increases and reference log-evidence for reporting
    %----------------------------------------------------------------------
    try
        F0;
        if ~M.noprint
            fprintf(' actual: %.3e (%.2f sec)\n',full(F - C.F),toc(tStart))
        end
    catch
        F0 = F;
    end

    % if F has increased, update gradients and curvatures for E-Step
    %----------------------------------------------------------------------
    if F > C.F || k < 3

        % accept current estimates
        %------------------------------------------------------------------
        C.p   = p;
        C.h   = h;
        C.F   = F;
        C.L   = L;
        C.Cp  = Cp;

        % E-Step: Conditional update of gradients and curvature
        %------------------------------------------------------------------
        dFdp  = -real(J'*iS*e) - ipC*p;
        dFdpp = -real(J'*iS*J) - ipC;

        % decrease regularization
        %------------------------------------------------------------------
        v     = min(v + 1/2,4);
        str   = 'EM:(+)';

    else

        % reset expansion point
        %------------------------------------------------------------------
        p     = C.p;
        h     = C.h;
        Cp    = C.Cp;

        % and increase regularization
        %------------------------------------------------------------------
        v     = min(v - 2,-4);
        str   = 'EM:(-)';

    end

    % E-Step: update
    %======================================================================
    dp    = spm_dx(dFdpp,dFdp,{v});
    p     = p + dp;
    Ep    = spm_unvec(spm_vec(pE) + V*p(ip),pE);

    % Graphics
    %======================================================================
    if exist('Fsi', 'var')
        spm_figure('Select', Fsi)

        % reshape prediction if necessary
        %------------------------------------------------------------------
        e  = spm_vec(e);
        f  = spm_vec(f);
        try
            e  = reshape(e,ns,nr);
            f  = reshape(f,ns,nr);
        end

        % subplot prediction
        %------------------------------------------------------------------
        x    = (1:size(e,1))*dt;
        xLab = 'time (seconds)';
        try
            if length(M.Hz) == ns
                x    = M.Hz;
                xLab = 'Frequency (Hz)';
            end
        end

        % plot real or complex predictions
        %------------------------------------------------------------------
        tstr = sprintf('%s: %i','prediction and response: E-Step',k);
        if isreal(spm_vec(y))

            subplot(2,1,1)
            plot(x,real(f)), hold on
            plot(x,real(f + e),':'), hold off
            xlabel(xLab)
            title(tstr,'FontSize',16)
            grid on

        else
            subplot(2,2,1)
            plot(x,real(f)), hold on
            plot(x,real(f + e),':'), hold off
            xlabel(xLab)
            ylabel('real')
            title(tstr,'FontSize',16)
            grid on

            subplot(2,2,2)
            plot(x,imag(f)), hold on
            plot(x,imag(f + e),':'), hold off
            xlabel(xLab)
            ylabel('imaginary')
            title(tstr,'FontSize',16)
            grid on
        end

        % subplot parameters
        %--------------------------------------------------------------
        subplot(2,1,2)
        bar(full(V*p(ip)))
        xlabel('parameter')
        tstr = 'conditional [minus prior] expectation';
        title(tstr,'FontSize',16)
        grid on
        drawnow

    end

    % convergence
    %----------------------------------------------------------------------
    dF  = dFdp'*dp;
    if ~M.noprint
        fprintf('%-6s: %i %6s %-6.3e %6s %.3e ',str,k,'F:',full(C.F - F0),'dF predicted:',full(dF))
    end
    criterion = [(dF < 1e-1) criterion(1:end - 1)];
    if all(criterion)
        if ~M.noprint
            fprintf(' convergence\n')
        end
        break
    end

end

if exist('Fsi', 'var')
    spm_figure('Focus', Fsi)
end

% outputs
%--------------------------------------------------------------------------
Ep     = spm_unvec(spm_vec(pE) + V*C.p(ip),pE);
Cp     = V*C.Cp(ip,ip)*V';
Eh     = C.h;
F      = C.F;
L      = C.L;


function [g1, g2, g3] = sdlfmXsdrbfKernGradientBlockIGJ(lfmKern, rbfKern, ...
    t1, t2, i, j, generalConst, generalConstGrad, covGrad, typeMeanGrad)

% SDLFMXSDRBFKERNGRADIENTBLOCKIGJ
% FORMAT
% DESC computes the gradients of the parameters for output system and
% latent force system when i is greater than j.
% ARG lfmKern : structure containing parameters for the output system
% ARG rbfKern : structure containing parameters for the latent force system
% ARG t1 : times at which the output system is evaluated
% ARG t2 : times at which the LF system is evaluated
% ARG i : interval in which the output is evaluated
% ARG j : interval in which the latent force is being evaluated
% ARG i : interval to be evaluated for system 1
% ARG j : interval to be evaluated for system 2
% ARG generalConstant : constants evaluated with sdlfmKernComputeConstant.m
% ARG generalConstGrad : derivatives of the constants computed with
% sdlfmKernGradientConstant.m
% ARG covGrad : partial derivatives of the objective function wrt portion
% of the corresponding kernel matrix
% ARG typeMeanGrad : specify the mean functions used to compute this part
% of the kernel
%
% COPYRIGHT : Mauricio A. Alvarez, 2010.

% KERN

if nargin < 10
    typeMeanGrad = {'lfm', 'lfmv'};
end

fhandleMeanParam = str2func(['sd' typeMeanGrad{1} 'MeanCompute']);
fhandleMeanGradParam = str2func(['sd' typeMeanGrad{1} 'MeanGradient']);
fhandleMeanGradSwitching = str2func(['sd' typeMeanGrad{2} 'MeanCompute']);

c1 = fhandleMeanParam(lfmKern, t1, 'Pos');
e1 = fhandleMeanParam(lfmKern, t1, 'Vel');

if isempty(generalConst{i,j})
    coeffPosRbf = c1;
    coeffVelRbf = e1;
else
    coeffPosRbf = generalConst{i,j}(1,1)*c1 + generalConst{i,j}(2,1)*e1;
    coeffVelRbf = generalConst{i,j}(1,2)*c1 + generalConst{i,j}(2,2)*e1;
end

PosRbf = lfmXrbfKernCompute(lfmKern, rbfKern, rbfKern.limit, t2);
[g1Kern, g2Kern] = lfmXrbfKernGradient(lfmKern, rbfKern, ...
        rbfKern.limit, t2, covGrad.', coeffPosRbf);
VelRbf = lfmvXrbfKernCompute(lfmKern, rbfKern, rbfKern.limit, t2);
[g1KLocal, g2KLocal] = lfmvXrbfKernGradient(lfmKern, rbfKern, ...
        rbfKern.limit, t2, covGrad.', coeffVelRbf);
g1Kern = g1Kern + g1KLocal;
g2Kern = g2Kern + g2KLocal;

[gcAlpha, gcOmega] = fhandleMeanGradParam(lfmKern, t1, 'Pos');
[geAlpha, geOmega] = fhandleMeanGradParam(lfmKern, t1, 'Vel');
[gradAlpha, gradOmega] = getLocalGradAlphaOmega(lfmKern);

g1Pos = fhandleMeanGradSwitching(lfmKern, t1, 'Pos');
h1Vel = fhandleMeanGradSwitching(lfmKern, t1, 'Vel');

% This means that are only two switching points involved, t1
% and t0 appear in k, like k_{ff}(t1-t0,t'-t0) and
% k_{mf}(t1-t0,t'-t0). The other points appear in c1(t - t1)
% and e1(t - t1)
gsp1Kern = 0;
gsp2Kern = 0;

temp = lfmvXrbfKernCompute(lfmKern, rbfKern, rbfKern.limit, t2);
covPartial = sum(sum((coeffPosRbf*temp).*covGrad));
gsp2Kern = gsp2Kern - covPartial;
gsp1Kern = gsp1Kern + covPartial;
temp = lfmXrbfvKernCompute(lfmKern, rbfKern, rbfKern.limit, t2);
covPartial = sum(sum((coeffPosRbf*temp).*covGrad));
gsp2Kern = gsp2Kern - covPartial;
temp = lfmaXrbfKernCompute(lfmKern, rbfKern, rbfKern.limit, t2);
covPartial = sum(sum((coeffVelRbf*temp).*covGrad));
gsp2Kern = gsp2Kern - covPartial;
gsp1Kern = gsp1Kern + covPartial;
temp = lfmvXrbfvKernCompute(lfmKern, rbfKern, rbfKern.limit, t2);
gsp2Kern = gsp2Kern - sum(sum((coeffVelRbf*temp).*covGrad));

% Organize gradients accordingly
if isempty(generalConst{i,j})
    matGradAlpha = gcAlpha*PosRbf + geAlpha*VelRbf;
    matGradOmega = gcOmega*PosRbf + geOmega*VelRbf;
    gCoeff = gradAlpha*sum(sum(matGradAlpha.*covGrad)) + ...
        gradOmega*(sum(sum(matGradOmega.*covGrad)));
    matGradSp1 = g1Pos*PosRbf + h1Vel*VelRbf;
    gsp1 = - sum(sum(matGradSp1.*covGrad));
    g3(i) = sum(gsp1Kern) + gsp1; % switching point 1
    g3(j) = sum(gsp2Kern);        % switching point 2
else
    constGradAlpha = generalConstGrad{1}{i,j};
    constGradOmega = generalConstGrad{2}{i,j};
    constVal = generalConst{i,j};
    % Derivative wrt parameters
    matGradAlpha = (constVal(1,1)*gcAlpha + constGradAlpha(1,1)*c1 ...
        + constVal(2,1)*geAlpha + constGradAlpha(2,1)*e1)*PosRbf ...
        + (constVal(1,2)*gcAlpha + constGradAlpha(1,2)*c1 ...
        + constVal(2,2)*geAlpha + constGradAlpha(2,2)*e1)*VelRbf;
    matGradOmega = (constVal(1,1)*gcOmega + constGradOmega(1,1)*c1 ...
        + constVal(2,1)*geOmega + constGradOmega(2,1)*e1)*PosRbf ...
        + (constVal(1,2)*gcOmega + constGradOmega(1,2)*c1 ...
        + constVal(2,2)*geOmega + constGradOmega(2,2)*e1)*VelRbf;
    gCoeff = gradAlpha*sum(sum(matGradAlpha.*covGrad)) + ...
        gradOmega*(sum(sum(matGradOmega.*covGrad)));
    % Derivative wrt switching points
    % Firts, notice that gsp1Kern doesn't correspond to the switching point
    % of the interval i (or say 1). It's just the derivative of the inner
    % switching point that leads to the actual switching point for the
    % interval 1. So we rename it first.
    gspInt = sum(gsp1Kern);
    % Compute the derivative of the swicthing point i, not appearing in the
    % constant
    matGradSp1 = (constVal(1,1)*g1Pos + constVal(2,1)*h1Vel)*PosRbf + ...
        (constVal(1,2)*g1Pos + constVal(2,2)*h1Vel)*VelRbf;
    gsp1 = - sum(sum(matGradSp1.*covGrad));
    % Compute the derivatives for all the switching points appearing in the
    % constant. The order is descending, i.e., t_3_, t_2, t_1
    constGradSPoint = generalConstGrad{3}{i,j};
    numberSP = size(constGradSPoint,2);
    gspInBetween = zeros(1, numberSP);
    for k=1:numberSP
        temp = (constGradSPoint(1,k)*c1 + constGradSPoint(3,k)*e1)*PosRbf + ...
            (constGradSPoint(2,k)*c1 + constGradSPoint(4,k)*e1)*VelRbf;
        gspInBetween(k) = sum(sum(temp.*covGrad));
    end
    % Assign derivatives wrt all other switching points, with a correction
    % for the derivative of the innermost switching point in the constant
    gspInBetween(end) =  gspInBetween(end) + gspInt;
    g3(i) = gsp1 + gspInBetween(1);
    g3(j) = sum(gsp2Kern);
    g3(j+1:i-1) = fliplr(gspInBetween(2:end));
end
g1 = zeros(1,5);
g2 = zeros(1,2);
% Assign derivatives wrt first system
g1(1:3) = g1Kern(1:3) + gCoeff;          % mass, spring, damper
g1(4) = g1Kern(4);                            % inverse width
g1(5) = g1Kern(5);                            % sensitivity
% Assign derivatives wrt second system
g2(1) = g2Kern(1);                      % variance
g2(2) = g2Kern(2);                   % inverse widths


function CP = runICPRejection(p, g, CP)

msg('S', {g.procICP{:} 'REJECTION'}, 'LogLevel', 'basic');

for i = 1:size(p.PairList,1)

    CP{i} = CP{i}.rejection('Attribute', ...
                            'AttributeName', 'dAlpha', ...
                            'AttributeMinMax', [p.MaxDeltaAngle*10/9 400]); % conversion from degree to gradian!

    % Rejection based on dp
    med = median(CP{i}.dp);
    sig_mad = 1.4826*mad(CP{i}.dp,1);

    CP{i} = CP{i}.rejection('Attribute', ...
                            'AttributeName', 'dp', ...
                            'AttributeMinMax', [-Inf med-p.MaxSigmaMad*sig_mad]);

    CP{i} = CP{i}.rejection('Attribute', ...
                            'AttributeName', 'dp', ...
                            'AttributeMinMax', [med+p.MaxSigmaMad*sig_mad Inf]);

    % Rejection based on ds
    CP{i} = CP{i}.rejection('Attribute', ...
                            'AttributeName', 'ds', ...
                            'AttributeMinMax', [p.MaxDistance Inf]);

    % Rejection based on roughness
    CP{i} = CP{i}.rejection('Attribute', ...
                            'AttributeName', 'roughness', ...
                            'AttributeMinMax', [p.MaxRoughness Inf]);

end

msg('E', {g.procICP{:} 'REJECTION'}, 'LogLevel', 'basic');

end


function title =  p11_title ( option )

%*****************************************************************************80
%
%% P11_TITLE sets the title for problem 11.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%
%    Output, string TITLE, the title of the problem.
%
  title = 'Torsion of a square rod, finite element solution.';

  return
end


%{
 * Copyright (C) 2013-2020, The Regents of The University of Michigan.
 * All rights reserved.
 * This software was developed in the Biped Lab (https://www.biped.solutions/)
 * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
 * be available under alternative licensing terms; contact the address above.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * The views and conclusions contained in the software and documentation are those
 * of the authors and should not be interpreted as representing official policies,
 * either expressed or implied, of the Regents of The University of Michigan.
 *
 * AUTHOR: Bruce JK Huang (bjhuang[at]umich.edu)
 * WEBSITE: https://www.brucerobot.com/
%}

function cost = costIoU(theta_x, theta_y, theta_z, T, X, Y, intrinsic)
    H = eye(4);
    H(1:3,1:3) = rotx(theta_x) * roty(theta_y) * rotz(theta_z);
    H(1:3,4) = T';
    P = intrinsic * [eye(3) zeros(3,1)] * H;
    L_X_transformed = P * X; % transformed points in LiDAR frame
    C_X_transformed = L_X_transformed ./ L_X_transformed(3,:);
    cost = 0;
    for i=1:size(C_X_transformed,2)/4
        k = 4*(i-1) + 1;
        vertices_X = [C_X_transformed(1,k) C_X_transformed(1,k+1) C_X_transformed(1,k+3) C_X_transformed(1,k+2);
                      C_X_transformed(2,k) C_X_transformed(2,k+1) C_X_transformed(2,k+3) C_X_transformed(2,k+2)];
        vertices_Y = [Y(1,k) Y(1,k+1) Y(1,k+3) Y(1,k+2);
                      Y(2,k) Y(2,k+1) Y(2,k+3) Y(2,k+2)];
%         vertices_X = C_X_transformed(1:2,k:k+3);
%         vertices_Y = Y(1:2,k:k+3);

%         conv_X = convhull(vertices_X(1,:)', vertices_X(2,:)');
%         conv_Y = convhull(vertices_Y(1,:)', vertices_Y(2,:)');
%         [vertices_X(1,:), vertices_X(2,:)] = poly2cw(vertices_X(1,conv_X(1:4)), vertices_X(2,conv_X(1:4)));
%         [vertices_Y(1,:), vertices_Y(2,:)] = poly2cw(vertices_Y(1,conv_Y(1:4)), vertices_Y(2,conv_Y(1:4)));
%         pgon_X =  polyshape(vertices_X(1,conv_X), vertices_X(2,conv_X));
%         pgon_Y =  polyshape(vertices_Y(1,conv_Y), vertices_Y(2,conv_Y));

        [vertices_X(1,:), vertices_X(2,:)] = poly2cw(vertices_X(1, :), vertices_X(2, :));
        [vertices_Y(1,:), vertices_Y(2,:)] = poly2cw(vertices_Y(1, :), vertices_Y(2, :));

        pgon_X =  polyshape(vertices_X(1, :), vertices_X(2, :));
        pgon_Y =  polyshape(vertices_Y(1, :), vertices_Y(2, :));
        if all(issimplified([pgon_X, pgon_Y]))
            [I_polyout,~,~] = intersect(pgon_X, pgon_Y);

            [U_polyout,~,~] = union(pgon_X ,pgon_Y);
            I_area = max(area(I_polyout), 1e-5);
            U_area = area(U_polyout);
            cost = cost + I_area/U_area;
        else
            cost = -1e3;
        end
    end
    cost = -cost;
end


report_this_filefun(mfilename('fullpath'));

%l = isnan(tmap);
%tmap(l) = 1;

l = tmap < 100;
tmap(l) = 100;

[lat,lon] = meshgrat(tmap,tmapleg);
%[smap,smapleg] = country2mtx('switzerland',100);
%[lat0, lon0] = meshgrat(smap,smapleg);

% tmap = km2deg(tmap/1);
[X , Y]  = meshgrid(gx,gy);

%sw = interp2(lon0,lat0,smap,lon,lat);

re4 = re3;

ren = interp2(X,Y,re4,lon,lat);

mi = min(min(ren));
l =  isnan(ren);
ren(l) = mi-20;

figure_w_normalized_uicontrolunits('pos',[150 500 1000 700])

hold on; axis off
axesm('MapProjection','eqaconic','MapParallels',[],...
    'MapLatLimit',[33.75 35.03],'MapLonLimit',[-116.8 -115.9])

meshm(ren,tmapleg,size(tmap),tmap);

daspectm('m',4);
tightmap
%view([10 30])
hl = camlight ; lighting phong
set(gca,'projection','perspective');

aa = a;

j = jet;
%j = j(64:-1:1,:);
j = [ [ 0.85 0.9 0.9] ; j];
caxis([ min(min(re4)) max(max(re4)) ]);

colormap(j);
caxis([0.60 1.5]);
axis off; set(gcf,'color','w')
ax = axis;
axis([ax(1:4) 0 5000])

setm(gca,'ffacecolor','w')
setm(gca,'fedgecolor','k','flinewidth',3);

setm(gca,'mlabellocation',0.5)
setm(gca,'meridianlabel','on')
setm(gca,'plabellocation',0.5)
setm(gca,'parallellabel','on','Labelrotation','on')
setm(gca,'Fontcolor','k','Fontweight','bold','FontSize',14,'Labelunits','dm')

h5 = colorbar;
set(h5,'position',[0.65 .13 0.015 0.3],'TickDir','out','Ycolor','k','Xcolor','k',...
    'Fontweight','bold','FontSize',14,'Ticklength',[0.025 0.02]);
set(gcf,'Inverthardcopy','off');

a = aa;


function x = mtimes( a, x )
    %MINUS Multiplication of TT/MPS block tensor by scalar or vector
    %   X = MTIMES(A, X) multiplies the TT/MPS tensor X by A.
    %       If A is a scalar, all blocks are multiplied by the x.order-th root of A.
    %       if A is a vector of size X.p, all X.p slices of the supercore X.U{x.mu} are multiplied
    %       by the corresponding entry in A.
    %
    %   See also PLUS, MINUS.

    %   TTeMPS Toolbox.
    %   Michael Steinlechner, 2013-2016
    %   Questions and contact: michael.steinlechner@epfl.ch
    %   BSD 2-clause license, see LICENSE.txt

    %x.U{1} = a*x.U{1};

    if length(a) == 1
        c = a^(1/x.order);
        for i = 1:x.order
            x.U{i} = c*x.U{i};
        end
    elseif length(a) == x.p
        for i = 1:x.p
            x.U{x.mu}(:,:,:,i) = a(i) * x.U{x.mu}(:,:,:,i);
        end
    else
        error('Dimension mismatch! Can only multiply block tensor X by scalar (whole tensor) or by a vector of length X.p')
    end
end


classdef nme_branch_acc < mp.nme_branch_ac & mp.form_acc

%   MATPOWER
%   Copyright (c) 2018-2022, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%   and Baljinnyam Sereeter, Delft University of Technology
%
%   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.

    methods
        function [h, dh] = ang_diff_fcn(obj, xx, Aang, lang, uang)
            if nargout > 1
                [va, dva] = obj.va_fcn(xx, [], 0);
                Adva = Aang * dva;
                dh = [ -Adva; Adva ];
            else
                va = obj.va_fcn(xx, [], 0);
            end
            Ax = Aang * va;
            h = [ lang - Ax;
                  Ax - uang ];
        end

        function d2H = ang_diff_hess(obj, xx, lambda, Aang)
            %% unpack data
            [vr, vi] = deal(xx{:});
            nn = length(vr);

            %% evaluate Hessian of voltage magnitude^2 function
            n = length(lambda) / 2;
            if n
                lam = lambda(n+1:2*n) - lambda(1:n);    %% lam_ub - lam_lb
            else
                lam = zeros(0,1);
            end

            d2H = obj.va_hess(xx, Aang' * lam, []);
        end
    end     %% methods
end         %% classdef