%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting %%	lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us %to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the BEAM-EXAMPLE geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function BeamCurve()

%
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%
Nx = 256;        % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Ny = 256;        % # of Eulerian Grid Pts. in y-Direction (MUST BE EVEN!!!)
Lx = 1.0;        % Length of Eulerian Grid in x-Direction
Ly = 1.0;        % Length of Eulerian Grid in y-Direction

% Immersed Structure Geometric / Dynamic Parameters %
N = 1.5*Nx;        % Number of Lagrangian Pts. (2x resolution of Eulerian grid)
a = 0.5;         % Length of beam (only in x-coordinate)
struct_name = 'BeamCurve'; % Name for .vertex, .spring, .beam, .target, etc files.

% Call function to construct geometry
[xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(N,Lx,a);

% Plot Geometry to test
plot(xLag,yLag,'r-'); hold on;
plot(xLag,yLag,'*'); hold on;
xlabel('x'); ylabel('y');
axis square;

% Prints .vertex file!
print_Lagrangian_Vertices(xLag,yLag,struct_name);

% Prints .beam file!
k_Beam = 5e11; C = 0.0;
print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name);

% Prints .target file!
k_Target = 1.75e8;
print_Lagrangian_Target_Pts(xLag,k_Target,struct_name)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called rubberband.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag);

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints Vertex points to a file called rubberband.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name)

    N = length(xLag);

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', 2 );

    fprintf(target_fid, '%d %1.16e\n', 1, k_Target);
    fprintf(target_fid, '%d %1.16e\n', N, k_Target);

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (Torsional Spring) points to a file called rubberband.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name)

    % k_Beam: beam stiffness
    % C: beam curvature

    N = length(xLag); % NOTE: Total number of beams = Number of Total Lag Pts. - 2

    beam_fid = fopen([struct_name '.beam'], 'w');

    fprintf(beam_fid, '%d\n', N-2 );

    %spring_force = kappa_spring*ds/(ds^2);

    %BEAMS BETWEEN VERTICES
    for s = 2:N-1
            if  s <= N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C);
            else
                %Case s=N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, 1,   k_Beam, C);
            end
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called rubberband.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Rest,struct_name)

    N = length(xLag);

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', N );

    %spring_force = kappa_spring*ds/(ds^2);

    %SPRINGS BETWEEN VERTICES
    for s = 1:N
            if s < N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            else
                %Case s=N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, 1,   k_Spring, ds_Rest);
            end
    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(N,Lx,a)

% The immsersed structure is a curved line %
ds = a/(N-1);

xLag(1) = -a/2;%Lx/2-a/2;
yLag(1) = 0.0;
for i=2:N

    xLag(i) = xLag(i-1) + ds;
    x = xLag(i);
    yLag(i) = -2*( (x)^2 - (a/2)^2 );

end

xLag = xLag + Lx/2;
yLag = yLag + Lx/2;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   FANUC MATE M710iC/50, FANUC Robotics Europe.
%
%   Authors: Carlos Carrazoni Perez, Juan Jose Perez Hernandez & David
%   Martinez Pascual
%   Universidad Miguel Hernandez de Elche.
%   date:   7/1/2019
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2019, by  Carlos Carrazoni Perez, Juan Jose Perez Hernandez
% & David Martinez Pascual
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.
function robot = parameters()

robot.name= 'Fanuc_M_710iC_50';

%Path where everything is stored for this robot
robot.path = 'robots/fanuc/M_710iC_50';
%Tabla de D-H
robot.DH.theta= '[q(1)+pi/2  q(2)+pi/2  q(3)  q(4)  q(5)  q(6)]';
robot.DH.d='[0.565 0 0 1.016 0 0.175]';
robot.DH.a='[0.15 0.87 0.17 0 0 0]';
robot.DH.alpha= '[pi/2  0  pi/2  pi/2  -pi/2  0]';
robot.J=[];

robot.inversekinematic_fn = 'inversekinematic_fanuc_m710(robot, T)';

%number of degrees of freedom
robot.DOF = 6;

%rotational: 0, translational: 1
robot.kind=['R' 'R' 'R' 'R' 'R' 'R'];

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-180) deg2rad(180); %Axis 1, minimum, maximum
                deg2rad(-135) deg2rad(90); %Axis 2, minimum, maximum
                deg2rad(-160) deg2rad(280); %Axis 3
                deg2rad(-360) deg2rad(360); %Axis 4
                deg2rad(-125) deg2rad(125); %Axis 5
                deg2rad(-360) deg2rad(360)]; %Axis 6

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(175); %Axis 1, rad/s
                deg2rad(175); %Axis 2, rad/s
                deg2rad(175); %Axis 3, rad/s
                deg2rad(250); %Axis 4, rad/s
                deg2rad(250); %Axis 5, rad/s
                deg2rad(355)];%Axis 6, rad/s
% end effectors maximum velocity
robot.linear_velmax = 1.0; %m/s, not specified
robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time

%base reference system
robot.T0 = eye(4);

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);
robot.tool_activated=1;

% GRAPHICS
robot.graphical.has_graphics=1;
robot.graphical.color = [0.9 0.9 0]%./255;
%for transparency
robot.graphical.draw_transparent=0;
%draw DH systems
robot.graphical.draw_axes=1;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=1;
%adjust for a default view of the robot
robot.axis=[-1.5 1.5 -1.5 1.5 0 2];
%read graphics files
robot = read_graphics(robot);

%robot.tool=load_robot('equipment/end_tools','water_cutter');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DYNAMIC PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
robot.has_dynamics=1;

%consider friction in the computations
robot.dynamics.friction=0;

%link masses (kg)
robot.dynamics.masses=[191.865 71.802 111.5 27.55 5.705 0.674];

%COM of each link with respect to own reference system
robot.dynamics.r_com=[-0.109  -0.099  0.053; %(rx, ry, rz) link 1
                      -0.466  -0.001 -0.287; %(rx, ry, rz) link 2
                      -0.086   0.032  0.007;  %(rx, ry, rz) link 3
                       0       0.342  0.011;  %(rx, ry, rz) link 4
                       0      -0.015  0.076 ; %(rx, ry, rz) link 5
                       0       0      0.010];%(rx, ry, rz) link 6

%Inertia matrices of each link with respect to its D-H reference system.
% Ixx	Iyy	Izz	Ixy	Iyz	Ixz, for each row
robot.dynamics.Inertia=[8.411      9.434	9.499   -2.810	0.691	1.396;
                        6.297     28.162	22.396  -0.049	-0.012	-9.398;
                        2.313      3.367	2.986	0.467	0.232	-0.285;
                        4.863	0.072	4.846	-0.002	0	0;
                        0.065	0.058	0.022	0	0.004   0;
                        0.000768	0.000767	0.001368	0	0	0];

%Speed reductor at each joint
robot.motors.G=[10 20 120 200 300 1];


%%*****************************************************************************
%% sqlp: solve an semidefinite-quadratic-linear program
%%       by infeasible path-following method.
%%
%%  [obj,X,y,Z,info,runhist] = sqlp(blk,At,C,b,OPTIONS,X0,y0,Z0);
%%
%%  Input: blk: a cell array describing the block diagonal structure of SQL data.
%%          At: a cell array with At{p} = [svec(Ap1) ... svec(Apm)]
%%         b,C: data for the SQL instance.
%%  (X0,y0,Z0): an initial iterate (if it is not given, the default is used).
%%     OPTIONS: a structure that specifies parameters required in sqlp.m,
%%              (if it is not given, the default in sqlparameters.m is used).
%%
%%  Output: obj  = [<C,X> <b,y>].
%%          (X,y,Z): an approximately optimal solution or a primal or dual
%%                   infeasibility certificate.
%%          info.termcode = termination-code
%%          info.iter     = number of iterations
%%          info.obj      = [primal-obj, dual-obj]
%%          info.cputime  = total-time
%%          info.gap      = gap
%%          info.pinfeas  = primal_infeas
%%          info.dinfeas  = dual_infeas
%%          runhist.pobj    = history of primal objective value.
%%          runhist.dobj    = history of dual   objective value.
%%          runhist.gap     = history of <X,Z>.
%%          runhist.pinfeas = history of primal infeasibility.
%%          runhist.dinfeas = history of dual   infeasibility.
%%          runhist.cputime = history of cputime spent.
%%----------------------------------------------------------------------------
%%  The OPTIONS structure specifies the required parameters:
%%      vers  gam  predcorr  expon  gaptol  inftol  steptol
%%      maxit  printlevel  scale_data ...
%%      (all have default values set in sqlparameters.m).
%%*************************************************************************
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*************************************************************************

function [obj,X,y,Z,info,runhist] = sqlp(blk,At,C,b,OPTIONS,X0,y0,Z0)

if (nargin < 5); OPTIONS = []; end

isemptyAtb = 0;
if isempty(At) && isempty(b);
    %% Add redundant constraint: <-I,X> <= 0
    b = 0;
    At = ops(ops(blk,'identity'),'*',-1);
    numblk = size(blk,1);
    blk{numblk+1,1} = 'l'; blk{numblk+1,2} = 1;
    At{numblk+1,1} = 1; C{numblk+1,1} = 0;
    isemptyAtb = 1;
end
%%
%%-----------------------------------------
%% get parameters from the OPTIONS structure.
%%-----------------------------------------
%%

matlabversion = sscanf(version,'%f');
if strcmp(computer,'PCWIN64') || strcmp(computer,'GLNXA64')
    par.computer = 64;
else
    par.computer = 32;
end
par.matlabversion = matlabversion(1);
par.vers        = 0;
par.predcorr    = 1;
par.gam         = 0;
par.expon       = 1;
par.gaptol      = 1e-8;
par.inftol      = 1e-8;
par.steptol     = 1e-6;
par.maxit       = 100;
par.printlevel  = 3;
par.stoplevel   = 1;
par.scale_data  = 0;
par.spdensity   = 0.4;
par.rmdepconstr = 0;
par.smallblkdim = 50;
par.schurfun     = cell(size(blk,1),1);
par.schurfun_par = cell(size(blk,1),1);
%%
parbarrier  = cell(size(blk,1),1);
for p = 1:size(blk,1)
    pblk = blk(p,:);
    if strcmp(pblk{1},'s') || strcmp(pblk{1},'q')
        parbarrier{p} = zeros(1,length(pblk{2}));
    elseif strcmp(pblk{1},'l') || strcmp(pblk{1},'u' )
        parbarrier{p} = zeros(1,sum(pblk{2}));
    end
end
parbarrier_0 = parbarrier;
%%
if nargin > 4,
    if isfield(OPTIONS,'vers');        par.vers     = OPTIONS.vers; end
    if isfield(OPTIONS,'predcorr');    par.predcorr = OPTIONS.predcorr; end
    if isfield(OPTIONS,'gam');         par.gam      = OPTIONS.gam; end
    if isfield(OPTIONS,'expon');       par.expon    = OPTIONS.expon; end
    if isfield(OPTIONS,'gaptol');      par.gaptol   = OPTIONS.gaptol; end
    if isfield(OPTIONS,'inftol');      par.inftol   = OPTIONS.inftol; end
    if isfield(OPTIONS,'steptol');     par.steptol  = OPTIONS.steptol; end
    if isfield(OPTIONS,'maxit');       par.maxit    = OPTIONS.maxit; end
    if isfield(OPTIONS,'printlevel');  par.printlevel  = OPTIONS.printlevel; end
    if isfield(OPTIONS,'stoplevel');   par.stoplevel   = OPTIONS.stoplevel; end
    if isfield(OPTIONS,'scale_data');  par.scale_data  = OPTIONS.scale_data; end
    if isfield(OPTIONS,'spdensity');   par.spdensity   = OPTIONS.spdensity; end
    if isfield(OPTIONS,'rmdepconstr'); par.rmdepconstr = OPTIONS.rmdepconstr; end
    if isfield(OPTIONS,'smallblkdim'); par.smallblkdim = OPTIONS.smallblkdim; end
    if isfield(OPTIONS,'parbarrier');
        parbarrier = OPTIONS.parbarrier;
        if isempty(parbarrier); parbarrier = parbarrier_0; end
        if ~iscell(parbarrier);
            tmp = parbarrier; clear parbarrier; parbarrier{1} = tmp;
        end
        if (length(parbarrier) < size(blk,1))
            len = length(parbarrier);
            parbarrier(len+1:size(blk,1)) = parbarrier_0(len+1:size(blk,1));
        end
    end
    if isfield(OPTIONS,'schurfun');
        par.schurfun = OPTIONS.schurfun;
        if ~isempty(par.schurfun); par.scale_data = 0; end
    end
    if isfield(OPTIONS,'schurfun_par'); par.schurfun_par = OPTIONS.schurfun_par; end
    if isempty(par.schurfun);     par.schurfun = cell(size(blk,1),1); end
    if isempty(par.schurfun_par); par.schurfun_par = cell(size(blk,1),1); end
