%-------------------------------------------------------------------------------------------------------------------%
%
% 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 JELLYFISH-EXAMPLE geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Jellyfish_Geometry()

% FLUID GRID PARAMETERS %
L = 8;                              % Length of computational domain (m)
N = 256;                            % # of Cartesian grid meshwidths
dx = L/N;                           % Cartesian mesh width (m)
ds = L/(2.0*N);                     % Ideal Lagrangian spacing (m)

% Construct Geometry
[xLag,yLag,ds] = give_Me_Immsersed_Boundary_Geometry(N,L);
plot(xLag,yLag,'*'); hold on;

% Translate Geometry
xLag = xLag + L/8;
yLag = yLag + L/4;
plot(xLag,yLag,'r*'); hold on;

% NAMING CONVENTION FOR SIMULATION
struct_name = 'jelly';      % structure name

%
% PRINT INPUT FILES (.vertex, .spring, .beam, etc) %
%

% print vertices
print_Lagrangian_Vertices(xLag,yLag,struct_name);

% print springs
k_Spring = 500*1.2750000000000000e+07; %500   % spring constant (Newton) dt=2.5e-6
print_Lagrangian_Springs(xLag,yLag,k_Spring,ds,struct_name);

% print beams
k_Beam = 1.0363359375000002e+13;  %100 % beam stiffness constant (Newton m^2) %5.1816796875000010e+12
print_Lagrangian_nonInv_Beams(xLag,yLag,k_Beam,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 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-1 );

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

    % SPRINGS BETWEEN VERTICES ON RHS
    for s = 1:ceil(N/2)
            if s <= floor(N/2)
                x1 = xLag(s); x2 = xLag(s+1);
                y1 = yLag(s); y2 = yLag(s+1);
                ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds);
            elseif s == ceil(N/2)
                x1 = xLag(1); x2 = xLag( ceil(N/2)+1 );
                y1 = yLag(1); y2 = yLag( ceil(N/2)+1 );
                ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', 1, ceil(N/2)+1, k_Spring, ds);
            end
    end

    % SPRINGS BETWEEN VERTICES ON LHS
    for s=1:floor(N/2)-1
                s1 = ceil(N/2)+s;
                s2 = ceil(N/2)+s+1;
                x1 = xLag(s1); x2 = xLag(s2);
                y1 = yLag(s1); y2 = yLag(s2);
                ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s1, s2, k_Spring, ds);
    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (NON-INVARIANT) points to a file called rubberband.nonInv_beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_nonInv_Beams(xLag,yLag,k_Beam,struct_name)

    % k_Beam: beam stiffness
    % Cx/Cy:  beam curvatures in x/y respestively

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

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

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

    % beam_force = kappa_beam*ds/(ds^4)

    %BEAMS BETWEEN VERTICES ON RHS
    for s = 1:ceil(N/2)+1
            if s <= floor(N/2)-1
                Cx = xLag(s) - 2*xLag(s+1) + xLag(s+2);
                Cy = yLag(s) - 2*yLag(s+1) + yLag(s+2);
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', s,s+1,s+2, k_Beam, Cx, Cy);
            elseif s == ceil(N/2)
                Cx = xLag(ceil(N/2)+1) - 2*xLag(1) + xLag(2);
                Cy = yLag(ceil(N/2)+1) - 2*yLag(1) + yLag(2);
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', ceil(N/2)+1,1,2, k_Beam, Cx,Cy);
            elseif s == ceil(N/2)+1
                Cx = xLag(ceil(N/2)+2) - 2*xLag( ceil(N/2)+1 ) + xLag(1);
                Cy = yLag(ceil(N/2)+2) - 2*yLag( ceil(N/2)+1 ) + yLag(1);
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', ceil(N/2)+2,ceil(N/2)+1,1, k_Beam, Cx,Cy);
            end
    end

    % BEAMS BETWEEN VERTICES ON LHS
    for s=1:floor(N/2)-2
                s1 = ceil(N/2)+s;
                s2 = ceil(N/2)+s+1;
                s3 = ceil(N/2)+s+2;
                Cx = xLag(s3) - 2*xLag(s2) + xLag(s1);
                Cy = yLag(s3) - 2*yLag(s2) + yLag(s1);
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', s3, s2, s1, k_Beam, Cx, Cy);
    end
    fclose(beam_fid);

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

function [xLag,yLag,ds] = give_Me_Immsersed_Boundary_Geometry(N,L)

% JELLYFISH GEOMETRY PARAMETERS %

bell_length = 2;                    % bell length (m)
npts_bell = ceil(2.0*(bell_length/L)*N); % number of points along the length of the entire bell
npts_circ = 1;                    %number of points along the circumference (=1 for 2D)
npts = npts_bell*npts_circ;	      % total number points
ds = bell_length/(npts_bell-1);   % mesh spacing along the length of bell (m)
Zs = 0;                          %distance to top of bell (m)
xb = zeros(npts);                 %holds resting positions to calculate curvatures
zb=zeros(npts);                   %holds resting positions to calculate curvatures

% Values to describe bell shape given in Alben, Peng and Miller %
betao = 0.5;
betam = 0.3;
to = 0.5;
t=0;
gamma = 1;

% Starting values
z = Zs;
r = 0;
x = 0;

% center line of jelly
xLag(1) = x;
yLag(1) = z;
zl=z;
rl=r;

%right side of bell
for s = 1:(ceil(npts_bell/2)-1)
    beta = betao+(betam-betao)*(t/to)^gamma;
    theta = -1.55*(1-exp(-(s)*ds/beta));
    z = zl + ds*sin(theta);
    r = rl + ds*cos(theta);
    x = r;
    zl=z;
    rl=r;
    xLag(s+1)=x;
    yLag(s+1)=z;
   %fprintf(vertex_fid, '%1.16e %1.16e\n', x, z);
end

% reinitialize values for centerline
z = Zs;
r = 0;
zl=z;
rl=r;

%left side of bell
for s = (ceil(npts_bell/2)):npts_bell-2
    s2=s-(ceil(npts_bell/2)-1);
    beta = betao+(betam-betao)*(t/to)^gamma;
    theta = -1.55*(1-exp(-(s2)*ds/beta));
    z = zl + ds*sin(theta);
    r = rl + ds*cos(theta);
    x = -1*r;
    zl=z;
    rl=r;
    xLag(s+1)=x;
    yLag(s+1)=z;
   %fprintf(vertex_fid, '%1.16e %1.16e\n', x, z);
end

%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% % Write out the beam information
%  beam_fid = fopen([mesh_name num2str(N) '.beam'], 'w');
%
%  fprintf(beam_fid, '%d\n', npts-3);
%
%  %right side of bell
%  for q = 0:npts_circ-1
%  for s = 0:(ceil(npts_bell/2)-3)
%     C1 = xb(s+1)+xb(s+3)-2*xb(s+2);
%     C2 = zb(s+1)+zb(s+3)-2*zb(s+2);
%     fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', q*npts_circ+s, q*npts_circ+s+1, q*npts_circ+s+2, kappa_beam*ds/(ds^4), C1, C2);
%  end
%
%  %top of bell
%     s=ceil(npts_bell/2);
%     C1 = xb(s+1)+xb(2)-2*xb(1);
%     C2 = zb(s+1)+zb(2)-2*zb(1);
%     fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', q*npts_circ+s, q*npts_circ+0, q*npts_circ+1, kappa_beam*ds/(ds^4), C1, C2);
%     s=ceil(npts_bell/2)+1;
%     C1 = xb(s+1)+xb(1)-2*xb(s);
%     C2 = zb(s+1)+zb(1)-2*zb(s);
%     fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', q*npts_circ+s, q*npts_circ+s-1, q*npts_circ+0, kappa_beam*ds/(ds^4), C1, C2);
%
%  %left side of bell
%  for s = ceil((npts_bell/2)+2):npts_bell-2
%     C1 = xb(s+1)+xb(s-1)-2*xb(s);
%     C2 = zb(s+1)+zb(s-1)-2*zb(s);
%     fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', q*npts_circ+s, q*npts_circ+s-1, q*npts_circ+s-2, kappa_beam*ds/(ds^4), C1, C2);
%  end
%  end
% %
% %
%  fclose(beam_fid);
%


function embedding = optimize_layout(head_embedding, tail_embedding, ...
    head, tail, n_epochs, n_vertices, epochs_per_sample, a, b, ...
    gamma, initial_alpha, negative_sample_rate, verbose)
%OPTIMIZE_LAYOUT Improve an embedding using stochastic gradient descent to
% minimize the fuzzy set cross entropy between the 1-skeletons of the high
% dimensional and low dimensional fuzzy simplicial sets. In practice this
% is done by sampling edges based on their membership strength (with the
% (1-p) terms coming from negative sampling similar to word2vec). This
% function is only called if the UMAP method is 'MATLAB'.
%
% embedding = OPTIMIZE_LAYOUT(head_embedding, tail_embedding, head, tail,
% n_epochs, n_vertices, epochs_per_sample, a, b)
%
% Parameters
% ----------
% head_embedding: array of size (n_samples, n_components)
%     The initial embedding to be improved by SGD.
%
% tail_embedding: array of size (source_samples, n_components)
%     The reference embedding of embedded points. If not embedding new
%     previously unseen points with respect to an existing embedding this
%     is simply the head_embedding (again); otherwise it provides the
%     existing embedding to embed with respect to.
%
% head: array of size (n_1_simplices, 1)
%     The indices of the heads of 1-simplices with non-zero membership.
%
% tail: array of size (n_1_simplices, 1)
%     The indices of the tails of 1-simplices with non-zero membership.
%
% n_epochs: double
%     The number of training epochs to use in optimization.
%
% n_vertices: double
%     The number of vertices (0-simplices) in the dataset.
%
% epochs_per_samples: array of size (n_1_simplices, 1)
%     A double value of the number of epochs per 1-simplex. 1-simplices with
%     weaker membership strength will have more epochs between being sampled.
%
% a: double
%     Parameter of differentiable approximation of right adjoint functor.
%
% b: double
%     Parameter of differentiable approximation of right adjoint functor.
%
% gamma: double (optional, default 1)
%     Weight to apply to negative samples.
%
% initial_alpha: double (optional, default 1)
%     Initial learning rate for the SGD.
%
% negative_sample_rate: double (optional, default 5)
%     Number of negative samples to use per positive sample.
%
% verbose: boolean (optional, default false)
%     Whether to report information on the current progress of the algorithm.
%
% Returns
% -------
% embedding: array of size (n_samples, n_components)
%     The optimized embedding.
%
% See also: OPTIMIZE_LAYOUT2
%
%   AUTHORSHIP
%   Math Lead & Primary Developer:  Connor Meehan <connor.gw.meehan@gmail.com>
%   Secondary Developer: Stephen Meehan <swmeehan@stanford.edu>
%   Bioinformatics Lead:  Wayne Moore <wmoore@stanford.edu>
%   Provided by the Herzenberg Lab at Stanford University
%   License: BSD 3 clause

    if nargin < 13
        verbose = false;
        if nargin < 12
            negative_sample_rate = 5;
            if nargin < 11
                initial_alpha = 1;
                if nargin < 10
                    gamma = 1;
                end
            end
        end
    end

    dim = size(head_embedding, 2);
    n_1_simplices = size(epochs_per_sample, 1);
    same_embedding = isequal(head_embedding, tail_embedding);
    alpha = initial_alpha;
    ONES=ones(1, dim);
    BG2S=2*gamma* b*ONES;
    FOURS=4*ONES;
    ABNEG2=-2.0*a*b;
    BNEG1=b-1;

    epochs_per_negative_sample = epochs_per_sample / single(negative_sample_rate);
    epoch_of_next_negative_sample = epochs_per_negative_sample;
    epoch_of_next_sample = epochs_per_sample;
    if verbose
        fprintf('\t0/%d epochs done\n', int32(n_epochs));
    end
    for n = 1:n_epochs
        for i = 1:n_1_simplices
            if epoch_of_next_sample(i) <= n
                j = head(i);
                k = tail(i);

                current = head_embedding(j,:);
                other = tail_embedding(k,:);

                dist_squared = norm(current - other).^2;

                grad_coeff = (dist_squared > 0)*(ABNEG2*(dist_squared).^BNEG1)./ (a*dist_squared.^b + 1);
                grad_coeff(isnan(grad_coeff)) = 0;

                grad= max(-4, min(4, grad_coeff .* (current - other)));
                current = current + grad * alpha;

                epoch_of_next_sample(i) = epoch_of_next_sample(i) + epochs_per_sample(i);

                n_neg_samples = floor((single(n) - epoch_of_next_negative_sample(i)) / epochs_per_negative_sample(i));

                if same_embedding
                    other = other - grad * alpha;
                    head_embedding(k,:) = other;
                    tail_embedding(k,:) = other;
                end

                for p = 1:n_neg_samples
                    k = int32(randi(n_vertices));

                    other = tail_embedding(k,:);

                    dist_squared = norm(current - other).^2;

                    if same_embedding && j == k
                        continue
                    end

                    grad_coeff = (dist_squared > 0)*BG2S./(0.001 + dist_squared)./(a*dist_squared.^b + 1);
                    grad_coeff(isnan(grad_coeff)) = 0;
                    grad=(grad_coeff > 0).*max(-4, min(4, grad_coeff .* (current - other))) + ~(grad_coeff > 0).*FOURS;
                    current = current + grad * alpha;
                end

                head_embedding(j,:) = current;
                if same_embedding
                    tail_embedding(j,:) = current;
                end

                epoch_of_next_negative_sample(i) = epoch_of_next_negative_sample(i)+(n_neg_samples * epochs_per_negative_sample(i));
            end

        end
        alpha = initial_alpha * (1 - single(n)/single(n_epochs));

        progress_checkpoint = min(floor(n_epochs / 10), 50);

        if verbose && mod(n, progress_checkpoint) == 0
            fprintf('\t%d/%d epochs done\n', int32(n), int32(n_epochs));
        end
    end

    embedding = head_embedding;
 end


