function output=fcn_LeaderAcceleration(u)
global leaderNum dim samplePos_all nodenum formationType
global rotateFlag tempp2 tempp3

u_all = reshape(u(1:dim*nodenum*3),dim,nodenum*3);
v_all=u_all(:,1:nodenum);
p_all=u_all(:,nodenum+1:2*nodenum);
control_all=u_all(:,2*nodenum+1:3*nodenum);
currentTime=u(end);
%output
a_leader_all=zeros(dim,leaderNum);

% speed up to speed=1
time_start_speedup=0;
time_span_speedup=5;
if currentTime>time_start_speedup && currentTime<time_start_speedup+time_span_speedup
    ax=fcn_sin(time_start_speedup,time_span_speedup,currentTime)/pi;
    a_leader_all(:,1)=[ax,0]';
    a_leader_all(:,2)=[ax,0]';
    a_leader_all(:,3)=[ax,0]';
end

% slow down to speed=0
time_start_slowdown=10;
time_span_slowdown=5;
if currentTime>time_start_slowdown && currentTime<time_start_slowdown+time_span_slowdown
    %ax=fcn_normalDistribution(time_start_slowdown,time_span_slowdown, 2, currentTime);
    ax=-fcn_sin(time_start_slowdown,time_span_slowdown,currentTime)/pi;
    a_leader_all(:,1)=[ax,0]';
    a_leader_all(:,2)=[ax,0]';
    a_leader_all(:,3)=[ax,0]';
end

% rotate
v3=v_all(:,3);
v2=v_all(:,2);
v1=v_all(:,1);
p3=p_all(:,3);
p2=p_all(:,2);
p1=p_all(:,1);
R90=[cos(pi/2),-sin(pi/2);
     sin(pi/2),cos(pi/2)];
time_start_rotate=20;
time_span_rotate=10;
if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate
    if rotateFlag==1
        tempp2=p2;
        tempp3=p3;
        rotateFlag=0;
    end
    % agent 2
    acce_mag=-fcn_SlowDownSpeedUp(time_start_rotate, time_span_rotate, currentTime, 4);