end
if (size(blk,2) > 2); par.smallblkdim = 0; end
%%
%%-----------------------------------------
%% convert matrices to cell arrays.
%%-----------------------------------------
%%
if ~iscell(At); At = {At}; end;
if ~iscell(C);  C = {C}; end;
if all(size(At) == [size(blk,1), length(b)]);
    convertyes = zeros(size(blk,1),1);
    for p = 1:size(blk,1)
        if strcmp(blk{p,1},'s') && all(size(At{p,1}) == sum(blk{p,2}))
            convertyes(p) = 1;
        end
    end
    if any(convertyes)
        if (par.printlevel);
            fprintf('\n sqlp: converting At into required format');
        end
        At = svec(blk,At,ones(size(blk,1),1));
    end
end
%%
%%-----------------------------------------
%% validate SQLP data.
%%-----------------------------------------
%%
% tstart = cputime;
[blk,At,C,b,blkdim,numblk,parbarrier] = validate(blk,At,C,b,par,parbarrier);
[blk,At,C,b,iscmp] = convertcmpsdp(blk,At,C,b);
if (iscmp) && (par.printlevel>=2);
    fprintf('\n SQLP has complex data');
end
if (nargin <= 5) || (isempty(X0) || isempty(y0) || isempty(Z0));
    par.startpoint = 1;
    [X0,y0,Z0] = infeaspt(blk,At,C,b);
else
    par.startpoint = 2;
    if ~iscell(X0);  X0 = {X0}; end;
    if ~iscell(Z0);  Z0 = {Z0}; end;
    y0 = real(y0);
    if (length(y0) ~= length(b));
        error('sqlp: length of b and y0 not compatible');
    end
    [X0,Z0] = validate_startpoint(blk,X0,Z0,par.spdensity,iscmp);
end
if (par.printlevel>=2)
    fprintf('\n num. of constraints = %2.0d',length(b));
    if blkdim(1);
        fprintf('\n dim. of sdp    var  = %2.0d,',blkdim(1));
        fprintf('   num. of sdp  blk  = %2.0d',numblk(1));
    end
    if blkdim(2);
        fprintf('\n dim. of socp   var  = %2.0d,',blkdim(2));
        fprintf('   num. of socp blk  = %2.0d',numblk(2));
    end
    if blkdim(3); fprintf('\n dim. of linear var  = %2.0d',blkdim(3)); end
    if blkdim(4); fprintf('\n dim. of free   var  = %2.0d',blkdim(4)); end
end
%%
%%-----------------------------------------
%% detect unrestricted blocks in linear blocks
%%-----------------------------------------
%%
user_supplied_schurfun = 0;
for p = 1:size(blk,1)
    if ~isempty(par.schurfun{p}); user_supplied_schurfun = 1; end
end
if (user_supplied_schurfun == 0)
    [blk2,At2,C2,ublkinfo,parbarrier2,X02,Z02] = ...
        detect_ublk(blk,At,C,parbarrier,X0,Z0,par.printlevel);
else
    blk2 = blk; At2 = At; C2 = C;
    parbarrier2 = parbarrier; X02 = X0; Z02 = Z0;
    ublkinfo = cell(size(blk2,1),1);
end
ublksize = blkdim(4);
for p = 1:size(ublkinfo,1)
    ublksize = ublksize + length(ublkinfo{p});
end
%%
%%-----------------------------------------
%% detect diagonal blocks in semidefinite blocks
%%-----------------------------------------
%%
if (user_supplied_schurfun==0)
    [blk3,At3,C3,diagblkinfo,diagblkchange,parbarrier3,X03,Z03] = ...
        detect_lblk(blk2,At2,C2,b,parbarrier2,X02,Z02,par.printlevel);
else
    blk3 = blk2; At3 = At2; C3 = C2;
    parbarrier3 = parbarrier2; X03 = X02; Z03 = Z02;
    diagblkchange = 0;
    diagblkinfo = cell(size(blk3,1),1);
end
%%
%%-----------------------------------------
%% main solver
%%-----------------------------------------
%%
%    exist_analytic_term = 0;
%    for p = 1:size(blk3,1);
%       idx = find(parbarrier3{p} > 0);
%       if ~isempty(idx); exist_analytic_term = 1; end
%    end
%
if (par.vers == 0);
    if blkdim(1); par.vers = 1; else par.vers = 2; end
end
par.blkdim = blkdim;
par.ublksize = ublksize;
[obj,X3,y,Z3,info,runhist] = ...
    sqlpmain(blk3,At3,C3,b,par,parbarrier3,X03,y0,Z03);
%%
%%-----------------------------------------
%% recover semidefinite blocks from linear blocks
%%-----------------------------------------
%%
if any(diagblkchange)
    X2 = cell(size(blk2,1),1); Z2 = cell(size(blk2,1),1);
    count = 0;
    for p = 1:size(blk2,1)
        pblk = blk2(p,:);
        n = sum(pblk{2});
        blkno      = diagblkinfo{p,1};
        idxdiag    = diagblkinfo{p,2};
        idxnondiag = diagblkinfo{p,3};
        if ~isempty(idxdiag)
            len = length(idxdiag);
            Xtmp = [idxdiag,idxdiag,X3{end}(count+1:count+len); n, n, 0];
            Ztmp = [idxdiag,idxdiag,Z3{end}(count+1:count+len); n, n, 0];
            if ~isempty(idxnondiag)
                [ii,jj,vv] = find(X3{blkno});
                Xtmp = [Xtmp; idxnondiag(ii),idxnondiag(jj),vv]; %#ok
                [ii,jj,vv] = find(Z3{blkno});
                Ztmp = [Ztmp; idxnondiag(ii),idxnondiag(jj),vv]; %#ok
            end
            X2{p} = spconvert(Xtmp);
            Z2{p} = spconvert(Ztmp);
            count = count + len;
        else
            X2(p) = X3(blkno); Z2(p) = Z3(blkno);
        end
    end
else
    X2 = X3; Z2 = Z3;
end
%%
%%-----------------------------------------
%% recover linear block from unrestricted block
%%-----------------------------------------
%%
numblk = size(blk,1);
numblknew = numblk;
X = cell(numblk,1); Z = cell(numblk,1);
for p = 1:numblk
    n = blk{p,2};
    if isempty(ublkinfo{p,1})
        X{p} = X2{p}; Z{p} = Z2{p};
    else
        Xtmp = zeros(n,1); Ztmp = zeros(n,1);
        Xtmp(ublkinfo{p,1}) = max(0,X2{p});
        Xtmp(ublkinfo{p,2}) = max(0,-X2{p});
        Ztmp(ublkinfo{p,1}) = max(0,Z2{p});
        Ztmp(ublkinfo{p,2}) = max(0,-Z2{p});
        if ~isempty(ublkinfo{p,3})
            numblknew = numblknew + 1;
            Xtmp(ublkinfo{p,3}) = X2{numblknew};
            Ztmp(ublkinfo{p,3}) = Z2{numblknew};
        end
        X{p} = Xtmp; Z{p} = Ztmp;
    end
