function  Raw = simObservation(SimRob, SimSen, SimLmk, Opt)

% SIMOBSERVATION Observe simulated landmarks.
%   RAW = SIMOBSERVATION(SIMROB,SIMSEN,SIMLMK) returns the raw data
%   captured for the sensor.
%       SIMROB: simulated robot structure
%       SIMSEN: simulated sensor strucure
%       SIMLMK: simulated landmarks strucure
%
%   See also SIMMOTION PROJEUCPNTINTOPINHOLEONROB

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

Raw.type = 'simu';

switch SimSen.type

    case {'pinHole'}      % camera pinHole

        % Project virtual world's points
        [Raw.data.points.coord, s] = projEucPntIntoPinHoleOnRob(...
            SimRob.frame, ...
            SimSen.frame, ...
            SimSen.par.k, ...
            SimSen.par.d, ...
            SimLmk.points.coord);
        Raw.data.points.app  = SimLmk.points.id;

        % Add sensor noise
        Raw.data.points.coord = Raw.data.points.coord + ...
            SimSen.par.pixErr*randn(size(Raw.data.points.coord));

        % Remove non visible
        vis = isVisible(Raw.data.points.coord,s,SimSen.par.imSize);
        Raw.data.points.coord(:, ~vis)  = [];
        Raw.data.points.app(~vis) = [];

        % Project virtual world's segments
        [Raw.data.segments.coord, s] = projSegLinIntoPinHoleOnRob(...
            SimRob.frame, ...
            SimSen.frame, ...
            SimSen.par.k, ...
            SimLmk.segments.coord);
        Raw.data.segments.app  = SimLmk.segments.id;

        % Add sensor noise
        Raw.data.segments.coord = Raw.data.segments.coord + ...
            SimSen.par.cov*randn(size(Raw.data.points.coord));

        % Remove non visible
        [Raw.data.segments.coord,vis] = visibleSegment( ...
            Raw.data.segments.coord,...
            s,...
            SimSen.par.imSize,...
            0,...                      % N pixels margin
            Opt.obs.lines.minLength);  % min segment length
        Raw.data.segments.coord(:, ~vis)  = [];
        Raw.data.segments.app(~vis) = [];

    case {'pinHoleDepth'}      % camera pinHole

        % Project virtual world's points
        Raw.data.points.coord = projEucPntIntoPhdOnRob(...
            SimRob.frame, ...
            SimSen.frame, ...
            SimSen.par.k, ...
            SimSen.par.d, ...
            SimLmk.points.coord);
        Raw.data.points.app  = SimLmk.points.id;

        % Add sensor noise
        Raw.data.points.coord = Raw.data.points.coord + ...
            SimSen.par.cov*randn(size(Raw.data.points.coord));

        % Remove non visible
        vis = isVisible(Raw.data.points.coord(1:2,:),Raw.data.points.coord(3,:),SimSen.par.imSize);
        Raw.data.points.coord(:, ~vis)  = [];
        Raw.data.points.app(~vis) = [];

        % TODO: Lines not implemented for sensor type 'pihHoleDepth'

        %         % Project virtual world's segments
        Raw.data.segments.coord = [];
        %         [Raw.data.segments.coord, s] = projSegLinIntoPinHoleOnRob(...
        %             SimRob.frame, ...
        %             SimSen.frame, ...
        %             SimSen.par.k, ...
        %             SimLmk.segments.coord);
        %         Raw.data.segments.app  = SimLmk.segments.id;
        Raw.data.segments.app = [];
        %
        %         % Add sensor noise
        %         Raw.data.segments.coord = Raw.data.segments.coord + ...
        %             SimSen.par.pixErr*randn(size(Raw.data.segments.coord));
        %
        %         % Remove non visible
        %         [Raw.data.segments.coord,vis] = visibleSegment( ...
        %             Raw.data.segments.coord,...
        %             s,...
        %             SimSen.par.imSize,...
        %             0,...                      % N pixels margin
        %             Opt.obs.lines.minLength);  % min segment length
        %         Raw.data.segments.coord(:, ~vis)  = [];
        %         Raw.data.segments.app(~vis) = [];

    case {'omniCam'}      % Omnidirectional camera

        % Project virtual world's points
        [Raw.data.points.coord, s] = projEucPntIntoOmniCamOnRob(...
            SimRob.frame, ...
            SimSen.frame, ...
            SimSen.par.k, ...
            SimSen.par.d, ...
            SimLmk.points.coord);
        Raw.data.points.app  = SimLmk.points.id;

        % Add sensor noise
        Raw.data.points.coord = Raw.data.points.coord + ...
            SimSen.par.pixErr*randn(size(Raw.data.points.coord));

        % Remove non visible
        vis = isVisible(Raw.data.points.coord,s,SimSen.par.imSize);
        Raw.data.points.coord(:, ~vis)  = [];
        Raw.data.points.app(~vis) = [];

    otherwise
        % Print an error and exit
        error('??? Unknown sensor type ''%s''.',SimSen.type);

end % end of the "switch" on sensor type

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


function [J,Msp,Nsp]=mtfftpt(data,tapers,nfft,t,f,findx)
% Multi-taper fourier transform for point process given as times
%
% Usage:
% [J,Msp,Nsp]=mtfftpt (data,tapers,nfft,t,f,findx) - all arguments required
% Input:
%       data        (struct array of times with dimension channels/trials;
%                   also takes in 1d array of spike times as a column vector)
%       tapers      (precalculated tapers from dpss)
%       nfft        (length of padded data)
%       t           (time points at which tapers are calculated)
%       f           (frequencies of evaluation)
%       findx       (index corresponding to frequencies f)
% Output:
%       J (fft in form frequency index x taper index x channels/trials)
%       Msp (number of spikes per sample in each channel)
%       Nsp (number of spikes in each channel)
if nargin < 6; error('Need all input arguments'); end;
if isstruct(data); C=length(data); else C=1; end% number of channels
K=size(tapers,2); % number of tapers
nfreq=length(f); % number of frequencies
if nfreq~=length(findx); error('frequency information (last two arguments) inconsistent'); end;
H=fft(tapers,nfft,1);  % fft of tapers
H=H(findx,:); % restrict fft of tapers to required frequencies
w=2*pi*f; % angular frequencies at which ft is to be evaluated
Nsp=zeros(1,C); Msp=zeros(1,C);
for ch=1:C;
  if isstruct(data);
     fnames=fieldnames(data);
     eval(['dtmp=data(ch).' fnames{1} ';'])
     indx=find(dtmp>=min(t)&dtmp<=max(t));
     if ~isempty(indx); dtmp=dtmp(indx);
     end;
  else
     dtmp=data;
     indx=find(dtmp>=min(t)&dtmp<=max(t));
     if ~isempty(indx); dtmp=dtmp(indx);
     end;
  end;
  Nsp(ch)=length(dtmp);
  Msp(ch)=Nsp(ch)/length(t);
  if Msp(ch)~=0;
      data_proj=interp1(t',tapers,dtmp);
      exponential=exp(-i*w'*(dtmp-t(1))');
      J(:,:,ch)=exponential*data_proj-H*Msp(ch);
  else
      J(1:nfreq,1:K,ch)=0;
  end;
end;


function filter = helperInitIMMFilter(detection)
% This is a helper function and may be removed in a future release.
% This function initializes an IMM filter for tracking using lidar data.

% Copyright 2019 The MathWorks, Inc.

% Create individual filters for constant turn rate and constant velocity.
trackingfilters = cell(2,1);
trackingfilters{1} = initCTCuboidFilter(detection);
trackingfilters{2} = initCVCuboidFilter(detection);

% Construct IMM and inform it about switching between models.
filter = trackingIMM(trackingfilters,'ModelConversionFcn',@switchimmCuboid);
filter.MeasurementNoise = detection.MeasurementNoise;
filter.TransitionProbabilities = 0.99;

end

function x = switchimmCuboid(modelType1,x1,modelType2,x2)
% This function simply removes helper and Cuboid from the model names and
% uses the switchimm function for switching kinematic state. It appends the
% non-kinematic part after the switch.
modelType1 = strrep(modelType1,'Cuboid','');
modelType1 = strrep(modelType1,'helper','');
modelType2 = strrep(modelType2,'Cuboid','');
modelType2 = strrep(modelType2,'helper','');

isCov = ~isvector(x1);
n1 = size(x1,1);
n2 = size(x2,1);
if ~isCov
    kinematicPartX1 = x1(1:(n1-4));
    shapePartX1 = x1((n1-3):n1);
    kinematicPartX2 = x2(1:(n2-4));
else
    kinematicPartX1 = x1(1:(n1-4),1:(n1-4));
    shapePartX1 = x1((n1-3):n1,(n1-3):n1);
    kinematicPartX2 = x1(1:(n2-4),1:(n2-4));
end

kinematicPartX = switchimm(modelType1,kinematicPartX1,modelType2,kinematicPartX2);

if isCov
    x = blkdiag(kinematicPartX,shapePartX1);
else
    x = [kinematicPartX;shapePartX1];
end

end

function filter = initCVCuboidFilter(detection)
    % This function initializes a constant-velocity cuboid filter from a
    % detection report.
    % detection contains measurement with the following convention:
    % [x;y;z;l;w;h];

    posIndex = [1 3 5];
    dimIndex = [8 9 10];
    yawIndex = 7;

    meas = detection.Measurement;
    measCov = detection.MeasurementNoise;
    dataType = class(meas);
    [pos,posCov,dim,dimCov,yaw,yawCov] = helperInverseLidarModel(meas,measCov);

    % Assemble state and state covariances
    state = zeros(10,1,dataType);
    state(posIndex) = pos;
    state(dimIndex) = dim;
    state(yawIndex) = yaw;
    cov = 100*eye(10,dataType);
    cov(posIndex,posIndex) = posCov;
    cov(dimIndex,dimIndex) = dimCov;
    cov(yawIndex,yawIndex) = yawCov;

    % processNoise. Acceleration and omega
    Q = eye(4);
    Q(4,4) = 2.5;

    % Use a UKF for capture non-linearity.
    filter = trackingUKF(@helperConstvelCuboid,@helperCvmeasCuboid,state,...
         'StateCovariance',cov,...
        'HasAdditiveProcessNoise',false,...
        'ProcessNoise',Q,'MeasurementNoise',detection.MeasurementNoise,...
        'Alpha',0.01);

    setMeasurementSizes(filter,6,6);
end

function filter = initCTCuboidFilter(detection)
    % This function initializes a constant-turn rate cuboid filter from a
    % detection report.
    % detection contains measurement with the following convention:
    % [x;y;z;l;w;h];

    posIndex = [1 3 6];
    dimIndex = [9 10 11];
    yawIndex = 8;

    meas = detection.Measurement;
    measCov = detection.MeasurementNoise;
    dataType = class(meas);
    [pos,posCov,dim,dimCov,yaw,yawCov] = helperInverseLidarModel(meas,measCov);

    % Assemble state and state covariances
    state = zeros(11,1,dataType);
    state(posIndex) = pos;
    state(dimIndex) = dim;
    state(yawIndex) = yaw;
    cov = 100*eye(11,dataType);
    cov(posIndex,posIndex) = posCov;
    cov(dimIndex,dimIndex) = dimCov;
    cov(yawIndex,yawIndex) = yawCov;

    % processNoise
    Q = eye(4);
    Q(4,4) = 25;

    % Use a UKF for capture non-linearity.
    filter = trackingUKF(@helperConstturnCuboid,@helperCtmeasCuboid,state,...
        'StateCovariance',cov,...
        'HasAdditiveProcessNoise',false,...
        'ProcessNoise',Q,...
        'MeasurementNoise',detection.MeasurementNoise,...
        'Alpha',0.01);

    setMeasurementSizes(filter,6,6);
end


function r = mrdivide(a,b)
%MRDIVIDE     Implements  a / b  for gradient
%

% written  10/16/98     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    complete redesign
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if prod(size(b))~=1
    error('Gradient division only for scalar denominator')
  end

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  if ~isa(a,'gradient')          % non-gradient / gradient scalar
    r.x = a / b.x;
    n = prod(size(a));
    if n==1                     % non-gradient scalar / gradient scalar
      D = -a / sqr(b.x);
      r.dx = b.dx * D;
    else                        % non-gradient array / gradient scalar
      D = 1 / sqr(b.x);
      if issparse(b.dx)
        ax = sparse(-a(:));
      else
        ax = -a(:);
      end
      r.dx = ax * ( b.dx * D );
    end
  elseif ~isa(b,'gradient')      % gradient / non-gradient scalar
    r.x = a.x / b;
    r.dx = a.dx / b;
  else                          % gradient / gradient scalar
    r.x = a.x / b.x;
    D = 1 / sqr(b.x);
    n = prod(size(a.x));
    if n==1                     % gradient scalar / gradient scalar
      Num = a.dx * b.x - a.x * b.dx;
      r.dx = Num * D;
    else                        % gradient array / gradient scalar
      if issparse(a.dx)
        ax = sparse(a.x(:));
      else
        ax = a.x(:);
      end
      Num = a.dx * b.x - ax * b.dx;
      r.dx = Num * D;
    end
  end

  r = class(r,'gradient');

  if rndold
    setround(rndold)
  end


function spy_ge ( m, n, a, header )

%*****************************************************************************80
%
%% SPY_GE plots a sparsity pattern for a general storage (GE) matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns
%    in the matrix.
%
%    Input, real A(M,N), the matrix.
%
%    Input, string HEADER, the name to be used for the
%    title of the plot, and as part of the names of the data, command
%    and plot files.
%

%
%  Create data file.
%
  data_filename = strcat ( header, '_data.txt' );
  data_unit = fopen ( data_filename, 'wt' );
  nz_num = 0;
  for j = 1 : n
    for i = 1 : m
      if ( a(i,j) ~= 0.0 )
        fprintf ( data_unit, '%d  %d\n', j, i );
        nz_num = nz_num + 1;
      end
    end
  end
  fclose ( data_unit );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Created sparsity data file "%s".\n', data_filename );
%
%  Create command file.
%
  command_filename = strcat ( header, '_commands.txt' );
  command_unit = fopen ( command_filename, 'wt' );

  fprintf ( command_unit, '# %s\n', command_filename );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, '# Usage:\n' );
  fprintf ( command_unit, '#  gnuplot < %s\n', command_filename );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, 'unset key\n' );
  fprintf ( command_unit, 'set term png\n' );

  png_filename = strcat ( header, '.png' );
  fprintf ( command_unit, 'set output "%s"\n', png_filename );
  fprintf ( command_unit, 'set size ratio -1\n' );
  fprintf ( command_unit, 'set xlabel "<--- J --->"\n' );
  fprintf ( command_unit, 'set ylabel "<--- I --->"\n' );
  fprintf ( command_unit, 'set title "%d nonzeros for ''%s''"\n', nz_num, header );
  fprintf ( command_unit, 'set timestamp\n' );
  fprintf ( command_unit, 'plot [y=1:%d] [x=%d:1] "%s" with points pt 5\n', ...
    n, m, data_filename );

  fclose ( command_unit );
  fprintf ( 1, '  Created graphics command file "%s".\n', command_filename );

  return