function  [X,Y,vals,labI]=mp_utm(optn,varargin)
% MP_UTM   Universal Transverse Mercator projection
%           This function should not be used directly; instead it is
%           is accessed by various high-level functions named M_*.

% mp_utm.m, Peter Lemmond (peter@whoi.edu)

% created mp_utm.m 13Aug98 from mp_tmerc.m, v1.2d distribution, by:
%
% Rich Pawlowicz (rich@ocgy.ubc.ca) 2/Apr/1997
%
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.
%
% Mathematical formulas for the projections and their inverses are taken from
%
%      Snyder, John P., Map Projections used by the US Geological Survey,
%      Geol. Surv. Bull. 1532, 2nd Edition, USGPO, Washington D.C., 1983.
%

% 10/Dec/98 - PL added various ellipsoids.
% 17/May/12 - clarified hemisphere setting

global MAP_PROJECTION MAP_VAR_LIST

% define a structure of various ellipsoids. each has a name, and
% a vector consisting of equatorial radius and flattening. the first
% two are somewhat special cases.

MAP_ELLIP = struct ( 'normal', [1.0, 0], ...
    'sphere', [6370997.0, 0], ...
    'grs80' , [6378137.0, 1/298.257], ...
    'grs67' , [6378160.0, 1/247.247], ...
    'wgs84' , [6378137.0, 1/298.257], ...
    'wgs72' , [6378135.0, 1/298.260], ...
    'wgs66' , [6378145.0, 1/298.250], ...
    'wgs60' , [6378165.0, 1/298.300], ...
    'clrk66', [6378206.4, 1/294.980], ...
    'clrk80', [6378249.1, 1/293.466], ...
    'intl24', [6378388.0, 1/297.000], ...
    'intl67', [6378157.5, 1/298.250]);

name={'UTM'};