end
%%
%%-----------------------------------------
%% recover complex solution
%%-----------------------------------------
%%
if (iscmp)
    for p = 1:numblk
        pblk = blk(p,:);
        n = sum(pblk{2})/2;
        if strcmp(pblk{1},'s');
            X{p} = X{p}(1:n,1:n) + sqrt(-1)*X{p}(n+1:2*n,1:n);
            Z{p} = Z{p}(1:n,1:n) + sqrt(-1)*Z{p}(n+1:2*n,1:n);
            X{p} = 0.5*(X{p}+X{p}');
            Z{p} = 0.5*(Z{p}+Z{p}');
        end
    end
end
if (isemptyAtb)
    X = X(1:end-1); Z = Z(1:end-1);
end
%%*****************************************************************************


function sdn = utc2sdn(utc)
% UTC2SDN   - Convert UTC seconds to matlab date format
%
% Use as: sdn = utc2sdn(utc);
%
% sdn = Matlab datenumber (se DATENUM and DATESTR)
% utc = Seconds since 01 Jan 1970 00:00:00

% Brian Schlining
% 12 Apr 2000

%datenum('01 Jan 1970 00:00:00') = 719529
sdn = utc/60/60/24 + 719529;


function hmm = states_supdate(hmm,hmm_noisy,rho,update)
% update==1, W; update==2, Omega; update==3, sigma; update==4, alpha
K = length(hmm_noisy.state);
Sind = hmm.train.Sind==1;
regressed = sum((hmm.train.Sind==1),1)>0;

for k = 1:K
    if any(update==1) && isfield(hmm_noisy.state(1),'W') && ~isempty(hmm_noisy.state(1).W.Mu_W)
        hmm.state(k).W.Mu_W = (1-rho) * hmm.state(k).W.Mu_W + ...
            rho * hmm_noisy.state(k).W.Mu_W;
        hmm.state(k).W.iS_W = (1-rho) * hmm.state(k).W.iS_W + ...
            rho * hmm_noisy.state(k).W.iS_W;
        if length(size(hmm.state(k).W.S_W))==2 && ... % full, uniquefull, uniqueAR or 1 dimension
               size(hmm.state(k).W.S_W,1)==size(hmm.state(k).W.S_W,2)
            hmm.state(k).W.S_W = inv(hmm.state(k).W.iS_W);
        else % diag or shareddiag
            for n = 1:size(hmm.state(k).W.S_W,1)
                hmm.state(k).W.S_W(n,Sind(:,n),Sind(:,n)) = ...
                    inv(permute(hmm.state(k).W.iS_W(n,Sind(:,n),Sind(:,n)),[2 3 1]));
            end
        end
    elseif any(update==2) && isfield(hmm_noisy.state(1),'Omega')
        hmm.state(k).Omega.Gam_rate = (1-rho) * hmm.state(k).Omega.Gam_rate + ...
            rho * hmm_noisy.state(k).Omega.Gam_rate;
        hmm.state(k).Omega.Gam_shape = (1-rho) * hmm.state(k).Omega.Gam_shape + ...
            rho * hmm_noisy.state(k).Omega.Gam_shape;
        if strcmp(hmm_noisy.train.covtype,'full') || ...
                strcmp(hmm_noisy.train.covtype,'uniquefull') || ...
                strcmp(hmm_noisy.train.covtype,'sharedfull')
            hmm.state(k).Omega.Gam_irate(regressed,regressed) = ...
                inv(hmm.state(k).Omega.Gam_rate(regressed,regressed));
        end
    elseif any(update==3) && isfield(hmm_noisy.state(1),'sigma')
        hmm.state(k).sigma.Gam_rate = (1-rho) * hmm.state(k).sigma.Gam_rate + ...
            rho * hmm_noisy.state(k).sigma.Gam_rate;
        hmm.state(k).sigma.Gam_shape = (1-rho) * hmm.state(k).sigma.Gam_shape + ...
            rho * hmm_noisy.state(k).sigma.Gam_shape;
    elseif any(update==4) && isfield(hmm_noisy.state(1),'alpha')
        hmm.state(k).alpha.Gam_rate = (1-rho) * hmm.state(k).alpha.Gam_rate + ...
            rho * hmm_noisy.state(k).alpha.Gam_rate;
        hmm.state(k).alpha.Gam_shape = (1-rho) * hmm.state(k).alpha.Gam_shape + ...
            rho * hmm_noisy.state(k).alpha.Gam_shape;
    elseif any(update==5) && isfield(hmm_noisy.state(1),'beta')
        hmm.state(k).beta.Gam_rate = (1-rho) * hmm.state(k).beta.Gam_rate + ...
            rho * hmm_noisy.state(k).beta.Gam_rate;
        hmm.state(k).beta.Gam_shape = (1-rho) * hmm.state(k).beta.Gam_shape + ...
            rho * hmm_noisy.state(k).beta.Gam_shape;
    end
end

if any(update==2) && isfield(hmm_noisy,'Omega')
    hmm.Omega.Gam_rate = (1-rho) * hmm.Omega.Gam_rate + ...
        rho * hmm_noisy.Omega.Gam_rate;
    hmm.Omega.Gam_shape = (1-rho) * hmm.Omega.Gam_shape + ...
        rho * hmm_noisy.Omega.Gam_shape;
    if strcmp(hmm_noisy.train.covtype,'uniquefull') || ...
            strcmp(hmm_noisy.train.covtype,'sharedfull')
        hmm.Omega.Gam_irate(regressed,regressed) = ...
            inv(hmm.Omega.Gam_rate(regressed,regressed));
    end
end

end


function [c, v, n] = ft_connectivity_psi(input, varargin)

% FT_CONNECTIVITY_PSI computes the phase slope index from a data-matrix
% containing the cross-spectral density. It implements the method described
% in: Nolte et al., Robustly estimating the flow direction of information
% in complex physical systems. Physical Review Letters, 2008; 100; 234101.
%
% Use as
%   [c, v, n] = ft_connectivity_psi(input, ...)
%
% The input data input should be organized as
%   Repetitions x Channel x Channel (x Frequency) (x Time)
% or
%   Repetitions x Channelcombination (x Frequency) (x Time)
%
% The first dimension should be singleton if the input already contains an
% average.
%
% Additional optional input arguments come as key-value pairs:
%   nbin			=	scalar, half-bandwidth parameter: the number of frequency bins
%								across which to integrate
%   hasjack		= 0 or 1, specifying whether the repetitions represent
%               leave-one-out samples (allowing for a variance estimate)
%   feedback	= 'none', 'text', 'textbar' type of feedback showing progress of
%               computation
%   dimord		= string, specifying how the input matrix should be interpreted
%   powindx   =
%   normalize =
%
% The output p contains the phase slope index, v is a variance estimate
% which only can be computed if the data contains leave-one-out samples,
% and n is the number of repetitions in the input data. If the phase slope
% index is positive, then the first chan (1st dim) becomes more lagged (or
% less leading) with higher frequency, indicating that it is causally
% driven by the second channel (2nd dim)
%
% See also FT_CONNECTIVITYANALYSIS

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

% FIXME: interpretation of the slope

hasjack   = ft_getopt(varargin, 'hasjack', 0);
feedback  = ft_getopt(varargin, 'feedback', 'none');
dimord    = ft_getopt(varargin, 'dimord');
powindx   = ft_getopt(varargin, 'powindx');
normalize = ft_getopt(varargin, 'normalize', 'no');
nbin      = ft_getopt(varargin, 'nbin');

if isempty(dimord)
  ft_error('input parameters should contain a dimord');
end

if (length(strfind(dimord, 'chan'))~=2 || contains(dimord, 'pos')>0) && ~isempty(powindx)
  %crossterms are not described with chan_chan_therest, but are linearly indexed

  siz = size(input);

  outsum = zeros(siz(2:end));
  outssq = zeros(siz(2:end));
  pvec   = [2 setdiff(1:numel(siz),2)];

  ft_progress('init', feedback, 'computing metric...');
  %first compute coherency and then phaseslopeindex
  for j = 1:siz(1)
    ft_progress(j/siz(1), 'computing metric for replicate %d from %d\n', j, siz(1));
    c      = reshape(input(j,:,:,:,:), siz(2:end));
    p1     = abs(reshape(input(j,powindx(:,1),:,:,:), siz(2:end)));
    p2     = abs(reshape(input(j,powindx(:,2),:,:,:), siz(2:end)));

    p      = ipermute(phaseslope(permute(c./sqrt(p1.*p2), pvec), nbin, normalize), pvec);

    outsum = outsum + p;
    outssq = outssq + p.^2;
  end
  ft_progress('close');

elseif length(strfind(dimord, 'chan'))==2 || length(strfind(dimord, 'pos'))==2
  %crossterms are described by chan_chan_therest

  siz = size(input);

  outsum = zeros(siz(2:end));
  outssq = zeros(siz(2:end));
  pvec   = [3 setdiff(1:numel(siz),3)];

  ft_progress('init', feedback, 'computing metric...');
  for j = 1:siz(1)
    ft_progress(j/siz(1), 'computing metric for replicate %d from %d\n', j, siz(1));
    p1  = zeros([siz(2) 1 siz(4:end)]);
    p2  = zeros([1 siz(3) siz(4:end)]);
    for k = 1:siz(2)
      p1(k,1,:,:,:,:) = input(j,k,k,:,:,:,:);
      p2(1,k,:,:,:,:) = input(j,k,k,:,:,:,:);
    end
    c      = reshape(input(j,:,:,:,:,:,:), siz(2:end));
    p1     = p1(:,ones(1,siz(3)),:,:,:,:);
    p2     = p2(ones(1,siz(2)),:,:,:,:,:);
    p      = ipermute(phaseslope(permute(c./sqrt(p1.*p2), pvec), nbin, normalize), pvec);
    p(isnan(p)) = 0;
    outsum = outsum + p;
    outssq = outssq + p.^2;
  end
  ft_progress('close');

end

n = siz(1);
c = outsum./n;

if n>1
  n = shiftdim(sum(~isnan(input),1),1);
  if hasjack
    bias = (n-1).^2;
  else
    bias = 1;
  end
  v = bias.*(outssq - (outsum.^2)./n)./(n - 1);
else
  v = [];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [y] = phaseslope(x, n, norm)

m   = size(x, 1); %total number of frequency bins
y   = zeros(size(x));
x(1:end-1,:,:,:,:) = conj(x(1:end-1,:,:,:,:)).*x(2:end,:,:,:,:);

if strcmp(norm, 'yes')
  coh = zeros(size(x));
  coh(1:end-1,:,:,:,:) = (abs(x(1:end-1,:,:,:,:)) .* abs(x(2:end,:,:,:,:))) + 1;
  %FIXME why the +1? get the coherence
  for k = 1:m
    begindx = max(1,k-n);
    endindx = min(m,k+n);
    y(k,:,:,:,:) = imag(nansum(x(begindx:endindx,:,:,:,:)./coh(begindx:endindx,:,:,:,:),1));
  end
else
  for k = 1:m
    begindx = max(1,k-n);
    endindx = min(m,k+n);
    y(k,:,:,:,:) = imag(nansum(x(begindx:endindx,:,:,:,:),1));
  end
end


function [flowx,flowy] = specific_wind(x,y,nel)
%constant_wind   Reference problem 3.3 convective wind
%   [flowx,flowy] = specific_wind(x,y,nel);
%   input
%          x          x coordinate vector
%          y          y coordinate vector
%          nel        number of elements
%   specifies constant wind at angle theta to vertical
%   IFISS function: DJS; 5 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      theta= -pi/6;
      flowx =  sin(theta)*ones(nel,1);
      flowy =  cos(theta)*ones(nel,1);
      return


function [x, cost, info, options] = trustregions(problem, x, options)
% Riemannian trust-regions solver for optimization on manifolds.
%
% function [x, cost, info, options] = trustregions(problem)
% function [x, cost, info, options] = trustregions(problem, x0)
% function [x, cost, info, options] = trustregions(problem, x0, options)
% function [x, cost, info, options] = trustregions(problem, [], options)
%
% This is the Riemannian Trust-Region solver (with tCG inner solve), named
% RTR. This solver will attempt to minimize the cost function described in
% the problem structure. It requires the availability of the cost function
% and of its gradient. It will issue calls for the Hessian. If no Hessian
% nor approximate Hessian is provided, a standard approximation of the
% Hessian based on the gradient will be computed. If a preconditioner for
% the Hessian is provided, it will be used.
%
% For a description of the algorithm and theorems offering convergence
% guarantees, see the references below. Documentation for this solver is
% available online at:
%
% http://www.manopt.org/solver_documentation_trustregions.html
%
%
% The initial iterate is x0 if it is provided. Otherwise, a random point on
% the manifold is picked. To specify options whilst not specifying an
% initial iterate, give x0 as [] (the empty matrix).
%
% The two outputs 'x' and 'cost' are the last reached point on the manifold
% and its cost. Notice that x is not necessarily the best reached point,
% because this solver is not forced to be a descent method. In particular,
% very close to convergence, it is sometimes preferable to accept very
% slight increases in the cost value (on the order of the machine epsilon)
% in the process of reaching fine convergence. In practice, this is not a
% limiting factor, as normally one does not need fine enough convergence
% that this becomes an issue.
%
% The output 'info' is a struct-array which contains information about the
% iterations:
%   iter (integer)
%       The (outer) iteration number, or number of steps considered
%       (whether accepted or rejected). The initial guess is 0.
%	cost (double)
%       The corresponding cost value.
%	gradnorm (double)
%       The (Riemannian) norm of the gradient.
%	numinner (integer)
%       The number of inner iterations executed to compute this iterate.
%       Inner iterations are truncated-CG steps. Each one requires a
%       Hessian (or approximate Hessian) evaluation.
%	time (double)
%       The total elapsed time in seconds to reach the corresponding cost.
%	rho (double)
%       The performance ratio for the iterate.
%	rhonum, rhoden (double)
%       Regularized numerator and denominator of the performance ratio:
%       rho = rhonum/rhoden. See options.rho_regularization.
%	accepted (boolean)
%       Whether the proposed iterate was accepted or not.
%	stepsize (double)
%       The (Riemannian) norm of the vector returned by the inner solver
%       tCG and which is retracted to obtain the proposed next iterate. If
%       accepted = true for the corresponding iterate, this is the size of
%       the step from the previous to the new iterate. If accepted is
%       false, the step was not executed and this is the size of the
%       rejected step.
%	Delta (double)
%       The trust-region radius at the outer iteration.
%	cauchy (boolean)
%       Whether the Cauchy point was used or not (if useRand is true).
%   And possibly additional information logged by options.statsfun.
% For example, type [info.gradnorm] to obtain a vector of the successive
% gradient norms reached at each (outer) iteration.
%
% The options structure is used to overwrite the default values. All
% options have a default value and are hence optional. To force an option
% value, pass an options structure with a field options.optionname, where
% optionname is one of the following and the default value is indicated
% between parentheses:
%
%   tolgradnorm (1e-6)
%       The algorithm terminates if the norm of the gradient drops below
%       this. For well-scaled problems, a rule of thumb is that you can
%       expect to reduce the gradient norm by 8 orders of magnitude
%       (sqrt(eps)) compared to the gradient norm at a "typical" point (a
%       rough initial iterate for example). Further decrease is sometimes
%       possible, but inexact floating point arithmetic will eventually
%       limit the final accuracy. If tolgradnorm is set too low, the
%       algorithm may end up iterating forever (or at least until another
%       stopping criterion triggers).
%   maxiter (1000)
%       The algorithm terminates if maxiter (outer) iterations were executed.
%   maxtime (Inf)
%       The algorithm terminates if maxtime seconds elapsed.
%	miniter (3)
%       Minimum number of outer iterations (used only if useRand is true).
%	mininner (1)
%       Minimum number of inner iterations (for tCG).
%	maxinner (problem.M.dim() : the manifold's dimension)
%       Maximum number of inner iterations (for tCG).
%	Delta_bar (problem.M.typicaldist() or sqrt(problem.M.dim()))
%       Maximum trust-region radius. If you specify this parameter but not
%       Delta0, then Delta0 will be set to 1/8 times this parameter.
%   Delta0 (Delta_bar/8)
%       Initial trust-region radius. If you observe a long plateau at the
%       beginning of the convergence plot (gradient norm VS iteration), it
%       may pay off to try to tune this parameter to shorten the plateau.
%       You should not set this parameter without setting Delta_bar.
%	useRand (false)
%       Set to true if the trust-region solve is to be initiated with a
%       random tangent vector. If set to true, no preconditioner will be
%       used. This option is set to true in some scenarios to escape saddle
%       points, but is otherwise seldom activated.
%	kappa (0.1)
%       Inner kappa convergence tolerance.
%	theta (1.0)
%       Inner theta convergence tolerance.
%	rho_prime (0.1)
%       Accept/reject ratio : if rho is at least rho_prime, the outer
%       iteration is accepted. Otherwise, it is rejected. In case it is
%       rejected, the trust-region radius will have been decreased.
%       To ensure this, rho_prime must be strictly smaller than 1/4.
%   rho_regularization (1e3)
%       Close to convergence, evaluating the performance ratio rho is
%       numerically challenging. Meanwhile, close to convergence, the
%       quadratic model should be a good fit and the steps should be
%       accepted. Regularization lets rho go to 1 as the model decrease and
%       the actual decrease go to zero. Set this option to zero to disable
%       regularization (not recommended). See in-code for the specifics.
%   statsfun (none)
%       Function handle to a function that will be called after each
%       iteration to provide the opportunity to log additional statistics.
%       They will be returned in the info struct. See the generic Manopt
%       documentation about solvers for further information. statsfun is
%       called with the point x that was reached last, after the
%       accept/reject decision. See comment below.
%   stopfun (none)
%       Function handle to a function that will be called at each iteration
%       to provide the opportunity to specify additional stopping criteria.
%       See the generic Manopt documentation about solvers for further
%       information.
%   verbosity (2)
%       Integer number used to tune the amount of output the algorithm
%       generates during execution (mostly as text in the command window).
%       The higher, the more output. 0 means silent. 3 and above includes a
%       display of the options structure at the beginning of the execution.
%   debug (false)
%       Set to true to allow the algorithm to perform additional
%       computations for debugging purposes. If a debugging test fails, you
%       will be informed of it, usually via the command window. Be aware
%       that these additional computations appear in the algorithm timings
%       too.
%   storedepth (20)
%       Maximum number of different points x of the manifold for which a
%       store structure will be kept in memory in the storedb. If the
%       caching features of Manopt are not used, this is irrelevant. If
%       memory usage is an issue, you may try to lower this number.
%       Profiling may then help to investigate if a performance hit was
%       incured as a result.
%
% Notice that statsfun is called with the point x that was reached last,
% after the accept/reject decision. Hence: if the step was accepted, we get
% that new x, with a store which only saw the call for the cost and for the
% gradient. If the step was rejected, we get the same x as previously, with
% the store structure containing everything that was computed at that point
% (possibly including previous rejects at that same point). Hence, statsfun
% should not be used in conjunction with the store to count operations for
% example. Instead, you could use a global variable and increment that
% variable directly from the cost related functions. It is however possible
% to use statsfun with the store to compute, for example, alternate merit
% functions on the point x.
%
% See also: steepestdescent conjugategradient manopt/examples

% This file is part of Manopt: www.manopt.org.
% This code is an adaptation to Manopt of the original GenRTR code:
% RTR - Riemannian Trust-Region
% (c) 2004-2007, P.-A. Absil, C. G. Baker, K. A. Gallivan
% Florida State University
% School of Computational Science
% (http://www.math.fsu.edu/~cbaker/GenRTR/?page=download)
% See accompanying license file.
% The adaptation was executed by Nicolas Boumal.
%
% Change log:
%
%   NB April 3, 2013:
%       tCG now returns the Hessian along the returned direction eta, so
%       that we do not compute that Hessian redundantly: some savings at
%       each iteration. Similarly, if the useRand flag is on, we spare an
%       extra Hessian computation at each outer iteration too, owing to
%       some modifications in the Cauchy point section of the code specific
%       to useRand = true.
%
%   NB Aug. 22, 2013:
%       This function is now Octave compatible. The transition called for
%       two changes which would otherwise not be advisable. (1) tic/toc is
%       now used as is, as opposed to the safer way:
%       t = tic(); elapsed = toc(t);
%       And (2), the (formerly inner) function savestats was moved outside
%       the main function to not be nested anymore. This is arguably less
%       elegant, but Octave does not (and likely will not) support nested
%       functions.
%
%   NB Dec. 2, 2013:
%       The in-code documentation was largely revised and expanded.
%
%   NB Dec. 2, 2013:
%       The former heuristic which triggered when rhonum was very small and
%       forced rho = 1 has been replaced by a smoother heuristic which
%       consists in regularizing rhonum and rhoden before computing their
%       ratio. It is tunable via options.rho_regularization. Furthermore,
%       the solver now detects if tCG did not obtain a model decrease
%       (which is theoretically impossible but may happen because of
%       numerical errors and/or because of a nonlinear/nonsymmetric Hessian
%       operator, which is the case for finite difference approximations).
%       When such an anomaly is detected, the step is rejected and the
%       trust region radius is decreased.
%
%   NB Dec. 3, 2013:
%       The stepsize is now registered at each iteration, at a small
%       additional cost. The defaults for Delta_bar and Delta0 are better
%       defined. Setting Delta_bar in the options will automatically set
%       Delta0 accordingly. In Manopt 1.0.4, the defaults for these options
%       were not treated appropriately because of an incorrect use of the
%       isfield() built-in function.

% Verify that the problem description is sufficient for the solver.
if ~canGetCost(problem)
    warning('manopt:getCost', ...
            'No cost provided. The algorithm will likely abort.');
end
if ~canGetGradient(problem)
    warning('manopt:getGradient', ...
            'No gradient provided. The algorithm will likely abort.');
end
if ~canGetHessian(problem)
    warning('manopt:getHessian:approx', ...
            'No Hessian provided. Using an approximation instead.');
end

% Define some strings for display
tcg_stop_reason = {'negative curvature',...
                   'exceeded trust region',...
                   'reached target residual-kappa',...
                   'reached target residual-theta',...
                   'dimension exceeded',...
                   'model increased'};

% Set local defaults here
localdefaults.verbosity = 2;
localdefaults.maxtime = inf;
localdefaults.miniter = 3;
localdefaults.maxiter = 1000;
localdefaults.mininner = 1;
localdefaults.maxinner = problem.M.dim();
localdefaults.tolgradnorm = 1e-6;
localdefaults.kappa = 0.1;
localdefaults.theta = 1.0;
localdefaults.rho_prime = 0.1;
localdefaults.useRand = false;
localdefaults.rho_regularization = 1e3;

% Merge global and local defaults, then merge w/ user options, if any.
localdefaults = mergeOptions(getGlobalDefaults(), localdefaults);
if ~exist('options', 'var') || isempty(options)
    options = struct();
end
options = mergeOptions(localdefaults, options);

% Set default Delta_bar and Delta0 separately to deal with additional
% logic: if Delta_bar is provided but not Delta0, let Delta0 automatically
% be some fraction of the provided Delta_bar.
if ~isfield(options, 'Delta_bar')
    if isfield(problem.M, 'typicaldist')
        options.Delta_bar = problem.M.typicaldist();
    else
        options.Delta_bar = sqrt(problem.M.dim());
    end
end
if ~isfield(options,'Delta0')
    options.Delta0 = options.Delta_bar / 8;
end

% Check some option values
assert(options.rho_prime < 1/4, ...
        'options.rho_prime must be strictly smaller than 1/4.');
assert(options.Delta_bar > 0, ...
        'options.Delta_bar must be positive.');
assert(options.Delta0 > 0 && options.Delta0 < options.Delta_bar, ...
        'options.Delta0 must be positive and smaller than Delta_bar.');

% It is sometimes useful to check what the actual option values are.
if options.verbosity >= 3
    disp(options);
end

% Create a store database
storedb = struct();

tic();

% If no initial point x is given by the user, generate one at random.
if ~exist('x', 'var') || isempty(x)
    x = problem.M.rand();
end

%% Initializations

% k counts the outer (TR) iterations. The semantic is that k counts the
% number of iterations fully executed so far.
k = 0;

% initialize solution and companion measures: f(x), fgrad(x)
[fx fgradx storedb] = getCostGrad(problem, x, storedb);
norm_grad = problem.M.norm(x, fgradx);

% initialize trust-region radius
Delta = options.Delta0;

% Save stats in a struct array info, and preallocate
% (see http://people.csail.mit.edu/jskelly/blog/?x=entry:entry091030-033941)
if ~exist('used_cauchy', 'var')
    used_cauchy = [];
end
stats = savestats(problem, x, storedb, options, k, fx, norm_grad, Delta);
info(1) = stats;
info(min(10000, options.maxiter+1)).iter = [];

% ** Display:
if options.verbosity == 2
   fprintf(['%3s %3s      %5s                %5s     ',...
            'f: %e   |grad|: %e\n'],...
           '   ','   ','     ','     ', fx, norm_grad);
elseif options.verbosity > 2
   fprintf('************************************************************************\n');
   fprintf('%3s %3s    k: %5s     num_inner: %5s     %s\n',...
           '','','______','______','');
   fprintf('       f(x) : %e       |grad| : %e\n', fx, norm_grad);
   fprintf('      Delta : %f\n', Delta);
end

% **********************
% ** Start of TR loop **
% **********************
while true

	% Start clock for this outer iteration
    tic();

    % Run standard stopping criterion checks
    [stop reason] = stoppingcriterion(problem, x, options, info, k+1);

    % If the stopping criterion that triggered is the tolerance on the
    % gradient norm but we are using randomization, make sure we make at
    % least miniter iterations to give randomization a chance at escaping
    % saddle points.
    if stop == 2 && options.useRand && k < options.miniter
        stop = 0;
    end

    if stop
        if options.verbosity >= 1
            fprintf([reason '\n']);
        end
        break;
    end

    if options.verbosity > 2 || options.debug > 0
        fprintf('************************************************************************\n');
    end

    % *************************
    % ** Begin TR Subproblem **
    % *************************

    % Determine eta0
    if ~options.useRand
        % Pick the zero vector
        eta = problem.M.zerovec(x);
    else
        % Random vector in T_x M (this has to be very small)
        eta = problem.M.lincomb(x, 1e-6, problem.M.randvec(x));
        % Must be inside trust-region
        while problem.M.norm(x, eta) > Delta
            eta = problem.M.lincomb(x, sqrt(sqrt(eps)), eta);
        end
    end

    % solve TR subproblem
    [eta Heta numit stop_inner storedb] = ...
                     tCG(problem, x, fgradx, eta, Delta, options, storedb);
    srstr = tcg_stop_reason{stop_inner};

    % This is only computed for logging purposes, because it may be useful
    % for some user-defined stopping criteria. If this is not cheap for
    % specific application (compared to evaluating the cost), we should
    % reconsider this.
    norm_eta = problem.M.norm(x, eta);

    if options.debug > 0
        testangle = problem.M.inner(x, eta, fgradx) / (norm_eta*norm_grad);
    end

    % If using randomized approach, compare result with the Cauchy point.
    % Convergence proofs assume that we achieve at least the reduction of
    % the Cauchy point. After this if-block, either all eta-related
    % quantities have been changed consistently, or none of them have
    % changed.
    if options.useRand
        used_cauchy = false;
        % Check the curvature,
        [Hg storedb] = getHessian(problem, x, fgradx, storedb);
        g_Hg = problem.M.inner(x, fgradx, Hg);
        if g_Hg <= 0
            tau_c = 1;
        else
            tau_c = min( norm_grad^3/(Delta*g_Hg) , 1);
        end
        % and generate the Cauchy point.
        eta_c  = problem.M.lincomb(x, -tau_c * Delta / norm_grad, fgradx);
        Heta_c = problem.M.lincomb(x, -tau_c * Delta / norm_grad, Hg);

        % Now that we have computed the Cauchy point in addition to the
        % returned eta, we might as well keep the best of them.
        mdle  = fx + problem.M.inner(x, fgradx, eta) ...
                   + .5*problem.M.inner(x, Heta,   eta);
        mdlec = fx + problem.M.inner(x, fgradx, eta_c) ...
                   + .5*problem.M.inner(x, Heta_c, eta_c);
        if mdle > mdlec
            eta = eta_c;
            Heta = Heta_c; % added April 11, 2012
            used_cauchy = true;
        end
    end

	% Compute the retraction of the proposal
	x_prop  = problem.M.retr(x, eta);

	% Compute the function value of the proposal
	[fx_prop storedb] = getCost(problem, x_prop, storedb);

	% Will we accept the proposed solution or not?
    % Check the performance of the quadratic model against the actual cost.
    rhonum = fx - fx_prop;
    rhoden = -problem.M.inner(x, fgradx, eta) ...
             -.5*problem.M.inner(x, eta, Heta);

    % Heuristic -- added Dec. 2, 2013 (NB) to replace the former heuristic.
    % This heuristic is documented in the book by Conn Gould and Toint on
    % trust-region methods, section 17.4.2.
    % rhonum measures the difference between two numbers. Close to
    % convergence, these two numbers are very close to each other, so
    % that computing their difference is numerically challenging: there may
    % be a significant loss in accuracy. Since the acceptance or rejection
    % of the step is conditioned on the ratio between rhonum and rhoden,
    % large errors in rhonum result in a large error in rho, hence in
    % erratic acceptance / rejection. Meanwhile, close to convergence,
    % steps are usually trustworthy and we should transition to a Newton-
    % like method, with rho=1 consistently. The heuristic thus shifts both
    % rhonum and rhoden by a small amount such that far from convergence,
    % the shift is irrelevant and close to convergence, the ratio rho goes
    % to 1, effectively promoting acceptance of the step.
    % The rationale is that close to convergence, both rhonum and rhoden
    % are quadratic in the distance between x and x_prop. Thus, when this
    % distance is on the order of sqrt(eps), the value of rhonum and rhoden
    % is on the order of eps, which is indistinguishable from the numerical
    % error, resulting in badly estimated rho's.
    % For abs(fx) < 1, this heuristic is invariant under offsets of f but
    % not under scaling of f. For abs(fx) > 1, the opposite holds. This
    % should not alarm us, as this heuristic only triggers at the very last
    % iterations if very fine convergence is demanded.
    rho_reg = max(1, abs(fx)) * eps * options.rho_regularization;
    rhonum = rhonum + rho_reg;
    rhoden = rhoden + rho_reg;

    if options.debug > 0
        fprintf('DBG:     rhonum : %e\n', rhonum);
        fprintf('DBG:     rhoden : %e\n', rhoden);
    end

    % This is always true if a linear, symmetric operator is used for the
    % Hessian (approximation) and if we had infinite numerical precision.
    % In practice, nonlinear approximations of the Hessian such as the
    % built-in finite difference approximation and finite numerical
    % accuracy can cause the model to increase. In such scenarios, we
    % decide to force a rejection of the step and a reduction of the
    % trust-region radius. We test the sign of the regularized rhoden since
    % the regularization is supposed to capture the accuracy to which
    % rhoden is computed: if rhoden were negative before regularization but
    % not after, that should not be (and is not) detected as a failure.
    model_decreased = (rhoden >= 0);

    if ~model_decreased
        srstr = [srstr ', model did not decrease']; %#ok<AGROW>
    end

    rho = rhonum / rhoden;

    if options.debug > 0
        m = @(x, eta) ...
          getCost(problem, x, storedb) + ...
          getDirectionalDerivative(problem, x, eta, storedb) + ...
          .5*problem.M.inner(x, getHessian(problem, x, eta, storedb), eta);
        zerovec = problem.M.zerovec(x);
        actrho = (fx - fx_prop) / (m(x, zerovec) - m(x, eta));
        fprintf('DBG:   new f(x) : %e\n', fx_prop);
        fprintf('DBG: actual rho : %e\n', actrho);
        fprintf('DBG:   used rho : %e\n', rho);
    end

    % Choose the new TR radius based on the model performance
    trstr = '   ';
    % If the actual decrease is smaller than 1/4 of the predicted decrease,
    % then reduce the TR radius.
    if rho < 1/4 || ~model_decreased
        trstr = 'TR-';
        Delta = Delta/4;
    % If the actual decrease is at least 3/4 of the precicted decrease and
    % the tCG (inner solve) hit the TR boundary, increase the TR radius.
    elseif rho > 3/4 && (stop_inner == 1 || stop_inner == 2)
        trstr = 'TR+';
        Delta = min(2*Delta, options.Delta_bar);
    end
    % Otherwise, keep the TR radius constant.

    % Choose to accept or reject the proposed step based on the model
    % performance.
    if model_decreased && rho > options.rho_prime
        accept = true;
        accstr = 'acc';
        x = x_prop;
        fx = fx_prop;
        [fgradx storedb] = getGradient(problem, x, storedb);
        norm_grad = problem.M.norm(x, fgradx);
    else
        accept = false;
        accstr = 'REJ';
    end

    % Make sure we don't use too much memory for the store database
    storedb = purgeStoredb(storedb, options.storedepth);

    % k is the number of iterations we have accomplished.
    k = k + 1;

    % Log statistics for freshly executed iteration.
    % Everything after this in the loop is not accounted for in the timing.
    stats = savestats(problem, x, storedb, options, k, fx, norm_grad, ...
                      Delta, info, rho, rhonum, rhoden, accept, numit, ...
                      norm_eta, used_cauchy);
    info(k+1) = stats; %#ok<AGROW>

    % ** Display:
    if options.verbosity == 2,
        fprintf(['%3s %3s   k: %5d     num_inner: %5d     ', ...
        'f: %e   |grad|: %e   %s\n'], ...
        accstr,trstr,k,numit,fx,norm_grad,srstr);
    elseif options.verbosity > 2,
        if options.useRand && used_cauchy,
            fprintf('USED CAUCHY POINT\n');
        end
		fprintf('%3s %3s    k: %5d     num_inner: %5d     %s\n', ...
				accstr, trstr, k, numit, srstr);
		fprintf('       f(x) : %e     |grad| : %e\n',fx,norm_grad);
		if options.debug > 0
			fprintf('      Delta : %f          |eta| : %e\n',Delta,norm_eta);
		end
		fprintf('        rho : %e\n',rho);
    end
    if options.debug > 0,
        fprintf('DBG: cos ang(eta,gradf): %d\n',testangle);
        if rho == 0
            fprintf('DBG: rho = 0, this will likely hinder further convergence.\n');
        end
    end

end  % of TR loop (counter: k)

% Restrict info struct-array to useful part
info = info(1:k+1);

if (options.verbosity > 2) || (options.debug > 0),
   fprintf('************************************************************************\n');
end
if (options.verbosity > 0) || (options.debug > 0)
    fprintf('Total time is %f [s] (excludes statsfun)\n', info(end).time);
end

% Return the best cost reached
cost = fx;

end

% Routine in charge of collecting the current iteration stats
function stats = savestats(problem, x, storedb, options, k, fx, ...
                           norm_grad, Delta, info, rho, rhonum, ...
                           rhoden, accept, numit, norm_eta, used_cauchy)
    stats.iter = k;
    stats.cost = fx;
    stats.gradnorm = norm_grad;
    stats.Delta = Delta;
    if k == 0
        stats.time = toc();
        stats.rho = inf;
        stats.rhonum = NaN;
        stats.rhoden = NaN;
        stats.accepted = true;
        stats.numinner = NaN;
        stats.stepsize = NaN;
        if options.useRand
            stats.cauchy = false;
        end
    else
        stats.time = info(k).time + toc();
        stats.rho = rho;
        stats.rhonum = rhonum;
        stats.rhoden = rhoden;
        stats.accepted = accept;
        stats.numinner = numit;
        stats.stepsize = norm_eta;
        if options.useRand,
          stats.cauchy = used_cauchy;
        end
    end

    % See comment about statsfun above: the x and store passed to statsfun
    % are that of the most recently accepted point after the iteration
    % fully executed.
    stats = applyStatsfun(problem, x, storedb, options, stats);

end


function [ cluass,cluNRG ] = bz_GradDescCluster( simMat, varargin )
%[cluass] = bz_GradDescCluster(simMat) clusters recording sites given a pairwise
%similarity matrix using gradient descent to find minimize within-cluster
%interaction energy. (i.e. maximize within-cluster coherence, see Berenyi
%et al 2014 for details).
%
%INPUT
%   simMat          undirected similarity matrix (coherence, correlation, or other)
%
%   (optional parameters)
%   'numsteps'      number of steps (default: 5e5)
%   'numinit'       number of initial clusters, should be larger than the
%                   number of clusters you expect (default: 20)
%   'stopthresh'    stability threshold to stop descending. probability of
%                   selected site changing cluster identity, P(switch)
%                   (default: 0.001)
%   'stopwin'       window of time for which the P(switch) must stay below
%                   stopthresh (default: 10000 steps)
%   'showplot'      true or false, to show the update plot as it goes
%   'brainmap'      cell array with two cells: {1} = linear indices, {2} = size map; 2 dim vector x and y dimensions of map

%OUTPUT
%   cluass      cluster assignments for each
%   cluNRG      negative energy (i.e. mean within-cluster coherence) for each cluster
%
%From Berenyi et al. (2014). Large-scale, high-density (up to 512
%   channels) recording of local circuits in behaving animals. Journal of
%   Neurophysiology, 111(5), 1132?1149. http://doi.org/10.1152/jn.00785.2013
%
%Code implementation by DLevenstein 2016. Updates DL and RS May 2017.
%
% TO DO
% - make brainmap general to any 2D ephys/imaging data
% - develop method for stopping num iterations
% - add consensus clustering option?

%% Parse the input parameters
parms = inputParser;
addParameter(parms,'numsteps',500000,@isnumeric);
addParameter(parms,'numinit',20,@isnumeric);
addParameter(parms,'showplot',true,@islogical);
addParameter(parms,'brainmap',{},@iscell);
addParameter(parms,'stopthresh',0.001,@(x) x>0 && x<=1)
addParameter(parms,'stopwin',10000,@isnumeric)

parse(parms,varargin{:})
numsteps = parms.Results.numsteps;
numinit = parms.Results.numinit;
SHOWPLOT = parms.Results.showplot;
LinearInds = parms.Results.brainmap{1};
SizeVid = parms.Results.brainmap{2};
stopthresh = parms.Results.stopthresh;
stopwin = parms.Results.stopwin;

%% Initialize and Run
numsites = size(simMat,1);
rng('shuffle') %Shuffle the random number generator seed... just in case
cluass = randi(numinit,numsites,1); %Start from random initial assignments

if SHOWPLOT; figure; switchtracker = zeros(numsteps,1); end
for ss = 1:numsteps
    %The time ticker...
    if mod(ss,1000)==1
        display(['Step: ',num2str(ss),' of ',num2str(numsteps)])
        [~,clusort] = sort(cluass);

        % Visuals
        if SHOWPLOT
            % plot changing clusters over time
            subplot(2,2,1)
                imagesc(simMat(clusort,clusort))
            %plot switching behavior
            subplot(2,2,2)
                plot(log10(smooth(switchtracker(1:ss),1000,'moving')))
                hold on
                plot(get(gca,'xlim'),log10(stopthresh).*[1 1],'r--')
                xlabel('Step #');ylabel('P(switch)')
                ylim([log10(stopthresh)-1 0])
                LogScale('y',10)
                hold off
            % plot map
            if ~isempty(LinearInds)
                map = ExpandVid(cluass, LinearInds, SizeVid);
                subplot(2,2,3)
                imagesc(map)
            end;
            %plot number of clusters
            subplot(2,2,4)
                plot(ss,length(unique(cluass)),'ko') %number clusters
                hold on
                xlabel('Step #');ylabel('# Clusters')
                drawnow
        end;
    end;

    % Fx meat
    [cluass,switchtracker(ss)] = DescOneStep(cluass,simMat);

    %Decide to exit the loop - if you've been consistent for a certain window
    if mean(switchtracker(max(ss-stopwin,1):ss)) <= stopthresh
        disp('Clustering has descended to the bottom of the (local) pit!')
        break
    end
end

%Calculate -Energy (mean similarity) for each final cluster
finalclus = unique(cluass);
numfinalclus = length(finalclus);
for ff = 1:numfinalclus
    %N = sum(cluass == finalclus(ff));
    clusmat = simMat(cluass == finalclus(ff),cluass == finalclus(ff));
    cluspairs = triu(clusmat,1);
    E(ff) = mean(cluspairs(:));
end
cluNRG = [finalclus,E'];

%% FUNCTION: OneStep
    function [newcluass,diditswitch] = DescOneStep(oldcluass,simMat)
        clus = unique(oldcluass);
        numclus = length(clus);

        %Pick a random site and calculate the energy for it's current cluster
        sitepick = randi(length(oldcluass),1);
        clu_A = oldcluass(sitepick); %Cluster the current site is in
        othersites = setdiff(1:length(oldcluass),sitepick);
        N_A = sum(oldcluass == clu_A)-1; %ALl OTHER elements of current cluster
        E_A = -(1./N_A).*sum(simMat(sitepick,oldcluass(othersites)==clu_A));

        %If only one site in cluster, energy is 0
        if N_A == 0
            E_A = 0;
        end

        %Calculate energy gap for switching site to other clusters
        energygap = zeros(size(clus));
        for cc = 1:numclus
            clu_B = clus(cc);
            if clu_B==clu_A
                continue
            end
            N_B = sum(oldcluass == clu_B);
            E_B = -(1./N_B).*(sum(simMat(sitepick,oldcluass==clu_B)));
            energygap(cc) = E_A - E_B;
        end

        %Switch site to cluster with largest energy gap
        newcluass = oldcluass;
        newcluass(sitepick) = clus(energygap==max(energygap));

        %Keep Track of switching - 1 if the channel stays in its cluster
        diditswitch = clus(energygap==max(energygap))~=clu_A;
    end
end

%% FUNCTION: ExpandVid

function map = ExpandVid(cluass, LinearInds, SizeVid)

    % Intialize
    map = single(nan(SizeVid(1)*SizeVid(2),1));

    % Put tmpR values int initialized 2D matrix
    map(LinearInds) = cluass;

    % Reshape into video
    map = reshape(map,SizeVid(1),SizeVid(2));

end


%IMATCH Template matching
%
% XM = IMATCH(IM1, IM2, U, V, H, S) is the position of the matching subimage of
% IM1 (template) within the image IM2.  The template in IM1 is centred at (U,V)
% and its half-width is H.
%
% The template is searched for within IM2 inside a rectangular region, centred
% at (U,V) and whose size is a function of S.  If S is a scalar the search
% region is [-S, S, -S, S] relative to (U,V).  More generally S is a 4-vector
% S=[umin, umax, vmin, vmax] relative to (U,V).
%
% The return value is XM=[DU,DV,CC] where (DU,DV) are the u- and v-offsets
% relative to (U,V) and CC is the similarity score for the best match in the
% search region.
%
% [XM,SCORE] = IMATCH(IM1, IM2, U, V, H, S) as above but also returns a matrix
% of matching score values for each template position tested. The rows
% correspond to horizontal positions of the template, and columns the vertical
% position.  The centre element corresponds to (U,V).
%
% Example::
%  Consider a sequence of images im(:,:,N) and we find corner points in the
%  k'th image
%        corners = icorner(im(:,:,k), 'nfeat', 20);
%  Now, for each corner we look for the 11x11 patch of surrounding pixels
%  in the next image, by searching within a 21x21 region
%        for corner=corners
%           xm = imatch(im(:,:,k), im(:,:,k+1), 5, 10);
%           if xm(3) > 0.8
%               fprintf('feature (%f,%f) moved by (%f,%f) pixels)\n', ...
%                   corner.u, corner.v, xm(1), xm(2) );
%           end
%        end
%
% Notes::
% - Useful for tracking a template in an image sequence where IM1 and IM2
%   are consecutive images in a template and (U,V) is the coordinate of
%   a corner point in IM1.
% - Is a MEX file.
% - IM1 and IM2 must be the same size.
% - ZNCC (zero-mean normalized cross correlation) matching is used as the
%   similarity measure. A perfect match score is 1.0 but anything above 0.8
%   is typically considered to be a good match.
%
% See also ISIMILARITY.

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB 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.
%
% MVTB 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 MVTB.  If not, see <http://www.gnu.org/licenses/>.

if ~exist('imatch', 'file')
    error('you need to build the MEX version of imatch, see vision/mex/README');
end


function [ vX ] = EstimateSignalSamples( vX, vH, vY, convShape, paramLambda, hSumLogProb )
% ----------------------------------------------------------------------------------------------- %
% [ mK ] = CreateConvMtx1D( vK, numElements, convShape )
% Generates a Convolution Matrix for 1D Kernel (The Vector vK) with
% support for different convolution shapes (Full / Same / Valid). The
% matrix is build such that for a signal 'vS' with 'numElements = size(vS
% ,1)' the following are equiavlent: 'mK * vS' and conv(vS, vK,
% convShapeString);
% Input:
%   - vK                -   Input 1D Convolution Kernel.
%                           Structure: Vector.
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - numElements       -   Number of Elements.
%                           Number of elements of the vector to be
%                           convolved with the matrix. Basically set the
%                           number of columns of the Convolution Matrix.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {1, 2, 3, ...}.
%   - convShape         -   Convolution Shape.
%                           The shape of the convolution which the output
%                           convolution matrix should represent. The
%                           options should match MATLAB's conv2() function
%                           - Full / Same / Valid.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {1, 2, 3}.
% Output:
%   - mK                -   Convolution Matrix.
%                           The output convolution matrix. The product of
%                           'mK' and a vector 'vS' ('mK * vS') is the
%                           convolution between 'vK' and 'vS' with the
%                           corresponding convolution shape.
%                           Structure: Matrix (Sparse).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References:
%   1.  MATLAB's 'convmtx()' - https://www.mathworks.com/help/signal/ref/convmtx.html.
% Remarks:
%   1.  The output matrix is sparse data type in order to make the
%       multiplication by vectors to more efficient.
%   2.  In caes the same convolution is applied on many vectors, stacking
%       them into a matrix (Each signal as a vector) and applying
%       convolution on each column by matrix multiplication might be more
%       efficient than applying classic convolution per column.
% TODO:
%   1.
%   Release Notes:
%   -   1.0.000     20/01/2019  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

CONVOLUTION_SHAPE_FULL         = 1;
CONVOLUTION_SHAPE_SAME         = 2;
CONVOLUTION_SHAPE_VALID        = 3;

switch(convShape)
    case(CONVOLUTION_SHAPE_FULL)
        convShapeString = 'full';
    case(CONVOLUTION_SHAPE_SAME)
        convShapeString = 'same';
    case(CONVOLUTION_SHAPE_VALID)
        convShapeString = 'valid';
end

vXX = vX;

hObjFun = @(vX) 0.5 * sum((conv(vX, vH, convShapeString) - vY) .^ 2) - (paramLambda * hSumLogProb(vX));

vX = fminunc(hObjFun, vX);

end


function [dStates, contactForces] = dynamics_singleStanceTwo(States, Actuators, Parameters)
% function [dStates, Actuators] = dynamics_singleStanceTwo(States, Actuators, Parameters)
%
% Computer Generated File -- DO NOT EDIT
%
% This function was created by the function Write_ContinuousDynamics()
% 10-Dec-2013 19:40:47
%
% Dymanics Model: retractable double pendulum biped
% Motion Phase: Single Stance Two
%
% Matthew Kelly
% Cornell University
%

x = States(1,:); % (m) Foot One horizontal position
y = States(2,:); % (m) Foot One vertical position
th1 = States(3,:); % (rad) Leg One absolute angle
th2 = States(4,:); % (rad) Leg Two absolute angle
L1 = States(5,:); % (m) Leg One length
L2 = States(6,:); % (m) Leg Two length
dx = States(7,:); % (m/s) Foot One horizontal velocity
dy = States(8,:); % (m/s) Foot One vertical velocity
dth1 = States(9,:); % (rad/s) Leg One absolute angular rate
dth2 = States(10,:); % (rad/s) Leg Two absolute angular rate
dL1 = States(11,:); % (m/s) Leg One extension rate
dL2 = States(12,:); % (m/s) Leg Two extensioin rate

F1 = Actuators(1,:); % (N) Compresive axial force in Leg One
F2 = Actuators(2,:); % (N) Compresive axial force in Leg Two
T1 = 0; %Foot One not in contact with ground!
T2 = Actuators(4,:); % (Nm) External torque applied to Leg Two
Thip = Actuators(5,:); % (Nm) Torque acting on Leg Two from Leg One

m1 = Parameters.m1; % (kg) Foot One mass
m2 = Parameters.m2; % (kg) Foot Two mass
M = Parameters.M; % (kg) Hip mass
g = Parameters.g; % (m/s^2) Gravity

% Constraints for this phase:
H1 = 0; %(N) Foot One, horizontal contact force
V1 = 0; %(N) Foot One, vertical contact force

dStates = zeros(size(States));
dStates(1:6,:) = States((1+6):(6+6),:);
dStates(7,:) = (T1.*sin(th1) - Thip.*sin(th1) + H1.*L1 - F1.*L1.*cos(th1))./(L1.*m1);
dStates(8,:) = -(T1.*cos(th1) - L1.*V1 - Thip.*cos(th1) + L1.*g.*m1 + F1.*L1.*sin(th1))./(L1.*m1);
dStates(9,:) = -(L2.*Thip.*m1 - L2.*T1.*m1 + L1.*T2.*m1.*cos(th1 - th2) + L1.*Thip.*m1.*cos(th1 - th2) - L2.*M.*T1.*cos(th1).^2 + L2.*M.*Thip.*cos(th1).^2 - L2.*M.*T1.*sin(th1).^2 + L2.*M.*Thip.*sin(th1).^2 + L1.*L2.*M.*V1.*cos(th1) - H1.*L1.*L2.*M.*sin(th1) - F2.*L1.*L2.*m1.*sin(th1 - th2) + 2.*L1.*L2.*M.*dL1.*dth1.*m1)./(L1.^2.*L2.*M.*m1);
dStates(10,:) = (L2.*M.*T1.*sin(th1).*sin(th2) - L2.*T1.*m1.*cos(th1).*cos(th2) - L2.*M.*Thip.*sin(th1).*sin(th2) + L2.*Thip.*m1.*cos(th1).*cos(th2) - L2.*T1.*m1.*sin(th1).*sin(th2) + L2.*Thip.*m1.*sin(th1).*sin(th2) - L2.*M.*T1.*cos(th1).^3.*cos(th2) + L2.*M.*Thip.*cos(th1).^3.*cos(th2) - L1.*L2.*M.*V1.*cos(th2) + H1.*L1.*L2.*M.*sin(th2) - L2.*M.*T1.*sin(th1).^3.*sin(th2) + L2.*M.*Thip.*sin(th1).^3.*sin(th2) + L2.*M.*T1.*cos(th1).*cos(th2) - L2.*M.*Thip.*cos(th1).*cos(th2) + L1.*T2.*m1.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*Thip.*m1.*cos(th1 - th2).*cos(th1).*cos(th2) - L2.*M.*T1.*cos(th1).*cos(th2).*sin(th1).^2 + L2.*M.*Thip.*cos(th1).*cos(th2).*sin(th1).^2 + L1.*T2.*m1.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*T2.*m1.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*T2.*m1.*sin(th1 - th2).*cos(th2).*sin(th1) + L1.*Thip.*m1.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*Thip.*m1.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*Thip.*m1.*sin(th1 - th2).*cos(th2).*sin(th1) - F1.*L1.*L2.*M.*cos(th1).*sin(th2) + F1.*L1.*L2.*M.*cos(th2).*sin(th1) - L2.*M.*T1.*cos(th1).^2.*sin(th1).*sin(th2) + L2.*M.*Thip.*cos(th1).^2.*sin(th1).*sin(th2) + F1.*L1.*L2.*m1.*cos(th1).*sin(th2) - F1.*L1.*L2.*m1.*cos(th2).*sin(th1) - F1.*L1.*L2.*M.*cos(th2).*sin(th1).^3 + F1.*L1.*L2.*M.*cos(th1).^3.*sin(th2) + L1.*L2.*M.*V1.*cos(th1).^2.*cos(th2) - H1.*L1.*L2.*M.*cos(th1).^2.*sin(th2) + L1.*L2.*M.*V1.*cos(th2).*sin(th1).^2 - H1.*L1.*L2.*M.*sin(th1).^2.*sin(th2) + L1.*L2.*M.*g.*m1.*cos(th2) - F2.*L1.*L2.*m1.*cos(th1 - th2).*cos(th1).*sin(th2) + F2.*L1.*L2.*m1.*cos(th1 - th2).*cos(th2).*sin(th1) - F2.*L1.*L2.*m1.*sin(th1 - th2).*cos(th1).*cos(th2) - F1.*L1.*L2.*M.*cos(th1).^2.*cos(th2).*sin(th1) + F1.*L1.*L2.*M.*cos(th1).*sin(th1).^2.*sin(th2) - F2.*L1.*L2.*m1.*sin(th1 - th2).*sin(th1).*sin(th2) - 2.*L1.*L2.*M.*dL2.*dth2.*m1.*cos(th2).^2 - 2.*L1.*L2.*M.*dL2.*dth2.*m1.*sin(th2).^2)./(L1.*L2.^2.*M.*m1.*(cos(th2).^2 + sin(th2).^2));
dStates(11,:) = -(T2.*m1.*sin(th1 - th2) + Thip.*m1.*sin(th1 - th2) - F1.*L2.*m1 + H1.*L2.*M.*cos(th1) + L2.*M.*V1.*sin(th1) + F2.*L2.*m1.*cos(th1 - th2) - F1.*L2.*M.*cos(th1).^2 - F1.*L2.*M.*sin(th1).^2 - L1.*L2.*M.*dth1.^2.*m1)./(L2.*M.*m1);
dStates(12,:) = (L2.*M.*T1.*cos(th1).*sin(th2) - L2.*M.*T1.*cos(th2).*sin(th1) - L2.*M.*Thip.*cos(th1).*sin(th2) + L2.*M.*Thip.*cos(th2).*sin(th1) - L2.*T1.*m1.*cos(th1).*sin(th2) + L2.*T1.*m1.*cos(th2).*sin(th1) + L2.*Thip.*m1.*cos(th1).*sin(th2) - L2.*Thip.*m1.*cos(th2).*sin(th1) - H1.*L1.*L2.*M.*cos(th2) + L2.*M.*T1.*cos(th2).*sin(th1).^3 - L2.*M.*T1.*cos(th1).^3.*sin(th2) - L2.*M.*Thip.*cos(th2).*sin(th1).^3 + L2.*M.*Thip.*cos(th1).^3.*sin(th2) - L1.*L2.*M.*V1.*sin(th2) + L1.*L2.*M.*g.*m1.*sin(th2) + L1.*L2.^2.*M.*dth2.^2.*m1.*cos(th2).^2 + L1.*L2.^2.*M.*dth2.^2.*m1.*sin(th2).^2 + L1.*T2.*m1.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*T2.*m1.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*T2.*m1.*sin(th1 - th2).*cos(th1).*cos(th2) + L1.*Thip.*m1.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*Thip.*m1.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*Thip.*m1.*sin(th1 - th2).*cos(th1).*cos(th2) + F1.*L1.*L2.*M.*cos(th1).*cos(th2) + L2.*M.*T1.*cos(th1).^2.*cos(th2).*sin(th1) - L2.*M.*Thip.*cos(th1).^2.*cos(th2).*sin(th1) - L2.*M.*T1.*cos(th1).*sin(th1).^2.*sin(th2) + L2.*M.*Thip.*cos(th1).*sin(th1).^2.*sin(th2) + L1.*T2.*m1.*sin(th1 - th2).*sin(th1).*sin(th2) + L1.*Thip.*m1.*sin(th1 - th2).*sin(th1).*sin(th2) + F1.*L1.*L2.*M.*sin(th1).*sin(th2) - F1.*L1.*L2.*m1.*cos(th1).*cos(th2) - F1.*L1.*L2.*m1.*sin(th1).*sin(th2) - F1.*L1.*L2.*M.*cos(th1).^3.*cos(th2) + H1.*L1.*L2.*M.*cos(th1).^2.*cos(th2) + H1.*L1.*L2.*M.*cos(th2).*sin(th1).^2 - F1.*L1.*L2.*M.*sin(th1).^3.*sin(th2) + L1.*L2.*M.*V1.*cos(th1).^2.*sin(th2) + L1.*L2.*M.*V1.*sin(th1).^2.*sin(th2) + F2.*L1.*L2.*m1.*cos(th1 - th2).*cos(th1).*cos(th2) - F1.*L1.*L2.*M.*cos(th1).*cos(th2).*sin(th1).^2 + F2.*L1.*L2.*m1.*cos(th1 - th2).*sin(th1).*sin(th2) - F2.*L1.*L2.*m1.*sin(th1 - th2).*cos(th1).*sin(th2) + F2.*L1.*L2.*m1.*sin(th1 - th2).*cos(th2).*sin(th1) - F1.*L1.*L2.*M.*cos(th1).^2.*sin(th1).*sin(th2))./(L1.*L2.*M.*m1.*(cos(th2).^2 + sin(th2).^2));

% contactForces(1,:) == H1 == (N) Foot One, horizontal contact force
% contactForces(2,:) == V1 == (N) Foot One, vertical contact force
% contactForces(3,:) == H2 == (N) Foot Two, horizontal contact force
% contactForces(4,:) == V2 == (N) Foot Two, vertical contact force
contactForces = zeros(4,size(States,2));
contactForces(1,:) = zeros(1,size(States,2));
contactForces(2,:) = zeros(1,size(States,2));
contactForces(3,:) = (L2.*M.*T1.*m2.*sin(th1).^3 - L2.*M.*Thip.*m2.*sin(th1).^3 + L1.*M.*T2.*m1.*sin(th2) - L2.*M.*T1.*m2.*sin(th1) + L1.*M.*Thip.*m1.*sin(th2) + L2.*M.*Thip.*m2.*sin(th1) + L2.*T1.*m1.*m2.*sin(th1) - L2.*Thip.*m1.*m2.*sin(th1) - H1.*L1.*L2.*M.*m2 + L1.*T2.*m1.*m2.*cos(th1 - th2).^2.*sin(th2) + L1.*Thip.*m1.*m2.*cos(th1 - th2).^2.*sin(th2) - F1.*L1.*L2.*M.*m2.*cos(th1).^3 + H1.*L1.*L2.*M.*m2.*cos(th1).^2 + H1.*L1.*L2.*M.*m2.*cos(th2).^2 + L1.*T2.*m1.*m2.*sin(th1 - th2).^2.*sin(th2) + L1.*Thip.*m1.*m2.*sin(th1 - th2).^2.*sin(th2) + H1.*L1.*L2.*M.*m2.*sin(th1).^2 + H1.*L1.*L2.*M.*m2.*sin(th2).^2 - L1.*T2.*m1.*m2.*cos(th1 - th2).*sin(th1) + L1.*T2.*m1.*m2.*sin(th1 - th2).*cos(th1) - L2.*T1.*m1.*m2.*cos(th1 - th2).*sin(th2) - L2.*T1.*m1.*m2.*sin(th1 - th2).*cos(th2) - L1.*Thip.*m1.*m2.*cos(th1 - th2).*sin(th1) + L1.*Thip.*m1.*m2.*sin(th1 - th2).*cos(th1) + L2.*Thip.*m1.*m2.*cos(th1 - th2).*sin(th2) + L2.*Thip.*m1.*m2.*sin(th1 - th2).*cos(th2) + F1.*L1.*L2.*M.*m2.*cos(th1) - F2.*L1.*L2.*M.*m1.*cos(th2) + L2.*M.*T1.*m2.*cos(th1).^2.*sin(th1) + L2.*M.*T1.*m2.*cos(th2).^2.*sin(th1) - L2.*M.*Thip.*m2.*cos(th1).^2.*sin(th1) - L2.*M.*Thip.*m2.*cos(th2).^2.*sin(th1) + L2.*M.*T1.*m2.*sin(th1).*sin(th2).^2 - L2.*M.*Thip.*m2.*sin(th1).*sin(th2).^2 - F1.*L1.*L2.*m1.*m2.*cos(th1) - F1.*L1.*L2.*M.*m2.*cos(th1).*cos(th2).^2 - F1.*L1.*L2.*M.*m2.*cos(th1).*sin(th1).^2 - F1.*L1.*L2.*M.*m2.*cos(th1).*sin(th2).^2 - F1.*L1.*L2.*m1.*m2.*sin(th1 - th2).*sin(th2) + F2.*L1.*L2.*m1.*m2.*sin(th1 - th2).*sin(th1) - F2.*L1.*L2.*m1.*m2.*cos(th1 - th2).^2.*cos(th2) - F2.*L1.*L2.*m1.*m2.*sin(th1 - th2).^2.*cos(th2) - L2.*M.*T1.*m2.*cos(th1 - th2).*cos(th1).^2.*sin(th2) - L2.*M.*T1.*m2.*sin(th1 - th2).*cos(th1).^2.*cos(th2) + L2.*M.*Thip.*m2.*cos(th1 - th2).*cos(th1).^2.*sin(th2) + L2.*M.*Thip.*m2.*sin(th1 - th2).*cos(th1).^2.*cos(th2) - L2.*M.*T1.*m2.*cos(th1 - th2).*sin(th1).^2.*sin(th2) - L2.*M.*T1.*m2.*sin(th1 - th2).*cos(th2).*sin(th1).^2 + L2.*M.*Thip.*m2.*cos(th1 - th2).*sin(th1).^2.*sin(th2) + L2.*M.*Thip.*m2.*sin(th1 - th2).*cos(th2).*sin(th1).^2 + F1.*L1.*L2.*m1.*m2.*cos(th1 - th2).*cos(th2) + F2.*L1.*L2.*m1.*m2.*cos(th1 - th2).*cos(th1) + F1.*L1.*L2.*M.*m2.*cos(th1 - th2).*cos(th1).^2.*cos(th2) + F1.*L1.*L2.*M.*m2.*cos(th1 - th2).*cos(th2).*sin(th1).^2 - F1.*L1.*L2.*M.*m2.*sin(th1 - th2).*cos(th1).^2.*sin(th2) - F1.*L1.*L2.*M.*m2.*sin(th1 - th2).*sin(th1).^2.*sin(th2) - H1.*L1.*L2.*M.*m2.*cos(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*V1.*m2.*cos(th1 - th2).*cos(th1).*sin(th2) - L1.*L2.*M.*V1.*m2.*cos(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*V1.*m2.*sin(th1 - th2).*cos(th1).*cos(th2) - H1.*L1.*L2.*M.*m2.*cos(th1 - th2).*sin(th1).*sin(th2) + H1.*L1.*L2.*M.*m2.*sin(th1 - th2).*cos(th1).*sin(th2) - H1.*L1.*L2.*M.*m2.*sin(th1 - th2).*cos(th2).*sin(th1) + L1.*L2.*M.*V1.*m2.*sin(th1 - th2).*sin(th1).*sin(th2))./(L1.*L2.*M.*m1.*(cos(th2).^2 + sin(th2).^2));
contactForces(4,:) = -(L1.*L2.*M.*V1.*m2 + L1.*M.*T2.*m1.*cos(th2) - L2.*M.*T1.*m2.*cos(th1) + L1.*M.*Thip.*m1.*cos(th2) + L2.*M.*Thip.*m2.*cos(th1) + L2.*T1.*m1.*m2.*cos(th1) - L2.*Thip.*m1.*m2.*cos(th1) + L2.*M.*T1.*m2.*cos(th1).^3 - L2.*M.*Thip.*m2.*cos(th1).^3 + L1.*T2.*m1.*m2.*cos(th1 - th2).^2.*cos(th2) + L1.*Thip.*m1.*m2.*cos(th1 - th2).^2.*cos(th2) + L1.*T2.*m1.*m2.*sin(th1 - th2).^2.*cos(th2) + L1.*Thip.*m1.*m2.*sin(th1 - th2).^2.*cos(th2) + F1.*L1.*L2.*M.*m2.*sin(th1).^3 - L1.*L2.*M.*V1.*m2.*cos(th1).^2 - L1.*L2.*M.*V1.*m2.*cos(th2).^2 - L1.*L2.*M.*V1.*m2.*sin(th1).^2 - L1.*L2.*M.*V1.*m2.*sin(th2).^2 - L1.*L2.*M.*g.*m1.*m2 - L1.*T2.*m1.*m2.*cos(th1 - th2).*cos(th1) - L2.*T1.*m1.*m2.*cos(th1 - th2).*cos(th2) - L1.*Thip.*m1.*m2.*cos(th1 - th2).*cos(th1) + L2.*Thip.*m1.*m2.*cos(th1 - th2).*cos(th2) + L2.*M.*T1.*m2.*cos(th1).*cos(th2).^2 - L2.*M.*Thip.*m2.*cos(th1).*cos(th2).^2 + L2.*M.*T1.*m2.*cos(th1).*sin(th1).^2 + L2.*M.*T1.*m2.*cos(th1).*sin(th2).^2 - L2.*M.*Thip.*m2.*cos(th1).*sin(th1).^2 - L2.*M.*Thip.*m2.*cos(th1).*sin(th2).^2 - L1.*T2.*m1.*m2.*sin(th1 - th2).*sin(th1) + L2.*T1.*m1.*m2.*sin(th1 - th2).*sin(th2) - L1.*Thip.*m1.*m2.*sin(th1 - th2).*sin(th1) - L2.*Thip.*m1.*m2.*sin(th1 - th2).*sin(th2) - F1.*L1.*L2.*M.*m2.*sin(th1) + F2.*L1.*L2.*M.*m1.*sin(th2) + F1.*L1.*L2.*m1.*m2.*sin(th1) + L2.*M.*T1.*m2.*sin(th1 - th2).*sin(th1).^2.*sin(th2) - L2.*M.*Thip.*m2.*sin(th1 - th2).*sin(th1).^2.*sin(th2) - F1.*L1.*L2.*m1.*m2.*cos(th1 - th2).*sin(th2) - F1.*L1.*L2.*m1.*m2.*sin(th1 - th2).*cos(th2) - F2.*L1.*L2.*m1.*m2.*cos(th1 - th2).*sin(th1) + F2.*L1.*L2.*m1.*m2.*sin(th1 - th2).*cos(th1) + F1.*L1.*L2.*M.*m2.*cos(th1).^2.*sin(th1) + F1.*L1.*L2.*M.*m2.*cos(th2).^2.*sin(th1) + F1.*L1.*L2.*M.*m2.*sin(th1).*sin(th2).^2 + F2.*L1.*L2.*m1.*m2.*cos(th1 - th2).^2.*sin(th2) + F2.*L1.*L2.*m1.*m2.*sin(th1 - th2).^2.*sin(th2) - L2.*M.*T1.*m2.*cos(th1 - th2).*cos(th1).^2.*cos(th2) + L2.*M.*Thip.*m2.*cos(th1 - th2).*cos(th1).^2.*cos(th2) - L2.*M.*T1.*m2.*cos(th1 - th2).*cos(th2).*sin(th1).^2 + L2.*M.*Thip.*m2.*cos(th1 - th2).*cos(th2).*sin(th1).^2 + L2.*M.*T1.*m2.*sin(th1 - th2).*cos(th1).^2.*sin(th2) - L2.*M.*Thip.*m2.*sin(th1 - th2).*cos(th1).^2.*sin(th2) - F1.*L1.*L2.*M.*m2.*cos(th1 - th2).*cos(th1).^2.*sin(th2) - F1.*L1.*L2.*M.*m2.*sin(th1 - th2).*cos(th1).^2.*cos(th2) - F1.*L1.*L2.*M.*m2.*cos(th1 - th2).*sin(th1).^2.*sin(th2) - F1.*L1.*L2.*M.*m2.*sin(th1 - th2).*cos(th2).*sin(th1).^2 + L1.*L2.*M.*V1.*m2.*cos(th1 - th2).*cos(th1).*cos(th2) + H1.*L1.*L2.*M.*m2.*cos(th1 - th2).*cos(th1).*sin(th2) - H1.*L1.*L2.*M.*m2.*cos(th1 - th2).*cos(th2).*sin(th1) + H1.*L1.*L2.*M.*m2.*sin(th1 - th2).*cos(th1).*cos(th2) + L1.*L2.*M.*V1.*m2.*cos(th1 - th2).*sin(th1).*sin(th2) - L1.*L2.*M.*V1.*m2.*sin(th1 - th2).*cos(th1).*sin(th2) + L1.*L2.*M.*V1.*m2.*sin(th1 - th2).*cos(th2).*sin(th1) + H1.*L1.*L2.*M.*m2.*sin(th1 - th2).*sin(th1).*sin(th2))./(L1.*L2.*M.*m1.*(cos(th2).^2 + sin(th2).^2));

end


function grid_params = bp_parse_grid_params( per_pulse_metadata, num_samples_used, varargin )
%BP_PARSE_GRID_PARAMS Compute grid for backprojection
% BP_PARSE_GRID_PARAMS(PER_PULSE_METADATA, NUM_SAMPLES_USED, ...
%    'PropertyName', PropertyValue, ...)
%
%       Property name     Description
%       center            Center of image to form in ECF coordinates.
%                         Default is scene reference point.
%       grid_type         'slant', 'ground', or 'DEM'.  Default is ground.
%       image_size_meters Scene size for image formation ([range_size
%                         azimuth_size]) in meters.  Default is total size
%                         supported by collect.
%       image_size_pixels Size of the image in pixels (only used for images
%                         formed to a DEM)
%       sample_rate       Samples per IPR.  Default is 1.5.
%       grid              The grid of image points (ECEF) onto which the
%                         image should be formed.  The format of the grid
%                         parameters is identical to the (MxNx3) grid
%                         returned from this routine if no grid is
%                         supplied.
%
% Output is a structure with the following fields:
%       center            Same as input property
%       grid_type         Same as input property
%       grid              Same as input property
%       sample_rate       Same as input property
%       range_vect_hat    Unit vector point from center of aperture to
%                         image center
%       image_size_meters Extent of the image in meters
%       image_size_pixels Size of the image in pixels
%       row_coords        Vector describing position of each row.  For
%                         planar grids, this is the distance from the
%                         center of the image in meters.  For DEMs, this is
%                         the latitude position in degrees of each row.
%       col_coords        Vector describing position of each column.
%       row_unit_vector   For planar grids, the unit vector in direction of
%                         increasing row
%       col_unit_vector   For planar grids, the unit vector in direction of
%                         increasing column
%
% Authors: Wade Schwartzkopf and Tom Krauss, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

% Calculate default image extent and supported resolution (in slant plane)
pulse_bandwidth = per_pulse_metadata.SCSS * (num_samples_used-1);
[max_resolution, max_extent] = pulse_info_to_resolution_extent(...
    per_pulse_metadata.TxPos - per_pulse_metadata.SRPPos, ... % Line-of-sight vector between ARP and ORP
    per_pulse_metadata.SC0 + (pulse_bandwidth/2), per_pulse_metadata.SCSS, ...
    pulse_bandwidth);

% Parse input parameters
p1 = inputParser;
p1.KeepUnmatched=true;
p1.addParamValue('grid_type',  'ground',                     @(x) any(strcmp(x,{'slant','ground','DEM'})));
p1.addParamValue('grid',       []); % Single grid that fits in memory.  Perhaps in the future, this could also allow for a file input.
p1.addParamValue('center',     mean(per_pulse_metadata.SRPPos), @(x) isempty(x) | (isvector(x)&&(length(x)==3))); % Centerpoint of formed image (ECEF, meters)
p1.addParamValue('image_size_meters', max_extent,            @(x) (isvector(x)&&(length(x)==2))); % Scene extent [x y] (m)
p1.addParamValue('image_size_pixels', [],                    @(x) (isvector(x)&&(length(x)==2))); % Scene extent [x y] (pixels)
p1.addParamValue('sample_rate',1.5,                          @(x) (isvector(x)&&(length(x)<=2)));
p1.FunctionName = mfilename;
p1.parse(varargin{:});
grid_params = p1.Results;

% Some geometry info
range_vect = grid_params.center - ... % Range vector points from center of aperture to image center
    ((per_pulse_metadata.TxPos(ceil(length(per_pulse_metadata.TxPos)/2),:) + ... % Use middle pulse
    per_pulse_metadata.RcvPos(ceil(length(per_pulse_metadata.RcvPos)/2),:))/2); % Bistatic definition, average of tx and rcv
% range_vect = grid_params.center - ... % Range vector points from center of aperture to image center
%     per_pulse_metadata.TxPos(ceil(length(per_pulse_metadata.TxPos)/2),:); % Use middle pulse
grid_params.range_vect_hat = range_vect/norm(range_vect); % Unit vector
vel_vector = per_pulse_metadata.TxPos(end,:)-per_pulse_metadata.TxPos(1,:); % Velocity vector
spn=cross(grid_params.range_vect_hat,vel_vector); % Slant plane normal
spn=spn/norm(spn); % Unit vector
% Slant plane normal to point "up" (away from origin).  This is affected by left/right looking.
spn = spn * sign(grid_params.center*cross(grid_params.range_vect_hat,vel_vector).'); % Left/right fix

% Determine final image size.  IMAGE_SIZE_PIXELS input parameter is ignored
% in all cases except for DEM.  We only allow it for the DEM case, since we
% don't currently have a robust automated way to determine it there.
if ~isempty(grid_params.grid) % Arbitrary grid was passed in.  Nothing to compute.
    grid_params.grid_type = 'ignore';
    grid_params.center = squeeze(mean(mean(grid_params.grid,1),2)); % Centroid of points in grid
    % grid_params.image_size_meters = ???
    grid_params.image_size_pixels = [size(grid_params.grid,1), size(grid_params.grid,2)];
    grid_params = rmfield(grid_params,'sample_rate'); % Not valid
    return; % Nothing left to do for explicitly passed grid
elseif strcmpi(grid_params.grid_type,'DEM')
    if isempty(grid_params.image_size_pixels) % Default
        % Assures that at least sample rate is achieved in all directions.
        grid_params.image_size_pixels = ceil(grid_params.image_size_meters.*...
            max(grid_params.sample_rate)./min(max_resolution));
    end
else % Planar image (slant or ground)
    % Adjust distances from slant to ground if necessary
    if strcmpi(grid_params.grid_type,'ground')
        gpn = wgs_84_norm( grid_params.center ); % Ground plane normal, based on (inflated) wgs_84 ellipsoid
        graze = asin(gpn.'*(-grid_params.range_vect_hat.')); % radians
        twist = asin(cross(gpn.',spn)*(-grid_params.range_vect_hat.')/...
            norm(cross(-grid_params.range_vect_hat.',gpn.'))); % radians
        if any(strcmp(p1.UsingDefaults,'image_size_meters'))
            grid_params.image_size_meters(1) = grid_params.image_size_meters(1)/cos(graze);
            grid_params.image_size_meters(2) = grid_params.image_size_meters(2)/cos(twist);
        end
        max_resolution(1) = max_resolution(1)/cos(graze);
        max_resolution(2) = max_resolution(2)/cos(twist);
    end
    grid_params.image_size_pixels = ceil(grid_params.image_size_meters.*...
        grid_params.sample_rate./max_resolution);
end

% Compute grid onto which the image will be formed
switch grid_params.grid_type
    % For planar images (ground and slant) compute the unit vectors in the
    % direction of increasing row/column direction
    case 'ground'
        row_vector = grid_params.range_vect_hat-(grid_params.range_vect_hat*gpn)*gpn.'; % Project range vector to ground
        grid_params.row_unit_vector = row_vector/norm(row_vector); % Unit vector
        col_vector = -cross(grid_params.row_unit_vector,gpn); % Vector in direction of increasing column (ECEF)
        grid_params.col_unit_vector = col_vector/norm(col_vector); % Unit vector
    case 'slant'
        % Slant plane really has no meaning in backprojection.  Any
        % arbitrary grid can be defined.  We define it here merely for
        % comparison against PFA.
        grid_params.row_unit_vector = grid_params.range_vect_hat; % Unit vector in direction of increasing row (ECEF)
        grid_params.col_unit_vector = -cross(grid_params.row_unit_vector,spn); % Unit vector in direction of increasing column (ECEF)
    case 'DEM'
        % We'll also get the DEM data for the image.  We assume that the
        % entire image fits within .2 degrees of lat and long and that the
        % entire DEM can fit into memory (reasonable for SRTM, but probably
        % not LIDAR).
        center_lla  = ecf_to_geodetic(grid_params.center);
        [lats,lons,elevations] = get_DEM_heights_region([center_lla(1)-0.1,center_lla(2)-0.1], ...
            [center_lla(1)+0.1,center_lla(2)+0.1]);
        [lats, lons] = meshgrid(lats, lons);
        % Return DEM interpolation function, rather than the grid itself,
        % since for image formation we will need values not lying exactly
        % on the DEM points.
        grid_params.F_1 = TriScatteredInterp(lats(:), lons(:), elevations(:));

        % Convert scene extent from meters to arc-degrees
        e2 = 6.6943799901377997e-3;  % eccentricity squared of Earth (WGS 84 value)
        a = 6378137.0;               % semimajor radius of the Earth (WGS 84 value)
        b = a .* sqrt( 1.0 - e2);    % semiminor radius of the Earth
        lat = pi/180*center_lla(1);
        M = (a*b)^2/((a*cos(lat))^2+(b*sin(lat))^2)^(3/2);
        N = a^2/sqrt((a*cos(lat))^2+(b*sin(lat))^2);
        arcradius_lat = pi/180*M;          % length of arc-degree in latitude direction (m)
        arcradius_lon = pi/180*cos(lat)*N; % length of arc-degree in longitude direction (m)
        scene_extent_lat = grid_params.image_size_meters(1)/arcradius_lat;
        scene_extent_lon = grid_params.image_size_meters(2)/arcradius_lon;
end
% Compute row/column coordinates
switch grid_params.grid_type
    % For a plane, coordinates are distances (in meters) from the center
    % of image for each row/column
    case {'ground','slant'}
        grid_params.row_coords = grid_params.image_size_meters(1)*...
            linspace(-1,1,grid_params.image_size_pixels(1))/2;
        grid_params.col_coords = grid_params.image_size_meters(2)*...
            linspace(-1,1,grid_params.image_size_pixels(2))/2;
    % For a DEM, coordinates of each row/column are in lat/lon degrees;
    % that is, angularly, rather than along a plane.
    case 'DEM'
        grid_params.row_coords = center_lla(1) + (scene_extent_lat*...
            linspace(-1,1,grid_params.image_size_pixels(1))/2);
        grid_params.col_coords = center_lla(2) + (scene_extent_lon*...
            linspace(-1,1,grid_params.image_size_pixels(2))/2);
end

end

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