end


function roomhyp = sample_roomtype7(n, lines, vp, imgwidth, imgheight)
% sample n room hyps
% 3 ways to create hypothesis
% recipe1: 1,right 2,top, junc2type,2
% recipe2: 1,right 3,top,right, junc2type,1
% recipe3: 2,top 3,top,right, junc2type,4

lc = [lines.lineclass]; % 0,1,2,3
lr = [lines.leftorright]; % 1 if right, -1 if left, 0 if neither
tb = [lines.above_horizon]; % 1 if above, -1 if below, 0 if neither

oneright = find((lc==1) & (lr==1));
twotop = find((lc==2) & (tb==1));
threetopright = find((lc==3) & (tb==1) & (lr==1));

linecode{1} = oneright;
linecode{2} = twotop;
linecode{3} = threetopright;

% recipe: [linecode1 linecode2 junc2type]
recipe(1,:) = [1 2 2];
recipe(2,:) = [1 3 1];
recipe(3,:) = [2 3 4];

%
% for i = 1:length(linecode),
%     num_linecode(i) = length(linecode{i});
% end
if (length(linecode{1})<1 || length(linecode{2})<1) && ...
   (length(linecode{1})<1 || length(linecode{3})<1) && ...
   (length(linecode{2})<1 || length(linecode{3})<1)
    roomhyp = [];
    return; % fail
end

count = 0;
num_continue_without_progress = 0;
while count < n && num_continue_without_progress < 30
    use_recipe_num = randsample(size(recipe,1), 1); % 1, 2, or 3
    cur_recipe = recipe(use_recipe_num,:);

    l1 = linecode{cur_recipe(1)};
    l2 = linecode{cur_recipe(2)};
    if length(l1)<1 || length(l2)<1
        num_continue_without_progress = num_continue_without_progress + 1;
        continue;
    end

    ls1 = l1(randsample(length(l1),1));
    ls2 = l2(randsample(length(l2),1));
    [cornerpt degen] = line_intersect(...
                        lines(ls1).point1, lines(ls1).point2, ...
                        lines(ls2).point1, lines(ls2).point2);
    if degen==1
        num_continue_without_progress = num_continue_without_progress + 1;
        continue;
    end
    type = getjunctype(cornerpt, lines(ls1), lines(ls2));
    if type ~= cur_recipe(3)
        num_continue_without_progress = num_continue_without_progress + 1;
        continue;
    end

    % check if inside img
    MARGIN = 5;
    if ~is_in_image(cornerpt, imgwidth, imgheight, MARGIN)
        num_continue_without_progress = num_continue_without_progress + 1;
        continue;
    end

    % found a valid way to generate a room hyp
    count = count+1;
    num_continue_without_progress = 0;
    roomhyp(count).corner(4).pt = []; % up to 4 corners
    roomhyp(count).corner(2).pt = cornerpt;
    roomhyp(count).type = 7;

%     global img;
%     disp_vanish(img, lines([ls1 ls2]), vp);
%     plot(cornerpt(1), cornerpt(2), 'x');
%     pause;
%     close;
end

if ~exist('roomhyp','var')
    roomhyp = [];
end


function s = ymdf_to_s_english ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_S_ENGLISH writes an English YMDF date into a string.
%
%  Format:
%
%    BC OS YYYY/MM/DD.FF
%    AD OS YYYY/MM/DD.FF
%    AD NS YYYY/MM/DD.FF
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, string S, a representation of the date.
%
  if ( y < 0 )
    s = sprintf ( 'BC OS %d/%02d/%05.2f', -y, m, d + f );
  elseif ( y < 1752 || ( y == 1752 && m < 9 ) || ( y == 1752 && m == 9 && d < 3 ) )
    s = sprintf ( 'AD OS %d/%02d/%05.2f', y, m, d + f );
  else
    s = sprintf ( 'AD NS %d/%02d/%05.2f', y, m, d + f );
  end

  return
end


function [dtf, dtfvar, n] = ft_connectivity_dtf(inputdata, varargin)

% FT_CONNECTIVITY_DTF computes the directed transfer function.
%
% Use as
%   [d, v, n] = ft_connectivity_dtf(inputdata, ...)
%
% The input should be a spectral transfer matrix organized as
%   Nrpt x Nchan x Nchan x Nfreq (x Ntime)
% where Nrpt can be 1.
%
% The output represents
%   d = partial directed coherence matrix Nchan x Nchan x Nfreq (x Ntime).
%       If multiple observations in the input, the average is returned.
%   v = variance of d across observations.
%   n = number of observations.
%
% Typically, nrpt should be 1 where the spectral transfer matrix is computed across
% observations. When nrpt>1 and hasjack=true, the input is assumed to contain the
% leave-one-out estimates of the spectral transfer matrix, thus a more reliable
% estimate of the relevant quantities.
%
% Additional optional input arguments come as key-value pairs:
%   'hasjack'   = boolean, specifying whether the input contains leave-one-outs,
%                 required for correct variance estimate (default = false)
%   'crsspctrm' = matrix containing the cross-spectral density. If this
%                 matrix is defined, the function returns the ddtf, which
%                 requires an estimation of partial coherence from this matrix.
%   'invfun'    = 'inv' (default) or 'pinv', the function used to invert the
%                 crsspctrm matrix to obtain the partial coherence. Pinv is
%                 useful if the data are poorly-conditioned.
%   'feedback'  = 'none', 'text', 'textbar', 'dial', 'etf', 'gui' type of feedback showing progress of computation, see FT_PROGRESS
%
% See also CONNECTIVITY, FT_CONNECTIVITYANALYSIS

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

hasjack   = ft_getopt(varargin, 'hasjack', 0);
powindx   = ft_getopt(varargin, 'powindx');
feedback  = ft_getopt(varargin, 'feedback', 'none');
crsspctrm = ft_getopt(varargin, 'crsspctrm');
invfun    = ft_getopt(varargin, 'invfun', 'inv');

switch invfun
  case {'inv' 'pinv'}
    invfun = str2func(invfun);
  otherwise
    ft_error('unknown specification of inverse function for the transfer matrix');
end