switch optn

  case 'name'

    X=name;

  case {'usage','set'}

    m_names=fieldnames(MAP_ELLIP);

    X=char({['     ''' varargin{1} ''''],...
	'     <,''lon<gitude>'',[min max]>',...
	'     <,''lat<itude>'',[min max]>',...
	'     <,''zon<e>'',value>',...
	'     <,''hem<isphere>'',[1|0] (0 for N)>',...
	'     <,''ell<ipsoid>'', one of',...
    reshape(sprintf('         %6s',m_names{:}),15,length(m_names))',...
        '                               >',...
	'     <,''rec<tbox>'', ( ''on'' | ''off'' )>'});

  case 'get'

    X=char([' Projection: ' MAP_PROJECTION.name '  (function: ' ...
	  MAP_PROJECTION.routine ')'],...
	[' longitudes: ' num2str(MAP_VAR_LIST.ulongs) ],...
	[' latitudes: ' num2str(MAP_VAR_LIST.ulats) ],...
	[' zone: ' num2str(MAP_VAR_LIST.zone) ],...
	[' hemisphere: ' num2str(MAP_VAR_LIST.hemisphere) ],...
	[' ellipsoid: ' MAP_VAR_LIST.ellipsoid ], ...
	[' Rectangular border: ' MAP_VAR_LIST.rectbox ]);

  case 'initialize'

    MAP_VAR_LIST=[];
    MAP_PROJECTION.name=varargin{1};
    MAP_VAR_LIST.ulongs = [-72 -68];
    MAP_VAR_LIST.ulats = [40 44];
    MAP_VAR_LIST.zone = 0;		% will be computed if not there
    MAP_VAR_LIST.hemisphere = -1;
    MAP_VAR_LIST.ellipsoid = 'wgs84';
    MAP_VAR_LIST.rectbox='off';
    k=2;

    while k<length(varargin)
      switch varargin{k}(1:3)
	case 'lon'
	  MAP_VAR_LIST.ulongs=varargin{k+1}(:)';
	case 'lat'
	  MAP_VAR_LIST.ulats=varargin{k+1}(:)';
	case 'zon'
	  MAP_VAR_LIST.zone=varargin{k+1};
	case 'hem'
	  MAP_VAR_LIST.hemisphere=varargin{k+1};
        case 'ell'
	  MAP_VAR_LIST.ellipsoid=varargin{k+1};
        case 'rec'
          MAP_VAR_LIST.rectbox=varargin{k+1};
	otherwise
	  disp(['Unknown option: ' varargin{k}]);
      end
      k=k+2;
    end

    % set the zone and hemisphere if not specified

    if (~MAP_VAR_LIST.zone)
      MAP_VAR_LIST.zone = 1 + fix((mod(mean(MAP_VAR_LIST.ulongs)+180,360))/6);
    end

    if MAP_VAR_LIST.hemisphere==-1
      MAP_VAR_LIST.hemisphere=mean(MAP_VAR_LIST.ulats)<0;
    end

    % check for a valid ellipsoid. if not, use the normalized sphere

    if ~isfield(MAP_ELLIP,MAP_VAR_LIST.ellipsoid)
       MAP_VAR_LIST.ellipsoid = 'normal';
    end

    % Get X/Y and (if rectboxs are desired) recompute lat/long limits.

    mu_util('xylimits');
    if strcmp(MAP_VAR_LIST.rectbox,'on'), mu_util('lllimits'); end

  case 'll2xy'

    long=varargin{1};
    lat=varargin{2};
    vals=zeros(size(long));

    % Clip out-of-range values (lat/long boxes)

    if  strcmp(MAP_VAR_LIST.rectbox,'off') && ~strcmp(varargin{4},'off')
        vals=vals | long<=MAP_VAR_LIST.longs(1)+eps*10 | long>=MAP_VAR_LIST.longs(2)-eps*10 | ...
	              lat<=MAP_VAR_LIST.lats(1)+eps*10 |   lat>=MAP_VAR_LIST.lats(2)-eps*10;
        [long,lat]=mu_util('clip',varargin{4},long,MAP_VAR_LIST.longs(1),long<MAP_VAR_LIST.longs(1),lat);
        [long,lat]=mu_util('clip',varargin{4},long,MAP_VAR_LIST.longs(2),long>MAP_VAR_LIST.longs(2),lat);
        [lat,long]=mu_util('clip',varargin{4},lat,MAP_VAR_LIST.lats(1),lat<MAP_VAR_LIST.lats(1),long);
        [lat,long]=mu_util('clip',varargin{4},lat,MAP_VAR_LIST.lats(2),lat>MAP_VAR_LIST.lats(2),long);
    end

    % do the forward transformation

    [X,Y] = mu_ll2utm(lat,long,MAP_VAR_LIST.zone,MAP_VAR_LIST.hemisphere, ...
	getfield(MAP_ELLIP,MAP_VAR_LIST.ellipsoid));

    % Clip out-of-range values (rectboxes)

    if strcmp(MAP_VAR_LIST.rectbox,'on')  && ~strcmp(varargin{4},'off')
        vals= vals | X<=MAP_VAR_LIST.xlims(1)+eps*10 | X>=MAP_VAR_LIST.xlims(2)-eps*10 | ...
                     Y<=MAP_VAR_LIST.ylims(1)+eps*10 | Y>=MAP_VAR_LIST.ylims(2)-eps*10;
        [X,Y]=mu_util('clip',varargin{4},X,MAP_VAR_LIST.xlims(1),X<MAP_VAR_LIST.xlims(1),Y);
        [X,Y]=mu_util('clip',varargin{4},X,MAP_VAR_LIST.xlims(2),X>MAP_VAR_LIST.xlims(2),Y);
        [Y,X]=mu_util('clip',varargin{4},Y,MAP_VAR_LIST.ylims(1),Y<MAP_VAR_LIST.ylims(1),X);
        [Y,X]=mu_util('clip',varargin{4},Y,MAP_VAR_LIST.ylims(2),Y>MAP_VAR_LIST.ylims(2),X);
    end

  case 'xy2ll'

    [Y,X] = mu_utm2ll(varargin{1}, varargin{2}, MAP_VAR_LIST.zone, ...
	MAP_VAR_LIST.hemisphere, getfield(MAP_ELLIP,MAP_VAR_LIST.ellipsoid));

  case 'xgrid'

    [X,Y,vals,labI]=mu_util('xgrid',MAP_VAR_LIST.longs,MAP_VAR_LIST.lats,varargin{1},31,varargin{2:3});

  case 'ygrid'

    [X,Y,vals,labI]=mu_util('ygrid',MAP_VAR_LIST.lats,MAP_VAR_LIST.longs,varargin{1},31,varargin{2:3});

  case 'box'

    [X,Y]=mu_util('box',31);

end

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

function [x,y] = mu_ll2utm (lat,lon, zone, hemisphere,ellipsoid)
%mu_ll2utm		Convert geodetic lat,lon to X/Y UTM coordinates
%
%	[x,y] = mu_ll2utm (lat, lon, zone, hemisphere,ellipsoid)
%
%	input is latitude and longitude vectors, zone number,
%		hemisphere(N=0,S=1), ellipsoid info [eq-rad, flat]
%	output is X/Y vectors
%
%	see also	mu_utm2ll, utmzone

% some general constants

DEG2RADS    = 0.01745329252;
RADIUS      = ellipsoid(1);
FLAT        = ellipsoid(2);
K_NOT       = 0.9996;
FALSE_EAST  = 500000;
FALSE_NORTH = 10000000;

% check for valid numbers

if (max(abs(lat)) > 90)
  error('latitude values exceed 89 degree');
  return;
end

if ((zone < 1) || (zone > 60))
  error ('utm zones only valid from 1 to 60');
  return;
end

% compute some geodetic parameters

lambda_not  = ((-180 + zone*6) - 3) * DEG2RADS;

e2  = 2*FLAT - FLAT*FLAT;
e4  = e2 * e2;
e6  = e4 * e2;
ep2 = e2/(1-e2);

% some other constants, vectors

lat = lat * DEG2RADS;
lon = lon * DEG2RADS;

sinL = sin(lat);
tanL = tan(lat);
cosL = cos(lat);

T = tanL.*tanL;
C = ep2 * (cosL.*cosL);
A = (lon - lambda_not).*cosL;
A2 = A.*A;
A4 = A2.*A2;
S = sinL.*sinL;

% solve for N

N = RADIUS ./ (sqrt (1-e2*S));

% solve for M

M0 = 1 - e2*0.25 - e4*0.046875 - e6*0.01953125;
M1 = e2*0.375 + e4*0.09375 + e6*0.043945313;
M2 = e4*0.05859375 + e6*0.043945313;
M3 = e6*0.011393229;
M = RADIUS.*(M0.*lat - M1.*sin(2*lat) + M2.*sin(4*lat) - M3.*sin(6*lat));

% solve for x

X0 = A4.*A/120;
X1 = 5 - 18*T + T.*T + 72*C - 58*ep2;
X2 = A2.*A/6;
X3 = 1 - T + C;
x = N.*(A + X3.*X2 + X1.* X0);

% solve for y

Y0 = 61 - 58*T + T.*T + 600*C - 330*ep2;
Y1 = 5 - T + 9*C + 4*C.*C;

y = M + N.*tanL.*(A2/2 + Y1.*A4/24 + Y0.*A4.*A2/720);

% finally, do the scaling and false thing. if using a unit-normal radius,
% we don't bother.

x = x*K_NOT + (RADIUS>1) * FALSE_EAST;

y = y*K_NOT;
if (hemisphere)
  y = y + (RADIUS>1) * FALSE_NORTH;
end

return

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

function [lat,lon] = mu_utm2ll (x,y, zone, hemisphere,ellipsoid)
%mu_utm2ll		Convert X/Y UTM coordinates to geodetic lat,lon
%
%	[lat,lon] = mu_utm2ll (x,y, zone, hemisphere,ellipsoid)
%
%	input is X/Y vectors, zone number, hemisphere(N=0,S=1),
%		ellipsoid info [eq-rad, flat]
%	output is lat/lon vectors
%
%	see also	mu_ll2utm, utmzone

% some general constants

DEG2RADS    = 0.01745329252;
RADIUS      = ellipsoid(1);
FLAT        = ellipsoid(2);
K_NOT       = 0.9996;
FALSE_EAST  = 500000;
FALSE_NORTH = 10000000;

if ((zone < 1) || (zone > 60))
  error ('utm zones only valid from 1 to 60');
  return;
end

% compute some geodetic parameters

e2  = 2*FLAT - FLAT*FLAT;
e4  = e2 * e2;
e6  = e4 * e2;
eps = e2 / (1-e2);
em1 = sqrt(1-e2);
e1  = (1-em1)/(1+em1);
e12 = e1*e1;

lambda_not  = ((-180 + zone*6) - 3) * DEG2RADS;

% remove the false things

x = x - (RADIUS>1)*FALSE_EAST;
if (hemisphere)
  y = y - (RADIUS>1)*FALSE_NORTH;
end

% compute the footpoint latitude

M = y/K_NOT;
mu = M/(RADIUS * (1 - 0.25*e2 - 0.046875*e4 - 0.01953125*e6));
foot = mu + (1.5*e1 - 0.84375*e12*e1)*sin(2*mu) ...
    + (1.3125*e12 - 1.71875*e12*e12)*sin(4*mu) ...
    + (1.57291666667*e12*e1)*sin(6*mu) ...
    + (2.142578125*e12*e12)*sin(8*mu);

% some other terms

sinF = sin(foot);
cosF = cos(foot);
tanF = tan(foot);

N = RADIUS ./ sqrt(1-e2*(sinF.*sinF));
T = tanF.*tanF;
T2 = T.*T;
C = eps * cosF.*cosF;
C2 = C.*C;
denom = sqrt(1-e2*(sinF.*sinF));
R = RADIUS * em1*em1 ./ (denom.*denom.*denom);
D = x./(N*K_NOT);
D2 = D.*D;
D4 = D2.*D2;

% can now compute the lat and lon

lat = foot - (N.*tanF./R) .* (0.5*D2 - (5 + 3*T + 10*C - 4*C2 - 9*eps).*D4/24 ...
    + (61 + 90*T + 298*C + 45*T2 - 252*eps - 3*C2) .* D4 .* D2/720);

lon = lambda_not + (D - (1 + 2*T +C).*D2.*D/6 + ...
    (5 - 2*C + 28*T - 3*C2 + 8*eps + 24*T2).*D4.*D./120)./cosF;

% convert back to degrees;

lat=lat/DEG2RADS;
lon=lon/DEG2RADS;

return


function [x,u,y] = spm_Markov_blanket(J,z,m,mj)
% FORMAT [x,u,y] = spm_Markov_blanket(J,z,m,mj)
% Markovian partition
% J  - Jacobian
% z  - {1 x N}  partition of states (indices)
% m  - number of internal states [default: 3]
%
% x  - {3 x n} particular partition of state indices
%     x{1,j} - active states of j-th partition
%     x{2,j} - sensory states of j-th partition
%     x{3,j} - internal states of j-th partition
%
% u  - location of partitions in scaling or embedding space
%
% y  - {3 x n} particular partition of partition indices
%     y{1,j} - active states of j-th partition
%     y{2,j} - sensory states of j-th partition
%     y{3,j} - internal states of j-th partition
%
% Partition or Grouping (coarse-scaling) operator
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_Markov_blanket.m 7655 2019-08-25 20:10:20Z karl $

% preliminaries
%--------------------------------------------------------------------------
GRAPHICS  = 1;                      % Graphics switch
nz        = length(z);              % number of partitions
if nargin < 3
    m = 3;                          % maximum size of internal states
end
if nargin < 4
   mj = ones(nz,1);                 % eligible internal states
end
if isempty(mj)
   mj = ones(nz,1);                 % eligible internal states
end

% Adjacency matrix (over z)
%--------------------------------------------------------------------------
for i = 1:nz
    for j = 1:nz
        Lij    = J(z{i},z{j});
        if any(any(Lij))
            L(i,j) = abs(norm(full(Lij)) > 1/128);
        else
            L(i,j) = 0;
        end
    end
end
L     = double(L);

% get Markov blanket
%--------------------------------------------------------------------------
B     = L + L' + L'*L;
B     = B - diag(diag(B));

% scaling space (defined by graph Laplacian)
%--------------------------------------------------------------------------
G     = L + L';
G     = G - diag(diag(G));
G     = G - diag(sum(G));
G     = expm(G);

% get principal dimensions of scaling space (u)
%--------------------------------------------------------------------------
if GRAPHICS
    [u,v] = eig(G,'nobalance');
    v     = abs(diag(v));
    for i = 1:nz
        [p,h] = hist(real(u(:,i)),16);
        dh    = h(2) - h(1) + exp(-16);
        p     = p(:)/sum(p)/dh;
        v(i)  = log(v(i)) - p'*log(p + exp(-16))*dh;
    end
    [v,j] = sort(real(v),'descend');
    u     = real(u(:,j));
end

% recursive (particular) partition into internal, sensory and active states
%--------------------------------------------------------------------------
nn    = zeros(nz,1);
for i = 1:nz

    % internal states (defined by graph Laplacian)
    %----------------------------------------------------------------------
    jj = ~(B*nn) & ~nn & mj;
    if any(jj)

        % find densely coupled internal states (using the graph Laplacian)
        %------------------------------------------------------------------
        [g,j] = max(diag(G).*jj);
        if m > 1
            g      = G(:,j);
            g(j)   = 0;
            g(~jj) = 0;
            [g,k]  = sort(g,'descend');
            try
                j = [j; k(1:m - 1)];
            end
        end

        jj    = sparse(j,1,1,size(L,1),1) & jj;         % internal states
        bb    = B*jj & ~jj & ~nn;                       % Markov blanket
        ee    =  ~bb & ~jj & ~nn;                       % external states
        b     = find(bb);
        e     = find(ee);
        s     = b(find( any(L(b,e),2)));
        a     = b(find(~any(L(b,e),2)));

        % indices of individual states in the i-th particle
        %------------------------------------------------------------------
        x{1,i} = spm_cat(z(a));
        x{2,i} = spm_cat(z(s));
        x{3,i} = spm_cat(z(j));

        % states accounted for (nn)
        %------------------------------------------------------------------
        nn   = nn | bb | jj;

    else

        % no internal states - find active states (not influenced by e)
        %------------------------------------------------------------------
        j = ~any(L(~nn,nn),2);
        if any(j)

            % sensory states connected with active states
            %--------------------------------------------------------------
            a  = find(~nn);
            a  = a(find(j,1));
            aa = sparse(a,1,1,size(L,1),1);
            ss = (L*aa | L'*aa) & ~aa & ~nn;
            a  = find(aa);
            s  = find(ss);
            j  = [];

            % indices of individual states in the i-th particle
            %--------------------------------------------------------------
            x{1,i} = spm_cat(z(a));
            x{2,i} = spm_cat(z(s));
            x{3,i} = [];

            % states accounted for (nn)
            %--------------------------------------------------------------
            nn   = nn | aa | ss;

        elseif any(~nn)

            % sensory states connected with sensory states
            %--------------------------------------------------------------
            s  = find(~nn);
            ss = sparse(s(1),1,1,nz,1);
            ss = ss | B*ss & ~nn;
            s  = find(ss);
            a  = [];
            j  = [];

            % indices of individual states in the i-th particle
            %--------------------------------------------------------------
            x{1,i} = [];
            x{2,i} = spm_cat(z(s));
            x{3,i} = [];

            % states accounted for (nn)
            %--------------------------------------------------------------
            nn   = nn | ss;
        end
    end

    % indices of partitions (i.e., n-states) in the i-th particle
    %----------------------------------------------------------------------
    y{1,i} = a;
    y{2,i} = s;
    y{3,i} = j;

    % plot
    %----------------------------------------------------------------------
    if all(nn) && numel(u) > 1

        % remove isolated (internal) states
        %--------------------------------------------------------------
        j     = [];
        for n = 1:size(x,2)
            if any(x{1,n}) || any(x{2,n})
                j = [j,n];
            end
        end
        x  = x(:,j);
        y  = y(:,j);

        if GRAPHICS,clf

            % colours for different particles
            %--------------------------------------------------------------
            nx            = size(x,2);
            [col,bol,msz] = spm_MB_col(nx);

            % plot partitions in embedding space (which particle)
            %--------------------------------------------------------------
            subplot(3,2,3)
            for k = 1:nx
                plot(u(y{1,k},1),u(y{1,k},2),'.','color',bol{k},'MarkerSize',msz), hold on
                plot(u(y{2,k},1),u(y{2,k},2),'.','color',bol{k},'MarkerSize',msz), hold on
                plot(u(y{3,k},1),u(y{3,k},2),'.','color',col{k},'MarkerSize',msz), hold on
            end
            axis square
            title(sprintf('Particles [%i n-states]',nz),'Fontsize',16)

            % plot particles in embedding space (which sort of state)
            %--------------------------------------------------------------
            subplot(3,2,4)
            for k = 1:nx
                plot(u(y{1,k},1),u(y{1,k},2),'.r','MarkerSize',msz), hold on
                plot(u(y{2,k},1),u(y{2,k},2),'.m','MarkerSize',msz), hold on
                plot(u(y{3,k},1),u(y{3,k},2),'.b','MarkerSize',msz), hold on
            end
            axis square
            title(sprintf('Markov partition [%i particles]',nx),'Fontsize',16)

            % plot particles in three embedding dimensions
            %--------------------------------------------------------------
            subplot(3,2,2)
            try
                for k = 1:nx
                    plot3(u(y{1,k},1),u(y{1,k},2),u(y{1,k},3),'.r','MarkerSize',msz), hold on
                    plot3(u(y{2,k},1),u(y{2,k},2),u(y{2,k},3),'.m','MarkerSize',msz), hold on
                    plot3(u(y{3,k},1),u(y{3,k},2),u(y{3,k},3),'.b','MarkerSize',msz), hold on
                end
            catch
                for k = 1:nx
                    plot(u(y{1,k},1),u(y{1,k},2),'.r','MarkerSize',msz), hold on
                    plot(u(y{2,k},1),u(y{2,k},2),'.m','MarkerSize',msz), hold on
                    plot(u(y{3,k},1),u(y{3,k},2),'.b','MarkerSize',msz), hold on
                end
            end

            axis square
            title('Embedding space','Fontsize',16)
            rotate3d(gca,'on')

            % Jacobian (ordered by partition and type)
            %--------------------------------------------------------------
            j = spm_vec(x');
            k = spm_vec(x );
            subplot(3,2,5),imagesc(-log(abs(J(k,k)) + exp(-4))),axis square
            subplot(3,2,6),imagesc(-log(abs(J(j,j)) + exp(-4))),axis square

            % Colors
            %--------------------------------------------------------------
            nj   = spm_length(x);
            msz  = fix(16 + 128/nj);
            j    = 1:nj;
            k    = spm_unvec(j,x')';
            j    = spm_unvec(j,x);
            subplot(3,2,5),hold on
            for q = 1:nx
                plot(j{1,q},ones(size(x{1,q})),'.','color',bol{q},   'MarkerSize',msz)
                plot(j{2,q},ones(size(x{2,q})),'.','color',bol{q},   'MarkerSize',msz)
                plot(j{3,q},ones(size(x{3,q})),'.','color',col{q},   'MarkerSize',msz)
                plot(j{1,q},zeros(size(x{1,q})) + nj,'.','color','r','MarkerSize',msz)
                plot(j{2,q},zeros(size(x{2,q})) + nj,'.','color','m','MarkerSize',msz)
                plot(j{3,q},zeros(size(x{3,q})) + nj,'.','color','b','MarkerSize',msz)
            end
            title(sprintf('Jacobian (by %i particles)',nx),'Fontsize',16)

            subplot(3,2,6),hold on
            for q = 1:nx
                plot(k{1,q},ones(size(x{1,q})),'.','color',bol{q},   'MarkerSize',msz)
                plot(k{2,q},ones(size(x{2,q})),'.','color',bol{q},   'MarkerSize',msz)
                plot(k{3,q},ones(size(x{3,q})),'.','color',col{q},   'MarkerSize',msz)
                plot(k{1,q},zeros(size(x{1,q})) + nj,'.','color','r','MarkerSize',msz)
                plot(k{2,q},zeros(size(x{2,q})) + nj,'.','color','m','MarkerSize',msz)
                plot(k{3,q},zeros(size(x{3,q})) + nj,'.','color','b','MarkerSize',msz)
            end
            title('Jacobian (by type)','Fontsize',16)

        end
        break
    end

end

return


%%******************************************************************
%% ops:
%%
%%   Z = ops(X,operand,Y,alpha);
%%
%%  INPUT:        X = a matrix or a scalar
%%                    or a CELL ARRAY consisting only of matrices
%%          operand = sym, transpose, triu, tril,
%%                    real, imag, sqrt, abs, max, min, nnz,
%%                    spdiags, ones, zeros, norm, sum, row-norm, blk-norm
%%                    rank1, rank1inv, inv
%%                    +,  -, *, .*,  ./, .^
%%     Y (optional) = a matrix or a scalar
%%                    or a CELL ARRAY consisting only of matrices
%% alpha (optional) = a scalar
%%                    or the variable blk.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

   function Z = ops(X,operand,Y,alpha);

   spdensity = 0.4;

   if (nargin == 2)
      if strcmp(operand,'sym');
         if ~iscell(X);
            [m,n] = size(X);
            if (m == n);
               Z = (X+X')/2;
            elseif (n == 1);
               Z = X;
            else;
               error('X must be square matrix or a column vector');
            end;
         else
            Z = cell(size(X));
            for p = 1:length(X);
                [m,n] = size(X{p});
                if (m == n);
                   Z{p} = (X{p}+X{p}')/2;
                elseif (n == 1);
                   Z{p} = X{p};
                else;
                   error('X{p} must be square matrix or a column vector');
                end;
            end;
         end;
      elseif strcmp(operand,'sqrt') | strcmp(operand,'abs') | ...
         strcmp(operand,'real') | strcmp(operand,'imag');
         if ~iscell(X);
            eval(['Z = ',operand,'(X);']);
         else;
            Z = cell(size(X));
            for p = 1:length(X);
               eval(['Z{p} = ',operand,'(X{p});']);
            end;
         end;
      elseif strcmp(operand,'max') | strcmp(operand,'min') | ...
             strcmp(operand,'sum');
         if ~iscell(X);
            eval(['Z = ',operand,'(X);']);
         else;
            Z = [];
            for p = 1:length(X);
                eval(['Z = [Z  ',operand,'(X{p})','  ];']);
            end;
         end;
         eval(['Z = ',operand,'(Z);']);
      elseif strcmp(operand,'transpose') | strcmp(operand,'triu') | ...
             strcmp(operand,'tril');
         if ~iscell(X);
            if (size(X,1) == size(X,2));
               eval(['Z = ',operand,'(X);']);
            elseif (size(X,2) == 1);
               eval(['Z = X;']);
            else;
               error('X must be square matrix or a column vector');
            end;
         else
            Z = cell(size(X));
            for p = 1:length(X);
                if (size(X{p},1) == size(X{p},2));
                   eval(['Z{p} = ',operand,'(X{p});']);
                elseif (size(X{p},2) == 1);
                   eval(['Z{p} = X{p};']);
                else;
                   error('X{p} must be square matrix or a column vector');
                end;
            end;
         end;
      elseif strcmp(operand,'norm');
         if ~iscell(X);
            Z = full(sqrt(sum(sum(X.*X))));
         else
            Z = 0;
            for p = 1:length(X); Z = Z + sum(sum(X{p}.*X{p})); end;
            Z = sqrt(Z);
         end;
      elseif strcmp(operand,'blk-norm');
         if ~iscell(X);
            Z = full(sqrt(sum(sum(X.*X))));
         else
            Z = zeros(length(X),1);
            for p = 1:length(X); Z(p) = sum(sum(X{p}.*X{p})); end;
            Z = sqrt(Z);
         end;
      elseif strcmp(operand,'inv');
         if ~iscell(X);
            [m,n] = size(X); n2 = n*n;
            if (m==n)
               Z = inv(X);
               if (nnz(Z) > spdensity*n2)
                  Z = full(Z);
               else
                  Z = sparse(Z);
               end
            elseif (m > 1 & n == 1);
               Z = 1./X;
               if (nnz(Z) > spdensity*n)
                  Z = full(Z);
	       else
	          Z = sparse(Z);
               end
            end
         else
            Z = cell(size(X));
            for p = 1:length(X);
               [m,n] = size(X{p}); n2 = n*n;
               if (m==n)
                  Z{p} = inv(X{p});
                  if (nnz(Z{p}) > spdensity*n2)
                     Z{p} = full(Z{p});
		  else
                     Z{p} = sparse(Z{p});
                  end
               elseif (m > 1 & n == 1);
                  Z{p} = 1./X{p};
                  if (nnz(Z{p}) > spdensity*n)
                     Z{p} = full(Z{p});
		  else
                     Z{p} = sparse(Z{p});
                  end
               end
            end
         end
      elseif strcmp(operand,'getM');
         if ~iscell(X);
            Z = size(X,1);
         else
            for p = 1:length(X); Z(p) = size(X{p},1); end;
            Z = sum(Z);
         end;
      elseif strcmp(operand,'nnz');
         if ~iscell(X);
            Z = nnz(X);
         else;
            for p = 1:length(X);
               Z(p) = nnz(X{p});
            end;
            Z = sum(Z);
         end;
      elseif strcmp(operand,'ones');
         if ~iscell(X);
            Z = ones(size(X));
         else
            Z = cell(size(X));
            for p = 1:length(X);
                Z{p} = ones(size(X{p}));
            end
         end
      elseif strcmp(operand,'zeros');
         if ~iscell(X);
            [m,n] = size(X);
            Z = sparse(m,n);
         else
            Z = cell(size(X));
            for p = 1:length(X);
               [m,n] = size(X{p});
               Z{p} = sparse(m,n);
            end
         end
      elseif strcmp(operand,'identity');
         blk = X;
         Z = cell(size(blk,1),1);
         for p = 1:size(blk,1)
            pblk = blk(p,:); n = sum(pblk{2});
            if strcmp(pblk{1},'s')
               Z{p} = speye(n,n);
            elseif strcmp(pblk{1},'q')
               s = 1+[0, cumsum(pblk{2})];
               len = length(pblk{2});
               Z{p} = zeros(n,1);
               Z{p}(s(1:len)) = ones(len,1);
            elseif strcmp(pblk{1},'l')
               Z{p} = ones(n,1);
            elseif strcmp(pblk{1},'u')
               Z{p} = zeros(n,1);
            end
         end
      elseif strcmp(operand,'row-norm');
         if ~iscell(X);
            if (size(X,2) == size(X,1));
               Z = sqrt(sum((X.*conj(X))'))';
            elseif (size(X,2) == 1);
               Z = abs(X);
            end
         else
            Z = cell(size(X));
            for p = 1:length(X);
               if (size(X{p},2) == size(X{p},1));
                  Z{p} = sqrt(sum((X{p}.*conj(X{p}))'))';
               elseif (size(X{p},2) == 1);
                  Z{p} = abs(X{p});
               end
            end
         end
      end
   end
%%
   if (nargin == 3)
      if strcmp(operand,'spdiags');
         if ~iscell(Y);
            [m,n] = size(Y);
            if (m == n);
               Z = spdiags(X,0,m,n);
            else
               Z = X;
            end
         else
            Z = cell(size(Y));
            for p = 1:length(Y);
               [m,n] = size(Y{p});
               if (m == n);
                  Z{p} = spdiags(X{p},0,m,n);
               else;
                  Z{p} = X{p};
               end
            end
         end
      elseif strcmp(operand,'inprod')
         if ~iscell(X) & ~iscell(Y)
    	    Z = (Y'*X)';
	 elseif iscell(X) & iscell(Y)
    	    Z = zeros(size(X{1},2),1);
   	    for p=1:length(X)
	        Z = Z + (Y{p}'*X{p})';
            end
         end
      elseif strcmp(operand,'+') | strcmp(operand,'-') | ...
             strcmp(operand,'/') | strcmp(operand,'./') | ...
             strcmp(operand,'*') | strcmp(operand,'.*') | ...
             strcmp(operand,'.^');
         if (~iscell(X) & ~iscell(Y));
            eval(['Z = X',operand,'Y;']);
         elseif (iscell(X) & iscell(Y))
            Z = cell(size(X));
            for p = 1:length(X);
 	       if (size(X{p},2) == 1) & (size(Y{p},2) == 1) & ...
                  (strcmp(operand,'*') | strcmp(operand,'/'));
                  eval(['Z{p} = X{p}.',operand,'Y{p};']);
               else
                  eval(['Z{p} = X{p} ',operand,'Y{p};']);
               end
            end
         elseif (iscell(X) & ~iscell(Y));
	    if (length(Y) == 1); Y = Y*ones(length(X),1); end
            Z = cell(size(X));
            for p = 1:length(X);
                eval(['Z{p} = X{p}',operand,'Y(p);']);
            end
         elseif (~iscell(X) & iscell(Y));
            Z = cell(size(Y));
	    if (length(X) == 1); X = X*ones(length(Y),1); end
            for p = 1:length(Y);
                eval(['Z{p} = X(p)',operand,'Y{p};']);
            end
         end
      else
         error([operand,' is not available, check input arguments']);
      end
   end
%%
   if (nargin == 4)
      if strcmp(operand,'rank1') | strcmp(operand,'rank1inv');
         Z = cell(size(alpha,1),1);
         for p = 1:size(alpha,1);
            if ~strcmp(alpha{p,1},'diag');
               blktmp = alpha{p,2};
               if (length(blktmp) == 1);
                  if strcmp(operand,'rank1');
                     Z{p} = (X{p}*Y{p}' + Y{p}*X{p}')/2;
                  else;
                     Z{p} = 2./(X{p}*Y{p}' + Y{p}*X{p}');
                  end
               else
                  Xp = X{p};
                  Yp = Y{p};
                  n = sum(blktmp);
                  Zp = sparse(n,n);
                  s = [0 cumsum(blktmp)];
                  if strcmp(operand,'rank1');
                     for i = 1:length(blktmp)
                        pos = [s(i)+1 : s(i+1)];
                        x = Xp(pos);
                        y = Yp(pos);
                        Zp(pos,pos) = sparse((x*y' + y*x')/2);
                     end;
                     Z{p} = Zp;
                  else
                     for i = 1:length(blktmp)
                        pos = [s(i)+1 : s(i+1)];
                        x = Xp(pos);
                        y = Yp(pos);
                        Zp(pos,pos) = sparse(2./(x*y' + y*x'));
                     end
                     Z{p} = Zp;
                  end
               end
            elseif strcmp(alpha{p,1},'diag');
               if strcmp(operand,'rank1');
                  Z{p} = X{p}.*Y{p};
               else
                  Z{p} = 1./(X{p}.*Y{p});
               end
            end
         end
      elseif strcmp(operand,'+') | strcmp(operand,'-');
         if ~iscell(X) & ~iscell(Y);
            eval(['Z = X',operand,'alpha*Y;']);
         elseif (iscell(X) & iscell(Y));
            Z = cell(size(X));
	    if (length(alpha) == 1);
               alpha = alpha*ones(length(X),1);
            end
            for p = 1:length(X);
               eval(['Z{p} = X{p}',operand,'alpha(p)*Y{p};']);
            end
         else
            error('X, Y are different objects');
         end
      else
         error([operand,' is not available']);
      end
   end
%%============================================================


%   SCRIPT TO COMPUTE THE TORQUES AT EACH JOINT FOR DIFFERENT MOTION STATES OF
%   a 3 DOF robotic arm.
%
%   In order to select an actuator (i.e. an electric motor, brushless for
%   example), we may need.
%   Nominal torque and speed: torque and speed for the 80% of the use of the motor.
%   Peak torque and speed: torque and speed for short periods of time. That
%   is: a higher torque that can be exerted at higher speeds during a
%   maximum of a 20% of the time.
%
%   Of course, the 80-20% are just bare numbers and should be given by the
%   robot manufacturer. Actually the torque in any motor depends directly
%   on the quantity of current that can be driven into the motor.
%   Typically, the peak torque is associated with a peak current. If the
%   peak current is maintained for a long time the motor will not be able
%   to dissipate the heat inside, thus generating high temperatures that
%   could melt the coils, conductors... etc.
%
%   The script uses the inverse dynamic model of the robot to simulate
%   different motion states and compute the torques for each situation.
%   The torques at each joint, as well as the torques at each motor are computed
%   (reduced by the gear ratio). A trapezoidal speed profile can be
%   selected at the parameters section of this file. This trapezoidal
%   profile should meet the desired features of the robot, such as maximum
%   joint speed, maximum acceleration/deceleration.
%   Please note that the movement of the robot is not simulated. The reader
%   should imagine that the manipulator is fixed at a given initial
%   position and the inverse dynamic function returns the torques at each
%   joint necessary to bring the arm to that motion state (defined by position q,
%   speed qd and acceleration qdd).
%
%   As a reference, below you can find the call to the inverse dynamics
%   function.
%
%   TAU = inversedynamic(robot, Q, QD, QDD, GRAV, FEXT)
%
% Copyright (C) 2012, by Arturo Gil Aparicio
%
% 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 motor_selection_3dofplanar

close all
global robot
robot.dynamics.friction = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETERS SECTION
%   Feel free to change the values of q, maximum_speeds and
%   maximum_accels.
%
%   This script tries to allow the student to test any mechanism
%   at the worst case. In this sense, q should be adjusted
%   as the pose where each joint would (statically) be needing a
%   higher torque.
%   The maximum_speeds and maximum_acceleration define a trapezoidal
%   speed profile. This trapezoidal speed is used by most machines
%   to command changes in speed in any of their joints.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% robot pose: experiment by changing the pose while observing the different
%             torques at each joint
q=[0 0 0]; %rad
%maximum speeds at for each joint
maximum_speeds=[pi pi pi];%rad/second
%maximum acceleration/deceleration for each joint
maximum_accels=[pi/4 pi/4 pi/4]; %rad/second^2

% time of the trapezoidal profile that the joint moves at maximum speed
time_at_constant_speed=2; %seconds

%load robot parameters. Just uncomment this line
robot=load_robot('example', '3dofplanar');
drawrobot3d(robot, q)

% CUIDADO: los resultados se calculan con estas ratios.
robot.motors.G = [1 1 1]

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  FIRST, COMPUTE TRAPEZOIDAL PROFILES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%compute acceleration plus deceleration times for every joint
time_acc = 2*maximum_speeds./maximum_accels+time_at_constant_speed;

%compute the total time for the slowest joint
total_time=max(time_acc);

% Trapezoidal speed profiles for each joint
[input_speeds, input_accels, time]=build_trapezoidal_speed_profile(maximum_speeds, maximum_accels, total_time);

% EJERCICIO:
% REALIZA UN PLOT DE input_speeds e input_accels.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FINALLY, COMPUTE TORQUES FOR EACH MOTION STATE.
% Please note that we consider that the robot is placed at a fixed position and consider
% different motion situations when we change the acceleration and speed at
% each joint. For each motion state, the inverse dynamic model returns the
% torques at each joint that would bring the robot to that motion.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
compute_inverse_dynamics(q, input_speeds, input_accels, time);

%Computes a trapezoidal speed profile for every joint given maximum
%permitted accelerations and maximum joint speeds
function [input_speeds, input_accelerations, time]=build_trapezoidal_speed_profile(maximum_speeds, maximum_accels, total_time)

delta_time=0.01;

%build time vector: twice acceleration time plus time at constant speed
time = 0:delta_time:total_time;

input_speeds=[];
input_accelerations=[];

for j=1:length(maximum_speeds),
    vel_row=[];
    acc_row=[];
    for i=1:length(time),
        [vel acc] = compute_values(time(i), maximum_speeds(j), maximum_accels(j), total_time);
        vel_row = [vel_row vel];
        acc_row = [acc_row acc];
    end
    input_speeds = [input_speeds; vel_row];
    input_accelerations = [input_accelerations; acc_row];
end

%returns the values of velocity and speed corresponding to a given time
function [vel acc]=compute_values(time_i, vel_max, acc_max, total_time)

tacc = vel_max/acc_max;
tdec = total_time-tacc;

if time_i < tacc
    vel = time_i.*acc_max;
    acc = acc_max;
    return;
elseif (time_i >= tacc) & (time_i < tdec)
    vel = vel_max;
    acc = 0;
    return;
else % time_i> tdec
    vel = vel_max-(time_i-tdec)*acc_max;
    acc = -acc_max;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   COMPUTE THE INVERSE DYNAMICS FOR EACH MOTION STATE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function compute_inverse_dynamics(q, input_speeds, input_accels, time)
global robot

%adjust_view(robot)
torques=[];
for j=1:length(time)
    fprintf('\nComputing time %d out of %d', j, length(time));
    % compute the torque to bring the robot instantaneously to this motion
    % state. change M=1  to add the effects of a 1kg mass load at the end effector
    M=0.1;
    %please note that the force due to the load acts on the z axis of
    tau=inversedynamic(robot, q, input_speeds(:,j), input_accels(:,j), [0  -9.81 0]', [0 -M*9.81 0 0 0 0]');
    torques=[torques tau];
end

%plot trapezoidal profiles
figure, hold, xlabel('time (s)'), ylabel('Input reference speeds (rad/s)')
plot(time, input_speeds(1,:), time, input_speeds(2,:), time, input_speeds(3,:) );
legend('Speed for joint 1 (qd1)','Speed for joint 2 (qd2)', 'Speed for joint 3 (qd3)')
%plot trapezoidal profiles, acceleration
figure, hold, xlabel('time (s)'), ylabel('Input reference acceleration (rad/s)')
plot(time, input_accels(1,:), time, input_accels(2,:), time, input_accels(3,:));
legend('Acceleration for joint 1 (qd1)','Acceleration for joint 2 (qd2)', 'Acceleration for joint 3 (qd3)')

% plot results. First, torques at each joint
figure, hold, xlabel('time (s)'), ylabel('Join Torques (N m)')
plot(time, torques(1,:), time, torques(2,:));
legend('Torque for joint 1 ','Torque  for joint 2 ')

% OTRA INFORMACION QUE ES ESENCIAL QUE PLOTEES:
% - Los pares en cada motor (utiliza robot.motors.G(1), .G(2)...

% - La potencia instantánea que realiza cada motor (la potencia es tau*w en
% cada articulación). La potencia pico del motor nos dará una idea de qué
% motor buscar en el catálogo.

% - Plotea la velocidad de cada motor en rpm: en efecto, los fabricantes
% tienen la costumbre de expresar la velocidad del motor en rpm y no en
% rad/s. Los fabricantes de robots tienen la costumbre de expresar las
% velocidades articulares en grados/s. Se te sugiere que plotees las
% velocidades de cada motor en rpm.

% Finalmente, en base al resultado, de cada motor, expresa:
% - El par pico (valor máximo en valor absoluto del par realizado por el motor).
% - El par nominal. Es el par en el punto intermedio del resultado del vector torques.
% - Velocidad nominal del motor en rpm.
% - La velocidad máxima en rpm del motor.


% DESCRIPTION:
%       subscript to display time steps and maximum supported frequency.
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 8th July 2014
%       last update - 8th July 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave 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.
%
% k-Wave 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 Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% display time step information
disp(['  dt: ' scaleSI(dt) 's, t_end: ' scaleSI(kgrid.t_array(end)) 's, time steps: ' num2str(length(kgrid.t_array))]);

% if using the elastic code, get the minimum sound speeds (not including
% zero if set for the shear speed)
if ~elastic_code
    c_min = min(medium.sound_speed(:));
else
    c_min_comp = min(medium.sound_speed_compression(:));
    c_min_shear = min(medium.sound_speed_shear(medium.sound_speed_shear ~= 0));
end

% get suitable scaling factor
grid_size_metric = [kgrid.x_size, kgrid.y_size, kgrid.z_size];
[x_sc, scale, prefix] = scaleSI( min(grid_size_metric(grid_size_metric ~= 0)) ); %#ok<*ASGLU>
clear grid_size_metric;

% display the grid size and maximum supported frequency
switch kgrid.dim
    case 1

        % display grid size
        disp(['  input grid size: ' num2str(kgrid.Nx) ' grid points (' scaleSI(kgrid.x_size) 'm)']);

        % display maximum supported frequency
        disp(['  maximum supported frequency: ' scaleSI( kgrid.k_max * c_min / (2*pi) ) 'Hz']);

    case 2

        % display grid size
        disp(['  input grid size: ' num2str(kgrid.Nx) ' by ' num2str(kgrid.Ny) ' grid points (' num2str(kgrid.x_size*scale) ' by ' num2str(kgrid.y_size*scale) prefix 'm)']);

        if ~elastic_code

            % display maximum supported frequency
            if kgrid.kx_max == kgrid.ky_max
                disp(['  maximum supported frequency: ' scaleSI( kgrid.k_max * c_min / (2*pi) ) 'Hz']);
            else
                disp(['  maximum supported frequency: ' scaleSI( kgrid.kx_max * c_min / (2*pi) ) 'Hz by ' scaleSI( kgrid.ky_max * c_min / (2*pi) ) 'Hz']);
            end

        else

            % display the maximum supported frequency
            if kgrid.kx_max == kgrid.ky_max
                disp(['  maximum supported compressional frequency: ' scaleSI( kgrid.k_max * c_min_comp / (2*pi) ) 'Hz']);
                if isempty(c_min_shear)
                    disp('  maximum supported shear frequency: 0Hz');
                else
                    disp(['  maximum supported shear frequency: ' scaleSI( kgrid.k_max * c_min_shear / (2*pi) ) 'Hz']);
                end
            else
                disp(['  maximum supported compressional frequency: ' scaleSI( kgrid.kx_max * c_min_comp / (2*pi) ) 'Hz by ' scaleSI( kgrid.ky_max * c_min_comp / (2*pi) ) 'Hz']);
                if isempty(c_min_shear)
                    disp('  maximum supported shear frequency: 0Hz');
                else
                    disp(['  maximum supported compressional frequency: ' scaleSI( kgrid.kx_max * c_min_shear / (2*pi) ) 'Hz by ' scaleSI( kgrid.ky_max * c_min_shear / (2*pi) ) 'Hz']);
                end
            end

        end

    case 3

        % display grid size
        disp(['  input grid size: ' num2str(kgrid.Nx) ' by ' num2str(kgrid.Ny) ' by ' num2str(kgrid.Nz) ' grid points (' num2str(kgrid.x_size*scale) ' by ' num2str(kgrid.y_size*scale) ' by ' num2str(kgrid.z_size*scale) prefix 'm)']);

        if ~elastic_code

            % display maximum supported frequency
            if (kgrid.kx_max == kgrid.kz_max) && (kgrid.kx_max == kgrid.ky_max)
                disp(['  maximum supported frequency: ' scaleSI( kgrid.k_max * c_min / (2*pi) ) 'Hz']);
            else
                disp(['  maximum supported frequency: ' scaleSI( kgrid.kx_max * c_min / (2*pi) ) 'Hz by ' scaleSI( kgrid.ky_max * c_min / (2*pi) ) 'Hz by ' scaleSI( kgrid.kz_max * c_min / (2*pi) ) 'Hz']);
            end

        else

            % display the maximum supported frequency
            if (kgrid.kx_max == kgrid.kz_max) && (kgrid.kx_max == kgrid.ky_max)
                disp(['  maximum supported compressional frequency: ' scaleSI( kgrid.k_max * c_min_comp / (2*pi) ) 'Hz']);
                if isempty(c_min_shear)
                    disp('  maximum supported shear frequency: 0Hz');
                else
                    disp(['  maximum supported shear frequency: ' scaleSI( kgrid.k_max * c_min_shear / (2*pi) ) 'Hz']);
                end
            else
                disp(['  maximum supported frequency: ' scaleSI( kgrid.kx_max * c_min_comp / (2*pi) ) 'Hz by ' scaleSI( kgrid.ky_max * c_min_comp / (2*pi) ) 'Hz by ' scaleSI( kgrid.kz_max * c_min_comp / (2*pi) ) 'Hz']);
                if isempty(c_min_shear)
                    disp('  maximum supported shear frequency: 0Hz');
                else
                    disp(['  maximum supported frequency: ' scaleSI( kgrid.kx_max * c_min_shear / (2*pi) ) 'Hz by ' scaleSI( kgrid.ky_max * c_min_shear / (2*pi) ) 'Hz by ' scaleSI( kgrid.kz_max * c_min_shear / (2*pi) ) 'Hz']);
                end
            end

        end
end

% cleanup unused variables
clear c_min c_min_comp c_min_shear grid_size_metric


function [inerr,dw,inLL,right ]=batch_equal_nomask_lstm(W,dataall  )
% gradient threshold for bptt need done   ,inLL,right
global   mzeros convert in_size gate_size out_size share_size usegpu...
    share_size2  in ingate cellstate cells outgate globalData    ...
    node_outgateInit cellinInit node_cellbiasInit delta_outInit
if nargin==1
    data=globalData;
else
    data=dataall;
end
if usegpu
    fun=@sigmoidnGpu;
    delta_fun=@delta_sigmoidnGpu;
    activation=@activationGpu;
    deactivation=@deactivationGpu;
else
    fun=@sigmoidnCpu;
    delta_fun=@delta_sigmoidnCpu;
    activation=@activationCpu;
    deactivation=@deactivationCpu;
end

actNum1=convert(1);
actNum2=2*actNum1;
numsamples=size(data,1) ;

dw= zeros(1 ,2*share_size2 +share_size2*numMmcell + (gate_size*numMmcell+1)*out_size);
[ Wingate,Wcell , Woutgate, Wout]=unpack(W);

[dwingate,dwcell,dwoutgate,dwout] = unpack2(dw);
dwingate=convert(dwingate);
dwcell=convert(dwcell);
dwoutgate=convert(dwoutgate);
dwout=convert(dwout);

right=zeros(numsamples,1,'int32');
inLL = 0;
inerr = zeros( numsamples,out_size);

timespan = size( data  , 3 );
node_tmpindex=cell(1,timespan);
node_tmpindex2=cell(1,timespan);
node_tmpindex3=cell(1,timespan);

node_outgate= node_outgateInit;
cellin=cellinInit;
node_cellbias=node_cellbiasInit;
delta_out=delta_outInit;
cellstatus= cellstatusInit;

node_ingate = node_outgate;
node_cell = cellin;
Y_cellout = cellin;
delta_outgate=node_outgate;

errorstate=cellin;
delta_cellin=cellin;
delta_ingate=node_outgate;

Wingate_in=convert( Wingate(in,:) );
Wingate_ingate= convert( Wingate(ingate,:) );
Wingate_cellstate=convert( Wingate(cellstate,:) );
Wingate_cell=convert( Wingate(cells,:) );
Wingate_outgate=convert( Wingate(outgate,:) );

% to cellin
Wcell_in=convert( Wcell(in,:) ) ;
Wcell_ingate=convert( Wcell(ingate,:) );
Wcell_cell=convert( Wcell(cells,:) );
Wcell_outgate=convert( Wcell(outgate,:) );

% to outgate
Woutgate_in=convert( Woutgate(in,:) );
Woutgate_ingate=convert( Woutgate(ingate,:) );
Woutgate_cellstate=convert( Woutgate(cellstate,:) );
Woutgate_cell = convert( Woutgate(cells,:) );
Woutgate_outgate=convert( Woutgate(outgate,:) );

t=2;
while t<=timespan

    output = data(:,1:out_size,t ) ;

    % forward pass
    node_in = data(:,1:in_size,t-1);
    node_cellbias{t } = data(:,in_size+1,t-1);

    % to ingate
    tmpp=node_in*Wingate_in + node_outgate{t-1}*Wingate_outgate +...
        cellstatus{t-1} * Wingate_cellstate + node_cell{t-1}*Wingate_cell +  node_ingate{t-1} * Wingate_ingate;
    node_ingate{t}=fun( tmpp ,signum);

    tmpp=node_in*Wcell_in +  node_ingate{t-1}  * Wcell_ingate  +...
        + node_cell{t-1}*Wcell_cell +node_outgate{t-1} * Wcell_outgate ;
    cellin{t} = activation( tmpp,actNum2 ) ;

    cellstatus{t}  =  cellstatus{t-1}  +    cellin{t }  .* repmat(node_ingate{t},1,numMmcell )  ;

    Y_cellout{t}= activation( cellstatus{t } ,actNum1 ) ;

    tmpp=node_in*Woutgate_in +   node_outgate{t-1} *Woutgate_outgate +...
        cellstatus{t}  * Woutgate_cellstate + node_cell{t-1}*Woutgate_cell +  node_ingate{t-1} * Woutgate_ingate;
    node_outgate{t }  = fun(tmpp  ,signum);

    node_cell{t }  = repmat(node_outgate{t},1,numMmcell )   .* Y_cellout{t }  ;

    node_tmpindex{t } = [ node_in  node_ingate{t-1} cellstatus{t-1} node_cell{t-1} node_outgate{t-1} ];
    node_tmpindex2{t} = [ node_in  node_ingate{t-1} cellstatus{t} node_cell{t-1} node_outgate{t-1} ];
    node_tmpindex3{t } = [ node_in  node_ingate{t-1}  node_cell{t-1} node_outgate{t-1} ];

    node_out  = fun(  [ node_cell{t}     node_cellbias{t}   ] * Wout    ,signum ) ;

    delta_out{t} =  ( - output + node_out  )  .* delta_fun(node_out,signum);

    inerr = inerr +     (  (output  - node_out  )).^2    ;
    %       right = right +   rightfun(masko,output,node_out) ;
    t=t+1;
end
t=t-1;
% just  cell
tmp_outgate=repmat(node_outgate{t},1,numMmcell);
tmp_ingate=repmat(node_ingate{t},1,numMmcell);
errorcell = delta_out{t} * Wout(1:end-1,:)';

% just cell to outgate
delta_outgate{t} =  squeezing(delta_fun( tmp_outgate,signum) .* errorcell  .* Y_cellout{t})  ;  %  .* nodenew_outgate.* ( 1 - nodenew_outgate  )  ;
% peephole
errorstate{t} = errorcell .* tmp_outgate .* deactivation(Y_cellout{t},actNum1)+...
    delta_outgate{t} * Woutgate_cellstate' ;
delta_cellin{t} =tmp_ingate .* errorstate{t}.* deactivation(cellin{t},actNum2);
delta_ingate{t} =  squeezing(cellin{t} .* errorstate{t} .* delta_fun(tmp_ingate,signum));
for t = timespan-1:-1:2
    % just  cell
    tmp_outgate=repmat(node_outgate{t},1,numMmcell);
    tmp_ingate=repmat(node_ingate{t},1,numMmcell);
    errorcell = delta_out{t } * Wout(1:end-1,:)' + delta_outgate{t+1} * Woutgate_cell' +...
        delta_cellin{t+1} * Wcell_cell' + delta_ingate{t+1} * Wingate_cell'; % W( out , %cell);

    % just cell to outgate
    delta_outgate{t} = delta_fun(  node_outgate{t},signum )  .* ( squeezing(errorcell  .* Y_cellout{t})  +...
        delta_ingate{t+1} * Wingate_outgate'+ delta_cellin{t+1} * Wcell_outgate'+...
        delta_outgate{t+1} *Woutgate_outgate') ;  %  .* nodenew_outgate.* ( 1 - nodenew_outgate  )  ;
    % peephole
    errorstate{t} = errorcell .* tmp_outgate .* deactivation( Y_cellout{t} ,actNum1 )  + ...
        errorstate{t+1} + delta_ingate{t+1}* Wingate_cellstate' +...
        delta_outgate{t} * Woutgate_cellstate' ;

    delta_cellin{t} = tmp_ingate .* deactivation( cellin{t } ,actNum2) .* errorstate{t} ;

    delta_ingate{t} = delta_fun( node_ingate{t} ,signum).* ( squeezing(cellin{t} .* errorstate{t})  +...
        + delta_ingate{t+1} * Wingate_ingate' +delta_cellin{t+1} * Wcell_ingate' + ...
        delta_outgate{t+1} * Woutgate_ingate' );
end

for t = 2:timespan
    dwout = dwout + [node_cell{t} node_cellbias{t} ]' *  delta_out{t} ;
    dwoutgate =dwoutgate + node_tmpindex2{t}' * delta_outgate{t} ;
    dwcell=dwcell+ node_tmpindex3{t}'*delta_cellin{t};
    dwingate = dwingate + node_tmpindex{t}'  * delta_ingate{t}   ;
end

inerr = gather(  1/2*    sum(    inerr)   /numsamples)  ;%  1/2*  for gradient checking
%right=gather(sum(right));

dw=pack2(dwingate,dwcell ,dwoutgate,dwout);
dw = gather(dw / numsamples );

    function [Wingate,Wcell ,Woutgate,Wout] = unpack(W)
        Wingate= reshape( W( 1:share_size2 )   , share_size ,gate_size );
        Wcell  = reshape( W( share_size2 +1 :   share_size2 *(1+numMmcell) ),  share_size, numMmcell*gate_size);
        Woutgate = reshape( W( (1+numMmcell) * share_size2 +1 : (2+numMmcell)* share_size2  ), share_size , gate_size);
        Wout = reshape( W( (2+numMmcell) * share_size2 +1 : end ) ,gate_size*numMmcell+1 ,  out_size);
    end
    function [Wingate,Wcell ,Woutgate,Wout] = unpack2(W)
        Wingate= reshape( W( 1:share_size2 )   , share_size ,gate_size );
        % Wcell  = reshape( W( share_size2 +1 : 2* share_size2  ),  share_size,gate_size);
        Wcell  = reshape( W( share_size2 +1 :   share_size2 *(1+numMmcell) ),  share_size, numMmcell*gate_size);
        %   share_size = in_size + gate_size * (2+2*problem.numMmcell) ;
        Wcell =  Wcell([1:in_size+gate_size (in_size+gate_size+gate_size*numMmcell+1):end],:) ;
        Woutgate = reshape( W( (1+numMmcell) * share_size2 +1 : (2+numMmcell)* share_size2  ), share_size , gate_size);
        Wout = reshape( W( (2+numMmcell) * share_size2 +1 : end ) ,gate_size*numMmcell+1 ,  out_size);
    end
    function [W]=pack2(Wingate,Wcell , Woutgate,Wout)
        tmp=mzeros(share_size,gate_size*numMmcell);
        tmp([1:in_size+gate_size (in_size+gate_size+gate_size*numMmcell+1):end],:)=Wcell;
        Wcell=tmp;
        W = [Wingate(:);Wcell(:) ;Woutgate(:);Wout(:)];
    end

    function y=sigmoidnGpu(x,num)
        y=arrayfun(@(x,num)1./(1+exp(- num*x)),x,num);
    end
    function y=delta_sigmoidnGpu(x,num)
        y=arrayfun(@(x,num)num*x.*(1-x),x,num);
    end
    function y=sigmoidnCpu(x,num)
        y=1./(1+exp(- num*x));
    end
    function y=delta_sigmoidnCpu(x,num)
        y=  num*x.*(1-x);
    end
    function y =  activationCpu(x,num)
        y=num*2./(1+exp(-x))-num;
    end
    function y =  deactivationCpu(x,num)
        y=0.5/num*(num+x).*(num-x);
    end
    function y =  activationGpu(x,num)
        y= arrayfun(@(x,num)num*2./(1+exp(-x))-num,x,num);
    end
    function y =  deactivationGpu(x,num)
        y=arrayfun(@(x,num)0.5/num*(num+x).*(num-x),x,num);
    end
    function y = squeezing(x)
        y=zeros(size(x,1),gate_size);
        for i = 1:numMmcell
            y = y + x(:,1+(i-1)*gate_size: i*gate_size);
        end
        %         x=mat2cell(x,size(x,1),gate_size*ones(1,numMmcell));
        %
        %         for i=2:numMmcell
        %             x{1}=x{1}+x{i};
        %         end
        %         y=x{1};
        %    y= cell2mat(cellfun(@plus  ,x, 'UniformOutput',false));
    end

end


function [mix, num_iter, ll] = gmmem_kpm(mix, x, varargin)
%GMMEM_KPM Like GMMEM, but with additional optional arguments
% function [mix, num_iter, ll] = gmmem_kpm(mix, x, varargin)
%
% Input:
% mix - structure created by gmminit or gmmem_multi_restart
% data - each row is an example
%
% Output:
% mix - modified structure
% num_iter - number of iterations needed to reach convergence
% ll - final log likelihood
%
% [ ... ] = gmmem_kpm(..., 'param1',val1, 'param2',val2, ...) allows you to
% specify optional parameter name/value pairs.
% Parameters are below [default value in brackets]
%
% 'max_iter' - maximum number of EM iterations [10]
% 'll_thresh' - change in log-likelihood threshold for convergence [1e-2]
% 'verbose' - 1 means display output while running [0]
% 'prior_cov' - this will be added to each estimated covariance
%               to prevent singularities  [1e-3*eye(d)]
% 'fn'        - this function, if non-empty,  will be called at every iteration
%               (e.g., to display the parameters as they evolve) [ [] ]
%               The fn is called as fn(mix, x, iter_num, fnargs).
%               It is also called before the iteration starts as
%               fn(mix, x, -1, fnargs), which can be used to initialize things.
% 'fnargs'    - additional arguments to be passed to fn [ {} ]
%
% Modified by Kevin P Murphy, 29 Dec 2002

% Check that inputs are consistent
errstring = consist(mix, 'gmm', x);
if ~isempty(errstring)
  error(errstring);
end

[ndata, xdim] = size(x);

[max_iter, ll_thresh, verbose, prior_cov, fn, fnargs] = ...
    process_options(varargin, ...
	'max_iter', 10, 'll_thresh', 1e-2, 'verbose', 1, ...
	'prior_cov', 1e-3*eye(xdim), 'fn', [], 'fnargs', {});

options = foptions;
if verbose, options(1)=1; else options(1)=-1; end
options(14) = max_iter;
options(3) = ll_thresh;

% Sort out the options
if (options(14))
  niters = options(14);
else
  niters = 100;
end

display = options(1);
test = 0;
if options(3) > 0.0
  test = 1;	% Test log likelihood for termination
end

check_covars = 0;
if options(5) >= 1
  if display >= 0
    disp('check_covars is on');
  end
  check_covars = 1;	% Ensure that covariances don't collapse
  MIN_COVAR = eps;	% Minimum singular value of covariance matrix
  init_covars = mix.covars;
end

mix0 = mix; % save init values for debugging

if ~isempty(fn)
  feval(fn, mix, x, -1, fnargs{:});
end

% Main loop of algorithm
for n = 1:niters

  % Calculate posteriors based on old parameters
  [post, act] = gmmpost(mix, x);

  % Calculate error value if needed
  if (display |  test)
    prob = act*(mix.priors)';
    % Error value is negative log likelihood of data
    e = - sum(log(prob + eps));
    if display > 0
      fprintf(1, 'Cycle %4d  Error %11.6f\n', n, e);
    end
    if test
      if (n > 1 & abs(e - eold) < options(3))
        options(8) = e;
	ll = -e;
	num_iter = n;
        return; %%%%%%%%%%%%%%%% Exit here if converged
      else
        eold = e;
      end
    end
  end

  if ~isempty(fn)
    feval(fn, mix, x, n, fnargs{:});
  end

  % Adjust the new estimates for the parameters
  new_pr = sum(post, 1);
  new_c = post' * x;

  % Now move new estimates to old parameter vectors
  mix.priors = new_pr ./ ndata;

  mix.centres = new_c ./ (new_pr' * ones(1, mix.nin));

  switch mix.covar_type
  case 'spherical'
    n2 = dist2(x, mix.centres);
    for j = 1:mix.ncentres
      v(j) = (post(:,j)'*n2(:,j));
    end
    mix.covars = ((v./new_pr) + sum(diag(prior_cov)))./mix.nin;
    if check_covars
      % Ensure that no covariance is too small
      for j = 1:mix.ncentres
        if mix.covars(j) < MIN_COVAR
          mix.covars(j) = init_covars(j);
        end
      end
    end
  case 'diag'
    for j = 1:mix.ncentres
      diffs = x - (ones(ndata, 1) * mix.centres(j,:));
      wts = (post(:,j)*ones(1, mix.nin));
      mix.covars(j,:) = sum((diffs.*diffs).*wts + prior_cov, 1)./new_pr(j);
    end
    if check_covars
      % Ensure that no covariance is too small
      for j = 1:mix.ncentres
        if min(mix.covars(j,:)) < MIN_COVAR
          mix.covars(j,:) = init_covars(j,:);
        end
      end
    end
  case 'full'
    for j = 1:mix.ncentres
      diffs = x - (ones(ndata, 1) * mix.centres(j,:));
      diffs = diffs.*(sqrt(post(:,j))*ones(1, mix.nin));
      mix.covars(:,:,j) = (diffs'*diffs + prior_cov)/new_pr(j);
    end
    if check_covars
      % Ensure that no covariance is too small
      for j = 1:mix.ncentres
        if min(svd(mix.covars(:,:,j))) < MIN_COVAR
          mix.covars(:,:,j) = init_covars(:,:,j);
        end
      end
    end
  case 'ppca'
    for j = 1:mix.ncentres
      diffs = x - (ones(ndata, 1) * mix.centres(j,:));
      diffs = diffs.*(sqrt(post(:,j))*ones(1, mix.nin));
      [mix.covars(j), mix.U(:,:,j), mix.lambda(j,:)] = ...
        ppca((diffs'*diffs)/new_pr(j), mix.ppca_dim);
    end
    if check_covars
      if mix.covars(j) < MIN_COVAR
        mix.covars(j) = init_covars(j);
      end
    end
    otherwise
      error(['Unknown covariance type ', mix.covar_type]);
  end
end

ll = sum(log(gmmprob(mix, x)));
num_iter = n;

%if (display >= 0)
%  disp('Warning: Maximum number of iterations has been exceeded');
%end


function [vs, output, v0] = computeConfidenceInterval2(v0, expdata, model, max_score)

majorIterationLimit = 2000;  %max number of iterations
minorIterationLimit = 1e7;   % essentially infinity
diffInterval = 1e-5;         %gradient step size.
feasibilityTolerance = max_score/20; % how close you need to be to the max score.

v0 = fitC13Data(v0,expdata,model);

if ~isfield(model, 'N')
   model.N = null(model.S);
end

x0 = model.N\v0; % back substitute

% safety check:
if (max(abs(model.S*v0))> 1e-6)
    display('v0 not quite in null space');
    pause;
end
if(max(abs(model.N*x0 - v0)) > 1e-6)
    display('null basis is weird');
    pause;
end

nalpha = size(model.N, 2);
x_L = -1000*ones(nalpha,1);
x_U = 1000*ones(nalpha,1);
Name = 't2';
[A, b_L, b_U] = defineLinearConstraints(model);

numpoints = size(x0,2);
scores = zeros(numpoints,1);

% compute scores for all points.
tProb.user.expdata = expdata;
tProb.user.model = model;
for i = 1:numpoints
    scores(i) = errorComputation2(x0(:,i),tProb);
end
valid_index = scores < max_score + feasibilityTolerance;
fprintf('found %d valid points\n', sum(valid_index));

x0_valid = x0(:,valid_index);
x0_invalid = x0;
scores_valid = scores(valid_index);

c = []; c_L = []; c_U = []; HessPattern = [];
f = 'errorComputation2';
g = 'errorComputation2_grad'; H = [];
%c = 'errorComputation2';
%c_L = 0;
%c_U = max_score;

dc = []; d2c = []; ConsPattern = [];
pSepFunc = [];
x_min = []; x_max = []; f_opt = []; x_opt = [];
Solver = 'snopt';

% pre-compute unnecesary directions.
% if checkedbefore(i) ~= 0 then direction i is redundant
% checkedbefore(i) < 0 means that the sign of all computations should be
% switched.
checkedbefore = zeros(length(model.lb),1);
for i = 2:length(model.lb)
    d = objective_coefficient(i, model);
    for j = 1:i-1
        dj = objective_coefficient(j, model);
        if max(abs(dj - d))< 1e-4
            checkedbefore(i) = j;
            break;
        elseif max(abs(dj + d))< 1e-4
            checkedbefore(i) = -j;
            break;
        end
    end
end

% initialize variables;
numpoints = size(x0, 2);

outputminv = 222*ones(length(model.lb), numpoints);
outputminexitflag = -222*ones(length(model.lb), numpoints);
outputminfinalscore = -222*ones(length(model.lb), numpoints);
outputmaxv = -222*ones(length(model.lb), numpoints);
outputmaxexitflag = -222*ones(length(model.lb), numpoints);
outputmaxfinalscore = -222*ones(length(model.lb), numpoints);
outputminstruct = cell(length(model.lb), numpoints);
outputmaxstruct = cell(length(model.lb), numpoints);

%iterate through directions
parfor i = 1:length(model.lb)
    if checkedbefore(i) ~= 0
        continue;
    end
    for j = -1:2:1 % max and min
        d = objective_coefficient(i,model)*j;
        lbprob = lpAssign(d, A, b_L, b_U, x_L, x_U, [], 'linear_bound', [],[],[],[],[],[],[]);
        lbresult = tomRun('cplex', lbprob, 0);
        fLowBnd = lbresult.f_k;
        fprintf('reaction %d of %d, direction %d, lowerbound %f\n', i,length(model.lb), j, fLowBnd);

        % short circuit if x0 already close to a bound.
        obj1 = d'*x0_valid;
        if(any(abs(obj1-fLowBnd)<.0001))
            display('short circuiting');
            [nil, index1] = min(obj1);
            if (j > 0)
                outputminv(i,:) = j*fLowBnd; % multiply by j to correct sign.
                outputminexitflag(i,:) = 111;
                outputminfinalscore(i,:) = scores_valid(index1);
            else
                outputmaxv(i,:) = j*fLowBnd; % multiply by j to correct sign.
                outputmaxexitflag(i,:) = 111;
                outputmaxfinalscore(i,:) = scores_valid(index1);
            end
        else % gotta actually do the computation.
            all_ds = d'*x0_invalid;
            [nil, index2] = sort(all_ds);

            % initialize temp variables to make parfor work.
            v = j*222*ones(1,numpoints);
            exitflag = -222*ones(1,numpoints);
            finalscore = 222*ones(1,numpoints);
            ostruct = cell(1,numpoints);

            for k = 1:length(index2)
                if exist('ttt.txt', 'file')
                    fprintf('quitting due to file found\n');
                    continue;
                end
                xinitial = x0_invalid(:,index2(k));
%                 Prob  = lpconAssign(d, x_L, x_U, Name, xinitial,...
%                                       A, b_L, b_U,...
%                                       c, dc, d2c, ConsPattern, c_L, c_U,...
%                                       fLowBnd, x_min, x_max, f_opt, x_opt);
                Prob = conAssign(f, g, H, HessPattern, x_L, x_U, Name, xinitial, ...
                                pSepFunc, fLowBnd, ...
                                A, b_L, b_U, c, dc, d2c, ConsPattern, c_L, c_U, ...
                                x_min, x_max, f_opt, x_opt);
                %Prob.NumDiff = 2; % central diff
                %Prob.optParam.CentralDiff = 1e-5;
                %pause;
                Prob.user.expdata = expdata;
                Prob.user.model = model;
                Prob.user.objective = d;
                Prob.user.max_error = max_score;
                Prob.user.diff_interval = diffInterval;
                Prob.user.multiplier = 10;

                Prob.optParam.IterPrint = 0;
                %Prob.optParam.cTol = .1*feasibilityTolerance;

                Prob.PriLevOpt = 0;
                Prob.SOL.PrintFile = strcat('temp/snoptp', num2str(i), 'x', num2str(j), 'x', num2str(k),'.txt');
                Prob.SOL.SummFile = strcat('temp/snopts', num2str(i), 'x', num2str(j), 'x', num2str(k),'.txt');
                Prob.SOL.optPar(35) = majorIterationLimit; %This is major iteration count.
                Prob.SOL.optPar(30) = minorIterationLimit; %total iteration limit;
                %Prob.SOL.optPar(11) = feasibilityTolerance; % feasibility tolerance

                Result = tomRun(Solver, Prob, 5);
                tscore = errorComputation2(Result.x_k, tProb);
                tbest = Result.f_k;

                fprintf('reaction %d (%d), x %d; x=%f (%f); score=%f (%f)\n', i,length(model.lb),j, tbest,fLowBnd, tscore, max_score)

                v(k) = j*tbest;
                exitflag(k) = Result.Inform;
                finalscore(k) = tscore;
                ostruct{k} = Result;
            end
            if (j > 0) %minimizing
                outputminv(i,:) = v; % multiply by j to correct sign.
                outputminexitflag(i,:) = exitflag;
                outputminfinalscore(i,:) = finalscore;
                outputminstruct(i,:) = ostruct;
            else
                outputmaxv(i,:) = v; % multiply by j to correct sign.
                outputmaxexitflag(i,:) = exitflag;
                outputmaxfinalscore(i,:) = finalscore;
                outputmaxstruct(i,:) = ostruct;
            end
        end
    end
end

output.minv = outputminv;
output.maxv = outputmaxv;
output.minexitflag = outputminexitflag;
output.maxexitflag = outputmaxexitflag;
output.minfinalscore = outputminfinalscore;
output.maxfinalscore = outputmaxfinalscore;
output.minstruct = outputminstruct;
output.maxstruct = outputmaxstruct;

for i = 1:length(model.lb)
    if checkedbefore(i) > 0 %short circuit if seen before.
        output.minv(i,:) = output.minv(checkedbefore(i),:);
        output.maxv(i,:) = output.maxv(checkedbefore(i),:);
        output.minexitflag(i,:) = output.minexitflag(checkedbefore(i),:);
        output.maxexitflag(i,:) = output.maxexitflag(checkedbefore(i),:);
        output.minfinalscore(i,:) = output.minfinalscore(checkedbefore(i),:);
        output.maxfinalscore(i,:) = output.maxfinalscore(checkedbefore(i),:);
        output.minstruct(i,:) = output.minstruct(checkedbefore(i),:);
        output.maxstruct(i,:) = output.maxstruct(checkedbefore(i),:);
    elseif checkedbefore(i) < 0
        output.minv(i,:) = -output.maxv(-checkedbefore(i),:);
        output.maxv(i,:) = -output.minv(-checkedbefore(i),:);
        output.minexitflag(i,:) = output.maxexitflag(-checkedbefore(i),:);
        output.maxexitflag(i,:) = output.minexitflag(-checkedbefore(i),:);
        output.minfinalscore(i,:) = output.maxfinalscore(-checkedbefore(i),:);
        output.maxfinalscore(i,:) = output.minfinalscore(-checkedbefore(i),:);
        output.minstruct(i,:) = output.maxstruct(-checkedbefore(i),:);
        output.maxstruct(i,:) = output.minstruct(-checkedbefore(i),:);
    end
end

vs = zeros(length(model.lb), 2);
for i = 1:length(model.lb)
    validindex = output.minfinalscore(i,:) < max_score + feasibilityTolerance;
    if any(validindex)
        vs(i,1) = min(output.minv(i,validindex));
    else
        vs(i,1) = 222;
    end
    validindex = output.maxfinalscore(i,:) < max_score + feasibilityTolerance;
    if any(validindex)
        vs(i,2) = max(output.maxv(i,validindex));
    else
        vs(i,2) = -222;
    end
end

return;

% function that returns the proper objective coefficient for each reaction
% takes into account the reversibility of reactinos etc.
function [d] =  objective_coefficient(i, model)
d = zeros(length(model.lb),1);
d(i) = 1;
if (model.match(i))
    d(model.match(i)) = -1;
end
d = (d'*model.N)'; % transform to null space;
return


function newSegmentation = grow_region(this, startingSegmentation, threshold, nIter)
% Iterative region growing using starting segmentation and threshold.
%
%   Y = MrImage()
%   Y.grow_region(startingSegmentation, threshold, nIter)
%
% This is a method of class MrImage.
%
% IN
%   startingSegmentation    MrImage-object containing the starting
%                           segmentation, e.g. obtained via binarizing
%                           this
%   threshold               Threshold above which voxel connected to the
%                           object are considered part of the object.
%   nIter                   Number of maximum iterations. Maximum path
%                           length that can be grown.
%
% OUT
%
% EXAMPLE
%   Y.grow_region(Y.binarize(500), 300, 20);
%
%   See also MrImage

% Author:   Saskia Bollmann
% Created:  2020-03-30
% Copyright (C) 2020 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public License (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.

% initialize
trueLocations = startingSegmentation.copyobj();

for n = 1:nIter

    % create searchLocations
    searchLocations = trueLocations.imdilate(strel('disk', 1));
    % exclude already found locations
    searchLocations = searchLocations - trueLocations;
    % apply in this
    searchVoxel = this .* searchLocations;
    % threshold
    newLocations = searchVoxel.binarize(threshold);
    % check new found locations
    disp([num2str(sum(newLocations.data(:))), ' new voxel(s) found.']);
    if ~any(newLocations.data(:))
        break
    end
    % add to true locations
    trueLocations = trueLocations + newLocations;

end
newSegmentation = trueLocations;

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_IRB7600_500_230(robot, T)
%   Solves the inverse kinematic problem for the ABB IRB7600_500_2300 robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_IRB7600_500_2300 returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   >>abb=load_robot('ABB', 'IRB7600_500_2300');
%   >>q = [0 0 0 0 0 0];
%   >>T = directkinematic(abb, q);
%   %Call the inversekinematic for this robot
%   >>qinv = inversekinematic(abb, T);
%
%   check that all of them are feasible solutions!
%   and every Ti equals T
%
%   for i=1:8,
%        Ti = directkinematic(abb, qinv(:,i))
%   end
%
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% 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 [q] = inversekinematic_irb7600_500_230(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry at the reference for this robot
L1=d(1);
L2=a(2);
L3=d(4);
A2=a(3);
L6=d(6);

A1 = a(1);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);

q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);

q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%Arrange solutions, there are 8 possible solutions so far.
% if q1 is a solution, q1* = q1 + pi is also a solution.
% For each (q1, q1*) there are two possible solutions
% for q2 and q3 (namely, elbow up and elbow up solutions)
% So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3),
% there exist two more possible solutions for the last three joints, generally
% called wrist up and wrist down solutions. For this reason,
%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    % use solve_spherical_wrist2 for the particular orientation
    % of the systems in this ABB robot
    % use either the geometric or algebraic method.
    % the function solve_spherical_wrist2 is used due to the relative
    % orientation of the last three DH reference systems.

    %use either one algebraic method or the geometric
    %qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1, 'geometric'); %wrist up
    qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1,'algebraic'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    %qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'geometric'); %wrist down
    qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'algebraic'); %wrist down
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=a(2);
L3=d(4);
A2=a(3);

%compute L4
L4=sqrt(A2^2+L3^2);
%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = (acos((L2^2+r^2-L4^2)/(2*r*L2)));

if ~isreal(gamma)
    disp('WARNING:inversekinematic_irb7600_500_230: the point is not reachable for this configuration, imaginary solutions');
%gamma = real(gamma);
end

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=a(2);
L3=d(4);
A2=a(3);

%See geometry of the robot
L4=sqrt(A2^2+L3^2);
%the angle phi is fixed
phi= acos((A2^2-L3^2+L4^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = (acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

if ~isreal(eta)
   disp('WARNING:inversekinematic_irb7600_500_230: the point is not reachable for this configuration, imaginary solutions');
    %eta = real(eta);
end

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi - eta;
q3(2) = pi - phi + eta;