%     acce_vec=(p1+R90'*(tempp2-p1))-tempp2;
%     acce_vec=acce_vec/norm(acce_vec);
    acce_vec=[1 0]';
    a_leader_all(:,2)=acce_mag*acce_vec;
    % agent 3
    acce_mag=-fcn_SlowDownSpeedUp(time_start_rotate, time_span_rotate, currentTime, 4);
%     acce_vec=(p1+R90'*(tempp3-p1))-tempp3;
%     acce_vec=acce_vec/norm(acce_vec);
    acce_vec=[0 1]';
    a_leader_all(:,3)=acce_mag*acce_vec;
end

% move downward
time_start_downward=time_start_rotate;
time_span_downward=time_span_rotate;
if currentTime>=time_start_downward && currentTime<=time_start_downward+time_span_downward
    % agent 1
    ay=fcn_sin(time_start_downward,time_span_downward,currentTime)/pi;
    a_leader_all(:,1)=[0,-ay]';
    % agent 2
    a_leader_all(:,2)=a_leader_all(:,2)+[0,-ay]';
    % agent 3
    a_leader_all(:,3)=a_leader_all(:,3)+[0,-ay]';
end

% slow down to speed=0
time_start_slowdown2=time_start_downward+time_span_downward+5;
time_span_slowdown2=5;
if currentTime>time_start_slowdown2 && currentTime<time_start_slowdown2+time_span_slowdown2
    %ax=fcn_normalDistribution(time_start_slowdown,time_span_slowdown, 2, currentTime);
    ay=-fcn_sin(time_start_slowdown2,time_span_slowdown2,currentTime)/pi;
    a_leader_all(:,1)=[0,-ay]';
    a_leader_all(:,2)=[0,-ay]';
    a_leader_all(:,3)=[0,-ay]';
end

% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate
%     % agent 3
%     aUnit=(p1-p3)/norm(p1-p3);
%     tht_dot=fcn_sin(time_start_rotate,time_span_rotate,currentTime);
%     speed_dot=fcn_sin(time_start_rotate,time_span_rotate,currentTime);
%     a_leader_all(:,3)=aUnit*tht_dot*norm(v3)+speed_dot*R90*aUnit;
%     % agent 2
%     aUnit=(p1-p2)/norm(p1-p2);
%     %tht_dot=fcn_sin(time_start_rotate,time_span_rotate,currentTime);
%     speed_dot=fcn_sin(time_start_rotate,time_span_rotate,currentTime);
%     a_leader_all(:,2)=aUnit*tht_dot*norm(v2)+speed_dot*R90*aUnit;
% end

% % scaling
% a_leader_all(2,2)=-fcn_TanhManeuver(20, 1, 10, currentTime);
% a_leader_all(2,3)=fcn_TanhManeuver(20, 1, 10, currentTime);

% % rotation
% v3=v_all(:,3);
% v2=v_all(:,2);
% v1=v_all(:,1);
% p3=p_all(:,3);
% p2=p_all(:,2);
% p1=p_all(:,1);
% R90=[cos(pi/2),-sin(pi/2);
%      sin(pi/2),cos(pi/2)];
% time_start_rotate=20;
% % agent 3
% time_span_rotate3=10;
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate3
%     if rotateCenterFlag==1
%         roateCenter(:,3)=p3-[0,5]';
%         rotateCenterFlag=0;
%         r3=norm(roateCenter(:,3)-p3);
%         roateCenter(:,2)=roateCenter(:,3);
%         r2=norm(roateCenter(:,2)-p2);
%         roateCenter(:,1)=roateCenter(:,3);
%         r1=norm(roateCenter(:,1)-p1);
%     end
%     tht_dot=pi/time_span_rotate3;
%     speed_dot=0;
%     a_leader_all(:,3)=R90'*v3/norm(v3)*tht_dot*norm(v3)+speed_dot*v3/norm(v3);
% end
% % agent 2
% time_span_rotate2=time_span_rotate3;
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate2
%     tht_dot=pi/time_span_rotate2;
%     if currentTime>=time_start_rotate && currentTime<=time_start_rotate+1
%         speed_dot=fcn_sin(time_start_rotate,1,currentTime)/2.6;
%     else
%         speed_dot=0;
%     end
%     a_leader_all(:,2)=R90'*v2/norm(v2)*tht_dot*norm(v2)+speed_dot*v2/norm(v2);
% end
% if currentTime>=time_start_rotate+time_span_rotate2 && currentTime<=time_start_rotate+time_span_rotate2+1
%         speed_dot=-fcn_sin(time_start_rotate,1,currentTime)/2.6;
%         tht_dot=0;
%         a_leader_all(:,2)=R90'*v2/norm(v2)*tht_dot*norm(v2)+speed_dot*v2/norm(v2);
% end
% % agent 1
% time_span_rotate1=time_span_rotate3;
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate1
%     tht_dot=pi/time_span_rotate3;%fcn_sin(time_start_rotate,time_span_rotate1,currentTime);
%     if currentTime>=time_start_rotate && currentTime<=time_start_rotate+1
%         speed_dot=fcn_sin(time_start_rotate,1,currentTime)/3;
% %         tht_dot=
%     else
%         speed_dot=0;
%     end
%     aUnit=(roateCenter(:,1)-p1)/norm(roateCenter(:,1)-p1);
%     a_leader_all(:,1)=aUnit*tht_dot*norm(v1)+speed_dot*v1/norm(v1);
% end
% if currentTime>=time_start_rotate+time_span_rotate2 && currentTime<=time_start_rotate+time_span_rotate2+1
%         speed_dot=-fcn_sin(time_start_rotate,1,currentTime)/2.6;
%         tht_dot=0;
%         a_leader_all(:,2)=R90'*v2/norm(v2)*tht_dot*norm(v2)+speed_dot*v2/norm(v2);
% end

% % agent 1: distance-based formation control law
% time_span_rotate1=time_span_rotate3;
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate1
%     delta12=norm(p1-p2)-2*sqrt(2);
%     delta13=norm(p1-p3)-2*sqrt(2);
%     a_leader_all(:,1)=5*(delta12*(p2-p1)+delta13*(p3-p1)) + 0.5*(delta12*(v2-v1)+delta13*(v3-v1));
% end

% % agent 2
% time_span_rotate2=26;
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate2
%     v2=v_all(:,2);
%     p2=p_all(:,2);
%     R90=[cos(pi/2),-sin(pi/2);
%              sin(pi/2),cos(pi/2)];
%     tht_dot=fcn_sin(time_start_rotate,time_span_rotate2,currentTime);
%     speed_dot=0;
%     a_leader_all(:,2)=R90'*v2/norm(v2)*tht_dot*norm(v2)+speed_dot*v2/norm(v2);
% end
% % agent 1
% time_span_rotate1=18;
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate1
%     v1=v_all(:,1);
%     p1=p_all(:,1);
%     R90=[cos(pi/2),-sin(pi/2);
%              sin(pi/2),cos(pi/2)];
%     tht_dot=fcn_sin(time_start_rotate,time_span_rotate1,currentTime);
%     speed_dot=0;
%     a_leader_all(:,1)=R90'*v1/norm(v1)*tht_dot*norm(v1)+speed_dot*v1/norm(v1);
% end
% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate
%     v3=v_all(:,3);
%     v2=v_all(:,2);
%     v1=v_all(:,1);
%     p3=p_all(:,3);
%     p2=p_all(:,2);
%     p1=p_all(:,1);
%     R90=[cos(pi/2),-sin(pi/2);
%          sin(pi/2),cos(pi/2)];
%     % agent 3
%     tht_dot=-fcn_normalDistribution(time_start_rotate,time_span_rotate, 3.15, currentTime);
%     speed_dot=0;
%     a_leader_all(:,3)=R90'*v3/norm(v3)*tht_dot*norm(v3)+speed_dot*v3/norm(v3);
%     % agent 2
%     tht_dot=-fcn_normalDistribution(time_start_rotate,time_span_rotate*1.5, 3.15, currentTime);
%     speed_dot=0;
%     a_leader_all(:,2)=R90'*v2/norm(v2)*tht_dot*norm(v2)+speed_dot*v2/norm(v2);
%     % agent 1
%     tht_dot=-fcn_normalDistribution(time_start_rotate,time_span_rotate*, 2, currentTime);
%     speed_dot=0;%fcn_normalDistribution(time_start_rotate,time_span_rotate, 1, currentTime);
%     a_leader_all(:,1)=R90'*v1/norm(v1)*tht_dot*norm(v1)+speed_dot*v1/norm(v1);
% end

% if currentTime>=time_start_rotate && currentTime<=time_start_rotate+time_span_rotate
%     offset=4;
%     v3=v_all(:,3);
%     v2=v_all(:,2);
%     v1=v_all(:,1);
%     p3=p_all(:,3);
%     p2=p_all(:,2);
%     p1=p_all(:,1);
%     R90=[cos(pi/2),-sin(pi/2);
%          sin(pi/2),cos(pi/2)];
%     threshhold=0.005;
%     if rotateCenterFlag==1
%         roateCenter(:,3)=p3-[0,offset]';
%         rotateCenterFlag=0;
%         r3=norm(roateCenter(:,3)-p3);
%         rotateTime(3)=pi*r3/norm(v3);
%         roateCenter(:,2)=roateCenter(:,3);
%         r2=norm(roateCenter(:,2)-p2);
%         rotateTime(2)=pi*r2/norm(v2);
% %         roateCenter(:,1)=roateCenter(:,3)+(p1-p3);
%         roateCenter(:,1)=roateCenter(:,2)+[p1(1)-p2(1),0]';
%         r1=norm(roateCenter(:,1)-p1);
%         rotateTime(1)=pi*r1/norm(v1);
%     end
%     % agent 3
%     tht=atan2(v3(2),v3(1));
%     if ~(tht>pi-threshhold || tht<-pi+threshhold)
%         r3=norm(roateCenter(:,3)-p3);
%         aUnit=(roateCenter(:,3)-p3)/norm(roateCenter(:,3)-p3);
%         a_leader_all(:,3)=aUnit*norm(v3)^2/r3;%+speed_dot*v3/norm(v3);
%     end
%     % agent 2
%     tht=atan2(v2(2),v2(1));
%     if ~(tht>pi-threshhold || tht<-pi+threshhold)
%         r2=norm(roateCenter(:,2)-p2);
%         aUnit=(roateCenter(:,2)-p2)/norm(roateCenter(:,2)-p2);
%         speed_dot=-fcn_SlowDownSpeedUp(time_start_rotate, 5, currentTime, 8);
%         a_leader_all(:,2)=aUnit*norm(v2)^2/r2;%+speed_dot*v2/norm(v2);
%     end
%     % agent 1
%     tht=atan2(v1(2),v1(1));
%     if ~(tht>pi-threshhold || tht<-pi+threshhold)
%         r1=norm(roateCenter(:,1)-p1);
%         aUnit=(roateCenter(:,1)-p1)/norm(roateCenter(:,1)-p1);
%         speed_dot=-fcn_SlowDownSpeedUp(time_start_rotate, 5, currentTime, 8);
%         a_leader_all(:,1)=aUnit*norm(v1)^2/r1;%+speed_dot*v1/norm(v1);
%     end
% end

% % slow down and speed up to align the agents
% time_start_align=20;
% time_span=5;
% if currentTime>time_start_align && currentTime<time_start_align+time_span
% %     % agent 3: back 4 meters
% %     distance_change=4;
% %     a_leader_all(2,3)=-fcn_SlowDownSpeedUp(time_start_align, time_span, currentTime, distance_change);
% %     % agent 2: foward 2 meters
% %     distance_change=2.3;
% %     a_leader_all(2,2)=fcn_SlowDownSpeedUp(time_start_align, time_span, currentTime, distance_change);
%     % agent 1 slow down
%     distance_change=2;
%     a_leader_all(1,1)=fcn_SlowDownSpeedUp(time_start_align, time_span, currentTime, distance_change);
% end

% output: replace the leaders' velocity with the one prescribed above
control_all(:,1:leaderNum) = a_leader_all;
output=reshape(control_all, dim*nodenum, 1);


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function varargout = regularizer(varargin)
%
% Main function for the regularization model, uses persistent parameter
%
% The regularization functional is either based on a linear differential
% operator B, i.e.
%
%        S(Y) = 0.5*alpha*hd*|B*Y|^2
%
%  or a general (non-linear) functional S(Y)
%
% Typical call
%    [Sc,dS,d2S] = regularizer(Y,omega,m)
%
%  1.     initialize (pick regularizer and set parameters)
%
%         regularizer('reset','regularizer','mfElastic','alpha',1e3,...
%               'mu',1,'lambda',0','rigid2D','center',center);
%
%  2.     [Sc,dS,d2S] = regularizer(Y,omega,m)
%        evaluates the regularization model, e.g. Sc = 0.5*yc'*B'*B*yc and its derivatives
%
%  A = alpha*hd*B'*B is made persistent for eficiency
%  hd = prod(omega./m)
%  an option grid is introduce to indicate the appropriate discretization:
%  staggered for elastic and diffusive,
%  cell-centered for curvature
%  nodal for TV, hyper elastic or diffusive EPI regularization
%==============================================================================

function varargout = regularizer(varargin)

persistent OPTN A

if nargin == 0 && nargout == 0 && isempty(OPTN),
    help(mfilename);
    return;
end;

% -----------------------------------------------------------------------------
% handle options
[method,OPTN,task,stop] = dealOptions(mfilename,OPTN,varargin{:});

% check the grid according to the regularizer to be used
% setup default solver for Gauss-Newton systems

if strcmp(task,'set') || strcmp(task,'reset'),

    switch method,
        case 'mfElastic',
            scheme     = 'elastic';
            matrixFree = 1;
            grid       = 'staggered';
            solver     = 'MG-elastic';

        case 'mbElastic',
            scheme     = 'elastic';
            matrixFree = 0;
            grid       = 'staggered';
            solver     = 'backslash';

        case 'mbElasticNodal',
            scheme     = 'elasticNodal';
            matrixFree = 0;
            grid       = 'nodal';
            solver     = 'backslash';

        case 'mfElasticNodal',
            scheme     = 'elasticNodal';
            matrixFree = 1;
            grid       = 'nodal';
            solver     = 'PCG';

        case 'mbCurvature',
            scheme     = 'curvature';
            matrixFree = 0;
            grid       = 'cell-centered';
            solver     = 'backslash';

        case 'mfCurvature',
            scheme     = 'curvature';
            matrixFree = 1;
            grid       = 'cell-centered';
            solver     = 'PCG-curvature';

        case 'mbHyperElastic',
            scheme     = 'hyperElastic';
            matrixFree = 0;
            grid       = 'nodal';
            solver     = 'backslash';

        case 'mfHyperElastic',
            scheme     = 'hyperElastic';
            matrixFree = 1;
            grid       = 'nodal';
            solver     = 'PCG-hyperElastic';

        case 'mbHyperElasticFEM',
            scheme     = 'hyperElasticFEM';
            matrixFree = 0;
            grid       = 'FEM';
            solver     = 'backslash';

        case 'mbElasticFEM',
            scheme     = 'elasticFEM';
            matrixFree = 0;
            grid       = 'FEM';
            solver     = 'backslash';

      case 'mfElasticFEM',
            scheme     = 'elasticFEM';
            matrixFree = 1;
            grid       = 'FEM';
            solver     = 'MG';

        otherwise
            scheme = method;
            [grid,matrixFree,solver] = feval(scheme,'para',[],[],OPTN{:});
    end;

    [dummy,OPTN] = dealOptions(mfilename,OPTN,'set','scheme',scheme,...
        'grid',grid,'matrixFree',matrixFree,'solver',solver);
end;

% return, if no further work has to be handled
if stop,
    varargout{1} = method;
    if nargout > 1, varargout{2} = OPTN;  varargout{3} = A;  end;
    return;
end
% -----------------------------------------------------------------------------
% do the work

% extract regularization parameters
scheme      = dealOptions(mfilename,OPTN,'get','scheme');
alpha       = dealOptions(mfilename,OPTN,'get','alpha');
matrixFree  = dealOptions(mfilename,OPTN,'get','matrixFree');

% extract variables
yc     = varargin{1};
omega  = varargin{2};
m      = varargin{3};
doDerivative = (nargout>1);
varargin = varargin(4:end);

for k=1:2:length(varargin), % overwrites defaults
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;
[Sc,dS,d2S] = feval(scheme,yc,omega,m,...
    'alpha',alpha,'matrixFree',matrixFree,...
    'doDerivative',doDerivative,OPTN{:},varargin{:});
varargout = {Sc,dS,d2S};

%------------------------------------------------------------------------------

function runMinimalExample
fprintf('run minimal example for %s\n',mfilename)
keyboard
%==============================================================================


function [lat, long ] = gcj2wgs_exact(gcjLat, gcjLng)
% adapted from https://github.com/googollee/eviltransform

    initDelta = 0.01;
    threshold = 0.000001;
    dLat = initDelta;
    dLng = initDelta;
    mLat = gcjLat - dLat;
    mLng = gcjLng - dLng;
    pLat = gcjLat + dLat;
    pLng = gcjLng + dLng;

    for i=1:30
        wgsLat = (mLat + pLat) / 2;
        wgsLng = (mLng + pLng) / 2;
        [tmplat, tmplng] = wgs2gcj(wgsLat, wgsLng);
        dLat = tmplat - gcjLat;
        dLng = tmplng - gcjLng;
        if all(abs(dLat) < threshold & abs(dLng) < threshold)
            lat = wgsLat;
            long =wgsLng;
            return;
        end
        pLat(dLat>=0) = wgsLat(dLat>=0);
        mLat(dLat<=0) = wgsLat(dLat<=0);
        pLng(dLng>=0) = wgsLng(dLng>=0);
        mLng(dLng<=0) = wgsLng(dLng<=0);
    end

   lat = wgsLat;
   long =wgsLng;

end


function ray = rayTracer(ray,ord,rMax)
%+========================================================================+
%|                                                                        |
%|           OPENRAY - LIBRARY FOR TRI-DIMENSIONAL RAY TRACING            |
%|           openRay is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : rayTracer.m                                   |
%|    #    |   VERSION    : 0.41                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.04.2018                                    |
%| ( === ) |   SYNOPSIS   : Ray tracer with octree speedup                |
%|  `---'  |                                                              |
%+========================================================================+

% Infos
disp('====> RAY TRACING <====')
tps = time();

% Mesh dimension
Nelt = length(ray.msh);

% Prepare sphere
tree = rayTreeInit(ray);

% Available ray
ind = find( (ray.dst<rMax) & (sum(ray.dir.^2,2)~=0) );

% Current order
n = length(ray.pos)-1;

% Infos
disp([' ~~> Tree with ',num2str(length(tree)),' stage(s) - Elapsed time is ', ...
                num2str(time()-tps),' seconds.'])

% Iterative loop
while ~isempty(ind) && (n < ord)
    % Infos
    tps = time();

    % Full repartition for smallest meshes
    if (Nelt < 50)
        Iray = cell(Nelt,1);
        for el = 1:Nelt
            Iray{el} = ind;
        end

    % Hierarchical repartition
    else
        Iray = rayTree(ray,tree,ind);
    end

    % Intersection between mesh and ray
    ray = rayCollision(ray,Iray);

    % Update available ray
    ind = find( (ray.dst<rMax) & (sum(ray.dir.^2,2)~=0) );

    % Order incrementation
    n = n + 1;

    % Infos
    disp([' + Step ',num2str(n), ' - Elapsed time is ', ...
                num2str(time()-tps),' seconds.'])
end
end

function tps = time()
tps = clock;
tps = tps(4)*3600 + tps(5)*60 + tps(6);
end

%     % Representation graphique
%     figure
%     hold on
%     for el = 1:Nelt
%         subElt = ray.msh.sub(el);
%         subRay = ray.sub(Iray{el});
%         plot(subElt,1)
%         plot(subRay)
%         pause
%     end
%     hold off


%kitexture 'Texture Feature Extraction Using LAW Metrics'
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros itexture.pane file
%
% Parameters:
% InputFile: i 'Input object ', required: 'input object'
% OutputFile: o 'Output object', required: 'output object'
% Toggle: L5L5 'L5L5', default: 0: 'If set, apply L5L5 texture kernel'
% Toggle: E5L5 'E5L5', default: 0: 'If set, apply E5L5 texture kernel'
% Toggle: S5L5 'S5L5', default: 0: 'If set, apply S5L5 texture kernel'
% Toggle: W5L5 'W5L5', default: 0: 'If set, apply W5L5 texture kernel'
% Toggle: R5L5 'R5L5', default: 0: 'If set, apply R5L5 texture kernel'
% Toggle: L5E5 'L5E5', default: 0: 'If set, apply L5E5 texture kernel'
% Toggle: E5E5 'E5E5', default: 0: 'If set, apply E5E5 texture kernel'
% Toggle: S5E5 'S5E5', default: 0: 'If set, apply S5E5 texture kernel'
% Toggle: W5E5 'W5E5', default: 0: 'If set, apply W5E5 texture kernel'
% Toggle: R5E5 'R5E5', default: 0: 'If set, apply R5E5 texture kernel'
% Toggle: L5S5 'L5S5', default: 0: 'If set, apply L5S5 texture kernel'
% Toggle: E5S5 'E5S5', default: 0: 'If set, apply E5S5 texture kernel'
% Toggle: S5S5 'S5S5', default: 0: 'If set, apply S5S5 texture kernel'
% Toggle: W5S5 'W5S5', default: 0: 'If set, apply W5S5 texture kernel'
% Toggle: R5S5 'R5S5', default: 0: 'If set, apply R5S5 texture kernel'
% Toggle: L5W5 'L5W5', default: 0: 'If set, apply L5W5 texture kernel'
% Toggle: E5W5 'E5W5', default: 0: 'If set, apply E5W5 texture kernel'
% Toggle: S5W5 'S5W5', default: 0: 'If set, apply S5W5 texture kernel'
% Toggle: W5W5 'W5W5', default: 0: 'If set, apply W5W5 texture kernel'
% Toggle: R5W5 'R5W5', default: 0: 'If set, apply R5W5 texture kernel'
% Toggle: L5R5 'L5R5', default: 0: 'If set, apply L5R5 texture kernel'
% Toggle: E5R5 'E5R5', default: 0: 'If set, apply E5R5 texture kernel'
% Toggle: S5R5 'S5R5', default: 0: 'If set, apply S5R5 texture kernel'
% Toggle: W5R5 'W5R5', default: 0: 'If set, apply W5R5 texture kernel'
% Toggle: R5R5 'R5R5', default: 0: 'If set, apply R5R5 texture kernel'
%
% Example: o = kitexture(i, {'i','';'o','';'L5L5',0;'E5L5',0;'S5L5',0;'W5L5',0;'R5L5',0;'L5E5',0;'E5E5',0;'S5E5',0;'W5E5',0;'R5E5',0;'L5S5',0;'E5S5',0;'S5S5',0;'W5S5',0;'R5S5',0;'L5W5',0;'E5W5',0;'S5W5',0;'W5W5',0;'R5W5',0;'L5R5',0;'E5R5',0;'S5R5',0;'W5R5',0;'R5R5',0})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% itexture - Texture Feature Extraction Using LAW Metrics
%
%  DESCRIPTION
% .I itexture
% will extract texture features from an image by convolving the original image
% with a set of special spatial filter kernels.  The spatial filters are
% comprised of 5 x 5 kernels
% derived from center-weighted vectors defined by the LAWS texture measure.
%
% Each texture measure calculated will be stored as width-height planes along
% the elements direction in the Output Object (o).
% The order in which the (selected) texture measures will be stored in the
% output object are:
% .DS
%         L5L5
%         L5E5
%         L5S5
%         L5W5
%         L5R5
%         E5L5
%         E5E5
%         E5S5
%         E5W5
%         E5R5
%         S5L5
%         S5E5
%         S5S5
%         S5W5
%         S5R5
%         W5L5
%         W5E5
%         W5S5
%         W5W5
%         W5R5
%         R5L5
%         R5E5
%         R5S5
%         R5W5
%         R5R5
% .DE
%
% If the input object elements dimension is greater than 1 then the results
% obtained by applying the first selected kernel to each band are stored by planes
% in the output object, followed by the results obtained by applying the
% next selected texture kernel to all bands, and so on. Thus a 3-band input
% image, operated on by the L5L5 and R5R5 kernels, will produce a 6-band
% output image with the results as follows:
% .DS
%       Band 0 - L5L5 on Band 0 of input image
%       Band 1 - L5L5 on Band 1 of input image
%       Band 2 - L5L5 on Band 2 of input image
%       Band 3 - R5R5 on Band 0 of input image
%       Band 4 - R5R5 on Band 1 of input image
%       Band 5 - R5R5 on Band 2 of input image
% .DE
%
% The five center-weighted vectors are:
% .DS
%          L5 = [ 1  4  6  4  1]
%          E5 = [-1 -2  0  2  1]
%          S5 = [-1  0  2  0 -1]
%          W5 = [-1  2  0 -2  1]
%          R5 = [ 1 -4  6 -4  1]
% .DE
%
% Each 5 x 5 kernel is derived from multiplying a particular combination of
% two of the above vectors.  This results in 25 possible 5 x 5 kernels.
% Note that 10 of the kernels are formed by taking the transpose.  For example,
% L5E5 is a 5 x 5 kernel formed by multiplying the vectors, L5 with E5.  It's
% transpose is E5L5, and is similarly formed by multiplying the vectors,
% E5 with L5.  One of these kernels is more sensitive to horizontal
% changes in texture, while the other is sensitive to vertical changes in
% texture.
%
% The kernels are applied with a centered hotspot.
%
% The sum of the elements of each kernel is zero, which results in the output
% image having a mean of zero.  Therefore the relevant texture information is
% contained in the image variance of the microtexture features.  The LAW
% kernels were designed to be sensitive to structures such as edges, ripples,
% and spots.
%
% Choosing which kernel to use will depend on the nature of the texture of
% interest, and will require trial and error to decide which produces the
% feature with the most discriminating power.  In general, if a texture kernel
% of say, L5S5 is used, one may also want to use its transpose, S5L5 for
% certain types of quasiperiodic variations commonly found in textured images.
%
% The colorspace model for the output object is always set to KNONE.
%
%
%
%  EXAMPLES
%
%  "SEE ALSO"
%
%  RESTRICTIONS
% .I itexture
% will operate on input objects of any data storage type.
%
% The output object will be of type KLONG, KDOUBLE, or
% KDCOMPLEX, determined by the data type of the input object.
% If the input object is of data type KBYTE, KUBYTE, KSHORT, or KUSHORT,
% then it is converted up to an KLONG image. If the input
% is of type KFLOAT or KDOUBLE, then the result will be KDOUBLE.
% Any complex input type is converted to type KDCOMPLEX.
%
%  REFERENCES
% K. I. Laws, "Rapid texture identification" Proc. SPIE, vol. 238, pp.376-380,
% 1980.
%
% J. Y. Hsiao, and A. A. Sawchuk, "Supervised textured image segmentation
% using feature smoothing and probabilistic relaxation techniques" IEEE Trans.
% Pattern Anal. Machine Intell., vol. 11, No. 12, pp 1279-1292, 1989.
%
%  COPYRIGHT
% Copyright (C) 1993 - 1997, Khoral Research, Inc. ("KRI")  All rights reserved.
%

function varargout = kitexture(varargin)
if nargin ==0
  Inputs={};arglist={'',''};
elseif nargin ==1
  Inputs=varargin{1};arglist={'',''};
elseif nargin ==2
  Inputs=varargin{1}; arglist=varargin{2};
else error('Usage: [out1,..] = kitexture(Inputs,arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={'i', '__input';'o', '__output';'L5L5', 0;'E5L5', 0;'S5L5', 0;'W5L5', 0;'R5L5', 0;'L5E5', 0;'E5E5', 0;'S5E5', 0;'W5E5', 0;'R5E5', 0;'L5S5', 0;'E5S5', 0;'S5S5', 0;'W5S5', 0;'R5S5', 0;'L5W5', 0;'E5W5', 0;'S5W5', 0;'W5W5', 0;'R5W5', 0;'L5R5', 0;'E5R5', 0;'S5R5', 0;'W5R5', 0;'R5R5', 0};
maxval={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
minval={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
istoggle=[0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1];
was_set=istoggle * 0;
paramtype={'InputFile','OutputFile','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle','Toggle'};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=1; nextinput=1; nextoutput=1;
  for ii=1:size(arglist,1)
  wasmatched=0;
  for jj=1:size(narglist,1)
   if strcmp(arglist{ii,1},narglist{jj,1})  % a given argument was matched to the possible arguments
     wasmatched = 1;
     was_set(jj) = 1;
     if strcmp(narglist{jj,2}, '__input')
      if (nextinput > length(Inputs))
        error(['Input ' narglist{jj,1} ' has no corresponding input!']);
      end
      narglist{jj,2} = 'OK_in';
      nextinput = nextinput + 1;
     elseif strcmp(narglist{jj,2}, '__output')
      if (nextoutput > nargout)
        error(['Output nr. ' narglist{jj,1} ' is not present in the assignment list of outputs !']);
      end
      if (isempty(arglist{ii,2}))
        narglist{jj,2} = 'OK_out';
      else
        narglist{jj,2} = arglist{ii,2};
      end

      nextoutput = nextoutput + 1;
      if (minval{jj} == 0)
         NumReqOutputs = NumReqOutputs - 1;
      end
     elseif isstr(arglist{ii,2})
      narglist{jj,2} = arglist{ii,2};
     else
        if strcmp(paramtype{jj}, 'Integer') & (round(arglist{ii,2}) ~= arglist{ii,2})
            error(['Argument ' arglist{ii,1} ' is of integer type but non-integer number ' arglist{ii,2} ' was supplied']);
        end
        if (minval{jj} ~= 0 | maxval{jj} ~= 0)
          if (minval{jj} == 1 & maxval{jj} == 1 & arglist{ii,2} < 0)
            error(['Argument ' arglist{ii,1} ' must be bigger or equal to zero!']);
          elseif (minval{jj} == -1 & maxval{jj} == -1 & arglist{ii,2} > 0)
            error(['Argument ' arglist{ii,1} ' must be smaller or equal to zero!']);
          elseif (minval{jj} == 2 & maxval{jj} == 2 & arglist{ii,2} <= 0)
            error(['Argument ' arglist{ii,1} ' must be bigger than zero!']);
          elseif (minval{jj} == -2 & maxval{jj} == -2 & arglist{ii,2} >= 0)
            error(['Argument ' arglist{ii,1} ' must be smaller than zero!']);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} < minval{jj})
            error(['Argument ' arglist{ii,1} ' must be bigger than ' num2str(minval{jj})]);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} > maxval{jj})
            error(['Argument ' arglist{ii,1} ' must be smaller than ' num2str(maxval{jj})]);
          end
        end
     end
     if ~strcmp(narglist{jj,2},'OK_out') &  ~strcmp(narglist{jj,2},'OK_in')
       narglist{jj,2} = arglist{ii,2};
     end
   end
   end
   if (wasmatched == 0 & ~strcmp(arglist{ii,1},''))
        error(['Argument ' arglist{ii,1} ' is not a valid argument for this function']);
   end
end
% match the remaining inputs/outputs to the unused arguments and test for missing required inputs
 for jj=1:size(narglist,1)
     if  strcmp(paramtype{jj}, 'Toggle')
        if (narglist{jj,2} ==0)
          narglist{jj,1} = '';
        end;
        narglist{jj,2} = '';
     end;
     if  ~strcmp(narglist{jj,2},'__input') && ~strcmp(narglist{jj,2},'__output') && istoggle(jj) && ~ was_set(jj)
          narglist{jj,1} = '';
          narglist{jj,2} = '';
     end;
     if strcmp(narglist{jj,2}, '__input')
      if (minval{jj} == 0)  % meaning this input is required
        if (nextinput > size(Inputs))
           error(['Required input ' narglist{jj,1} ' has no corresponding input in the list!']);
        else
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        end
      else  % this is an optional input
        if (nextinput <= length(Inputs))
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end;
     else
     if strcmp(narglist{jj,2}, '__output')
      if (minval{jj} == 0) % this is a required output
        if (nextoutput > nargout & nargout > 1)
           error(['Required output ' narglist{jj,1} ' is not stated in the assignment list!']);
        else
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
          NumReqOutputs = NumReqOutputs-1;
        end
      else % this is an optional output
        if (nargout - nextoutput >= NumReqOutputs)
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end
     end
  end
end
if nargout
   varargout = cell(1,nargout);
else
  varargout = cell(1,1);
end
global KhorosRoot
if exist('KhorosRoot') && ~isempty(KhorosRoot)
w=['"' KhorosRoot];
else
if ispc
  w='"C:\Program Files\dip\khorosBin\';
else
[s,w] = system('which cantata');
w=['"' w(1:end-8)];
end
end
[varargout{:}]=callKhoros([w 'itexture"  '],Inputs,narglist);


classdef ParadigmTFR < ParadigmDataflowSimplified
    % Time/Frequency Regression. This is a new approach that is not yet published.
    %
    % This method learns a linear classifier (or regressor) of second-order dynamics in the EEG over
    % a particular set of time and frequency bins. The key trick lies in a set of extra assumptions
    % that make the (very high-dimensional) solution statistically tractable. The algorithm amounts
    % to a single convex optimization problem dependent on a small number of hyper-parameters that can
    % be optimized using grid search. For a reasonably exhaustive grid the solution is the globally
    % optimal second-order dynamics solution.
    %
    % Notes:
    %  This implementation is not yet optimally tuned -- the spectral estimation should be replaced by a
    %  similar approach to multi-taper CSP.
    %
    % Examples:
    %   % learn a spectral classifier for features within -2 to 2 seconds relative to some marker in some standard EEG frequency bands
    %   % here using area under curve (AUC) to optimize the regularization parameter (assuming that the number of exemplars per class is imbalanced)
    %   % and setting the set of relative regularization term weights that should be searched over to {[1 1 1 1]}, i.e., not searching over those, for speed.
    %   myapproach = {'TFR' 'SignalProcessing',{'EpochExtraction',[-2 2]},'Prediction',{'FeatureExtraction',{'Times',[-1.5,-1,-0.5,0,0.5,1,1.5],'Frequencies',[4 7 11 14 20 30]}, ...
    %       'MachineLearning',{'Learner',{'proximal','LambdaSearch',{'ParameterMetric','auc'},'TermWeights',{[1 1 1 1]}}}}};
    %
    % Name:
    %   Time/Frequency Regression
    %
    %                            Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
    %                            2013-03-08

    methods

        function defaults = preprocessing_defaults(self)
            % define the default pre-processing parameters of this paradigm
            defaults = {'FIRFilter',{[0.5 1],'highpass'}, 'EpochExtraction',[-3 3], 'Resampling',100};
        end

        function defaults = machine_learning_defaults(self)
            % global sharing approach
            defaults = {'proximal', 'lambdaSearch',{'lambdas',2.^(5:-0.1:-4),'foldmargin',0},'regularizers',{ ...
                    'term1',{'trace'}, ...                                                                      % the weights are a small sum of second-order spatial filters per T/F resel
                    'term2',{'trace','LinearOperator','@(x)reshape(x,a*b,c*d)'}, ...                            % across all of time/frequency we learn combinations of few latent spatial filters
                    'term3',{'l2', 'LinearOperator','@(x)vec(diff(x,[],4))','NonorthogonalTransform',true}, ... % temporal smoothness
                    'term4',{'l2', 'LinearOperator','@(x)vec(diff(x,[],3))','NonorthogonalTransform',true}, ... % spectral smoothness
                }, 'regweights',{[1 1 1 1],[1 1 0.5 0.5],[1 1 2 2],[1 1 0.25 0.25],[1 1 4 4],[1 2 1 2],[2 1 1 2],[1 2 2 1],[2 1 2 1]}};
        end

        function model = feature_adapt(self,varargin)
            % adapt a feature representation using the CSP algorithm
            args = arg_define(varargin, ...
                arg_norep('signal'), ...
                arg({'freqpoints','Frequencies'},[4 7 11 14 20 30],[0 0.2 200 1000],'Frequency points to consider.'),...
                arg({'timepoints','Times'},-2.5:0.5:2.5,[],'Time points to consider.'), ...
                arg({'from_edges','FromEdges'},true,[],'Run up to epoch edges. This is both in time and frequency.'), ...
                arg({'vectorize_features','VectorizeFeatures'},false,[],'Vectorize the features. For compatibility with basic classifiers.'));
            model.args = rmfield(args,'signal');
            model.chanlocs = args.signal.chanlocs;
        end

        function [features,shape] = feature_extract(self,signal,featuremodel)
            % first perform the time/freq decomposition
            args = featuremodel.args;
            if args.from_edges
                args.timepoints = [signal.xmin args.timepoints signal.xmax];
                args.freqpoints = [0 args.freqpoints signal.srate/2];
            end
            time2idx = @(t) min(signal.pnts,max(1,1+round((t-signal.xmin)*signal.srate)));
            half_hann = @(a,b) 0.5*(1-cos(pi*((((a+1):b) - a) / (b - a))));
            shape = [size(signal.data,1),size(signal.data,1),length(args.freqpoints)-2,length(args.timepoints)-2];
            features = zeros([shape signal.trials]);
            for t=2:length(args.timepoints)-1
                % calculate time window function
                last_tp = time2idx(args.timepoints(t-1));
                cur_tp = time2idx(args.timepoints(t));
                next_tp = time2idx(args.timepoints(t+1));
                wndrange = last_tp+1:next_tp;
                wndfunc = [half_hann(last_tp,cur_tp) 1-half_hann(cur_tp,next_tp)];
                % window and fourier-transform the data
                X = fft(bsxfun(@times,wndfunc,signal.data(:,wndrange,:)),[],2);
                % multiply out the cross-spectral covariance
                tmp = zeros(size(X,1),size(X,1),ceil(size(X,2)/2),size(X,3));
                freq2idx = @(f) min(size(tmp,3),max(1,1+round(f*length(wndrange)/signal.srate)));
                for f=1:size(tmp,3)
                    for n=1:size(tmp,4)
                        tmp(:,:,f,n) = 2*real(X(:,f,n)*X(:,f,n)');
                    end
                end
                % average around frequency centers
                for f=2:length(args.freqpoints)-1
                    last_fp = freq2idx(args.freqpoints(f-1));
                    cur_fp = freq2idx(args.freqpoints(f));
                    next_fp = freq2idx(args.freqpoints(f+1));
                    wndrange = last_fp+1:next_fp;
                    wndfunc = [half_hann(last_fp,cur_fp) 1-half_hann(cur_fp,next_fp)];
                    features(:,:,f-1,t-1,:) = mean(bsxfun(@times,tmp(:,:,wndrange,:),reshape(wndfunc,1,1,[])),3);
                end
            end
            % do final vectorization if desired
            if featuremodel.args.vectorize_features
                features = reshape(features,[],signal.trials)'; end
        end

        function layout = dialog_layout_defaults(self)
            layout = {'SignalProcessing.Resampling.SamplingRate','SignalProcessing.FIRFilter.Frequencies', ...
                '', 'SignalProcessing.EpochExtraction','', ...
                'Prediction.FeatureExtraction.Frequencies', 'Prediction.FeatureExtraction.Times', ...
                'Prediction.FeatureExtraction.FromEdges','', ...
                'Prediction.MachineLearning.Learner.LossType', '', ...
                'Prediction.MachineLearning.Learner.LambdaSearch.Lambdas', ...
                'Prediction.MachineLearning.Learner.TermWeights', ...
                'Prediction.MachineLearning.Learner.LambdaSearch.ParameterMetric'};
        end

    end
end


function DEMO_GROUP_PEB
% Demonstration routine for empirical Bayes and Bayesian model reduction
%--------------------------------------------------------------------------
% This routine illustrates the use of Bayesian model reduction when
% inverting hierarchical (linear) models - it is essentially a software
% validation demo and proof of concept. It uses a parametric empirical
% Bayesian model (i.e., nested linear models) to eschew local minima issues
% and to assure the Laplace assumption is correct. In brief, the data are
% generated for multiple subjects, under a linear model with subject
% specific parameters at the first level and group specific parameters at
% the second. These model a group effect common to all subjects in a subset
% of parameters and differences in a further subset. In this demo, we
% consider the full hierarchical inversion of a multisubject  study by
% updating the priors at the first level, using the empirical priors from
% the second level. Crucially, this is done during the optimisation  at the
% first level (i.e., after  every iteration - or small number of iterations
% - at the first level.
%
% This provides a generic scheme for the hierarchical inversion of
% nonlinear and possibly dynamic models in which the first level
% optimisation is informed by the sufficient statistics of the second level
% (namely the empirical priors). This should be contrasted with the summary
% statistic approach, in which the second level optimisation, based upon
% the sufficient statistics of the first level (posteriors and priors) are
% computed after convergence of the first level. The results of inversion
% are compared in terms of the second level posteriors (and the second
% level free energy over iterations). Specifically, we compare a gold
% standard (PEB) inversion, with the summary statistic approach to
% empirical Bayes and the hierarchical inversion demonstrated in this
% routine.
%
% The parameterisation of the models uses the format of DCM. This means
% parameters are specified as a structure with key parameters being in the
% fields A, B and C.
%
% See also: spm_dcm_bmr, spm_dcm_peb and spm_dcm_peb_bma
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Karl Friston, Peter Zeidman
% $Id: DEMO_GROUP_PEB.m 6737 2016-03-03 12:05:51Z karl $

% set up
%==========================================================================
rng('default')

% model space - defined in terms of combinations of some parameters
%--------------------------------------------------------------------------
k     = spm_perm_mtx(3);
for i = 1:8;
    B{i} = k(i,:);
end

% model space
%--------------------------------------------------------------------------
mw  = 3;                              % true model (within)
mx  = 4;                              % true model (between)
Ns  = 16;                             % number of subjects
C   = 32;                             % within:between [co]variance ratio

% create subject-specifc GLM
%==========================================================================

% within subject effects:  condition specific effects 'B' (2 s.d.)
%--------------------------------------------------------------------------
pC          = 1/8;
sd          = sqrt(pC/C);
DCM.Ep.A    = randn(4,1)*sd;
DCM.Ep.B{1} = B{mw}*2*sd;
Np          = spm_length(DCM.Ep);
DCM.M.pE    = spm_zeros(DCM.Ep);
DCM.M.pC    = eye(Np,Np)*pC;

% between subject effects: constant and group difference
%--------------------------------------------------------------------------
X           = [ones(Ns,1) kron([-1;1],ones(Ns/2,1))];
DCM.Ex      = spm_zeros(DCM.Ep);
DCM.Ex.B{1} = B{mx}*2*sd;

% create subject-specifc DCM
%--------------------------------------------------------------------------
Ex    = spm_vec(DCM.Ex);
Ep    = spm_vec(DCM.Ep);
pC    = DCM.M.pC;
Cp    = sd*diag(~~spm_vec(Ep));
Ny    = 16;
for i = 1:Ns

    % generate data
    %----------------------------------------------------------------------
    Pp    = X(i,1)*Ep + X(i,2)*Ex + Cp*randn(Np,1);

    % generate data
    %----------------------------------------------------------------------
    Z{i,i} = randn(Ny,Np);
    y{i,1} = Z{i,i}*Pp + randn(Ny,1)/8;

    % design matrix and data
    %----------------------------------------------------------------------
    GCM{i,1}.xU    = Z{i,i};
    GCM{i,1}.xY.y  = y{i,1};
    GCM{i,1}.xY.X0 = [];

    % likelihood model and priors
    %----------------------------------------------------------------------
    GCM{i,1}.M.IS  = @(P,M,U) U*spm_vec(P);
    GCM{i,1}.M.pE  = DCM.M.pE;
    GCM{i,1}.M.pC  = pC;
    GCM{i,1}.Tp    = Pp;

end

% PEB (GLM) for inversion to provide a reference for BMR
%==========================================================================
Nx    = size(X,2);
Q     = spm_Ce(ones(1,Np));
for i = 1:Np
    Q{i} = kron(eye(Ns,Ns),Q{i})/128;
end
P{1}.X = spm_cat(Z);
P{1}.C = spm_Ce(ones(1,Ns)*Ny);
P{2}.X = kron(X,eye(Np,Np));
P{2}.C = Q;
P{3}.X = kron(zeros(Nx,1),zeros(Np,1));
P{3}.C = kron(eye(Nx,Nx),pC);

% Full hierarchical parametric empirical Bayes inversion
%--------------------------------------------------------------------------
[qP,~,F] = spm_PEB(spm_cat(y),P,1);

% record estimates as a reference
%--------------------------------------------------------------------------
PB.F  = F;
PB.Ep = qP{3}.E;
PB.Cp = qP{3}.C;

% repeat using non-linear empirical Bayes
%==========================================================================

% second level model
%--------------------------------------------------------------------------
M.X   = X;
M.pE  = DCM.M.pE;
M.pC  = DCM.M.pC;

% hierarchical inversion
%--------------------------------------------------------------------------
[gcm,peb,M] = spm_dcm_peb_fit(GCM,M);

% repeated using Bayesian model reduction summary statistic approach
%==========================================================================
GCM  = spm_dcm_fit(GCM);
PEB  = spm_dcm_peb(GCM,M);

% second level parameter estimates
%==========================================================================
spm_figure('GetWin','Figure 1'); clf

% estimated and true second level parameters
%--------------------------------------------------------------------------
subplot(2,2,1), spm_plot_ci(PB.Ep,PB.Cp), hold on, bar([Ep;Ex],1/2), hold off
xlabel('parameters'), ylabel('expectation'),
title('Parametric Bayes','FontSize',16), axis square

subplot(2,2,2), spm_plot_ci(PEB.Ep(:),PEB.Cp), hold on, bar([Ep;Ex],1/2), hold off
xlabel('parameters'), ylabel('expectation'),
title('Sufficient statistics','FontSize',16), axis square

subplot(2,2,3), spm_plot_ci(peb.Ep(:),peb.Cp), hold on, bar([Ep;Ex],1/2), hold off
xlabel('parameters'), ylabel('expectation'),
title('Hierarchical inversion','FontSize',16), axis square

subplot(2,2,4), bar(gcm{1}.FEB)
xlabel('iterations'), ylabel('free energy'),
title('second level free energy','FontSize',16), axis square

return

% Bayesian model reduction with and without hierarchical inversion
%==========================================================================

% define the model space in terms of a matrix
%--------------------------------------------------------------------------
K     = ones(length(B),spm_length(DCM.Ep));
k     = spm_fieldindices(DCM.M.pE,'B');
for i = 1:length(B)
    K(i,k) = spm_vec(B{i})';
end

% defined model in terms of prior covariance
%--------------------------------------------------------------------------
Nm    = size(K,2);
for i = 1:Ns
    for j = 1:Nm
        gcm{i,j}      = gcm{i,1};
        GCM{i,j}      = GCM{i,1};
        gcm{i,j}.M.pC = diag(K(j,:))*M.pC*diag(K(j,:));
        GCM{i,j}.M.pC = diag(K(j,:))*M.pC*diag(K(j,:));
    end
end

rcm   = spm_dcm_bmr(gcm);
RCM   = spm_dcm_bmr(GCM);

% Free energies
%--------------------------------------------------------------------------
for i = 1:Ns
    for j = 1:Nm
        G(i,j,1) = rcm{i,j}.F - rcm{i,1}.F;
        G(i,j,2) = RCM{i,j}.F - RCM{i,1}.F;
    end
end

%  free energy model comparison
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 2');clf

p  = spm_softmax(sum(G(:,:,1))'); [m i] = max(p);
subplot(2,2,1), bar(p)
text(i - 1/4,m/2,sprintf('%-2.0f%%',m*100),'Color','w','FontSize',8)
xlabel('model'), ylabel('probability'), title('Iterative PEB','FontSize',16)
axis([0 (length(p) + 1) 0 1]), axis square

p  = spm_softmax(sum(G(:,:,1))'); [m i] = max(p);
subplot(2,2,2), bar(p)
text(i - 1/4,m/2,sprintf('%-2.0f%%',m*100),'Color','w','FontSize',8)
xlabel('model'), ylabel('probability'), title('Bayesian model reduction','FontSize',16)
axis([0 (length(p) + 1) 0 1]), axis square


% Jiao Xianjun (putaoshu@msn.com; putaoshu@gmail.com)
% analyze PSS detection under big frequency offset
% A script of project: https://github.com/JiaoXianjun/rtl-sdr-LTE

% according to simulation result, gold configuration is that use whole PSS (length 137), and fo step 50kHz
% simulation result:
% len pss 137 fo step 50kHz SNR 0dB 10000 cost 29.9569s fail rate 0.003
% len pss 137 fo step 50kHz SNR 3dB 10000 cost 27.4461s fail rate 0
% len pss 137 fo step 25kHz SNR 0dB 10000 cost 40.6559s fail rate 0.0025
% len pss 137 fo step 25kHz SNR 3dB 10000 cost 41.6056s fail rate 0
% len pss 128 fo step 50kHz SNR 0dB 10000 cost 23.6994s fail rate 0.0091
% len pss 128 fo step 50kHz SNR 3dB 10000 cost 23.0074s fail rate 0.0002
% len pss 128 fo step 25kHz SNR 0dB 10000 cost 41.1991s fail rate 0.0078
% len pss 128 fo step 25kHz SNR 3dB 10000 cost 41.4224s fail rate 0

% function test_pss_fo_corr
clear all;
close all;

[fd_pss, td_pss] = pss_gen;

pss_idx = 1;
% pss = td_pss(end-127:end, pss_idx);
pss = td_pss(:, pss_idx);
len_pss = length(pss);

fo_search_set = [-200e3:50e3:200e3];
% fo_search_set = 0;
pss_set = kron(ones(1, length(fo_search_set)), pss).*exp(1i.*2.*pi.*(1./1.92e6).*(0:(length(pss)-1)).'*fo_search_set);
fo_step = (fo_search_set(2) - fo_search_set(1))/1e3;

var_pss = mean(abs(td_pss(:, pss_idx)).^2);

r = zeros(1, 2*137);
tmp_total = zeros(1, length(fo_search_set));
num_test = 10000;
num_fail = 0;
snr = 3;
sigma2 = var_pss/(10^(snr/10));
correct_pos = 147 - (length(pss)-128);
tic;
for idx = 1:num_test
    r_pss = [(randn(137,1)+1i.*randn(137,1)).*sqrt(var_pss./2); td_pss(:, pss_idx); (randn(137,1)+1i.*randn(137,1)).*sqrt(var_pss./2)];
    r_pss = r_pss + sqrt(sigma2/2).*(randn(length(r_pss),1)+1i.*randn(length(r_pss),1));

    fo = (2*rand-1)*200e3;
%     fo = (-7.5e3/1) + 0*15e3;
    r_pss_fo = r_pss.*exp(1i.*2.*pi.*fo.*(1./1.92e6).*(0:(length(r_pss)-1))');

    for i=1:(2*137)
        s = r_pss_fo(i:(i+length(pss)-1));

%         %  %---------method 1-----------------------
%         for j=1:length(fo_search_set)
%             tmp = conj(pss_set(:,j)).*s;
%             tmp = vec2mat(tmp, 8);
%             tmp = sum( abs(sum(tmp, 2)).^2 );
% %             r(i) = r(i) + tmp;
%             tmp_total(j) = tmp;
%         end
%         r(i) = max(tmp_total);
%         %  %---------end of method 1-----------------------

%         %  %---------method 2-----------------------
%         r(i) = abs(s(1:64)'*s(128:-1:65))^2;
%         %  %---------end of method 2-----------------------

        %  %---------method 3-----------------------
        tmp = conj(pss).*s;
        r(i) = max(abs(fft(tmp)).^2);
        %  %---------end of method 3-----------------------
    end
    plot(r); drawnow;
    % plot(138, r(138), 'rs');

    if mod(idx, 100) == 0
        disp(['len pss ' num2str(len_pss) ' fo step ' num2str(fo_step) 'kHz SNR ' num2str(snr) 'dB ' num2str(idx) ' cost ' num2str(toc) 's fail rate ' num2str(num_fail/idx) ]);
        tic;
    end

    [~, max_idx] = max(r);
    if max_idx ~= correct_pos
%         plot(r); hold on;
%         plot(correct_pos, r(correct_pos), 'rs');
        num_fail = num_fail + 1;
        disp(['idx ' num2str(idx) ' max_idx ' num2str(max_idx) ' fail rate ' num2str(num_fail/idx)]);
%         break;
    end
end

disp(['fail rate ' num2str(num_fail/idx)]);


function [ gx, gy ] = g05_xy ( gn )

%*****************************************************************************80
%
%% G05_XY returns the grid points for grid 5.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer GN, the grid size.
%
%    Output, real GX(GN,1), GY(GN,1), the grid coordinates.
%
  gx = [ ...
    0.000,  0.000,  0.000,  0.000,  0.000,  0.000,  0.000,  0.000,  0.000, ...
    0.125,  0.125,  0.125,  0.125,  0.125,  0.125,  0.125,  0.125,  0.125, ...
    0.250,  0.250,  0.250,  0.250,  0.250,  0.250,  0.250,  0.250,  0.250, ...
    0.375,  0.375,  0.375,  0.375,  0.375,  0.375,  0.375,  0.375,  0.375, ...
    0.500,  0.500,  0.500,  0.500,  0.500,  0.500,  0.500,  0.500,  0.500, ...
    0.625,  0.625,  0.625,  0.625,  0.625,  0.625,  0.625,  0.625,  0.625, ...
    0.750,  0.750,  0.750,  0.750,  0.750,  0.750,  0.750,  0.750,  0.750, ...
    0.875,  0.875,  0.875,  0.875,  0.875,  0.875,  0.875,  0.875,  0.875, ...
    1.000,  1.000,  1.000,  1.000,  1.000,  1.000,  1.000,  1.000,  1.000 ]';

  gy = [ ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000, ...
    0.000,  0.125,  0.250,  0.375,  0.500,  0.625,  0.750,  0.875,  1.000 ]';

  return
end


function value = p12_exact ( dim_num )

%*****************************************************************************80
%
%% P12_EXACT returns the exact integral for problem 12.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Output, real VALUE, the exact value of the integral.
%
  value = 1.0;
  for i = 1 : dim_num
    value = value * sin ( i );
  end

  return
end


function [cost,costs] = rcrossvalidate(model, L, wfun, estfct,combinefct)

% Estimate the model performance of a model with robust [$ l$] -fold crossvalidation

% CAUTION!! Use this function only to obtain the value of the rcrossvalidation score
% function given the tuning parameters. Do not use this function together with
% 'tunelssvm', but use 'rcrossvalidatelssvm' instead. The latter is a faster
% implementation which uses previously computed results.
%
%
% >> cost = rcrossvalidate({Xtrain,Ytrain,type,gam,sig2})
% >> cost = rcrossvalidate( model)
%
% Robustness in the $l$-fold crossvalidation score function is obtained by
% iteratively reweighting schemes.
%
% This routine is computational intensive.
%
%
% Some commonly used criteria are:
%
% >> cost = rcrossvalidate(model, 10, 'whuber', 'mae')
% >> cost = rcrossvalidate(model, 10, 'whampel', 'mae')
% >> cost = rcrossvalidate(model, 10, 'wlogistic', 'mae')
% >> cost = rcrossvalidate(model, 10, 'wmyriad', 'mae')
%
% Full syntax
%
%     1. Using LS-SVMlab with the functional interface:
%
% >> [cost, costs] = rcrossvalidate({X,Y,type,gam,sig2,kernel,preprocess}, L, wfun, estfct, combinefct)
%
%       Outputs
%         cost          : Cost estimation of the L-fold cross validation
%         costs(*)      : L x 1 vector with costs estimated on the L different folds
%       Inputs
%         X             : Training input data used for defining the LS-SVM and the preprocessing
%         Y             : Training output data used for defining the LS-SVM and the preprocessing
%         type          : 'function estimation' ('f') or 'classifier' ('c')
%         gam           : Regularization parameter
%         sig2          : Kernel parameter (bandwidth in the case of the 'RBF_kernel')
%         kernel(*)     : Kernel type (by default 'RBF_kernel')
%         preprocess(*) : 'preprocess'(*) or 'original'
%         L(*)          : Number of folds (by default 10)
%         wfun(*)       : weighting scheme (by default: whuber)
%         estfct(*)     : Function estimating the cost based on the residuals (by default mse)
%         combinefct(*) : Function combining the estimated costs on the different folds (by default mean)
%
%
%     2. Using the object oriented interface:
%
% >> [cost, costs] = crossvalidate(model, L, wfun, estfct, combinefct)
%
%       Outputs
%         cost          : Cost estimation of the L-fold cross validation
%         costs(*)      : L x 1 vector with costs estimated on the L different folds
%         ec(*)         : N x 1 vector with residuals of all data
%       Inputs
%         model         : Object oriented representation of the LS-SVM model
%         Xval          : Nt x d matrix with the inputs of the validation points used in the procedure
%         Yval          : Nt x m matrix with the outputs of the validation points used in the procedure
%         L(*)          : Number of folds (by default 10)
%         wfun(*)       : weighting scheme (by default: whuber)
%         estfct(*)     : Function estimating the cost based on the residuals (by default mse)
%         combinefct(*) : Function combining the estimated costs on the different folds (by default mean)
%
% See also:
% mae,whuber,wlogistic,whampel,wmyriad, crossvalidate, trainlssvm, robustlssvm

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

% LS-SVMlab
eval('model = initlssvm(model{:});',' ');
eval('L;','L=min(ceil(sqrt(model.nb_data)),10);');
eval('estfct;','estfct=''mae'';');
eval('combinefct;','combinefct=''mean'';');
eval('wfun;','wfun=''whuber'';');
%
% initialisation and defaults
%
nb_data = size(model.ytrain,1);

if L==nb_data, p = 1:nb_data; else p = randperm(nb_data); end
px = model.xtrain(p,:);
py = model.ytrain(p,:);

[~,Y] = postlssvm(model,[],py);

%initialize: no incremental  memory allocation
costs = zeros(L,length(model.gam));
block_size = floor(nb_data/L);

S = ones(nb_data,1);
Atot = kernel_matrix(px,model.kernel_type,model.kernel_pars)+eye(nb_data)./model.gam;
%
%
% start loop over l validations
%
for l = 1:L,

    % divide in data and validation set, trainings data set is a copy
    % of permutated_data, validation set is just a logical index
    if l==L,
        train = 1:block_size*(l-1);
        validation = block_size*(l-1)+1:nb_data;
    else
        train = [1:block_size*(l-1) block_size*l+1:nb_data];
        validation = block_size*(l-1)+1:block_size*l;
    end

    A = [0 S(train)';S(train) Atot(train,train)];
    b = [0;py(train)];

    % Solve linear system
    sol = linsolve(A,b,struct('SYM',true));

    % Determine residuals ek
    ek = sol(2:end)./model.gam;
    g = model.gam;
    %for i=2:size(A,1), A(i,i) = A(i,i) - 1/g; end
    A = A-eye(size(train,2)+1)./g; A(1,1)=0;
    Ah = A;
    %
    % robust estimation of the variance
    %
    for k = 1:20
        vare = 1.483*median(abs((ek)-median(ek)));
        alphaold = sol(2:end);
        %
        % robust re-estimation of the alpha's and the b
        %
        cases = reshape((ek./vare),1,size(ek,1));
        W = g*weightingscheme(cases,wfun);

        for t=1:size(train,2), A(t+1,t+1) = A(t+1,t+1)+1./W(t); end

        sol = linsolve(A,b,struct('SYM',true));

        ek = sol(2:end)./W';
        A = Ah;
        if norm(abs(alphaold-sol(2:end)),'fro')<=1e-4,
            %fprintf('\n Converged after %.0f iteration(s)', k);
            k = inf;
        end
        model.status = 'changed';
    end

    % regression
    % Simulate system on validation data
    yh = Atot(train,validation)'*sol(2:end) + ones(numel(validation),1)*sol(1);
    [~,yh] = postlssvm(model,[],yh);
    costs(l,1) = feval(estfct,yh - Y(validation,:));
end
cost = feval(combinefct, costs);


function [res] = tt_dot(tt1,tt2)
%Scalar product of two TT-tensors in the TT1.0 format
%   [RES]=TT_DOT(TT1,TT2) Scalar product of two TT-tensors in the TT1.0
%   format. Please avoid its usage, use dot function from the
%   object-oriented version
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------
d=size(tt1,1);
g0=(tt1{1})'*(tt2{1});
gd=(tt1{d})'*(tt2{d});
r1=size(gd,1); r2=size(gd,2);
gd=reshape(gd,1,r1*r2);
for i=2:d-1
    core1=tt1{i};
    core2=tt2{i};
    core1=ten_conv(core1,2,g0);
    ncur=size(core1,1); r21=size(core1,2); r31=size(core1,3);
    r22=size(core2,2); r32=size(core2,3);
    core1=reshape(core1,[ncur*r21,r31]);
    core2=reshape(core2,[ncur*r22,r32]);
    g0=core1'*core2;
end
r1=size(g0,1); r2=size(g0,2);
g0=reshape(g0,1,r1*r2);
res=g0*gd';
return

end


function [pointst,pointsf]=ridges(tfr,hat,t,method,trace);
%RIDGES	Extraction of ridges.
%	[POINTST,POINTSF]=RIDGES(TFR,HAT,T,METHOD,TRACE) extracts the
%	ridges of a time-frequency distribution. These ridges are some
%	particular sets of curves deduced from the stationary points of
%	their  reassignment operators.
%
%	TFR    : time-frequency representation
%	HAT    : complex matrix of the reassignment vectors.
%	T      : the time instant(s).
%	METHOD : the chosen representation (default: 'tfrrsp').
%	TRACE  : if nonzero, the progression of the algorithm is shown
%					   (default : 0).
%
%	POINTST,POINTSF are two vectors for the time and frequency
%	coordinates of the stationary points of the reassignment.
%	Therefore, PLOT(POINTST,POINTSF,'.') shows the squeleton of the
%	representation.
%
%	Example :
%	 sig=fmlin(128,0.1,0.4); g=tftb_window(21,'kaiser');
%	 h=tftb_window(47,'Kaiser'); t=1:2:127;
%	 figure(1), [tfr,rtfr,hat]=tfrrspwv(sig,t,128,g,h);
%	 ridges(tfr,hat,t,'tfrrspwv',1);
%	 figure(2), [tfr,rtfr,hat]=  tfrrsp(sig,t,128,h);
%	 ridges(tfr,hat,t,'tfrrsp',1);
%
%	See also : FRIEDMAN.

%	F. Auger, August 1994, December 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  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 2 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, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin<2),
 error('at least 2 parameters required');
end;

[tfrrow,tfrcol]=size(tfr);
[hatrow,hatcol]=size(hat);

if (nargin==2),
 t=1:tfrcol; method='tfrrsp'; trace=0;
elseif (nargin==3),
 method='tfrrsp'; trace=0;
elseif (nargin==4),
 trace=0;
end;

[trow,tcol] = size(t);
if (trow~=1),
 error('T must only have one row');
elseif (tfrrow~=hatrow)|(tfrcol~=hatcol),
 error('TFR and HAT must have the same size');
end;

Nf=tfrrow; frequencies=(1:Nf)';
threshold=sum(sum(tfr))*0.5/(tfrrow*tfrcol);

pointst=[]; pointsf=[];

if trace, fprintf ('\nRidge extraction: \n'); end;

method=upper(method);
if strcmp(method,'TFRRPWV') | strcmp(method,'TFRRPMH'),
 for icol=1:tfrcol, ti=t(icol);
  if trace, disprog(icol,tfrcol,10); end;
  indices=find((tfr(:,icol)>threshold)&(hat(:,icol)-frequencies==0));
  nbindices=length(indices);
  if (nbindices>0),
   pointst=[pointst;ones(nbindices,1)*ti];
   pointsf=[pointsf;indices/(2.0*Nf)];
  end;
 end;
elseif strcmp(method,'TFRRSPWV'),
 for icol=1:tfrcol, ti=t(icol);
  if trace, disprog(icol,tfrcol,10); end;
  indices=find((real(hat(:,icol))-frequencies==0)&...
               (imag(hat(:,icol))-icol==0)&...
               (tfr(:,icol)>threshold));
  nbindices=length(indices);
  if (nbindices>0),
   pointst=[pointst;ones(nbindices,1)*ti];
   pointsf=[pointsf;indices/(2.0*Nf)];
  end;
 end;
elseif strcmp(method,'TFRRSP')|strcmp(method,'TYPE1')
 for icol=1:tfrcol, ti=t(icol);
  if trace, disprog(icol,tfrcol,10); end;
  indices=find((real(hat(:,icol))-frequencies==0)&...
               (imag(hat(:,icol))-icol==0)&...
               (tfr(:,icol)>threshold));
  nbindices=length(indices);
  if (nbindices>0),
   pointst=[pointst;ones(nbindices,1)*ti];
   pointsf=[pointsf;indices/Nf];
  end;
 end;
else
 error('unknown representation');
end;

if (nargout==0),
 clf;
 plot(pointst,pointsf,'.')
 axis([min(t) max(t) 0 0.5]);
end;


function value = p06_param ( action, name, value )

%*****************************************************************************80
%
%% P06_PARAM gets or sets parameters for problem 6.
%
%  Discussion:
%
%    The parameter is named "M", and it represents the value of the exponent
%    in the integrand function:
%
%    Integral ( -oo < x < +oo ) x^m exp (-x*x) dx
%
%    M must be greater than -1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 May 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, character ACTION, the action.
%    'S' to set the value,
%    'G' to get the value.
%
%    Input, character NAME, the parameter name.
%    'M', the exponent.
%
%    Input/output, integer VALUE, the parameter value.
%    If ACTION = 'S', then VALUE is an input quantity, and M is set to VALUE.
%    If ACTION = 'G', then VALUE is an output quantity, and VALUE is set to M.
%
  persistent m;

  if ( action == 'S' || action == 's' )

    if ( value <= -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'P06_PARAM - Fatal error!\n' );
      fprintf ( 1, '  Parameter M must be greater than -1.\n' );
      error ( 'P06_PARAM - Fatal error!' );
    end

    m = value;

  elseif ( action == 'G' || action == 'g' )

    value = m;

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'P06_PARAM - Fatal error!\n' );
    fprintf ( 1, '  Unrecognized value of ACTION = "%s".\n', action );
    error ( 'P06_PARAM - Fatal error!' );

  end

  return
end


%MDL_IR140 Create model of the ABB IRB 140 manipulator
%
%      mdl_irb140_mod
%
% Script creates the workspace variable irb which describes the
% kinematic characteristics of an ABB IRB 140 manipulator using
% modified DH conventions.
%
% Also define the workspace vectors:
%   qz         zero joint angle configuration
%
% Reference::
% - ABB IRB 140 data sheet
% - "THE MODELING OF A SIX DEGREE-OF-FREEDOM INDUSTRIAL ROBOT FOR
%   THE PURPOSE OF EFFICIENT PATH PLANNING"
%   Master of Science Thesis, Penn State U, May 2009
%   Tyler Carter
%
% See also SerialLink, mdl_irb140, mdl_puma560, mdl_stanford, mdl_twolink.
%
% Notes::
% - SI units of metres are used.
% - The tool frame is in the centre of the tool flange.
% - Zero angle configuration has the upper arm vertical and lower arm
%   horizontal.

% MODEL: ABB, IRB140, 6DOF, modified_DH

% Reference::

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

clear L

% joint angle limits from
% A combined optimization method for solving the inverse kinematics problem...
% Wang & Chen
% IEEE Trans. RA 7(4) 1991 pp 489-
L(1) = Revolute('d', 0.352, 'a', 0, 'alpha', 0, 'offset', 0, 'modified');
L(2) = Revolute('d', 0, 'a', 0.070, 'alpha', pi/2, 'offset', 0, 'modified');
L(3) = Revolute('d', 0, 'a', 0.360, 'alpha', 0, 'offset', 0, 'modified');
L(4) = Revolute('d', 0.380, 'a', 0, 'alpha', pi/2, 'offset', 0, 'modified');
L(5) = Revolute('d', 0, 'a', 0, 'alpha', -pi/2, 'offset', 0, 'modified');
L(6) = Revolute('d', 0, 'a', 0, 'alpha', pi/2, 'offset', 0, 'modified');

L(1).m = 34655.36e-3;
L(1).r = [27.87 43.12 -89.03]*1e-3;
L(1).I = [
    512052539.74 1361335.88 51305020.72
    1361335.88 464074688.59 70335556.04
    51305020.72 70335556.04 462745526.12]*1e-9;

L(2).m = 15994.59e-3;
L(2).r = [ 198.29 9.73 92.43]*1e03;
L(2).I = [
    94817914.40 -3859712.77 37932017.01
    -3859712.77 328604163.24 -1088970.86
    37932017.01 -1088970.86 277463004.88]*1e-9;

L(3).m = 20862.05e-3;
L(3).r = [ -4.56 -79.96 -5.86];
L(3).I = [
    500060915.95 -1863252.17 934875.78
    -1863252.17 75152670.69 -15204130.09
    934875.78 -15204130.09 515424754.34]*1e-9;

irb = SerialLink(L, 'name', 'IRB 140', ...
    'manufacturer', 'ABB', 'comment', 'modified DH');

%
% some useful poses
%
qz = [0 0 0 0 0 0]; % zero angles, L shaped pose

clear L


function [u,flag,relres,iter,resvec] = minres(L,f,tol,maxit,R1,R2,u0,varargin)
%MINRES    A Preconditioned Minimal Residual method for ODEs.
%   U = MINRES(L,F) attempts to solve the linear ODE L(U) = F on DOMAIN(L) with
%   boundary conditions. The CHEBOP L must be a self-adjoint, uniformly elliptic
%   second-order differential operator of the form
%          L(U) = (a(x)*U')' + c(x)U,
%   with dirichlet boundary conditions. The righthand side F should be a chebfun
%   on DOMAIN(L). By default the indefinite integral operator is employed as a
%   preconditioner.
%
%   U = MINRES(L,F,TOL) specifies the tolerance of the method. If TOL is [] then
%   MINRES uses the default in cheboppref.
%
%   U = MINRES(L,F,TOL,MAXIT) specifies the maximum number of iterations. If
%   MAXIT is [] then MINRES uses the default in cheboppref.
%
%   U = MINRES(L,F,TOL,MAXIT,R1,R2) solves the preconditioned linear ODE of
%   (R2*L*R1)(V) = R2*f, where R2 must be the adjoint of R1. R1 and R2 must be
%   function handles. If R1 = [], then the default preconditioner is employed.
%   Only the default preconditioner is currently supported.
%
%   U = MINRES(L,F,TOL,MAXIT,R1,R2,U0) specifies the initial guess. If U0 is []
%   then MINRES MINRES the default, the zero function on DOMAIN(L).
%
%   [U,FLAG] = MINRES(L,F,...) also returns a convergence FLAG:
%    0 MINRES converged to the desired tolerance TOL within MAXIT iterations
%    1 MINRES iterated MAXIT times but did not converge.
%    2 preconditioner R1 was an unbounded operator.
%    3 MINRES stagnated (two consecutive iterates were the same).
%    4 one of the scalar quantities calculated during MINRES became too
%      small or too large to continue computing.
%
%   [U,FLAG,RELRES] = MINRES(L,F,...) also returns the relative residual
%    NORM(R2(F)-R2(L(U)),2)/NORM(F,2). If FLAG is 0, then RELRES <= TOL.
%
%   [U,FLAG,RELRES,ITER] = MINRES(L,F,...) also returns the iteration number at
%   which U was computed: 0 <= ITER <= MAXIT.
%
%   [U,FLAG,RELRES,ITER,RESVEC] = MINRES(L,F,...) returns a vector of estimated
%   residual norms at each iteration including NORM(R2(F)-R2(L(Uk)),2).
%
% See also CHEBOP/PCG and CHEBOP/GMRES.

% Copyright 2017 by The University of Oxford and The Chebfun Developers. See
% http://www.chebfun.org/ for Chebfun information.

% Only continue if L is a linear chebop:
if ( ~all(islinear(L)) )
    error('CHEBFUN:CHEBOP:pcg:nonlinear', ...
        'MINRES supports only linear CHEBOP instances.');
end

% At the moment, we need a second-order differential equation:
LinearOp = linop( L );
if ( LinearOp.diffOrder ~=2 )
    error('CHEBFUN:CHEBOP:pcg:DiffOrder', ...
        'MINRES supports only second-order ODEs.');
end

if ( nargin < 2 )
    error(message('chebop:minres:NotEnoughInputs'));
end

% Norm of righthand side function:
n2f = norm(f, 2);
n = length( f );
dom = domain( f );

% Grab boundary conditions from chebop:
if ( isa( L, 'chebop' ) )
    if isempty(L.lbc)
        left_bc = 0;
    else
        left_bc = L.lbcShow;

        % Current implementation requires Dirichlet boundary conditions:
        if ( ~isa(left_bc, 'double' ) )
            error('CHEBFUN:CHEBOP:pcg:leftbc', ...
                   'Currently, we require Dirichlet boundary conditions. Please supply N.lbc = double.');
        end
    end

    if ( isempty(L.rbc) )
        right_bc = 0;
    else
        right_bc = L.rbcShow;
        % Current implementation requires Dirichlet boundary conditions:
        if ( ~isa(right_bc, 'double' ) )
            error('CHEBFUN:CHEBOP:pcg:rightbc', ...
                   'Currently, we require Dirichlet boundary conditions. Please supply N.rbc = double.');
        end
    end

    L = L.op;
elseif ( isa( L, 'function_handle' ) )
    left_bc = 0;
    right_bc = 0;
else
    error(message('chebop:pcg:DiffOperatorIllDefined'));
end

% Ensure that L = @(x,u) ...
if ( nargin(L) == 1 )
    L = @(x,u) L(u);
elseif ( nargin(L) ~=2 )
    error(message('chebop:pcg:DiffOpNargin'));
end

% Data mine L for faster "mat-vec" computations in the CG iteration:
x = chebfun( @(x) x );
c = L(x, 1+0*x);
a = -L(x, x.^2/2) - (-L(x, x) + c.*x).*x + c.*(x.^2/2);
L = @(v) -diff( a.*diff( v ) ) + c.*v;

% Assign default values to unspecified parameters
if ( (nargin < 3) || isempty(tol) )
    tol = cheboppref().bvpTol;
end
warned = 0;
if ( tol <= eps )
    warning(message('chebop:pcg:tooSmallTolerance'));
    warned = 1;
    tol = eps;
elseif ( tol >= 1 )
    warning(message('chebop:pcg:tooBigTolerance'));
    warned = 1;
    tol = 1-eps;
end
if ( (nargin < 4) || isempty(maxit) )
    maxit = cheboppref().maxIter;
end

if ( (nargin < 5) || isempty(R1) )
    % Preconditioner:
    R1 = @(u) cumsum(u);
else
    % The method only works with the default preconditioner:
    error(message('chebop:pcg:OnlyDefaultPreconditionerAllowed'))
end

if ( (nargin < 6) || isempty(R2) )
    % Adjoint operator to R1:
    R2 = @(u) sum(u) - cumsum(u);
else
    % The method only works with the default preconditioner:
    error(message('chebop:pcg:OnlyDefaultPreconditionerAllowed'))
end

% Projection operator
Pi = @(g) g - mean(g);

% Build modified differential operator:
x = chebfun( @(x) x, dom );
T = @(v) Pi( R2( L( R1( v ) ) ) );

if ( (nargin >= 7) && ~isempty(u0) )
    if ( ~domainCheck(f, u0) )
        error(message('chebop:pcg:WrongInitGuessDomain'));
    else
        u = u0;
    end
    Tu = T(u);
else
    % Otherwise, initial guess is the zero function:
    u = 0*f;
    Tu = u;
end

if ( nargin > 7 )
    error(message('chebop:minres:TooManyInputs'));
end

% Ensure that rhs is within the correct space, and if not then solve a modified
% problem.
R2f = R2( f );
PiR2f = Pi( R2f );
if ( norm( R2f - PiR2f ) > tol || norm(left_bc)> tol || norm(right_bc) > tol )
    % f is NOT in the space Wn = {v in L_2: R1(v) in V_{n,0} }:
    basis = x.^(0:4);
    A = zeros(4, size(basis,2));
    for jj = 1:size(basis,2)
        A(1:2,jj) = feval( R1( R2( L(basis(:,jj)) ) ), dom([1,length(dom)])');
        A(3:4,jj) = feval(basis(:,jj),dom([1,length(dom)])');
    end
    b = [ feval( R1( R2f ), dom([1,length(dom)])') ; left_bc ; right_bc];
    warning('off','all')
    z = basis*(A\b);
    warning('on','all');
    % g is now in the space Wn = {v in L_2: R1(v) in V_{n,0} }:
    g = Pi( R2f - R2( L(z) ) );
else
    % f is in the space Wn = {v in L_2: R1(v) in V_{n,0} }:
    g = PiR2f;
    z = 0*f;
end

% Set up for the method
flag = 1;
iter = 0;
umin = u;                          % Iterate which has minimal residual so far
imin = 0;                          % Iteration at which xmin was computed
tolg = tol * normest(g);           % Relative tolerance
r = g - Tu;
normr = norm(r,2);                 % Norm of residual
normr_act = normr;

if ( normr <= tolg )               % Initial guess is a good enough solution
    flag = 0;
    relres = normr / n2f;
    resvec = normr;
    % Undo preconditioner:
    u = R1( Pi( u ) );
    return
end

resvec = zeros(maxit+1,1);         % Preallocate vector for MINRES residuals
resvec(1) = normr;                 % resvec(1) = norm(b-A*x0)
normrmin = normr;                  % Norm of minimum residual

vold = r;
v = vold;
beta1 = innerProduct(vold, v);
if ( beta1 <= 0 )
    flag = 5;
    relres = normr / n2b;
    resvec = resvec(1);
    u = R1( Pi( u ) );
    return
end
beta1 = sqrt(beta1);
snprod = beta1;
vv = v / beta1;
v = T(vv);
Amvv = v;
alpha = innerProduct(vv,v);
v = v - (alpha/beta1) * vold;

% Local reorthogonalization
numer = innerProduct(vv, v);
denom = innerProduct(vv, vv);
v = v - (numer/denom) * vv;
volder = vold;
vold = v;
betaold = beta1;
beta = innerProduct(v,v);
if ( beta < 0 )
    flag = 5;
    relres = normr / n2b;
    resvec = resvec(1);
    u = R1( Pi( u ) );
    return
end
iter = 1;
beta = sqrt(beta);
gammabar = alpha;
epsilon = 0;
deltabar = beta;
gamma = sqrt(gammabar^2 + beta^2);
mold = chebfun('0');
Amold = mold;
m = vv / gamma;
Am = Amvv / gamma;
cs = gammabar / gamma;
sn = beta / gamma;
u = u + snprod * cs * m;
snprod = snprod * sn;

normr = abs(snprod);
resvec(2,1) = normr;

% Check for convergence after first step.
if ( normr <= tolg )
    flag = 0;
    relres = normr / n2f;
    resvec = resvec(1:2);
    u = R1( Pi( u ) );
    return
end

stag = 0;                          % stagnation of the method
moresteps = 0;
maxmsteps = min([floor(n/50),5,n-maxit]);
maxstagsteps = 3;

% loop over maxit iterations (unless convergence or failure)
for ii = 2:maxit

    vv = v * (1/beta);
    v = T(vv);
    Amolder = Amold;
    Amold = Am;
    Am = v;
    v = v - (beta / betaold) * volder;
    alpha = innerProduct(vv, v);
    v = v - (alpha / beta) * vold;
    volder = vold;
    vold = v;
    betaold = beta;
    beta = innerProduct(v, v);
    if (beta < 0)
        flag = 5;
        break
    end
    beta = sqrt(beta);
    delta = cs * deltabar + sn * alpha;
    molder = mold;
    mold = m;
    m = vv - delta * mold - epsilon * molder;
    Am = Am - delta * Amold - epsilon * Amolder;
    gammabar = sn * deltabar - cs * alpha;
    epsilon = sn * beta;
    deltabar = - cs * beta;
    gamma = sqrt(gammabar^2 + beta^2);
    m = m / gamma;
    Am = Am / gamma;
    cs = gammabar / gamma;
    sn = beta / gamma;
    % Check for stagnation of the method
    if (snprod*cs == 0) || (abs(snprod*cs)*norm(m) < eps*norm(u))
        % increment the number of consecutive iterates which are the same
        stag = stag + 1;
    else
        stag = 0;
    end
    u = u + (snprod * cs) * m;
    snprod = snprod * sn;
    normr = abs(snprod);

    resvec(ii+1,1) = normr;

    % Check for convergence
    if ( ( normr <= tolg ) || ( stag >= maxstagsteps ) || moresteps)
        % double check residual norm is less than tolerance
        r = g - T( u );
        normr_act = norm(r);
        resvec(ii+1, 1) = normr_act;
        if (normr_act <= tolg)
            flag = 0;
            iter = ii;
            break
        else
            if stag >= maxstagsteps && moresteps == 0
                stag = 0;
            end
            moresteps = moresteps + 1;
            if moresteps >= maxmsteps
                if ~warned
                    warning('tooSmallTolerance');
                end
                flag = 3;
                iter = ii;
                break;
            end
        end
    end

    if ( normr < normrmin )        % update minimal norm quantities
        normrmin = normr;
        umin = u;
        imin = ii;
    end

    if ( stag >= maxstagsteps )    % 3 iterates are the same
        flag = 3;
        break
    end
end                                % for ii = 1 : maxit
if ( isempty(ii) )
    ii = 1;
end

% Returned solution is first with minimal residual:
if ( flag == 0 )
    relres = normr_act / n2f;
else
    r_comp = g - T(u);
    if ( norm(r_comp) <= normr_act )
        u = umin;
        iter = imin;
        relres = norm(r_comp) / n2f;
    else
        iter = ii;
        relres = normr_act / n2f;
    end
end

% Undo preconditioner:
u = R1( u ) + z;

% truncate the zeros from resvec
if ( (flag <= 1) || (flag == 3) )
    resvec = resvec(1:ii+1);
else
    resvec = resvec(1:ii);
end

end


function [c, s, kernel, iter] = deconvCa(y, kernel, smin, fit_gt, debug_on, sn, maxIter, theta, lambda)
%% deconvolve calcium traces to infer spike counts
%% inputs:
%   y:  1*T vector, observed calcium traces
%   kernel: struct variable with two fields {'fhandle', 'pars', nMax}. kernel
%   deterines the convolution kernel
%      fhandle: function handle, the function form of the response function
%       pars: p*1 vector, parameters for fhandle
%       nMax: scalar, maximum number of frames required by calcium indicators
%           to return resting states
%   smin: scalar, minimum number of nonzero spike count within each bin
%   fit_gt: true or false. iteratively fit gt or not
%   debug_on: play and save video of the whole procedure.
%   sn:     noise power
%   maxIter: scalar, maximum iterations for running OASIS, default (3)
%   theta: 1*T vecotr,  weight vector at each time point
%   lambda: scalar, tuning parameter

%% outputs:
%   c: 1*T vector, inferred calcium trace
%   s: 1*T vector, inferred spike train
%   kernel: convolution kernel
%   iters: number of iterations for updating irf

%% Author: Pengcheng Zhou, Carnegie Mellon University, 2016
% This work is based on one NIPS paper by Johannes Friedrich & Liam
% Paninski

%% input arugments
y = reshape(y, 1, []);  % convert data into row vector
if ~exist('sn', 'var') || isempty(sn)
    sn = get_noise_fft(y);
end
T = length(y);          % number of frames

% get convolution kernel
if ~exist('kernel', 'var') || isempty(kernel)
    kernel = create_kernel('exp2', ar2exp(estimate_time_constant(y,2)));
end
fhandle = kernel.fhandle;
nMax = kernel.nMax;

y = [y, zeros(1, nMax)];  % add few more elements for computation convenience

% threshold of the spike
if ~exist('smin', 'var') || isempty(smin)
    smin = 3*sn;               % min spike count
else
    smin = smin * sn;
end

% fit response function or not
if ~exist('fit_gt', 'var') || isempty(fit_gt)
    fit_gt = false;
else
    kernel.pars = ar2exp(estimate_time_constant(y,2));
end
thresh = 1e-3;

% debug mode
if ~exist('debug_on', 'var') || isempty(debug_on)
    debug_on = false;
end

% maximum iterations for running OASIS
if ~exist('maxIter', 'var') || isempty(maxIter)
    maxIter = 5;
end

% tuning parameter for enforcing sparsity
if ~exist('lambda', 'var') || isempty(lambda)
    lambda = 0;   % tuning parameter
end

% weight for each frame
if ~exist('theta', 'var')|| isempty(theta) || (length(theta)==1)     % weight vector
    theta = ones(1, T);
elseif length(theta)==T
    theta = reshape(theta, 1, T);
else
    disp('elements in theta should be equal to elements in y');
    return;
end

% tuning parameter for enforcing sparsity
if ~exist('lambda', 'var') || isempty(lambda)
    lambda = 0;   % tuning parameter
end

%% running OASIS
if debug_on
    figure('position', [1, 1 1500, 250]);
    plot(y); hold on;
    a = plot(y, 'r');
    avi_file = VideoWriter('example.avi');
    avi_file.open();
    xlim([1, T]);
end

f0 = inf;       % objective function
y0 = y;         % backup the raw trace
iter = 1;       % iteratiosn
for iter = 1:maxIter
    if debug_on
        title(sprintf('Iter %d, norm(residual) = %.4f', iter, f0));
    end
    % normalize gt to make its maximum value to be 1
    gt = fhandle(kernel.pars, 1:nMax); % 1*nMax, response function of calcium transients
    ind0 = 1; %correct the response function of the first event
    gt = [reshape(gt, 1, nMax), zeros(1, T)]; % add few more elements to gt for computation convenience

    gt_max = max(gt);
    gt = gt/gt_max;
    gt1 = [gt(ind0:end), zeros(1, ind0-1)]; % kernel for the first frame

    %% initialize values for the results
    s = zeros(size(y));     % spike count within each bin
    v = zeros(size(y));     % sum(theta_t*y_t) - lambda
    w = zeros(size(y));     % sum(theta_t^2 * g(t-ti+1)^2))
    pool_ti = zeros(size(y)); % first frame of each pool

    % initialize the first pool
    v(1) = theta(1)*y(1)*gt1(1) - lambda;
    w(1) = (theta(1)*gt1(1))^2;
    s(1) = max(0, v(1) / w(1));
    %     if s(1)<smin
    %         s(1) = 0;
    %     end
    pool_ti(1) = 1;
    tpre = 1;   % time of the last event

    %% start to initialize the next pool
    ii = 2;     % start to initialize the next pool
    t = 2;      % frame to begin
    frame_valid = true;
    while t<=T
        % plot results
        if debug_on
            c = zeros(1,T);
            c(1:T) = s(1) * gt1(1:T);
            for m=2:(ii-1)
                t0 = pool_ti(m);
                c(t0:T) = c(t0:T)+s(t0) * gt(1:(T-t0+1));
            end
            delete(a);
            a = plot(c(1:t), 'r', 'linewidth', 2);
            drawnow;
            temp = getframe();
            temp.cdata = imresize(temp.cdata, [200, 1500]);
            avi_file.writeVideo(temp);
        end
        %
        % find the spike count that minimizes the objective function
        if (t-tpre)>=nMax  % no influences from the previous events
            vi = theta(t)*y(t)*gt(1) - lambda;
        elseif tpre>1        % estimate vi and subtract the effect of the previous event
            vi = theta(t) * (y(t)-s(tpre)*gt(t-tpre+1)) * gt(1) - lambda;
        else   % special treatment with the first event
            vi = theta(t) * (y(t)-s(tpre)*gt1(t-tpre+1)) * gt(1) - lambda;
        end
        wi = (theta(t) * gt(1))^2;
        si = vi/wi;

        % check the violatoin of si
        if si>smin && frame_valid % no violation, create a new pool to save the result and move to the next new pool
            % peel off the previous event
            if (t-tpre)<nMax
                if tpre>1
                    y(tpre+(1:nMax)-1) = y(tpre+(1:nMax)-1) - s(tpre)*gt(1:nMax);
                else
                    y(tpre+(1:nMax)-1) = y(tpre+(1:nMax)-1) - s(tpre)*gt1(1:nMax);
                end
            end
            pool_ti(ii) = t;   % create a new pool
            tpre = t;
            v(t) = vi;
            w(t) = wi;
            s(t) = si;
            ii = ii+1;  % move to the next pool
            t = t+1;
        else  % with violation
            frame_valid = true; % allows the next frame to be valid
            if (t-tpre)>=nMax
                % ignore this frame directly and move to the next frame
                t = t + 1;
                continue;
            elseif tpre>1         % merge it to the current pool
                v(tpre) = v(tpre) + theta(t)*y(t)*gt(t-tpre+1);
                w(tpre) = w(tpre) + (theta(t)*gt(t-tpre+1))^2;
            else
                v(tpre) = v(tpre) + theta(t)*y(t)*gt1(t-tpre+1);
                w(tpre) = w(tpre) + (theta(t)*gt1(t-tpre+1));
            end
            s(tpre) = v(tpre)/w(tpre);  % update the current event

            % check the violation of the current pool
            if s(tpre)>smin      % the previous event is still avaiable
                t = t+1;
                continue;
            elseif ii==2  %the previous pool is not available anymore, but it's the first pool
                s(tpre) = max(0, s(tpre));
                t = t+1;
            else   % not available, then delete the current pool and force its first frame to be
                % event-free.
                t = tpre;  % go back to the first frame of the current pull

                frame_valid = false; % force the frame t to be invalid
                ii = ii-1;
                tpre = pool_ti(ii-1);

                % add back the signal of the previous pool
                if (t-tpre)>nMax
                    t = t+1;
                    frame_valid = true;
                    continue;
                elseif tpre>1
                    y(tpre+(1:nMax)-1) = y(tpre+(1:nMax)-1) + s(tpre)*gt(1:nMax);
                else
                    y(tpre+(1:nMax)-1) = y(tpre+(1:nMax)-1) + s(tpre)*gt1(1:nMax);
                end
            end
        end
    end
    %% collect the results
    pool_ti(ii:end) = [];
    temp = s(pool_ti);
    s = zeros(1, T);
    s(pool_ti) = temp;
    temp = s;
    temp(1) = 0;
    c = conv(temp, gt(1:nMax));
    c = c(1:T) + s(1)*gt1(1:T);
    f1 = norm(y(1:T)-c, 2);
    s = s*sum(gt);
    if debug_on
        delete(a);
        a = plot(c, 'r');
        drawnow();
        temp = getframe();
        temp.cdata = imresize(temp.cdata, [200, 1500]);
        avi_file.writeVideo(temp);
    end

    %% break the while loop
    if ~fit_gt  % don't iterate gt
        break;
    elseif (f0-f1)/f0 <= thresh % improvement is small, stop
        break;
    else  % move to the next iteration
        y = y0;
        if strcmpi(kernel.type, 'exp2')
            [kernel, ~] = update_kernel_exp2(y(1:T), s, kernel);
        else
            break;
        end
        f0 = f1;
        iter = iter + 1;
        %             return;
    end
end

if debug_on
    avi_file.close();
    close(gcf);
end
end

%% estimate the noise power
function [sn,psdx,ff] = get_noise_fft(Y,options)
% Written by:
% Eftychios A. Pnevmatikakis, Simons Foundation, 2015
% with minor adaption by Pengcheng Zhou, Carnegie Mellon University, 2015
options.noise_range = [.25, .5];
range_ff = options.noise_range;
options.noise_method = 'logmexp';
method = options.noise_method;
options.block_size = [64, 64];
block_size = options.block_size;
options.split_data = false;
split_data = options.split_data;
options.max_timesteps = 3000;

dims = ndims(Y);
sizY = size(Y);
N = min(sizY(end),options.max_timesteps);
if N < sizY(end)
    %Y = reshape(Y,prod(sizY(1:end-1)),[]);
    Y(prod(sizY(1:end-1))*N+1:end) = [];
    Y = reshape(Y,[sizY(1:end-1),N]);
end

Fs = 1;
ff = 0:Fs/N:Fs/2;
indf=ff>range_ff(1);
indf(ff>range_ff(2))=0;
if dims > 1
    d = prod(sizY(1:dims-1));
    Y = reshape(Y,d,N);
    Nb = prod(block_size);
    SN = cell(ceil(d/Nb),1);
    PSDX = cell(ceil(d/Nb),1);
    if ~split_data
        for ind = 1:ceil(d/Nb);
            xdft = fft(Y((ind-1)*Nb+1:min(ind*Nb,d),:),[],2);
            xdft = xdft(:,1: floor(N/2)+1); % FN: floor added.
            psdx = (1/(Fs*N)) * abs(xdft).^2;
            psdx(:,2:end-1) = 2*psdx(:,2:end-1);
            %SN{ind} = mean_psd(psdx(:,indf),method);
            switch method
                case 'mean'
                    SN{ind}=sqrt(mean(psdx(:,indf)/2,2));
                case 'median'
                    SN{ind}=sqrt(median(psdx(:,indf)/2),2);
                case 'logmexp'
                    SN{ind} = sqrt(exp(mean(log(psdx(:,indf)/2),2)));
            end
            PSDX{ind} = psdx;
        end
    else
        nc = ceil(d/Nb);
        Yc = mat2cell(Y,[Nb*ones(nc-1,1);d-(nc-1)*Nb],N);
        parfor ind = 1:ceil(d/Nb);
            xdft = fft(Yc{ind},[],2);
            xdft = xdft(:,1:floor(N/2)+1);
            psdx = (1/(Fs*N)) * abs(xdft).^2;
            psdx(:,2:end-1) = 2*psdx(:,2:end-1);
            Yc{ind} = [];
            switch method
                case 'mean'
                    SN{ind}=sqrt(mean(psdx(:,indf)/2,2));
                case 'median'
                    SN{ind}=sqrt(median(psdx(:,indf)/2),2);
                case 'logmexp'
                    SN{ind} = sqrt(exp(mean(log(psdx(:,indf)/2),2)));
            end

        end
    end
    sn = cell2mat(SN);
else
    xdft = fft(Y);
    xdft = xdft(:,1:floor(N/2)+1);
    psdx = (1/(Fs*N)) * abs(xdft).^2;
    psdx(:,2:end-1) = 2*psdx(:,2:end-1);
    switch method
        case 'mean'
            sn = sqrt(mean(psdx(:,indf)/2,2));
        case 'median'
            sn = sqrt(median(psdx(:,indf)/2),2);
        case 'logmexp'
            sn = sqrt(exp(mean(log(psdx(:,indf)/2),2)));
    end
end
psdx = cell2mat(PSDX);
if dims > 2
    sn = reshape(sn,sizY(1:dims-1));
end
end