if ~isempty(powindx)
  % this error message is rather uninformative, but is kept for now for
  % backward compatibility reasons (i.e. it might exist when called from
  % ft_connectivityanalysis
  ft_error('linearly indexed data for DTF computation is at the moment not supported');
end

siz    = [size(inputdata) 1];
n      = siz(1);
outsum = zeros(siz(2:end));
outssq = zeros(siz(2:end));

% check the crsspctrm; if it is present, compute the partial coherence
if ~isempty(crsspctrm)
  assert(isequal(size(crsspctrm),size(inputdata)), 'the input data should be of the same size as the crsspctrm');
  fprintf('computing dDTF in the presence of a crsspctrm\n');

  % the crsspctrm allows for the partial coherence to be computed
  pdim   = prod(siz(4:end));
  tmpcrs = reshape(crsspctrm, [siz(1:3) pdim]);
  ft_progress('init', feedback, 'computing partial coherence...');
  for k = 1:n
    ft_progress(k/n, 'computing partial coherence for replicate %d from %d\n', k, n);
    tmp = reshape(tmpcrs(k,:,:,:), [siz(2:3) pdim]);
    for m = 1:pdim
      tmp(:,:,m) = invfun(tmp(:,:,m));
      tmp(:,:,m) = abs(tmp(:,:,m))./sqrt(abs(diag(tmp(:,:,m))*diag(tmp(:,:,m))'));
    end
    tmpcrs(k,:,:,:) = tmp;
  end
  ft_progress('close');
  crsspctrm = reshape(tmpcrs, siz);
end

% data should be represented as chan_chan_therest
for j = 1:n
  tmph   = reshape(inputdata(j,:,:,:,:), siz(2:end));
  if isempty(crsspctrm)
    % plain DTF
    den    = sum(abs(tmph).^2,2);
    tmpdtf = abs(tmph)./sqrt(repmat(den, [1 siz(2) 1 1 1]));
  else
    % dDTF
    tmpc   = reshape(crsspctrm(j,:,:,:,:), siz(2:end));
    den    = sum(sum(abs(tmph).^2,3),2);
    tmpdtf = abs(tmph)./sqrt(repmat(den, [1 siz(2) siz(4) 1 1 1]));
    tmpdtf = tmpdtf.*tmpc;
  end
  outsum = outsum + tmpdtf;
  outssq = outssq + tmpdtf.^2;
end
dtf = outsum./n;

if n>1 % FIXME this is strictly only true for jackknife, otherwise other bias is needed
  if hasjack
    bias = (n - 1).^2;
  else
    bias = 1;
  end
  dtfvar = bias.*(outssq - (outsum.^2)/n)./(n-1);
else
  dtfvar = [];
end

% this is to achieve the same convention for all connectivity metrics:
% row -> column
for k = 1:prod(siz(4:end))
  dtf(:,:,k)    = transpose(dtf(:,:,k));
  if ~isempty(dtfvar)
    dtfvar(:,:,k) = transpose(dtfvar(:,:,k));
  end
end


%  Figure 9.54      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
function y = fas(u)
if u < 0.0606,
   y = 0.1 ;
elseif u < 0.0741,
   y = 0.1 + (u-0.0606)*20;
else y = 0.9;
end


function RV = fmri_vrestriction(nH, nC, AC, CC, nHTest)
%
% RV = fmri_vrestriction(nH, nC, AC, CC, nHTest)
%
% Creates a restriction vector of dimension 1 X nC*nH
%
% nH - total number of elements in HDIR
% nC - total number of stimulus conditions (including fixation)
% AC - list of active conditions
% CC - list of control conditions
% nHTest - list of HDIR components to test (default: all).
%
% Eg: fmri_vrestriction(10, 6, [1 3], [2 5], [3 6:9])
% Generates an RV for testing conditions (1+3)-(2+5) using
% components 3,6,7,8, and 9 in the HDIR.  Conditions 0 and 4
% are not tested. There are 10 components in the HDIR, and 5
% stimulus conditions.
%
%
% $Id: fmri_vrestriction.m,v 1.1 2004/03/12 07:31:56 sayres Exp $

% Check for the correct number of arguments %
if(nargin ~= 4 & nargin ~= 5)
  error('USAGE: fmri_vrestriction(nH, nC, AC, CC, <nHTest>');
  return;
end

%% check that the input parameters are correct %%
if(nH < 1)
  msg = 'nH must be greater than 0';
  qoe(msg); error(msg);
end
if(nC < 1)
  msg = 'Number of conditions must be greater than 0';
  qoe(msg); error(msg);
end
if(length(find(AC<0 | AC > nC-1)))
  msg = 'Invalid condition number in AC';
  qoe(msg); error(msg);
end
if(length(find(CC<0 | CC > nC-1)))
  msg = 'Invalid condition number in CC';
  qoe(msg); error(msg);
end

% Test that the same condition is not in both AC and CC %
C = [AC CC];
for n = 1:nC,
  if(length(find(C==n)) > 1)
    msg = 'Same condition is in both AC and CC';
    qoe(msg); error(msg);
  end
end

% Set defaults for nHTest, if needed, else test range %
if(nargin == 4) nHTest = [1:nH];
else
  if(length(find(nHTest<1 | nHTest > nH)))
    msg = 'Invalid nHTest Component';
    qoe(msg); error(msg);
  end
end

% Strip Condition 0 from AC (if there) %
iACnz = find(AC ~= 0);
if( ~ isempty(iACnz) )
  AC = AC(iACnz);
end
% Strip Condition 0 from CC (if there) %
iCCnz = find(CC ~= 0);
if( ~ isempty(iCCnz) )
  CC = CC(iCCnz);
end

%% -------- Generate the restriction matrix -------- %%
RV = zeros(nC-1,nH); % -1 excludes fixation
if( ~ isempty(iACnz) )
  RV(AC,nHTest) =  ones(length(AC),length(nHTest));
end
if( ~ isempty(iCCnz) )
  RV(CC,nHTest) = -ones(length(CC),length(nHTest));
end
RV = reshape(RV', [1 prod(size(RV))]);

return;


%io_loadjmrui.m
%Jamie Near, McGill University 2014.
%
% USAGE:
% out=io_loadjmrui(filename);
%
% DESCRIPTION:
% Load a jMRUI text file into matlab structure format.
%
% INPUTS:
% filename   = filename of the jMRUI txt file.
%
% OUTPUTS:
% out = Input dataset in FID-A structure format.

function out=io_loadjmrui(filename);

%LOAD IN JMRUI .txt FILE
[RF,info]=io_readjmrui(filename);

txfrq=str2num(info.TransmitterFrequency);
sz=[str2num(info.PointsInDataset) 1];
dwelltime=str2num(info.SamplingInterval)*1e-3;
spectralwidth=1/dwelltime;
date=str2num(info.DateOfExperiment);
Bo=txfrq/42577000;  %JMRUI HEADER INCORRECTLY SAYS 3.0

t=[0:dwelltime:sz(1)*dwelltime-dwelltime];

dims.t=1;
dims.coils=0;
dims.averages=0;
dims.subSpecs=0;
dims.extras=0;

fids=RF(:,1);

specs=fftshift(ifft(fids,[],dims.t),dims.t);

f=[(-spectralwidth/2)+(spectralwidth/(2*sz(1))):spectralwidth/(sz(1)):(spectralwidth/2)-(spectralwidth/(2*sz(1)))];
ppm=-f/(Bo*42.577);
ppm=ppm+4.65;

out.fids=fids;
out.specs=specs;
out.sz=sz;
out.ppm=ppm;
out.t=t;
out.spectralwidth=spectralwidth;
out.dwelltime=dwelltime;
out.txfrq=txfrq;
out.date=date;
out.dims=dims;
out.Bo=Bo;
out.seq='';
out.te=[];
out.tr=[];
out.pointsToLeftshift=0;

%Write flags
out.flags.writtentostruct=1;
out.flags.gotparams=1;
out.flags.leftshifted=0;
out.flags.filtered=0;
out.flags.zeropadded=0;
out.flags.freqcorrected=0;
out.flags.phasecorrected=0;
out.flags.averaged=1;
out.flags.addedrcvrs=1;
out.flags.writtentotext=0;
out.flags.downsampled=0;
out.flags.isFourSteps=0;
out.flags.subtracted=1;


function w=pdeleft(u)
% PDELEFT W=PDELEFT(U) is the nonlinear residual of the left
%         preconditioned pde example with C=20.
%
global rhsf prhsf
%
% Compute the low-order nonlinear term.
%
v=20*u.*(dxmf(u)+dymf(u));
%
% Apply fish2d to the entire pde.
%
w=u+fish2d(v)-prhsf;


function Az = ComputeAz(meta, percent, pixel_coords)
% COMPUTAZ Compute azimuth angle (angle from north) across spatial frequency
%
% Should work generically for many types of SAR complex data, not just
% spotlight collects.
%
% INPUTS:
%   meta         - required: SICD metadata structure
%   percent      - optional: Array of values from 0 to 100.  Indicates the
%                  fraction across the spatial frequency in azimuth for
%                  which to compute azimuth angle (not including zeropad).
%                  Default is scene center point center of aperture.
%   pixel_coords - optional: [column_index, row_index] (az,rng) index to
%                  point of interest.  Default is scene center point (SCP).
%
% OUTPUTS:
%   Az - computed azimuth angle for each PERCENT value
%
% Author: Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

%% Handle default values for input parameters
% Default for percent is just SCPCOA azimuth
% If percent is not passed, we might not need TimeCOAPoly, so we check this first.
if ~exist('percent','var')
    % SCPCOA.AzimAng is an actual field in latest SICD spec
    if isfield(meta,'SCPCOA') && isfield(meta.SCPCOA,'AzimAng')
        Az = meta.SCPCOA.AzimAng;
        return;
    end
    percent = 50; % If SCPCOA.AzimAng not available, we will compute it.
end
if ~isfield(meta.Grid,'TimeCOAPoly') % TimeCOAPoly field is required
    % For spotlight, we can take some shortcuts with missing metadata
    if isfield(meta,'CollectionInfo') &&...
            isfield(meta.CollectionInfo,'RadarMode')&&...
            isfield(meta.CollectionInfo.RadarMode,'ModeType')&&...
            strcmpi(meta.CollectionInfo.RadarMode.ModeType,'SPOTLIGHT')
        if isfield(meta,'SCPCOA') && isfield(meta.SCPCOA,'SCPTime') &&...
                isfield(meta,'Position') && isfield(meta.Position,'ARPPoly')
            meta.Grid.TimeCOAPoly = meta.SCPCOA.SCPTime;
        elseif isfield(meta,'SCPCOA') && isfield(meta.SCPCOA,'ARPPos') &&...
                isfield(meta.SCPCOA,'ARPVel')
            meta.Grid.TimeCOAPoly = 0;
            meta.Position.ARPPoly.X = [meta.SCPCOA.ARPPos.X; meta.SCPCOA.ARPVel.X];
            meta.Position.ARPPoly.Y = [meta.SCPCOA.ARPPos.Y; meta.SCPCOA.ARPVel.Y];
            meta.Position.ARPPoly.Z = [meta.SCPCOA.ARPPos.Z; meta.SCPCOA.ARPVel.Z];
        end
    end
    if ~isfield(meta.Grid,'TimeCOAPoly')
        error('COMPUTEAZ:TIMECOAPOLY_MISSING','Unable to compute SICD Grid.TimeCOAPoly field from complex data.');
    end
end
% Point of interest (POI) only required for spatially variant COA
if exist('pixel_coords','var') && ~isempty(pixel_coords)
    % Convention for point_slant_to_ground() pixel coordinates is reverse
    % of what is passed to this function (column/row), so we have to swap
    % values.
    poi_ecf = geodetic_to_ecf(point_slant_to_ground([pixel_coords(2); pixel_coords(1)], meta)).';
    % Calculate center of aperture (COA) for given point
    time_coa = sicd_polyval2d(meta.Grid.TimeCOAPoly,pixel_coords(1),pixel_coords(2),meta);
else % Default to SCP
    poi_ecf = [meta.GeoData.SCP.ECF.X, meta.GeoData.SCP.ECF.Y, meta.GeoData.SCP.ECF.Z];
    time_coa = meta.Grid.TimeCOAPoly(1); % SCP COA time is this by definition
    if any(meta.Grid.TimeCOAPoly(2:end)~=0) % Spatially variant COA
        warning('COMPUTEAZ:PIXEL_COORDS_REQUIRED','For non-spotlight data, point of interest must be specified for accurate azimuth angles.');
    end
end

%% Calculate geometry info for center of aperture
pos_coefs=[meta.Position.ARPPoly.X(end:-1:1)...
    meta.Position.ARPPoly.Y(end:-1:1)...
    meta.Position.ARPPoly.Z(end:-1:1)]; % Position polynomial
ARP=[polyval(pos_coefs(:,1),time_coa)...
    polyval(pos_coefs(:,2),time_coa)...
    polyval(pos_coefs(:,3),time_coa)]; % Aperture position at COA
% Velocity polynomial is derivative of position polynomial
vel_coefs=pos_coefs(1:end-1,:).*repmat(((size(pos_coefs,1)-1):-1:1)',[1 3]);
ARV=[polyval(vel_coefs(:,1), time_coa)...
    polyval(vel_coefs(:,2), time_coa)...
    polyval(vel_coefs(:,3), time_coa)]; % Aperture velocity at COA
LOS = poi_ecf - ARP; % Line of site vector at COA
R = norm(LOS); % Range from sensor to POI at COA
V = norm(ARV); % Speed at COA
DCA =  acos((ARV/V)*(LOS.'/R)); % Doppler Cone Angle to POI at COA

%% Compute effective aperture positions for each percentage
Theta = meta.Grid.Col.ImpRespBW/meta.Grid.Row.KCtr; % Angular aperture spanned by this complex image
EffectiveDuration = (Theta*R/V)/sin(DCA); % Theta is expressed as a ratio, rather than an angle
% For the purposes of this computation, we assume a linear flight path
% centered around the COA ARP and ARV, and that the center of the deskewed
% spatial frequency in azimuth is the azimuth angle at COA.
t = EffectiveDuration * ((-1/2) + (percent(:) / 100));
pos = [ARP(1) + (t * ARV(1)), ...
    ARP(2) + (t * ARV(2)), ...
    ARP(3) + (t * ARV(3))];
% We could also compute position given the position given the full position
% polynomial.  However, given that the PERCENT parameter is actually a
% fraction of the spatial frequency space and not time, it is not clear
% that this will be any more accurate.
% t = time_coa + (EffectiveDuration * ((-1/2) + (percent(:) / 100)));
% pos=[polyval(pos_coefs(:,1),t)...
%     polyval(pos_coefs(:,2),t)...
%     polyval(pos_coefs(:,3),t)];

%% Calculate actual azimuth angles
gpn = wgs_84_norm(poi_ecf); % Ground plane normal, based on wgs_84 ellipsoid
range = pos-repmat(poi_ecf,[size(t,1),1]); % Range vectors for each time
north_ground = [ 0 0 1 ]-([ 0 0 1 ]*gpn)*gpn.'; % Project north onto ground plane
range_ground=range-(range*gpn)*gpn.'; % Project range vector onto ground plane
Az = atan2( cross( range_ground, repmat(north_ground,[size(t,1),1]), 2 ) * gpn, ...
    range_ground * north_ground.')*180/pi; % Angle between the two projected vectors
Az(Az < 0) = Az(Az < 0) + 360; % Assure in [0,360], not [-180,180]
Az = reshape(Az,size(percent)); % Reform to shape of original input

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function [simple_rules,skipped] = simplifyGrRules(grRules,expanded)
%simplifyGrRules  Simplify and condense the logic of model grRules.
%
% simplifyGrRules uses Matlab's symbolic toolbox to mathematically simplify
% gene rules. Each rule is converted into a symbolic boolean expression,
% and these expressions are simplified yielding rules that are as reduced/
% simplified as possible, while still maintaining functional equivalency to
% the original grRule logic.
%
% The function will also remove unnecessary parentheses, trailing ANDs/ORs,
% and duplicate genes in model grRules. The function requires that the gene
% names/IDs do not contain spaces, parentheses, "&", "|", or "#".
% Furthermore, grRules must separate gene names/IDs from ANDs and ORs by at
% least one space. For example,
%
%       OK: "GENE1 and GENE2", or "GENE1 & GENE2"
%   NOT OK: "GENE1andGENE2", or "GENE1&GENE2"
%
%
% *!WARNING!* Simplifying grRules with this function may eliminate some
%             gene-reaction associations.
%             For example: 'G1 or (G1 and G2)' simplifies to 'G1'
%
%
% USAGE:
%
%   [simple_rules,skipped] = simplifyGrRules(grRules,expanded);
%
%
% INPUT:
%
%   grRules        The grRules field from a genome-scale metabolic model.
%                  Note that grRules can use written ("and" "or") or
%                  symbolic ("&" "|") boolean operators.
%
%   expanded       (Optional, default FALSE). If TRUE, the grRule will be
%                  returned in its expanded form, where groups of ANDs
%                  (enzyme complexes) are separated by ORs, rather than
%                  using a shorter nested format.
%
%                  For example:
%
%                   input: G1 and G2 and (G3 or G4) and (G4 or G3 or G3) and G1
%
%              simplified: G1 and G2 and (G3 or G4)
%
%       simplify + expand: (G1 and G2 and G3) or (G1 and G2 and G4)
%
%                  NOTE: Expanding rules can make them very long, and in
%                        some cases will exceed Matlab's limit on the
%                        length of a symbolic expression, requiring them to
%                        be skipped.
%
%
% OUTPUT:
%
%   simple_rules   Updated/simplified grRules.
%
%   skipped        Logical vector indicating which grRules were skipped
%                  because they were too long to be converted into a
%                  symbolic equation. For these cases, the grRule will be
%                  copied into simple_rules without any simplification.
%

if nargin < 2
    expanded = false;
end

% perform a preliminary "clean" of the gene rules
grRules = cleanGrRules(grRules);

% check if the grRules use written or symbolic boolean operators
if any(contains(grRules,{'&','|'}))
    Btype = 'symbol';  % record rule type, to revert back at the end
else
    % convert "and" to "&" and "or" to "|"
    grRules = regexprep(grRules, ' or ', ' | ');
    grRules = regexprep(grRules, ' and ', ' & ');
    Btype = 'text';  % record rule type, to revert back at the end
end

% get list of all gene IDs present in grRules
rxnGenes = cellfun(@(r) unique(regexp(r,'[^&|\(\) ]+','match')),grRules,'UniformOutput',false);
nonEmpty = ~cellfun(@isempty,rxnGenes);
allGenes = unique([rxnGenes{nonEmpty}]');

% Replace all gene IDs with G1, G2, G3, etc. to avoid potential problems
% with certain gene ID formats (e.g., Entrez IDs are numbers, which will
% cause problems).
tempIDs = strcat('G',arrayfun(@num2str,(1:length(allGenes))','UniformOutput',false));  % construct new ID vector
convertGene = @(g) tempIDs{ismember(allGenes,g)};  % define conversion function
grRules = regexprep(grRules, '[^&|\(\) ]+', '${convertGene($0)}');  % find and convert all gene IDs to new temporary type

% identify all rules containing both ANDs and ORs
AndOrInd = find(contains(grRules,'&') & contains(grRules,'|'));

% initialize output specifying which rules were skipped due to complexity
skipped = false(size(grRules));

% iterate through complex rules (those containing both ANDs and ORs)
for i = 1:length(AndOrInd)

    r = grRules{AndOrInd(i)};

    % attempt to convert string to symbolic equation
    try
        reqn = str2sym(r);
    catch
        fprintf('grRule #%u was too long (%u characters), and therefore skipped.\n',AndOrInd(i),numel(r));
        skipped(AndOrInd(i)) = true;
        continue
    end

    % simplify eqn, allowing for a max of 10 steps
    reqn = simplify(reqn,10);

    % attempt to expand equation if requested
    reqn_noexpand = reqn;
    if ( expanded )
        try
            reqn = expand(reqn);
        catch
            fprintf('Failed to expand grRule #%u due to excess length. Rule will remain in simplified form.\n',AndOrInd(i));
        end
    end

    % Add parentheses around groups of genes. This may add some
    % unnecessary parentheses, but they will be cleaned up later.
    if is_or(reqn) || is_and(reqn)
        % the input and output of this function are of the type "char"
        try
            % sometimes an expanded equation will fail here
            r = add_parentheses(char(reqn));
        catch
            fprintf('Failed to expand grRule #%u due to excess length. Rule will remain in simplified form.\n',AndOrInd(i));
            reqn = reqn_noexpand;
            r = add_parentheses(char(reqn));
        end
        r = regexprep(r,'^\(|\)$','');  % remove the extra set of parentheses that always enclose the expression
    end

    % update rule in grRule vector
    grRules{AndOrInd(i)} = r;

end

% restore original gene IDs
restoreGene = @(g) allGenes{ismember(tempIDs,g)};  % define conversion function
grRules = regexprep(grRules, '[^&|\(\) ]+', '${restoreGene($0)}');  % find and restore all gene IDs to original ID

% change boolean operators back to original type
if strcmpi(Btype,'text')
    grRules = regexprep(grRules, ' \| ', ' or ');
    grRules = regexprep(grRules, ' & ', ' and ');
end

% assign output
simple_rules = grRules;

end

%% Add parentheses around groups of equation terms
function eqn_str_new = add_parentheses(eqn_str)
% This is a recursive function, which groups terms in an equation with
% parentheses in order to clearly indicate the order of operations. This is
% necessary to avoid ambiguity when the equations are converted back into
% gene rules.
%
% NOTE: both eqn_str_new and eqn_str are strings (char).

% obtain symbolic form of equation
eqn = str2sym(eqn_str);

% separate equation into terms/pieces ("children")
eqn_pieces = arrayfun(@char,children(eqn),'UniformOutput',false);

% find pieces that are further separable, and recursively separate until
% all pieces are single genes
ind = find(contains(eqn_pieces,{'|','&'}));
for i = 1:length(ind)
    eqn_pieces{ind(i)} = add_parentheses(eqn_pieces{ind(i)});
end

% re-assemble equation pieces, adding | or &, depending on the input eqn
if is_or(eqn)
    eqn_str_new = char(strcat('(',join(eqn_pieces,' | '),')'));
elseif is_and(eqn)
    eqn_str_new = char(strcat('(',join(eqn_pieces,' & '),')'));
else
    % if the input eqn was simply one gene, just return the input
    eqn_str_new = eqn_str;
end

end

%% Functions to test whether an equation is a collection of ORs or ANDs
function res = is_or(eqn)
% check if outermost operations contain ORs
res = length(regexp(char(eqn),'\|')) > length(regexp(char(children(eqn)),'\|'));
end

function res = is_and(eqn)
% check if outermost operations contain ANDs
res = length(regexp(char(eqn),'&')) > length(regexp(char(children(eqn)),'&'));
end


% STD_SPEC - Returns the data or ICA component spectra for a dataset. Updates the EEG structure
%              in the Matlab environment and in the .set file as well. Saves the spectra
%              in a file.
% Usage:
%           >> [spec freqs] = std_spec(EEG, 'key', 'val', ...);
%
%              Computes the mean spectra of the data channels or activities of specified
%              components of the supplied dataset. The spectra are saved in a Matlab file.
%              If such a file already exists, loads the spectral information from this file.
%              Options (below) specify which components to use, and the desired frequency
%              range. There is also an option to specify other SPECTOPO input variables
%              (see >> help spectopo for details).
%
%              Returns the removed mean spectra of the selected ICA components in the
%              requested frequency range. If the spectra were computed previously but a
%              different frequency range is selected, there is an overwrite option.
%              so. The function will load previously computed log spectra, if any, and
%              will remove the mean from the requested frequency range. The frequencies
%              vector is also returned.
% Inputs:
%   EEG - a loaded epoched EEG dataset structure.
%
% Optional inputs:
%   'components' - [numeric vector] components of the EEG structure for which
%                  activation spectrum will be computed. Note that because
%                  computation of ERP is so fast, all components spectrum are
%                  computed and saved. Only selected component
%                  are returned by the function to Matlab
%                  {default|[] -> all}
%   'channels'   - [cell array] channels of the EEG structure for which
%                  activation spectrum will be computed. Note that because
%                  computation of ERP is so fast, all channels spectrum are
%                  computed and saved. Only selected channels
%                  are returned by the function to Matlab
%                  {default|[] -> none}
%   'recompute'  - ['on'|'off'] force recomputing ERP file even if it is
%                  already on disk.
%   'trialindices' - [cell array] indices of trials for each dataset.
%                  Default is all trials.
%   'recompute'  - ['on'|'off'] force recomputing data file even if it is
%                  already on disk.
%   'rmcomps'    - [integer array] remove artifactual components (this entry
%                  is ignored when plotting components). This entry contains
%                  the indices of the components to be removed. Default is none.
%   'interp'     - [struct] channel location structure containing electrode
%                  to interpolate ((this entry is ignored when plotting
%                  components). Default is no interpolation.
%   'output'     - ['power'|'fft'] compute power of keep single complex
%                  'fft' estimate. Default is 'power'.
%   'fileout'    - [string] name of the file to save on disk. The default
%                  is the same name (with a different extension) as the
%                  dataset given as input.
%  'savetrials'  - ['on'|'off'] save single-trials ERSP. Requires a lot of disk
%                  space (dataset space on disk times 10) but allow for refined
%                  single-trial statistics.
%
% spectrum specific optional inputs:
%   'specmode'   - ['psd'|'fft'|'pburg'|'pmtm'] method to compute spectral
%                  decomposition. 'psd' uses the spectopo function (optional
%                  parameters to this function may be given as input). 'fft'
%                  uses a simple fft on each trial. For continuous data
%                  data trials are extracted automatically (see 'epochlim'
%                  and 'epochrecur' below). Two experimental modes are
%                  'pmtm' and 'pbug' which use multitaper and the Burg
%                  method to compute spectrum respectively. NOTE THAT SOME
%                  OF THESE OPTIONS REQUIRE THE SIGNAL PROCESSING TOOLBOX.
%   'epochlim'   - [min max] for FFT on continuous data, extract data
%                  epochs with specific epoch limits in seconds (see also
%                  'epochrecur' below). Default is [0 1].
%   'epochrecur' - [float] for FFT on continuous data, set the automatic
%                  epoch extraction recurence interval (default is 0.5 second).
%   'timerange'  - [min max] use data within a specific time range before
%                  computing the data spectrum. For instance, for evoked
%                  data trials, it is recommended to use the baseline time
%                  period.
%   'logtrials'  - ['on'|'off'] compute single-trial log transform before
%                  averaging them. Default is 'off' for 'psd' specmode and
%                  'on' for 'fft' specmode. Ignored when output is set to
%                  'fft'.
%   'continuous' - ['on'|'off'] force epoch data to be treated as
%                  continuous so small data epochs can be extracted for the
%                  'fft' specmode option. Default is 'off'.
%   'freqrange'  - [minhz maxhz] frequency range (in Hz) within which to
%                  return the spectrum {default|[]: [0 sample rate/2]}.
%                  Note that this does not affect the spectrum computed on
%                  disk, only the data returned by this function as output.
%   'nw'         - [integer] number of tapers for the 'pmtm' spectral
%                  method. Default is 4.
%   'burgorder'  - [integet] order for the Burg spectral method.
%
% Changes between EEGLAB 13 and later EEGLAB versions:
% For the 'specmode' option 'fft', EEGLAB 14 and later version detrend the
% data and apply hamming taper to it. EEGLAB 13 and earlier remove the
% baseline from the data and apply a hamming taper but only for continuous data.
%
% Other optional spectral parameters:
%   All optional parameters to the spectopo function may be provided to this
%   function as well (requires the 'specmode' option above to be set to
%   'psd').
%
% Outputs:
%   spec      - the mean spectra (in dB) of the requested ICA components in the selected
%               frequency range (with the mean of each spectrum removed).
%   freqs     - a vector of frequencies at which the spectra have been computed.
%
% Files output or overwritten for ICA:
%               [dataset_filename].icaspec,   % raw spectrum of ICA components
% Files output or overwritten for data:
%               [dataset_filename].datspec,
%
% See also  SPECTOPO, STD_ERP, STD_ERSP, STD_MAP, STD_PRECLUST
%
% Authors: Arnaud Delorme, SCCN, INC, UCSD, January, 2005

% Defunct:      0 -> if frequency range is different from saved spectra, ask via a
%                    pop-up window whether to keep existing spectra or to overwrite them.

% Copyright (C) Arnaud Delorme, SCCN, INC, UCSD, October 11, 2004, arno@sccn.ucsd.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% 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 HOLDER 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.

function [X, f, overwrt] = std_spec(EEG, varargin)

overwrt = 1; % deprecated
if nargin < 1
    help std_spec;
    return;
end

% decode inputs
% -------------
if ~isempty(varargin)
    if ~ischar(varargin{1})
        varargin = { varargin{:} [] [] };
        if all(varargin{1} > 0)
            options = { 'components' varargin{1} 'freqrange' varargin{2} };
        else
            options = { 'channels' -varargin{1} 'freqrange' varargin{2} };
        end
    else
        options = varargin;
    end
else
    options = varargin;
end

[g spec_opt] = finputcheck(options, { 'components' 'integer' []         [];
                                      'channels'   'cell'    {}         {};
                                      'timerange'  'float'   []         [];
                                      'specmode'   'string'  {'fft','psd','pmtm','pburg'} 'psd';
                                      'recompute'  'string'  { 'on','off' } 'off';
                                      'savetrials' 'string'  { 'on','off' } 'off';
                                      'continuous' 'string'  { 'on','off' } 'off';
                                      'logtrials'  'string'  { 'on','off' 'notset' } 'notset';
                                      'output'     'string'  { 'power','fft' } 'power';
                                      'savefile'   'string'  { 'on','off' } 'on';
                                      'epochlim'   'real'    []         [0 1];
                                      'trialindices' { 'integer','cell' } []         [];
                                      'epochrecur' 'real'    []         0.5;
                                      'rmcomps'    'cell'    []         cell(1,length(EEG));
                                      'nw'         'float'   []         4;
                                      'fileout'    'string'  []         '';
                                      'trialinfo'  'struct'  []         struct([]);
                                      'burgorder'  'integer' []         20;
                                      'interp'     'struct'  { }        struct([]);
                                      'nfft'       'integer' []         [];
                                      'freqrange'  'real'    []         [] }, 'std_spec', 'ignore');
if ischar(g), error(g); end
if isempty(g.trialindices), g.trialindices = cell(1, length(EEG)); end
if ~iscell(g.trialindices), g.trialindices = { g.trialindices }; end
if ~strcmpi(g.specmode, 'fft') && strcmpi(g.output, 'ftt'), error('FFT option only valid when computing FFT'); end
if isfield(EEG,'icaweights')
   numc = size(EEG(1).icaweights,1);
else
   error('EEG.icaweights not found');
end
if isempty(g.components)
    g.components = 1:numc;
end

EEG_etc = [];

% filename
% --------
if isempty(g.fileout), g.fileout = fullfile(EEG(1).filepath, EEG(1).filename(1:end-4)); end
if ~isempty(g.channels)
    filename = [ g.fileout '.datspec'];
    prefix = 'chan';
else
    filename = [ g.fileout '.icaspec'];
    prefix = 'comp';
end

% SPEC information found in datasets
% ---------------------------------
if exist(filename) && strcmpi(g.recompute, 'off')

    fprintf('File "%s" found on disk, no need to recompute\n', filename);
    if strcmpi(prefix, 'comp')
        [X,tmp,f] = std_readfile(filename, 'components', g.components, 'freqlimits', g.freqrange, 'measure', 'spec');
    else
        [X,tmp,f] = std_readfile(filename, 'channels', g.channels, 'freqlimits', g.freqrange, 'measure', 'spec');
    end
    if ~isempty(X), return; end
end

% No SPEC information found
% -------------------------
if isempty(g.channels)
     [X,boundaries]  = eeg_getdatact(EEG, 'component', [1:size(EEG(1).icaweights,1)], 'trialindices', g.trialindices );
else [X,boundaries]  = eeg_getdatact(EEG, 'trialindices', g.trialindices, 'rmcomps', g.rmcomps, 'interp', g.interp);
end
if ~isempty(boundaries) && boundaries(end) ~= size(X,2), boundaries = [boundaries size(X,2)]; end

% get specific time range for epoched and continuous data
% -------------------------------------------------------
oritrials = EEG.trials;
if ~isempty(g.timerange)
    if oritrials > 1
        timebef  = find(EEG(1).times >= g.timerange(1) & EEG(1).times < g.timerange(2) );
        X        = X(:,timebef,:);
        EEG(1).pnts = length(timebef);
    else
        disp('warning: ''timerange'' option cannot be used with continuous data');
    end
end

% extract epochs if necessary
% ---------------------------
if all([ EEG.trials] == 1) || strcmpi(g.continuous, 'on')
    epochCount  = 1;
    sampleCount = 1;
    for iEEG = 1:length(EEG)
        TMP = EEG(1);
        TMP.data = X;
        TMP.icaweights = [];
        TMP.icasphere  = [];
        TMP.icawinv    = [];
        TMP.icaact     = [];
        TMP.icachansind = [];
        TMP.trials = size(TMP.data,3);
        TMP.pnts   = size(TMP.data,2);
        TMP.event  = [];
        TMP.urevent  = [];
        TMP.epoch  = [];
        TMP.chanlocs = [];
        for index = 1:length(boundaries)
            TMP.event(index).type = 'boundary';
            TMP.event(index).latency = boundaries(index);
        end
        TMP = eeg_checkset(TMP);
        if TMP.trials > 1
            % epoch data - need to re-extract data
            TMP = pop_select(TMP, 'trial', [epochCount:(epochCount+EEG(iEEG).trials-1)]);
            epochCount = epochCount+EEG(iEEG).trials;
            TMP = eeg_epoch2continuous(TMP);
        else
            % continuous data - need to re-extract data
            TMP = pop_select(TMP, 'point', [sampleCount:(sampleCount+EEG(iEEG).pnts-1)]);
            sampleCount = sampleCount+EEG(iEEG).pnts;
        end
        TMP = eeg_regepochs(TMP, g.epochrecur, g.epochlim);
        disp('Warning: continuous data, extracting 1-second epochs');
        if iEEG == 1,
            XX = TMP.data;
            newTrialInfo = g.trialinfo(iEEG);
            newTrialInfo(1:size(TMP.data,3)) = g.trialinfo(iEEG);
        else
            XX(:,:,end+1:end+size(TMP.data,3)) = TMP.data;
            newTrialInfo(end+1:end+size(TMP.data,3)) = g.trialinfo(iEEG);
        end
    end
    g.trialinfo = newTrialInfo;
    X = XX;
end

% compute spectral decomposition
% ------------------------------
if strcmpi(g.logtrials, 'notset'), if strcmpi(g.specmode, 'fft') g.logtrials = 'on'; else g.logtrials = 'off'; end; end
if strcmpi(g.logtrials, 'on'), datatype = 'SPECTRUMLOG'; else datatype = 'SPECTRUMABS'; end
if strcmpi(g.specmode, 'psd')
    if strcmpi(g.savetrials, 'on') || strcmpi(g.logtrials, 'on')
        if all([ EEG.trials] == 1) || strcmpi(g.continuous, 'on')
            if isequal(g.epochlim, [0 1])
                fprintf('Spectopo(psd): randomly extracted epochs are only 1 seconds. PSD is better suited for longer epochs.\n');
            end
        end
        fprintf('Computing spectopo (psd) across trials: ');
        for iTrial = 1:size(X,3)
            [tmp, f] = spectopo(X(:,:,iTrial), size(X,2), EEG(1).srate, 'plot', 'off', 'boundaries', boundaries, 'nfft', g.nfft, 'verbose', 'off', spec_opt{:});
            if iTrial == 1
                XX = zeros(size(tmp,1), size(tmp,2), size(X,3));
            end
            XX(:,:,iTrial) = tmp;
            %if iTrial == 1 && size(X,3) > 1, XX(:,:,size(X,3)) = 0; end
            if mod(iTrial,10) == 0, fprintf('%d ', iTrial); end
        end
        fprintf('\n');
        if strcmpi(g.logtrials, 'off')
             X = 10.^(XX/10);
        else X = XX;
        end
        if strcmpi(g.savetrials, 'off')
            X = mean(X,3);
        end
    else
        [X, f] = spectopo(X, size(X,2), EEG(1).srate, 'plot', 'off', 'boundaries', boundaries, 'nfft', g.nfft, 'verbose', 'off', spec_opt{:});
        X = 10.^(X/10);
    end
elseif strcmpi(g.specmode, 'pmtm')
    if strcmpi(g.logtrials, 'on')
        error('Log trials option cannot be used in conjunction with the PMTM option');
    end
    if all([ EEG.trials ] == 1) && ~isempty(boundaries), disp('Warning: multitaper does not take into account boundaries in continuous data (use ''psd'' method instead)'); end
    fprintf('Computing spectrum using multitaper method:');
    for cind = 1:size(X,1)
        fprintf('.');
        for tind = 1:size(X,3)
            [tmpdat f] = pmtm(X(cind,:,tind), g.nw, g.nfft, EEG.srate);
            if cind == 1 && tind == 1
                X2 = zeros(size(X,1), length(tmpdat), size(X,3));
            end
            X2(cind,:,tind) = tmpdat;
        end
    end
    fprintf('\n');
    X = X2;
    if strcmpi(g.savetrials, 'off'), X = mean(X,3); end
elseif strcmpi(g.specmode, 'pburg')
    if strcmpi(g.logtrials, 'on')
        error('Log trials option cannot be used in conjunction with the PBURB option');
    end
    fprintf('Computing spectrum using Burg method:');
    if all([ EEG.trials ] == 1) && ~isempty(boundaries), disp('Warning: pburg does not take into account boundaries in continuous data (use ''psd'' method instead)'); end
    for cind = 1:size(X,1)
        fprintf('.');
        for tind = 1:size(X,3)
            [tmpdat f] = pburg(X(cind,:,tind), g.burgorder, g.nfft, EEG.srate);
            if cind == 1 && tind == 1
                X2 = zeros(size(X,1), length(tmpdat), size(X,3));
            end
            X2(cind,:,tind) = tmpdat;
        end
    end
    fprintf('\n');
    X = X2;
    if strcmpi(g.savetrials, 'off'), X = mean(X,3); end
else % fft mode
    %
    if size(X,3) > 1
        for iTrial = 1:size(X,3)
            X(:,:,iTrial) = detrend(X(:,:,iTrial)')';
        end
    else
        X = detrend(X')';
    end
    try
        X = bsxfun(@times, X, hamming(size(X,2))'); % apply hamming window even for data trials (not the case in EEGLAB 13)
    catch
        X = bsxfun(@times, X, hamming2(size(X,2))');
    end
    disp('Warning: std_spec function computation has changed since version 13 (see help message)');
    %end
    % if all([ EEG.trials ] == 1) && ~isempty(boundaries), disp('Warning: fft does not take into account boundaries in continuous data (use ''psd'' method instead)'); end
    tmp   = fft(X, g.nfft, 2);
    f     = linspace(0, EEG(1).srate/2, floor(size(tmp,2)/2));
    f     = f(2:end); % remove DC (match the output of PSD)
    tmp   = tmp(:,2:floor(size(tmp,2)/2),:);

    % To compute spectral density (but still need FFT correction
    %     dens  = f(3)-f(2)
    %     tmp   = tmp(:,2:floor(size(tmp,2)/2),:)/dens;

    if strcmpi(g.output, 'power')
        X     = tmp.*conj(tmp);
        if strcmpi(g.logtrials, 'on'),  X = 10*log10(X); end
    else
        X = tmp;
        datatype = 'SPECTRUMFFT';
    end
end
eeglab_options;
if option_single
    X = single(X);
end

% Save SPECs in file (all components or channels)
% -----------------------------------------------
fileNames = computeFullFileName( { EEG.filepath }, { EEG.filename });
if strcmpi(g.savefile, 'on')
    options = { options{:} spec_opt{:} 'timerange' g.timerange 'nfft' g.nfft 'specmode' g.specmode };
    if strcmpi(prefix, 'comp')
        savetofile( filename, f, X, 'comp', 1:size(X,1), options, {}, fileNames, g.trialindices, datatype , g.trialinfo);
    else
        if ~isempty(g.interp)
            savetofile( filename, f, X, 'chan', 1:size(X,1), options, { g.interp.labels }, fileNames, g.trialindices, datatype , g.trialinfo);
        else
            tmpchanlocs = EEG(1).chanlocs;
            savetofile( filename, f, X, 'chan', 1:size(X,1), options, { tmpchanlocs.labels }, fileNames, g.trialindices, datatype , g.trialinfo);
        end
    end
end
return;

% compute full file names
% -----------------------
function res = computeFullFileName(filePaths, fileNames);
for index = 1:length(fileNames)
    res{index} = fullfile(filePaths{index}, fileNames{index});
end

% -------------------------------------
% saving SPEC information to Matlab file
% -------------------------------------
function savetofile(filename, f, X, prefix, comps, params, labels, dataFiles, dataTrials, datatype , trialInfo);

    disp([ 'Saving SPECTRAL file ''' filename '''' ]);
    allspec = [];
    for k = 1:length(comps)
        allspec = setfield( allspec, [ prefix int2str(comps(k)) ], squeeze(X(k,:,:)));
    end
    if nargin > 6 && ~isempty(labels)
        allspec.labels = labels;
    end
    allspec.freqs      = f;
    allspec.parameters = params;
    allspec.datatype   = datatype;
    allspec.datafiles   = dataFiles;
    allspec.datatrials  = dataTrials;
    allspec.trialinfo   = trialInfo;
    allspec.average_spec = mean(X,1);
    std_savedat(filename, allspec);

% -------------------------------------
% Adapted from Octave version
% -------------------------------------
function c = hamming2(m, opt)

if (nargin < 1 || nargin > 2)
    help hamming;
    return;
end

if (~(isscalar (m) && (m == fix (m)) && (m > 0)))
    error ('hamming: M must be a positive integer');
end

N = m - 1;
if (nargin == 2)
    switch (opt)
        case 'periodic'
            N = m;
        case 'symmetric'
            % Default option, same as no option specified.
        otherwise
            error ('hamming: window type must be either "periodic" or "symmetric"');
    end
end

if (m == 1)
    c = 1;
else
    m = m - 1;
    c = 0.54 - 0.46 * cos (2 * pi * (0 : m)' / N);
end


function [R T] = fX4_to_RT(Tm)

R = Tm(1:3,1:3);
T = Tm(1:3,4)';


function r = eq(p,q)
% MEAS/EQ  Implement p == q for meas.

% find the difference between the two
diff = meas();
diff = p - q;

% see if p == q at the 1-sigma level
if abs(diff.value) < diff.error
    r = 1;
else
    r = 0;
end


function jed = yjf_to_jed_islamic_a ( y, j, f )

%*****************************************************************************80
%
%% YJF_TO_JED_ISLAMIC_A converts an Islamic-A YJF date to a JED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, J, real F, the YJF date.
%
%    Output, real JED, the Julian Ephemeris Date.
%

%
%  Copy the input.
%
  y1 = y;
  j1 = j;
  f1 = f;
%
%  Check the input.
%
  [ y1, j1, ierror ] = yj_check_islamic ( y1, j1 );

  if ( ierror ~= 0 )
    jed = -1.0;
    return
  end
%
%  Convert the input.
%
  [ y2, m2, d2, f2 ] = yjf_to_ymdf_islamic ( y1, j1, f1 );

  jed = ymdf_to_jed_islamic_a ( y2, m2, d2, f2 );

  return
end


function status = test_modal_weighting(modus)
%TEST_MODAL_WEIGHTING evaluates the modal weighting implementation
%
%   Usage: status = test_modal_weighting(modus)
%
%   Input parameters:
%       modus   - 0: numerical
%                 1: visual
%
%   Output parameters:
%       status  - true or false
%
%   TEST_MODAL_WEIGHTING(modus) tests the implementation of the modal weighting
%   window for different local sound field synthesis techniques.

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

status = false;

%% ===== Checking of input parameters ====================================
nargmin = 1;
nargmax = 1;
narginchk(nargmin,nargmax);

%% ===== Configuration ==================================================
window_type = {...
    'rect'; ...
    'max-rE'; ...
    'kaiser'; ...
    'tukey'; ...
    };
% For plotting
conf = SFS_config;
conf.dimension = '2.5D';
conf.secondary_sources.number = 16;
conf.secondary_sources.geometry = 'circular';
conf.secondary_sources.size = 3;
conf.resolution = 300;
conf.plot.usedb = true;
conf.t0 = 'source';
X = [-2 2];
Y = [-2 2];
Z = 0;
src = 'pw';

%% ===== Computation and Plotting ========================================
if modus % graphical mode
    for window = window_type'
        conf.modal_window = window{:};
        update_title = false;
        base_title = ['Window: ' window{:}];
        switch conf.modal_window
        case {'rect', 'max-rE'}
            parameter = 1;
        otherwise
            parameter = [0 0.5 1];
            update_title = true;
        end
        for pp = 1:length(parameter)
            conf.modal_window_parameter = parameter(pp);
            sound_field_imp_nfchoa(X,Y,Z,[0 -1 0],'pw',0,conf);
            if update_title
                figure_title = [base_title ...
                                ', Parameter: ' ...
                                num2str(parameter(pp))];
            else
                figure_title = base_title;
            end
            title(figure_title);
        end
    end
else % numerical mode
    for window = window_type'
        conf.modal_window = window{:};
        conf.modal_window_parameter = 0.5;
        for order = [1 2]
            for ndtft = [2*order+1 3*order+1]
                [win,Win,Phi] = ...
                    modal_weighting(order,ndtft,conf);
                [win_ref,Win_ref,Phi_ref] = ...
                    ref_modal_weighting(order,ndtft,conf);
                if any([abs(win-win_ref) ...
                        abs(Win-Win_ref) ...
                        abs(Phi-Phi_ref)])>1e14
                    return
                end
            end
        end
    end
end

status = true;

end

function [win,Win,Phi] = ref_modal_weighting(order,ndtft,conf)
    % Returns reference values for some orders as calculated by the Matlab
    % implementation of commit ...
    switch conf.modal_window
    case 'rect'
        if order==1 && ndtft==2*order+1
            win = [1 1];
            Win = [1 0 0];
            Phi = [0 2.094395102393196 4.188790204786391];
        elseif order==1 && ndtft==3*order+1
            win = [1 1];
            Win = [0.75 0.25 -0.25 0.25];
            Phi = [0 1.570796326794897 3.141592653589793 4.712388980384690];
        elseif order==2 && ndtft==2*order+1
            win = [1 1 1];
            Win = [1 0 0 0 0];
            Phi = [0 ...
                   1.256637061435917 ...
                   2.513274122871834 ...
                   3.769911184307752 ...
                   5.026548245743669];
        elseif order==2 && ndtft==3*order+1
            win = [1 1 1];
            Win = [0.714285714285714 ...
                   0.257419676543548 ...
                  -0.178139943388210 ...
                   0.063577409701804 ...
                   0.063577409701804 ...
                  -0.178139943388210 ...
                   0.257419676543548];
            Phi = [0 ...
                   0.897597901025655 ...
                   1.795195802051310 ...
                   2.692793703076966 ...
                   3.590391604102621 ...
                   4.487989505128276 ...
                   5.385587406153931];
        end
    case 'max-rE'
        if order==1 && ndtft==2*order+1
            win = [1 0.707106781186548];
            Win = [0.804737854124365 0.097631072937817 0.097631072937817];
            Phi = [0 2.094395102393196 4.188790204786391];
        elseif order==1 && ndtft==3*order+1
            win = [1 0.707106781186548];
            Win = [0.603553390593274 ...
                   0.250000000000000 ...
                  -0.103553390593274 ...
                   0.250000000000000];
            Phi = [0 ...
                   1.570796326794897 ...
                   3.141592653589793 ...
                   4.712388980384690];
        elseif order==2 && ndtft==2*order+1
            win = [1 0.866025403784439 0.500000000000000];
            Win = [0.746410161513775 ...
                   0.145243228056937 ...
                  -0.018448308813825 ...
                  -0.018448308813825 ...
                   0.145243228056937];
            Phi = [0 ...
                   1.256637061435917 ...
                   2.513274122871834 ...
                   3.769911184307752 ...
                   5.026548245743669];
        elseif order==2 && ndtft==3*order+1
            win = [1 0.866025403784439 0.500000000000000];
            Win = [0.533150115366983 ...
                   0.265342154409152 ...
                  -0.040912347323205 ...
                   0.008995135230562 ...
                   0.008995135230562 ...
                  -0.040912347323205 ...
                   0.265342154409152];
            Phi = [0 ...
                   0.897597901025655 ...
                   1.795195802051310 ...
                   2.692793703076966 ...
                   3.590391604102621 ...
                   4.487989505128276 ...
                   5.385587406153931];
        end
    case {'kaiser', 'kaiser-bessel'}
        % conf.modal_window_parameter == 0.5
        if order==1 && ndtft==2*order+1
            win = [1 0.581816879161903];
            Win = [0.721211252774602 0.139394373612699 0.139394373612699];
            Phi = [0 2.094395102393196 4.188790204786391];
        elseif order==1 && ndtft==3*order+1
            win = [1 0.581816879161903];
            Win = [0.540908439580952 ...
                   0.250000000000000 ...
                  -0.040908439580952 ...
                   0.250000000000000];
            Phi = [0 ...
                   1.570796326794897 ...
                   3.141592653589793 ...
                   4.712388980384690];
        elseif order==2 && ndtft==2*order+1
            win = [1 0.883766863470753 0.581816879161903];
            Win = [0.786233497053062 ...
                   0.120959694808693 ...
                  -0.014076443335224 ...
                  -0.014076443335224 ...
                   0.120959694808693];
            Phi = [0 ...
                   1.256637061435917 ...
                   2.513274122871834 ...
                   3.769911184307752 ...
                   5.026548245743669];
        elseif order==2 && ndtft==3*order+1
            win = [1 0.883766863470753 0.581816879161903];
            Win = [0.561595355037902 ...
                   0.263300911786297 ...
                  -0.063101577944049 ...
                   0.019002988638802 ...
                   0.019002988638802 ...
                  -0.063101577944049 ...
                   0.263300911786297];
            Phi = [0 ...
                   0.897597901025655 ...
                   1.795195802051310 ...
                   2.692793703076966 ...
                   3.590391604102621 ...
                   4.487989505128276 ...
                   5.385587406153931];
        end
    case 'tukey'
        % conf.modal_window_parameter == 0.5
        if order==1 && ndtft==2*order+1
            win = [1 0.75];
            Win = [0.833333333333333 0.083333333333333 0.083333333333333];
            Phi = [0 2.094395102393196 4.188790204786391];
        elseif order==1 && ndtft==3*order+1
            win = [1 0.75];
            Win = [0.625 0.250 -0.125 0.250];
            Phi = [0 1.570796326794897 3.141592653589793 4.712388980384690];
        elseif order==2 && ndtft==2*order+1
            win = [1 1 0.5];
            Win = [0.800000000000000 ...
                   0.161803398874989 ...
                  -0.061803398874989 ...
                  -0.061803398874989 ...
                   0.161803398874989];
            Phi = [0 ...
                   1.256637061435917 ...
                   2.513274122871834 ...
                   3.769911184307752 ...
                   5.026548245743669];
        elseif order==2 && ndtft==3*order+1
            win = [1 1 0.5];
            Win = [0.571428571428571 ...
                   0.289208381394450 ...
                  -0.049430105116435 ...
                  -0.025492561992301 ...
                  -0.025492561992301 ...
                  -0.049430105116435 ...
                   0.289208381394450];
            Phi = [0 ...
                   0.897597901025655 ...
                   1.795195802051310 ...
                   2.692793703076966 ...
                   3.590391604102621 ...
                   4.487989505128276 ...
                   5.385587406153931];
        end
    end
end


function X = multi_transpose (X)
% Multi-task cell array transpose.
%
%% INPUT
%   X: {n1 * n2} * t - input matrix
%
%% OUTPUT
%   X: {n2 * n1} * t - transpose every matrix in the cells.
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%

for i = 1:length(X)
    X{i} = X{i}';
end

end


function [f] = spm_fx_fnirs(x,u,P,M)
% State equation for a dynamic model of fNIRS responses
% FORMAT [f] = spm_fx_nirs(x,u,P,M)
%
% x      - state vector
%--------------------------------------------------------------------------
%   x(:,1) - excitatory neuronal activity            ue
%   x(:,2) - vasodilatory signal                          s
%   x(:,3) - rCBF                                  ln(f)
%   x(:,4) - venous volume                         ln(v)
%   x(:,5) - deoxyHb                               ln(q)
%   x(:,6) - totalHb                               ln(p)
%   [x(:,7)] - inhibitory neuronal activity            ui
%--------------------------------------------------------------------------
% u     experimental inputs
% P     prior of latent variables
% M    model structure
%
% f      - dx/dt
%___________________________________________________________________________
%
% References for hemodynamic & neuronal state equations:
% 1. Friston KJ, Mechelli A, Turner R, Price CJ. Nonlinear responses in
%    fMRI: the Balloon model, Volterra kernels, and other hemodynamics.
%    Neuroimage 12:466-477, 2000.
% 2. Stephan KE, Kasper L, Harrison LM, Daunizeau J, den Ouden HE,
%    Breakspear M, Friston KJ. Nonlinear dynamic causal models for fMRI.
%    Neuroimage 42:649-662, 2008.
% 3. Marreiros AC, Kiebel SJ, Friston KJ. Dynamic causal modelling for
%    fMRI: a two-state model.
%    Neuroimage. 39(1):269-78, 2008.
% 4. Buxton RB, Uludag, K, Dubowitz, DJ, Liu, TT. Modeling the hemodynamic
%    response to brain activation. Neuroimage. 2004, 23: 220-233.
% 5. X Cui and S Bray and A Reiss. Functional near infrared spectroscopy (NIRS)
%    signal improvement based on negative correlation between oxygenated and
%    deoxygenated hemoglobin dynamics.
%    Neuroimage 49:3039-3046, 2010.
%
% This script is based on spm_fx_fmri.m written by
% Karl Friston & Klaas Enno Stephan.
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny & Sungho Tak
% $Id: spm_fx_fnirs.m 6942 2016-11-21 13:17:44Z guillaume $

% Neuronal motion
%==========================================================================
P.A   = full(P.A);                       %    linear parameters
P.B   = full(P.B);                       % bi-linear parameters
P.C   = P.C/16;                          % exogenous parameters

% implement differential state equation y = dx/dt (neuronal)
%--------------------------------------------------------------------------
f    = x;

% input dependent modulation
%--------------------------------------------------------------------------
for i = 1:size(P.B,3)
    P.A(:,:,1) = P.A(:,:,1) + u(i)*P.B(:,:,i);
end

if size(x, 2) == 6 % one neuronal state per region
    %======================================================================

    % one neuronal state per region: diag(A) is a log self-inhibition
    %----------------------------------------------------------------------
    SI     = diag(P.A);
    P.A    = P.A - diag(exp(SI)/2 + SI);

    % flow
    %----------------------------------------------------------------------
    f(:,1) = P.A*x(:,1) + P.C*u(:);

elseif size(x,2) == 7 % two neuronal states per region
    %======================================================================

    % extrinsic (two neuronal states): enforce positivity
    %----------------------------------------------------------------------
    n     = size(P.A,1);            % number of regions
    EE    = exp(P.A(:,:,1))/8;
    IE    = diag(diag(EE));         % intrinsic inhibitory to excitatory
    EE    = EE - IE;                % extrinsic excitatory to excitatory
    EI    = eye(n,n);               % intrinsic excitatory to inhibitory
    SE    = eye(n,n)/2;             % intrinsic self-inhibition (excitatory)
    SI    = eye(n,n);               % intrinsic self-inhibition (inhibitory)

    % excitatory proportion
    %----------------------------------------------------------------------
    if size(P.A,3) > 1
        phi = spm_phi(P.A(:,:,2)*2);
        EI  = EI + EE.*(1 - phi);
        EE  = EE.*phi - SE;
    else
        EE  = EE - SE;
    end

    % motion - excitatory and inhibitory: f = dx/dt
    %----------------------------------------------------------------------
    f(:,1) = EE*x(:,1) - IE*x(:, 7) + P.C*u(:);
    f(:,6 + 1) = EI*x(:,1) - SI*x(:, 7);
end

% Hemodynamic motion
%==========================================================================

% hemodynamic parameters
%--------------------------------------------------------------------------
%   H(1) - signal decay                                   d(ds/dt)/ds)
%   H(2) - autoregulation                                 d(ds/dt)/df)
%   H(3) - transit time                                      (t0)
%   H(4) - exponent for Fout(v)                           (alpha)
%   H(5) - resting oxygen extraction                      (E0)
%   H(6) - viscoelastic time constant              (tv)
%--------------------------------------------------------------------------
H = [0.64 0.32 2.00 0.32 0.32 2.00];

% exponentiation of hemodynamic state variables
%--------------------------------------------------------------------------
x(:,3:6) = exp(x(:,3:6));

% signal decay
sd = H(1)*exp(P.decay);

% autoregulatory feedback
af = H(2).*exp(P.afback);

% transit time
tt = H(3).*exp(P.transit);
% viscoelastic time constant
tv = H(6).*exp(P.tv);

% outflow f(v) (steady state)
fv_s = x(:,4).^(1/H(4));

% implement differential state equation f = dx/dt (hemodynamic)
%--------------------------------------------------------------------------
f(:,2)   = x(:,1) - sd.*x(:,2) - af.*(x(:,3) - 1);
f(:,3)   = x(:,2)./x(:,3);
f(:,4)   = (x(:,3) - fv_s)./((tt+tv).*x(:,4));

% outflow (dynamic state)
fv_d = fv_s + tv.*x(:,4).*f(:,4);

% oxygen extraction fraction
ff = (1 - (1 - H(5)).^(1./x(:,3)))/H(5);

f(:,5)   = (x(:,3).*ff - fv_d.*x(:,5)./x(:,4))./(tt.*x(:,5));
f(:,6)   = (x(:,3) - fv_d) ./ (tt.*x(:,4));

f        = f(:);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This code was written by Marcello Torchio, University of Pavia.
% Please send comments or questions to
% marcello.torchio01@ateneopv.it
%
% Copyright 2017: 	Marcello Torchio, Lalo Magni, and Davide M. Raimondo, University of Pavia
%					Bhushan Gopaluni, University of British Columbia
%                 	Richard D. Braatz, MIT.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% RETREIVEDATA retreives the data which is returned in the results structure.

function [ce_t, cs_barrato_t, T_t, jflux_t, Phis_t, Phie_t, cs_star_t, SOC_t, film_t, js_t, Q_t,t_tot] = retreiveData(ce_t, cs_barrato_t, T_t, jflux_t, Phis_t, Phie_t,cs_star_t, SOC_t, film_t, js_t, Q_t,t_tot, y, t, param)
	% Extract differential variables after the integration process
	ce_t            = [ce_t;y(param.ce_indices)];
	cs_barrato_t    = [cs_barrato_t;y(param.cs_average_indices)];
	T_t             = [T_t;y(param.T_indices)];
	film_t          = [film_t;y(param.film_indices)];
    Q_t             = [Q_t;y(param.Q_indices)];
	% Extract the algebraic variables after the integration process
	jflux_t         = [jflux_t;y(param.jflux_indices)];
	Phis_t          = [Phis_t;y(param.Phis_indices)];
	Phie_t          = [Phie_t;y(param.Phie_indices)];
	js_t            = [js_t;y(param.js_indices)];

	% Check if Fick's law of diffusion is used. This is required to define the
    % correct way how to evaluate the SOC.
    if(param.SolidPhaseDiffusion~=3)
        cs_average = cs_barrato_t(end,param.Np+1:end);
    else
        start_index = param.Nr_p*param.Np+1;
        end_index   = start_index+param.Nr_n-1;
        cs_average  = zeros(param.Nn,1);
        for n=1:param.Nn
            cs_average(n)   = 1/param.Rp_n*(param.Rp_n/param.Nr_n)*sum(cs_barrato_t(end,start_index:end_index));
            start_index     = end_index + 1;
            end_index       = end_index + param.Nr_n;
        end
    end
    Csout           = sum(cs_average);
    Sout            = 100*(1/param.len_n*(param.len_n/(param.Nn))*Csout/param.cs_maxn);
    cs_star_t       = [cs_star_t;surfaceConcentration(cs_barrato_t(end,:)',jflux_t(end,:)',Q_t(end,:)',T_t(end,:)',param)'];
    SOC_t           = [SOC_t;Sout];
    t_tot           = [t_tot;t];
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%	Implemetation of the tracker described in paper
%	"MEEM: Robust Tracking via Multiple Experts using Entropy Minimization",
%   Jianming Zhang, Shugao Ma, Stan Sclaroff, ECCV, 2014
%
%	Copyright (C) 2014 Jianming Zhang
%
%	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/>.
%
%	If you have problems about this software, please contact: jmzhang@bu.edu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function iou = getIOU(r1,r2)

left = max((r1(:,1)),(r2(:,1)));
top = max((r1(:,2)),(r2(:,2)));
right = min((r1(:,1)+r1(:,3)),(r2(:,1)+r2(:,3)));
bottom = min((r1(:,2)+r1(:,4)),(r2(:,2)+r2(:,4)));
ovlp = max(right - left,0).*max(bottom - top, 0);
iou = ovlp./(r1(:,3).*r1(:,4)+r2(:,3).*r2(:,4)-ovlp);

end


function [hwm,hwmm]=webmap_of_catalog(catalog,force)
    % webmap_of_catalog plot earthquakes in a browser window
    % using the geoweb toolbox
    %
    % see webmap, webmarker
    iconFolder=fullfile('resrc','img','ico');
    icons=strcat('ball_',{'cyan';'blue1';'blue2';'purp1';'puce';'pink';'red';'orange';'yellow';'lime';'green';'mint'},'_30.png');
    %icons=icons(1:2:end);
    % TODO: future idea: this can  execute scripts in links. eg: col
    % TODO: future idea: allow this to be colored by depth, magnitude, or date
    disp(['Plotting events on web map.  They will plot from smallest to largest. ', newline,...
        'Within each magnitude bin, they are plotted according to depth range']);
    MAXQUAKES=1000;
    if catalog.Count >= 1000 && (~exist('force','var') || ~force)
        errordlg('Too many events for webmap.  Reduce events to < 1000, and try again');
        return
    end
    %catalog.sort('Depth','descend')
    % dep2color= @(x) [(x - min(x)) /max(x- min(x)), repmat(0,numel(x),1), sqrt(1-(x - min(x)) /max(x- min(x)))];
    smallestEvent=min(catalog.Magnitude);
    biggestEvent=max(catalog.Magnitude);
    eventRange=biggestEvent-smallestEvent;

    scaleit = @(n) (((n-smallestEvent) ./ eventRange ).*1.4 + 0.6).^1.8 ./ 4;

    %scaleit = @(n)(( n + abs(min(n)) + 1 )/4) .^ (1/3);
    %TODO include the MagnitudeType
    %disp('desc')
    desc={};
    for i=1:catalog.Count
        desc(i)={sprintf('(%8.4f, %8.4f) depth: %4.2f km, mag %3.2f\n',...
        catalog.Latitude(i), catalog.Longitude(i), catalog.Depth(i), catalog.Magnitude(i))};
    end
    %disp('title');
    tit=cellstr(char(catalog.Date,'uuuu-MM-dd HH:mm:ss'));
    hwm=webmap('World Topographic Map');
    wmlimits(hwm,bounds2(catalog.Latitude), bounds2(catalog.Longitude))
    %disp('wmmarker');

    magrange = [10:-1:-2];
    cmp=jet(numel(magrange));

    for j=2:numel(magrange)
        name(j)={sprintf('%.1f %c M < %.1f',magrange(j),char(8804),magrange(j-1))};
        idx(j)={catalog.Magnitude >= magrange(j) & catalog.Magnitude < magrange(j-1)};
        nInRange(j)=sum(idx{j});
    end

    % get rid of empty categories
    magrange(nInRange==0)=[];
    name(nInRange==0)=[];
    idx(nInRange==0)=[];
    nInRange(nInRange==0)=[];
    tot=cumsum(nInRange);
    toomany = find(tot > MAXQUAKES,1,'first');
    nDepthBins=numel(icons);
    [bins,edgs]=discretize(catalog.Depth,nDepthBins);
    cmp=jet(nDepthBins);
    %quakeIcons={}
    for q=1:numel(edgs)-1
        depthnames(q) ={ sprintf(' %.1f %c Z < %.1f km', edgs(q), char(8804), edgs(q+1))};
    end

    %% using multi-color option for wmmarker is super slow. Either
    % add specialized icons or do a double loop.
    %for j=numel(magrange) : -1 :1
        %if j > toomany+1
        %    continue;
        %end

        %for q=nDepthBins:-1:1
        %    quakeIcons(bins==q)={fullfile( iconFolder , icons{mod(q,numel(icons))+1})};
        %end

        for q=nDepthBins:-1:1
            %thisidx = idx{j};
            thisidx = bins==q;
            %sum(thisidx)
            if ~any(thisidx), continue, end
            % thiscolor = cmp(q,:);
            %thisIcon=fullfile( iconFolder , icons{mod(q,numel(icons))+1});
            if ~exist('hwmm','var')
                hwmm=wmmarker(catalog.Latitude(thisidx),catalog.Longitude(thisidx),...
                    'OverlayName',[depthnames{q}],...
                    ... 'OverlayName',[name{j}],...
                    ...'Color',thiscolor,...
                    ...'Icon',quakeIcons(thisidx),...
                    'Icon',fullfile( iconFolder , icons{mod(q,numel(icons))+1}),...
                    'Description',desc(thisidx),...
                    'FeatureName',tit(thisidx),...
                    'Alpha',0.7,...
                    'IconScale',scaleit(catalog.Magnitude(thisidx))./1.5,...
                    'Autofit',false);
            else
                hwmm(end+1)=wmmarker(catalog.Latitude(thisidx),catalog.Longitude(thisidx),...
                    ...'OverlayName',[name{j}],...
                    'OverlayName',[depthnames{q}],...
                    ...'Color',thiscolor,...
                    ...'Icon',quakeIcons(thisidx),...
                    'Icon',fullfile( iconFolder , icons{mod(q,numel(icons))+1}),...
                    ...'Icon',quakeIcons{q},...
                    'Description',desc(thisidx),...
                    'FeatureName',tit(thisidx),...
                    'Alpha',0.7,...
                    'IconScale',scaleit(catalog.Magnitude(thisidx))./1.5,...
                    'Autofit',false);
            end
        end
    %end
    %if ~isempty(toomany)
    %    fprintf('Only the largest %d events (of %d) were shown\n',tot(toomany),sum(nInRange));
    %end
    %{
    hwmm=wmmarker(catalog.Latitude, catalog.Longitude, 'FeatureName',tit);...,...
        ...'IconScale',scaleit(catalog.Magnitude),...
        ...'Description',desc,'Color',dep2color(catalog.Depth), 'FeatureName',tit);
    %}
end


function [alpha,b,eta,zeta,kappa] = solve_hkl_logistic_dual_fast(eta,data,Y,lambda,varargin);

% solve the milasso for a given regularization parameter
n = size(data.X,1);
p =length(data.affinity);
pX = size(data.X,2);
affs = data.affinity;
weights = data.weights;

% optional parameters
mingap = 1e-3;
display = 0;
gapeta = 1e-3;

% READ OPTIONAL PARAMETERS
args = varargin;
nargs = length(args);
for i=1:2:nargs
	switch args{i},
		case 'mingap',        mingap = args{i+1};
		case 'gapeta',        gapeta = args{i+1};
		case 'display',        display = args{i+1};
	end
end

if isempty(eta)
	eta = 1/p ./ weights.^2;
end
x = eta .* weights.^2;
x = max(x,0);
x = x / sum(x);

if display==1
	display = Inf;
end
beta_ARMIJO = .25 ;
sigma_ARMIJO = .1 ;
kmax_ARMIJO = 20;
kmax = 200;
tol_dx = 1e-12;
k = 1;
alpha=1;

% % optimization parameters for the dual problem
% optparam.tol = 1e-16;
% optparam.kmax = 200;
% optparam.tol_df = 1e-20;
% optparam.display= 0;
fx0 = Inf;

% solve the regular kernel learning problem
X = data.X;	% no need to center here for logistic
mY = mean(Y);
G = zeros(size(data.X,2),p);
for i=1:p
	G(data.groups{i},i) = 1;
end

% optimization parameter for logistic regression
optparam.tol = 1e-16;
optparam.kmax = 50;
optparam.tol_df = 1e-14;
optparam.display= 0;

%nu =  1e-6; % added ridge on kernels
while k<=kmax;
	x = max(x,0);
	x = x / sum(x);

	% compute value of function
	eta = ( x*(1-gapeta) + gapeta/p )./ weights.^2 ;
	zeta = zeros(p,1);
	for i=1:p
		zeta(affs{i}) = zeta(affs{i}) + 1./eta(i);
	end
	zeta = 1./zeta;
		reweight = G * sqrt(zeta);

	% solve the regular kernel learning problem in the primal
	if k==1
		% first iteration -> initialize with square loss
		% otherwise, take previous iteration weights
		temp = ( reweight .* ( X' * (Y - mY ) ) );
		wt = (  (reweight*reweight') .* ( X' * X ) + n * lambda * eye(pX) ) \   temp ;
	end

	Xr = repmat(reweight',n,1) .* X;
	[wt,exit_parameters] = minimize_newton(wt,@logistic_cost,optparam, Xr,Y,lambda);
	fx = logistic_cost(wt,Xr,Y,lambda);

	gradient_zeta = zeros(p,1);
	for i=1:p
		gradient_zeta(i) = - lambda/2 *  norm(wt(data.groups{i}))^2 / zeta(i);
	end

	gradient_eta = zeros(p,1);
	for i=1:p;
		gradient_eta(i) = sum( gradient_zeta(affs{i}) .* zeta(affs{i}).^2 ) / eta(i)^2;
	end
	gradient = (1-gapeta) * gradient_eta ./ weights.^2;

 alpha = 1;
	% check optimality condition
	optcond = max(x .* ( gradient - min(gradient)));
	if max(x .* ( gradient - min(gradient))) < mingap, break; end

	fx0=fx;

	% projected gradient
	xbar = x -   gradient;
	d = - gradient;
	x0=x;
	ialpha = 1;
	while ialpha<kmax_ARMIJO
		xalpha = simplex_projection(x + d * alpha);
 		% fxalpha = feval(fun,xalpha,varargin{:});
		eta = ( xalpha*(1-gapeta) + gapeta/p )./ weights.^2 ;
		zeta = zeros(p,1);
		for i=1:p
			zeta(affs{i}) = zeta(affs{i}) + 1./eta(i);
		end
		zeta = 1./zeta;

		reweight = G * sqrt(zeta);
		Xr = repmat(reweight',n,1) .* X;
		[wt0,exit_parameters] = minimize_newton(wt,@logistic_cost,optparam, Xr,Y,lambda);
		fxalpha = logistic_cost(wt0,Xr,Y,lambda);

 		if fx - fxalpha >= - sigma_ARMIJO * gradient' * ( xalpha - x );
 			% start to go up
			while ( fx - fxalpha >= - sigma_ARMIJO * gradient' * ( xalpha - x ) ) & ialpha<kmax_ARMIJO;
				alpha = alpha / beta_ARMIJO;
 				xalpha = simplex_projection(x + d * alpha);
				%fxalpha = feval(fun,xalpha,varargin{:});
				eta = ( xalpha*(1-gapeta) + gapeta/p )./ weights.^2 ;
				zeta = zeros(p,1);
				for i=1:p
					zeta(affs{i}) = zeta(affs{i}) + 1./eta(i);
				end
				zeta = 1./zeta;

				reweight = G * sqrt(zeta);
				Xr = repmat(reweight',n,1) .* X;
				[wt0,exit_parameters] = minimize_newton(wt,@logistic_cost,optparam, Xr,Y,lambda);
				fxalpha = logistic_cost(wt0,Xr,Y,lambda);

				% fprintf('+');
				ialpha = ialpha+1;
			end
			alpha = alpha * beta_ARMIJO;
			xalpha = simplex_projection(x + d * alpha);
			x = xalpha;
			break;
		else
			alpha = alpha * beta_ARMIJO;
 		end
		ialpha = ialpha+1;
	end
 	if ialpha==kmax_ARMIJO, alpha=1; end

	if ~isinf(display)
		if display==1,
			fprintf('k=%d - f=%f - armijo=%d - dx=%e - gap=%f\n',k,fx,ialpha,norm(x-x0),optcond);
		end
		if display>1,
			if mod(k,display)==1
				fprintf('k=%d - f=%f - armijo=%d - dx=%e - gap=%f\n',k,fx,ialpha,norm(x-x0),optcond);

			end
		end
	end
	if norm(x-x0)<tol_dx,    break; end

	if sum(x) > 1+1e-1, keyboard; end

	k=k+1;
end

% if k==kmax+1
%     keyboard;
% end

eta = ( x*(1-gapeta) + gapeta/p )./ weights.^2 ;
zeta = zeros(p,1);
for i=1:p
	zeta(affs{i}) = zeta(affs{i}) + 1./eta(i);
end
zeta = 1./zeta;
reweight = G * sqrt(zeta);
Xr = repmat(reweight',n,1) .* X;
[wt,exit_parameters] = minimize_newton(wt,@logistic_cost,optparam, Xr,Y,lambda);

pred = data.X * ( reweight.* wt);
b= center_logistic(pred,Y);

alpha = 1/(n*lambda) * ( Y - 1./( 1 + exp( - pred - b ) ) );

kappa = zeros(p,p);
for i=1:p
	kappa(i,affs{i}) = zeta(affs{i})./eta(i);
end

if isinf(display)
	fprintf('k=%d - f=%f -  gap=%f\n',k,fx,optcond);
end
% if optcond> 1,
% keyboard
% end


filename='Gripping_hexahedra_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'nonadjoint_compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
constraint_case = 'INEQUALITY';
optimizer = 'IPOPT'; incrementFactor = 1;
filterType = 'P1';

nsteps = 1;
Vfrac_final = 0.2;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function v = tfocs_normsq( x, scaling )

% TFOCS_NORMSQ    Squared norm.
%    By default, TFOCS_NORMSQ(X) = TFOCS_DOT(X,X). However, certain
%    objects may have more efficient ways of computing this value.
%    If so, TFOCS_NORMSQ should be overloaded to take advantage of
%    this. However, the numerical equivalence to TFOCS_DOT(X,X) must
%    be preserved.
%
%   TFOCS_NORMSQ( X, D ) = TFOCS_DOT( X, D.*X ) is a scaled norm

if nargin < 2 || isempty(scaling)
    v = tfocs_dot( x, x );
elseif numel(scaling) == 1
    v = scaling*tfocs_dot( x, x );
else
    v = tfocs_dot( x, scaling.*x );
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.