% ==============================================================================
%
%    Software License Agreement (BSD License)
%    Copyright (c) 2019
%    (www.aimlab.wpi.edu)
%
%    All rights reserved.
%
%    Redistribution and use in source and binary forms, with or without
%    modification, are permitted provided that the following conditions
%    are met:
%
%    * Redistributions of source code must retain the above copyright
%    notice, this list of conditions and the following disclaimer.
%
%    * Redistributions in binary form must reproduce the above
%    copyright notice, this list of conditions and the following
%    disclaimer in the documentation and/or other materials provided
%    with the distribution.
%
%    * Neither the name of authors nor the names of its contributors may
%    be used to endorse or promote products derived from this software
%    without specific prior written permission.
%
%    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
%    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
%    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
%    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
%    COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
%    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
%    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
%    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
%    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
%    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
%    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
%    POSSIBILITY OF SUCH DAMAGE.
%
%    \author:    <http://www.aimlab.wpi.edu>
%    \author:    <amunawar@wpi.edu>
%    \author:    Adnan Munawar
%    \version:   0.1$
% ==============================================================================

function [its,sizePath,run_time] =  RRTstar3D(dim,segmentLength,radius,random_world,show_output,samples)
% if samples < 4000
%     disp('ERROR! SPECIFY ATLEAST 4000 SAMPLES')
%     return
% end

% dim = 2;
% radius =0;
% segmentLength = 5;
% random_world = 0;
% n_its = 1000;
% standard length of path segments
if dim ==2
    start_cord = [5,5];
    goal_cord = [95,95];

else

    start_cord = [5,5,5];
    goal_cord = [95,95,95];
end

% create random world
Size = 100;
NumObstacles = 100;

if random_world ==1
    world = createWorld(NumObstacles,ones(1,dim)*Size,zeros(1,dim),dim);
else
    [world NumObstacles] = createKnownWorld(ones(1,dim)*Size,[0;0;0],dim);
end
% randomly select start and end nodes
%start_node = generateRandomNode(world,dim)
%end_node   = generateRandomNode(world,dim)
start_node = [start_cord,0,0,0];
end_node = [goal_cord,0,0,0];
% establish tree starting with the start node
tree = start_node;

numPaths = 0;
a = clock;
% check to see if start_node connects directly to end_node
if ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )...
        &&(collision(start_node,end_node,world,dim)==0) )

    disp('CASE 1')
    path = [start_node; end_node];
else
    disp('CASE 2')
    if samples >0
        draw = floor(samples/8);
        its = 0;
        for i = 1:samples
            flag = 0;
            [tree,flag] = extendTree(tree,end_node,segmentLength,radius,world,flag,dim);
            numPaths = numPaths + flag;
            its = its+1;

            if its == draw
                tree_1 = tree;
            elseif its == draw*2
                tree_2 = tree;
            elseif its == draw*3
                tree_3 = tree;
            elseif its == draw*4
                tree_4 = tree;
            elseif its == draw*5
                tree_5 = tree;
            elseif its == draw*6
                tree_6 = tree;
            elseif its == draw*7
                tree_7 = tree;
            elseif its == samples
                tree_8 = tree;
            end
        end

    else
        its = 0;
        numPaths = 0;
        flag = 0;
        while numPaths < 1,
            [tree,flag] = extendTree(tree,end_node,segmentLength,radius,world,flag,dim);
            numPaths = numPaths + flag;
            its = its+1;
        end
    end

end
numPaths

% find path with minimum cost to end_node
path = findMinimumPath(tree,end_node,dim);

b = clock;
run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));

path_1 = findMinimumPath(tree_1,end_node,dim);

path_2 = findMinimumPath(tree_2,end_node,dim);

path_3 = findMinimumPath(tree_3,end_node,dim);

path_4 = findMinimumPath(tree_4,end_node,dim);

path_5 = findMinimumPath(tree_5,end_node,dim);

path_6 = findMinimumPath(tree_6,end_node,dim);

path_7 = findMinimumPath(tree_7,end_node,dim);

path_8 = findMinimumPath(tree_8,end_node,dim);

sizePath = size(path,1);

if show_output == 1

    if size(path_1, 1) > 0
        figure;
        plotExpandedTree(world,tree_1,dim);
        plotWorld(world,path_1,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 1/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_2, 1) > 0
        figure;
        plotExpandedTree(world,tree_2,dim);
        plotWorld(world,path_2,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 2/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_3, 1) > 0
    figure;
    plotExpandedTree(world,tree_3,dim);
    plotWorld(world,path_3,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 3/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_4, 1) > 0
    figure;
    plotExpandedTree(world,tree_4,dim);
    plotWorld(world,path_4,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 4/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_5, 1) > 0
    figure;
    plotExpandedTree(world,tree_5,dim);
    plotWorld(world,path_5,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 5/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_6, 1) > 0
    figure;
    plotExpandedTree(world,tree_6,dim);
    plotWorld(world,path_6,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 6/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_7, 1) > 0
    figure;
    plotExpandedTree(world,tree_7,dim);
    plotWorld(world,path_7,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 7/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_8, 1) > 0
    figure;
    plotExpandedTree(world,tree_8,dim);
    plotWorld(world,path_8,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 8/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path, 1) > 0
    figure;
    plotExpandedTree(world,tree,dim);
    plotWorld(world,path,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE FOR THE SPECIFIED SAMPLES. PLEASE INCREASE THE NUMBER OF SAMPLES')
    end
end
end

function world = createWorld(NumObstacles, endcorner, origincorner,dim)

if dim == 2

    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2))
        disp('Not valid corner specifications!')
        world=[];

        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        maxRadius = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2));
        maxRadius = 5*maxRadius/NumObstacles/2;
        for i=1:NumObstacles,
            % randomly pick radius
            world.radius(i) = maxRadius*rand;
            % randomly pick center of obstacles
            cx = origincorner(1) + world.radius(i)...
                + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;
            cy = origincorner(2) + world.radius(i)...
                + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;
            world.cx(i) = cx;
            world.cy(i) = cy;
        end
    end

elseif dim ==3;
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))
        disp('Not valid corner specifications!')
        world=[];

        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)];
        maxRadius = min(bounds);
        maxRadius = 5*maxRadius/NumObstacles;
        for i=1:NumObstacles,
            % randomly pick radius
            world.radius(i) = maxRadius*rand;
            % randomly pick center of obstacles
            cx = origincorner(1) + world.radius(i)...
                + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;
            cy = origincorner(2) + world.radius(i)...
                + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;
            cz = origincorner(2) + world.radius(i)...
                + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;
            world.cx(i) = cx;
            world.cy(i) = cy;
            world.cz(i) = cz;
        end
    end
end
end

function [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;
if dim == 2
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)),
        disp('Not valid corner specifications!')
        world=[];
        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        maxRadius = 10;

        world.radius(1) = maxRadius;
        cx = 50;
        cy = 50;
        world.cx(1) = cx;
        world.cy(1) = cy;

        world.radius(2) = maxRadius;
        cx = 75;
        cy = 25;
        world.cx(2) = cx;
        world.cy(2) = cy;

        world.radius(3) = maxRadius;
        cx = 25;
        cy = 75;
        world.cx(3) = cx;
        world.cy(3) = cy;

        world.radius(4) = maxRadius;
        cx = 25;
        cy = 25;
        world.cx(4) = cx;
        world.cy(4) = cy;

        world.radius(5) = maxRadius;
        cx = 75;
        cy = 75;
        world.cx(5) = cx;
        world.cy(5) = cy;
    end

elseif dim == 3

    NumObstacles = 9;
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)),
        disp('Not valid corner specifications!')
        world=[];

        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        maxRadius = 10;

        world.radius(1) = maxRadius;
        cx = 50;
        cy = 50;
        cz = 50;
        world.cx(1) = cx;
        world.cy(1) = cy;
        world.cz(1) = cz;

        world.radius(2) = maxRadius;
        cx = 25;
        cy = 25;
        cz = 25;
        world.cx(2) = cx;
        world.cy(2) = cy;
        world.cz(2) = cz;

        world.radius(3) = maxRadius;
        cx = 75;
        cy = 75;
        cz = 75;
        world.cx(3) = cx;
        world.cy(3) = cy;
        world.cz(3) = cz;

        world.radius(4) = maxRadius;
        cx = 25;
        cy = 25;
        cz = 75;
        world.cx(4) = cx;
        world.cy(4) = cy;
        world.cz(4) = cz;

        world.radius(5) = maxRadius;
        cx = 75;
        cy = 75;
        cz = 25;
        world.cx(5) = cx;
        world.cy(5) = cy;
        world.cz(5) = cz;

        world.radius(6) = maxRadius;
        cx = 25;
        cy = 75;
        cz = 25;
        world.cx(6) = cx;
        world.cy(6) = cy;
        world.cz(6) = cz;

        world.radius(7) = maxRadius;
        cx = 75;
        cy = 25;
        cz = 25;
        world.cx(7) = cx;
        world.cy(7) = cy;
        world.cz(7) = cz;

        world.radius(8) = maxRadius;
        cx = 75;
        cy = 25;
        cz = 75;
        world.cx(8) = cx;
        world.cy(8) = cy;
        world.cz(8) = cz;

        world.radius(9) = maxRadius;
        cx = 25;
        cy = 75;
        cz = 75;
        world.cx(9) = cx;
        world.cy(9) = cy;
        world.cz(9) = cz;
    end
end
end

function node=generateRandomNode(world,dim)

if dim ==2;
    % randomly pick configuration
    px       = (world.endcorner(1)-world.origincorner(1))*rand;
    py       = (world.endcorner(2)-world.origincorner(2))*rand;

    chi      = 0;
    cost     = 0;
    node     = [px, py, chi, cost, 0];

    % check collision with obstacle
    while collision(node, node, world,dim),
        px       = (world.endcorner(1)-world.origincorner(1))*rand;
        py       = (world.endcorner(2)-world.origincorner(2))*rand;

        chi      = 0;
        cost     = 0;
        node     = [px, py, chi, cost, 0];
    end

elseif dim ==3;
    % randomly pick configuration
    px       = (world.endcorner(1)-world.origincorner(1))*rand;
    py       = (world.endcorner(2)-world.origincorner(2))*rand;
    pz       = (world.endcorner(3)-world.origincorner(3))*rand;

    chi      = 0;
    cost     = 0;
    node     = [px, py, pz, chi, cost, 0];

    % check collision with obstacle
    while collision(node, node, world,dim),
        px       = (world.endcorner(1)-world.origincorner(1))*rand;
        py       = (world.endcorner(2)-world.origincorner(2))*rand;
        pz       = (world.endcorner(3)-world.origincorner(3))*rand;

        chi      = 0;
        cost     = 0;
        node     = [px, py, pz, chi, cost, 0];
    end

end

end

function collision_flag = collision(node, parent, world,dim)

collision_flag = 0;

for i=1:dim
    if (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i))
        collision_flag = 1;
    end
end

if collision_flag == 0 && dim ==2
    for sigma = 0:.2:1,
        p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
        % check each obstacle
        for i=1:world.NumObstacles,
            if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),
                collision_flag = 1;
                break;
            end
        end
    end

elseif collision_flag == 0 && dim ==3
    for sigma = 0:.2:1,
        p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
        % check each obstacle
        for i=1:world.NumObstacles,
            if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)),
                collision_flag = 1;
                break;
            end
        end
    end
end
end

function flag = canEndConnectToTree(tree,end_node,minDist,world,dim)
flag = 0;
% check only last node added to tree since others have been checked
if ( (norm(tree(end,1:dim)-end_node(1:dim))<minDist)...
        & (collision(tree(end,1:dim), end_node(1:dim), world,dim)==0) ),
    flag = 1;
end

end

function [new_tree,flag] = extendTree(tree,end_node,segmentLength,r,world,flag_chk,dim)

flag1 = 0;
while flag1==0,
    % select a random point
    randomPoint = ones(1,dim);
    for i=1:dim
        randomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand;
    end

    % find leaf on node that is closest to randomPoint
    tmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint;
    sqrd_dist = sqr_eucl_dist(tmp,dim);
    [min_dist,idx] = min(sqrd_dist);
    min_parent_idx = idx;

    new_point = (randomPoint-tree(idx,1:dim));
    new_point = tree(idx,1:dim)+(new_point/norm(new_point))*segmentLength;

    min_cost  = cost_np(tree(idx,:),new_point,dim);
    new_node  = [new_point, 0, min_cost, idx];

    if collision(new_node, tree(idx,:), world,dim)==0

        tmp_dist = tree(:,1:dim)-(ones(size(tree,1),1)*new_point);
        dist = sqr_eucl_dist(tmp_dist,dim);
        near_idx = find(dist <= r^2);

        if size(near_idx,1)>1
            size_near = size(near_idx,1);

            for i = 1:size_near
                if collision(new_node, tree(near_idx(i),:), world,dim)==0

                    cost_near = tree(near_idx(i),dim+2)+line_cost(tree(near_idx(i),:),new_point,dim);

                    if  cost_near < min_cost
                        min_cost = cost_near;
                        min_parent_idx = near_idx(i);
                    end

                end
            end
        end

        new_node = [new_point, 0 , min_cost, min_parent_idx];
        new_tree = [tree; new_node];
        new_node_idx = size(new_tree,1);

        if size(near_idx,1)>1
            reduced_idx = near_idx;
            for j = 1:size(reduced_idx,1)
                near_cost = new_tree(reduced_idx(j),dim+2);
                lcost = line_cost(new_tree(reduced_idx(j),:),new_point,dim);
                if near_cost > min_cost + lcost ...
                        && collision(new_tree(reduced_idx(j),:),new_node,world,dim)
                    before = new_tree(reduced_idx(j),dim+3)
                    new_tree(reduced_idx(j),dim+3) = new_node_idx;
                    after = new_tree(reduced_idx(j),dim+3)
                end

            end
        end
        flag1=1;
    end
end

if flag_chk == 0
    % check to see if new node connects directly to end_node
    if ( (norm(new_node(1:dim)-end_node(1:dim))<segmentLength )...
            && (collision(new_node,end_node,world,dim)==0) )
        flag = 1;
        new_tree(end,dim+1)=1;  % mark node as connecting to end.
    else
        flag = 0;
    end

else flag = 1;
end
end

function e_dist = sqr_eucl_dist(array,dim)

sqr_e_dist = zeros(size(array,1),dim);
for i=1:dim

    sqr_e_dist(:,i) = array(:,i).*array(:,i);

end
e_dist = zeros(size(array,1),1);
for i=1:dim

    e_dist = e_dist+sqr_e_dist(:,i);

end

end

%calculate the cost from a node to a point
function [cost] = cost_np(from_node,to_point,dim)

diff = from_node(:,1:dim) - to_point;
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;

end

%calculate the cost from a node to a node
function [cost] = cost_nn(from_node,to_node,dim)

diff = from_node(:,1:dim) - to_node(:,1:dim);
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;

end

function [cost] = line_cost(from_node,to_point,dim)
diff = from_node(:,1:dim) - to_point;
cost = norm(diff);
end

function path = findMinimumPath(tree,end_node,dim)

% find nodes that connect to end_node
connectingNodes = [];
for i=1:size(tree,1),
    if tree(i,dim+1)==1,
        connectingNodes = [connectingNodes ; tree(i,:)];
    end
end

if size(connectingNodes, 1) > 0

    % find minimum cost last node
    [tmp,idx] = min(connectingNodes(:,dim+2));

    % construct lowest cost path
    path = [connectingNodes(idx,:); end_node];
    parent_node = connectingNodes(idx,dim+3);
    while parent_node>1,
        parent_node = tree(parent_node,dim+3);
        path = [tree(parent_node,:); path];
    end

else
    path = [];
end

end

function plotExpandedTree(world,tree,dim)
ind = size(tree,1);
while ind>0
    branch = [];
    node = tree(ind,:);
    branch = [ branch ; node ];
    parent_node = node(dim+3);
    while parent_node > 1
        cur_parent = parent_node;
        branch = [branch; tree(parent_node,:)];
        parent_node = tree(parent_node,dim+3);
    end
    ind = ind - 1;

    if dim == 2
        X = branch(:,1);
        Y = branch(:,2);

        p = plot(X,Y);
        set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');
        hold on;

    elseif dim == 3
        X = branch(:,1);
        Y = branch(:,2);
        Z = branch(:,3);

        p = plot3(X,Y,Z);
        set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');
        hold on;
    end
end
end

function plotWorld(world,path,dim)
% the first element is the north coordinate
% the second element is the south coordinate
if dim ==2

    N = 10;
    th = 0:2*pi/N:2*pi;
    axis([world.origincorner(1),world.endcorner(1),...
        world.origincorner(2), world.endcorner(2)]);
    hold on

    for i=1:world.NumObstacles,
        X = world.radius(i)*sin(th) + world.cx(i);
        Y = world.radius(i)*cos(th) + world.cy(i);
        fill(X,Y,'blue');
    end

    X = path(:,1);
    Y = path(:,2);
    p = plot(X,Y);

elseif dim ==3
    axis([world.origincorner(1),world.endcorner(1),...
        world.origincorner(2), world.endcorner(2),...
        world.origincorner(3), world.endcorner(3)]);
    hold on

    for i=1:world.NumObstacles,
        [X Y Z] = sphere(10);
        X = (X*world.radius(i));
        Y = (Y*world.radius(i));
        Z = (Z*world.radius(i));
        surf(X+world.cx(i),Y+world.cy(i),Z+world.cz(i));
        colormap([0.5 0.2 0.3]);
    end

    X = path(:,1);
    Y = path(:,2);
    Z = path(:,3);
    p = plot3(X,Y,Z);
end
set(p,'Color','black','LineWidth',3)
xlabel('X axis');
ylabel('Y axis');
zlabel('Z axis');
title('RRT Star Algorithm');
end


function model = Learning_MLE_Basis_Stoc( Seqs, model, alg )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Learning Hawkes processes via maximum likelihood estimation
% Different regularizers (low-rank, sparse, group sparse) of parameters and
% their combinations are considered, which are solved via StochasticADMM.
%
% Reference:
% Xu, Hongteng, Mehrdad Farajtabar, and Hongyuan Zha.
% "Learning Granger Causality for Hawkes Processes."
% International Conference on Machine Learning (ICML). 2016.
%
% Provider:
% Hongteng Xu @ Georgia Tech
% June. 10, 2017
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% initial
Aest = model.A;
muest = model.mu;
% KerFint = struct('dGK', []);
% KerF = struct('gij', []);

if alg.LowRank
    UL = zeros(size(Aest));
    ZL = Aest;
end

if alg.Sparse
    US = zeros(size(Aest));
    ZS = Aest;
end

if alg.GroupSparse
    UG = zeros(size(Aest));
    ZG = Aest;
end

D = size(Aest, 1);

if alg.storeLL
    model.LL = zeros(alg.epoch, 1);
end
if alg.storeErr
    model.err = zeros(alg.epoch, 3);
end

tic;
for o = 1:alg.epoch % the number of epoch for stochastic opt

    rho = alg.rho * (1.1^o);

    idx_seq = randperm(length(Seqs));
    if length(Seqs)<alg.seqbatch;
        N_seq_batch = 1;
        Ns = length(Seqs);
    else
        N_seq_batch = floor(length(Seqs)/alg.seqbatch);
        Ns = alg.seqbatch;
    end

    for n = 1:N_seq_batch

        % initial para
        Amu = zeros(D, 1);
        Bmu = Amu;

        CmatA = zeros(size(Aest));
        AmatA = CmatA;
        BmatA = CmatA;
        if alg.LowRank
            BmatA = BmatA + rho*(UL-ZL);
            AmatA = AmatA + rho;
        end
        if alg.Sparse
            BmatA = BmatA + rho*(US-ZS);
            AmatA = AmatA + rho;
        end
        if alg.GroupSparse
            BmatA = BmatA + rho*(UG-ZG);
            AmatA = AmatA + rho;
        end

        % E-step: evaluate the responsibility using the current parameters
        for ns = 1:Ns
            c = idx_seq(Ns*(n-1)+ns); % the id of current sequence

            if ~isempty(Seqs(c).Time)
                Time = Seqs(c).Time;
                Event = Seqs(c).Mark;
                Tstart = Seqs(c).Start;

                if isempty(alg.Tmax)
                    Tstop = Seqs(c).Stop;
                else
                    Tstop = alg.Tmax;
                    indt = Time < alg.Tmax;
                    Time = Time(indt);
                    Event = Event(indt);
                end

                Nc = length(Time);
%                 if o == 1
%                     KerFint(c).dGK = zeros(alg.historyL, length(model.landmark), Nc-1);
%                     KerF(c).gij = zeros(alg.historyL, length(model.landmark), Nc-1);
%                 end

                idx_event = randperm(Nc);
                if Nc<alg.eventbatch
                    N_event_batch = 1;
                    Ne = Nc;
                else
                    N_event_batch = alg.eventbatch;
                    Ne = floor(Nc/alg.eventbatch);
                end
                TimeNew = [Tstart, Time];
                for m = 1:N_event_batch
                    for ne = 1:Ne
                        i = idx_event(Ne*(m-1)+ne);
                        ui = Event(i);
                        ti = Time(i);
                        ti1 = TimeNew(i);

                        Amu = Amu + ti - ti1;

                        % calculate intensity
                        lambdai = muest(ui);
                        pii = muest(ui);
                        pij = [];

                        if i>1
                            start = max([1, i-alg.historyL]);
                            tj = Time(start:i-1);
                            uj = Event(start:i-1);

                            %if o==1 || isempty(KerF(c).gij)
                                dt = ti - tj;
                                dt1 = ti1 - tj;
                                gij = Kernel(dt, model);

                                %KerF(c).gij(1:length(dt),:,i-1) = gij;

                                GK = Kernel_Integration(dt, model);
                                GK1 = Kernel_Integration(dt1, model);
                                dGK = GK - GK1;
                                %KerFint(c).dGK(1:length(dt),:,i-1) = dGK;
                            %end

                            auiuj = Aest(uj, :, ui);
                            %pij = auiuj .* KerF(c).gij(1:length(uj),:,i-1);%
                            pij = auiuj .* gij;
                            lambdai = lambdai + sum(pij(:));
                        end
                        pii = pii./lambdai;

                        if i>1
                            pij = pij./lambdai;
                            if ~isempty(pij) && sum(pij(:))>0
                                for j = 1:length(uj)
                                    uuj = uj(j);
                                    CmatA(uuj,:,ui) = CmatA(uuj,:,ui) - pij(j,:);

                                    BmatA(uuj,:,:) = BmatA(uuj,:,:) + ...
                                        double(Aest(uuj,:,:)>0).*...
                                        repmat( dGK(j,:), [1,1,D] );
                                        %repmat( KerFint(c).dGK(j,:,i-1), [1,1,D] );
                                end
                            end
                        end
                        Bmu(ui) = Bmu(ui) + pii;

                    end
                end

%                 Amu = Amu + Tstop - Tstart;
%
%                 dT = Tstop - Time;
%                 GK = Kernel_Integration(dT, model);
%
%                 Nc = length(Time);
%                 idx_event = randperm(Nc);
%
%                 if Nc<alg.eventbatch
%                     N_event_batch = 1;
%                     Ne = Nc;
%                 else
%                     N_event_batch = alg.eventbatch;
%                     Ne = floor(Nc/alg.eventbatch);
%                 end
%
%                 for m = 1:N_event_batch
%                     for ne = 1:Ne
%                         i = idx_event(Ne*(m-1) + ne);
%                         ui = Event(i);
%
%                         BmatA(ui,:,:) = BmatA(ui,:,:)+...
%                             double(Aest(ui,:,:)>0).*repmat( GK(i,:), [1,1,D] );
%
%                         ti = Time(i);
%                         lambdai = muest(ui);
%                         pii = muest(ui);
%                         pij = [];
%
%                         if i>1
%                             start = max([1, i-alg.historyL]);
%                             tj = Time(start:i-1);
%                             uj = Event(start:i-1);
%
%                             dt = ti - tj;
%                             gij = Kernel(dt, model);
%                             auiuj = Aest(uj, :, ui);
%                             pij = auiuj .* gij;
%                             lambdai = lambdai + sum(pij(:));
%                         end
%
%                         pii = pii./lambdai;
%
%                         if i>1
%                             pij = pij./lambdai;
%                             if ~isempty(pij) && sum(pij(:))>0
%                                 for j = 1:length(uj)
%                                     uuj = uj(j);
%                                     CmatA(uuj,:,ui) = CmatA(uuj,:,ui) - pij(j,:);
%                                 end
%                             end
%                         end
%
%                         Bmu(ui) = Bmu(ui) + pii;
%                     end
%                 end
            else
                warning('Sequence %d is empty!', c)
            end
        end

        % M-step: update parameters
        mu = Bmu./Amu;
        if alg.Sparse==0 && alg.GroupSparse==0 && alg.LowRank==0
            A = -CmatA./BmatA;%( -BA+sqrt(BA.^2-4*AA*CA) )./(2*AA);
            A(isnan(A))=0;
            A(isinf(A))=0;
        else
            A = ( -BmatA + sqrt(BmatA.^2 - 4*AmatA.*CmatA) )./(2*AmatA);
            A(isnan(A))=0;
            A(isinf(A))=0;
        end

        % check convergence
        Err=sum(sum(sum(abs(A-Aest))))/sum(abs(Aest(:)));
        Aest = A;
        muest = mu;
        model.A = Aest;
        model.mu = muest;
        fprintf('Epoch=%d, EventBatch=%d, RelErr=%f, Time=%0.2fsec\n',...
                o, n, Err, toc);

        if Err<alg.thres || (o==alg.epoch && n==N_seq_batch)
            break;
        end
    end

    % calculate Loglikelihood
    if alg.storeLL
        Loglike = Loglike_Basis( Seqs, model, alg );
        model.LL(o) = Loglike;
    end
    % calculate error
    if alg.storeErr
        Err = zeros(1,3);
        Err(1) = norm(model.mu(:) - alg.truth.mu(:))/norm(alg.truth.mu(:));
        Err(2) = norm(model.A(:) - alg.truth.A(:))/norm(alg.truth.A(:));
        Err(3) = norm([model.mu(:); model.A(:)]-[alg.truth.mu(:); alg.truth.A(:)])...
            /norm([alg.truth.mu(:); alg.truth.A(:)]);
        model.err(o,:) = Err;
    end

    if alg.LowRank
        threshold = alg.alphaLR/rho;
        ZL = SoftThreshold_LR( Aest+UL, threshold );
        UL = UL + (Aest-ZL);
    end

    if alg.Sparse
        threshold = alg.alphaS/rho;
        ZS = SoftThreshold_S( Aest+US, threshold );
        US = US + (Aest-ZS);
    end

    if alg.GroupSparse
        threshold = alg.alphaGS/rho;
        ZG = SoftThreshold_GS( Aest+UG, threshold );
        UG = UG + (Aest-ZG);
    end

end


function [opt_radius,opt_interval,result,r,alldt,nummod,numreal,sigma] = calc_optim(a,rmin,rstep,rmax,tmin,tstep,Nmin,bootloops,maepi,choice)

    % function [opt_radius,opt_interval,result,r,alldt,nummod,numreal,sigma] = calc_optim(a,rmin,rstep,rmax,tmin,tstep,Nmin,bootloops,maepi,choice);
    % -------------------------------------------------------------------------
    %
    % optimisation of free parameters looking for seismic quiescences preceding large aftershocks
    %
    % Input parameters:
    %   a               Earthquake catalog (has to be complete in magnitude!)
    %   rmin,rstep,rmax Radius [degrees]
    %   tmin,tstep      Time [days]
    %   Nmin            minimum earthquake number in catalog
    %   bootloops       number of bootstrap loops
    %   maepi           mainshock (->ZMAP)
    %   choice          Nr. of M5+ aftershock
    %
    % Output parameters:
    %   opt_radius      radius around large aftershock
    %   opt_interval    time interval before large aftershock
    %   result          matrix containing relative seismic quiescences
    %   r, alldt        vectors with radii, forecast intervals
    %   numreal,nummod  observed/modeled nr. of aftershocks in interval
    %   sigma           uncertainty of forecast
    %
    % Samuel Neukomm
    % last update 25.02.2004

report_this_filefun(mfilename('fullpath'));
    % get mainshock / calculate delay times / cut catalogue at mainshock
    [m_main, main] = max(a.Magnitude);
    if size(a,2) == 9
        date_matlab = datenum(a.Date.Year,a.Date.Month,a.Date.Day,a.Date.Hour,a.Date.Minute,zeros(size(a,1),1));
    else
        date_matlab = datenum(a.Date.Year,a.Date.Month,a.Date.Day,a.Date.Hour,a.Date.Minute,a(:,10));
    end
    date_main = date_matlab(main);
    time_aftershock = date_matlab-date_main;
    l = time_aftershock(:) > 0;
    tas = time_aftershock(l);
    eqcatalogue = a.subset(l);

    % choose one of large aftershocks
    l = eqcatalogue(:,6) >= 5;
    largeas = eqcatalogue(l,:);
    largetime = tas(l);
    eq = largeas(choice,:);
    eqt = largetime(choice);

    % get optimum parameters
    dist_lon = eqcatalogue(:,1)-eq(1);
    dist_lat = eqcatalogue(:,2)-eq(2);
    dist_dep = km2deg(eqcatalogue(:,7)-eq(7),6371);
    result = []; i = 1;
    numreal = []; nummod = []; sigma = [];
    for r = rmin:rstep:rmax % range of collection radius
        disp(num2str(r/rmax))
        l = (dist_lon.^2+dist_lat.^2+dist_dep.^2).^0.5 <= r;
        gpi = eqcatalogue(l,:);
        time_as = tas(l);
        l = time_as < eqt;
        gpi = gpi(l,:); % sub-catalogue to be analysed
        time_as = time_as(l); % corresponding delay times

        if length(time_as) >= Nmin
            dt = tmin; % interval before large aftershock
            j = 1; alldt = [];
            while dt < eqt % Changed eqt/2 to eqt
                [rc,realnum,modnum,sig] = calc_optrc(gpi,time_as,eqt-dt,eqt,bootloops,maepi); % determine significance
                if isnan(rc)==0
                    result(i,j) = rc;
                    numreal(i,j) = realnum;
                    nummod(i,j) = modnum;
                    sigma(i,j) = sig;
                else
                    result(i,j) = 0;
                    numreal(i,j) = 0;
                    nummod(i,j) = 0;
                    sigma(i,j) = 0;
                end
                alldt = [alldt; dt];
                dt = dt+tstep;
                j = j+1;
            end
        end
        i = i+1;
    end
    r = (rmin:rstep:rmax)';

    if isempty(result) == 0  &&  max(max(result)) > 0
        % find minimum
        [dum, j] = min(min(result));
        [dum, i] = min(result(:,j));
        opt_radius = r(i);
        opt_interval = alldt(j);

        % contourplot of results
        figure_w_normalized_uicontrolunits('Numbertitle','off','Name','Optimum quiescence')
        lim = ceil(abs(dum));
        pcolor(alldt,r,result)
        set(gca,'pos',[0.15 0.15 0.7 0.7])
        shading interp
        caxis([-lim lim]);
        load fourcolors
        j = [j(256:-1:1,:) ];
        colormap(j)
        set(gcf,'renderer','zbuffer');
        set(gca,'TickDir','out')
        h5 = colorbar;
        set(h5,'Tickdir','out','pos',[0.8 0.3 0.02 0.4]);
        hold on
        plot(opt_interval,opt_radius','*','Markersize',8,'linewidth',2,'color','k');
        string =['Radius = ' num2str(opt_radius) ' deg; Interval = ' num2str(opt_interval) ' days; RC = ' num2str(dum,3) ];
        title(string)
        string = ['Time interval before t1 = ' num2str(round(100*eqt)/100) ' [days]'];
        xlabel(string)
        ylabel('Radius [deg]');
        set(gcf,'color','w');

        l = (dist_lon.^2+dist_lat.^2+dist_dep.^2).^0.5 <= opt_radius;
        optcat = eqcatalogue(l,:);
        time_as = tas(l);
        l = time_as < eqt;
        optcat = optcat(l,:);
        plot_optfit(optcat,eqt-opt_interval,opt_interval,bootloops,maepi);
    else
        opt_radius = NaN;
        opt_interval = NaN;
        alldt = NaN;
    end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Spherical Harmonic Modeling and Analysis Toolkit (SPHARM-MAT) is a 3D
% shape modeling and analysis toolkit.
% It is a software package developed at Shenlab in Center for Neuroimaging,
% Indiana University (SpharmMat@gmail.com, http://www.iupui.edu/~shenlab/)
% It is available to the scientific community as copyright freeware
% under the terms of the GNU General Public Licence.
%
% Copyright 2009, 2010, ShenLab, Center for Neuroimaging, Indiana University
%
% This file is part of SPHARM-MAT.
%
% SPHARM-MAT is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% SPHARM-MAT is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with SPHARM-MAT. If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% ============================================
% read_coef.m
% function for reading *.coef file and saving the coefficients to *_des.mat
% file

% INPUT: filename, which is the name of the input coef file.
% If not specifying a filename, a dialogue window will be activated for choosing a
% gipl file.
%
% Wan Jing
% 11/06/2008 - create
%============================================

function fvec = read_coef(filename)

fid = fopen(filename,'r');
if(fid<0)
    fprintf('could not open file %s\n',filename);
    return
end

Count = fscanf(fid, '{ %d,', 1);
if(isempty(Count))
    fprintf('The type of this file is not coef. \n');
    return;
end

% read the coefficients to C
C = fscanf(fid, '{%lf, %lf, %lf},\n', [3 inf]);
C = C';
fprintf('The number of coefficients are: %d .\n ',length(C));

if (Count ~= length(C))
    fprintf('The file has error about the number of coefficients');
end

fvec = [];
i = 1;

md = sqrt(length(C))-1;
for L = 0:md
    for M=-L:L
%         disp(sprintf('i=%d, L=%d, M=%d',i,L,M));
        switch sign(M)
            case -1
                fvec(end+1,:) = [0 0 0];
            case 0
                fvec(end+1,:) = C(i,:); i=i+1;
            case 1
                fvec(end+1,:) = complex(C(i,:),C(i+1,:)); i=i+2;
        end
    end
end

fclose('all');

return


function sys = sdpvar(varargin)
%SDPVAR Create symbolic decision variable
%
%   You can create a sdpvar variable by:
%     X = SDPVAR(n)               Symmetric nxn matrix
%     X = SDPVAR(n,n)             Symmetric nxn matrix
%     X = SDPVAR(n,m)             Full nxm matrix (n~=m)
%
%   Definition of multiple scalars can be simplified
%     SDPVAR x y z w
%
%   The parametrizations supported are
%     X = SDPVAR(n,n,'full')      Full nxn matrix
%     X = SDPVAR(n,n,'symmetric') Symmetric nxn matrix
%     X = SDPVAR(n,n,'toeplitz')  Symmetric Toeplitz
%     X = SDPVAR(n,n,'hankel')    Symmetric Hankel
%     X = SDPVAR(n,n,'skew')      Skew-symmetric
%
%   The letters 'sy','f','ha', 't' and 'sk' are searched for in the third argument
%   hence sdpvar(n,n,'toeplitz') gives the same result as sdpvar(n,n,'t')
%
%   Only square Toeplitz and Hankel matries are supported
%
%   A scalar is defined as a 1x1 matrix
%
%   Higher-dimensional matrices are also supported, although this currently
%   is an experimental feature with limited use. The type flag applies to
%   the lowest level slice.
%
%     X = SDPVAR(n,n,n,'full')      Full nxnxn matrix
%
%   In addition to the matrix type, a fourth argument
%   can be used to obtain a complex matrix. All the
%   matrix types above apply to a complex matrix, and
%   in addition a Hermitian type is added
%
%     X = SDPVAR(n,n,'hermitian','complex') Complex Hermitian nxn matrix (X=X'=conj(X.'))
%
%   The other types are obtained as above
%     X = SDPVAR(n,n,'symmetric','complex') Complex symmetric nxn matrix (X=X.')
%     X = SDPVAR(n,n,'full','complex')      Complex full nxn matrix
%     ... and the same for Toeplitz, Hankel and skew-symmetric
%
%   See also @SDPVAR/SET, INTVAR, BINVAR, methods('sdpvar'), SEE

superiorto('sdpvar');
if nargin==0
    return
end

if isstruct(varargin{1})
    sys = class(varargin{1},'ncvar');
    return
end

% To speed up dualization, we keep track of primal SDP cones
% [0 0] :  Nothing known (cleared in some operator, or none-cone to start with)
% [1 0] :  Primal cone
% [1 1] :  Primal cone + DOUBLE
% [1 2 x] :  Primal cone + SDPVAR
% [-1 1] : -Primal cone + DOUBLE
% [-1 2 x] : -Primal cone + SDPVAR

conicinfo = [0 0];

if ischar(varargin{1})
    switch varargin{1}
        case 'clear'
            disp('Obsolete comand');
            return
        case 'nvars'
            sys = yalmip('nvars');%THIS IS OBSAOLETE AND SHOULD NOT BE USED
            return
        otherwise
            n = length(varargin);
            varnames = varargin;
            for k = 1:n
                varcmd{k}='(1,1)';
                lp=strfind(varargin{k},'(');
                rp=strfind(varargin{k},')');
                if isempty(lp) & isempty(rp)
                    if ~isvarname(varargin{k})
                        error('Not a valid variable name.')
                    end
                else
                    if (~isempty(lp))&(~isempty(rp))
                        if min(lp)<max(rp)
                            varnames{k} = varargin{k}(1:lp-1);
                            varcmd{k}=varargin{k}(lp:rp);
                        else
                            error('Not a valid variable name.')
                        end
                    else
                        error('Not a valid variable name.')
                    end
                end
            end
            for k = 1:n
                if isequal(varnames{k},'i') | isequal(varnames{k},'j')
                    if length(dbstack) == 1
                        assignin('caller',varnames{k},eval(['sdpvar' varcmd{k}]));
                    else
                        error(['Due to a bug in MATLAB, use ' varnames{k} ' = sdpvar' varcmd{k} ' instead.']);
                    end
                else
                    assignin('caller',varnames{k},eval(['ncvar' varcmd{k}]));
                end
            end
            return
    end
end

% *************************************************************************
% Maybe new NDSDPVAR syntax
% *************************************************************************
if nargin > 2 & isa(varargin{3},'double') & ~isempty(varargin{3})
    sys = ndsdpvar(varargin{:});
    return
end

% Supported matrix types
% - symm
% - full
% - skew
% - hank
% - toep
switch nargin
    case 1 %Bug in MATLAB 5.3!! sdpvar called from horzcat!!!!????
        if isempty(varargin{1})
            sys = varargin{1};
            return
        end
        if isa(varargin{1},'sdpvar')
            sys = varargin{1};
            sys.typeflag = 0;
            return
        end
        n = varargin{1};
        m = varargin{1};
        if sum(n.*m)==0
            sys = zeros(n,m);
            return
        end
        if (n==m)
            matrix_type = 'symm';
            nvar = sum(n.*(n+1)/2);
            conicinfo = [1 0];
        else
            matrix_type = 'full';
            nvar = sum(n.*m);
        end
    case 2
        n = varargin{1};
        m = varargin{2};
        if length(n)~=length(m)
            error('The dimensions must have the same lengths')
        end
        if sum(n.*m)==0
            sys = zeros(n,m);
            return
        end
        if (n==m)
            matrix_type = 'symm';
            nvar = sum(n.*(n+1)/2);
            conicinfo = [1 0];
        else
            matrix_type = 'full';
            nvar = sum(n.*m);
        end
    case {3,4}
        n = varargin{1};
        m = varargin{2};
        if sum(n.*m)==0
            sys = zeros(n,m);
            return
        end

        % Check for complex or real
        if (nargin == 4)
            if isempty(varargin{4})
                varargin{4} = 'real';
            else
                if ~ischar(varargin{4})
                    help sdpvar
                    error('Fourth argument should be ''complex'' or ''real''')
                end
            end
            index_cmrl = strmatch_octavesafe(varargin{4},{'real','complex'});
            if isempty(index_cmrl)
                error('Fourth argument should be ''complex'' or ''real''. See help above')
            end
        else
            if ~ischar(varargin{3})
                help sdpvar
                error('Third argument should be ''symmetric'', ''full'', ''hermitian'',...See help above')
            end
            index_cmrl = 1;
        end;

        if isempty(varargin{3})
            if n==m
                index_type = 7; %Default symmetric
            else
                index_type = 4;
            end
        else
            if ~isempty(strmatch_octavesafe(varargin{3},{'complex','real'}))
                % User had third argument as complex or real
                error(['Third argument should be ''symmetric'', ''full'', ''toeplitz''... Maybe you meant sdpvar(n,n,''full'',''' varargin{3} ''')'])
            end
            index_type = strmatch_octavesafe(varargin{3},{'toeplitz','hankel','symmetric','full','rhankel','skew','hermitian'});
        end

        if isempty(index_type)
            error(['Matrix type "' varargin{3} '" not supported'])
        else
            switch index_type+100*(index_cmrl-1)
                case 1
                    if n~=m
                        error('Toeplitz matrix must be square')
                    else
                        matrix_type = 'toep';
                        nvar = n;
                    end

                case 2
                    if n~=m
                        error('Hankel matrix must be square')
                    else
                        matrix_type = 'hank';
                        nvar = n;
                    end

                case 3
                    if n~=m
                        error('Symmetric matrix must be square')
                    else
                        matrix_type = 'symm';
                        nvar = sum(n.*(n+1)/2);
                    end

                case 4
                    matrix_type = 'full';
                    nvar = sum(n.*m);
                    if nvar==1
                        matrix_type = 'symm';
                    end

                case 5
                    if n~=m
                        error('Hankel matrix must be square')
                    else
                        matrix_type = 'rhankel';
                        nvar = 2*n-1;
                    end

                case 6
                    if n~=m
                        error('Skew symmetric matrix must be square')
                    else
                        matrix_type = 'skew';
                        nvar = (n*(n+1)/2)-n;
                    end

                case 7
                    if n~=m
                        error('Symmetric matrix must be square')
                    else
                        matrix_type = 'symm';
                        nvar = n*(n+1)/2;
                    end

                case 101
                    if n~=m
                        error('Toeplitz matrix must be square')
                    else
                        matrix_type = 'toep complex';
                        nvar = 2*n;
                    end

                case 102
                    if n~=m
                        error('Hankel matrix must be square')
                    else
                        matrix_type = 'hank complex';
                        nvar = (2*n);
                    end

                case 103
                    if n~=m
                        error('Symmetric matrix must be square')
                    else
                        matrix_type = 'symm complex';
                        nvar = 2*n*(n+1)/2;
                    end

                case 104
                    matrix_type = 'full complex';
                    nvar = 2*n*m;
                    if nvar==1
                        matrix_type = 'symm complex';
                    end

                case 105
                    if n~=m
                        error('Hankel matrix must be square')
                    else
                        matrix_type = 'rhankel complex';
                        nvar = 2*(2*n-1);
                    end

                case 106
                    if n~=m
                        error('Skew symmetric matrix must be square')
                    else
                        matrix_type = 'skew complex';
                        nvar = 2*((n*(n+1)/2)-n);
                    end

                case 107
                    if n~=m
                        error('Hermitian matrix must be square')
                    else
                        matrix_type = 'herm complex';
                        nvar = n*(n+1)/2+(n*(n+1)/2-n);
                    end

                otherwise
                    error('Bug! Report!');
            end

        end

    case 5 % Fast version for internal use
        sys.basis = varargin{5};
        sys.lmi_variables=varargin{4};
        sys.dim(1) = varargin{1};
        sys.dim(2) = varargin{2};
        sys.typeflag = 0;
        sys.savedata = [];
        sys.extra = [];
        sys.extra.expanded = [];
        sys.extra.createTime = definecreationtime;
        sys.originalbasis = [];
        sys.leftfactors{1} = [];
        sys.rightfactors{1} = [];
        sys.midfactors{1} = [];
        sys.conicinfo = 0;

        % Find zero-variables
        constants = find(sys.lmi_variables==0);
        if ~isempty(constants);
            sys.lmi_variables(constants)=[];
            sys.basis(:,1) = sys.basis(:,1) + sum(sys.basis(:,1+constants),2);
            sys.basis(:,1+constants)=[];
        end
        if isempty(sys.lmi_variables)
            sys = full(reshape(sys.basis(:,1),sys.dim(1),sys.dim(2)));
        else
            sys = class(sys,'sdpvar');
        end
        return
    case 6 % Fast version for internal use
        sys.basis = varargin{5};
        sys.lmi_variables=varargin{4};
        sys.dim(1) = varargin{1};
        sys.dim(2) = varargin{2};
        sys.typeflag = varargin{6};
        sys.savedata = [];
        sys.extra = [];
        sys.extra.expanded = [];
        sys.extra.createTime = definecreationtime;
        sys.conicinfo = 0;
        sys.originalbasis = [];
        sys.leftfactors{1} = [];
        sys.rightfactors{1} = [];
        sys.midfactors{1} = [];
        % Find zero-variables
        constants = find(sys.lmi_variables==0);
        if ~isempty(constants);
            sys.lmi_variables(constants)=[];
            sys.basis(:,1) = sys.basis(:,1) + sum(sys.basis(:,1+constants),2);
            sys.basis(:,1+constants)=[];
        end
        if isempty(sys.lmi_variables)
            sys = full(reshape(sys.basis(:,1),sys.dim(1),sys.dim(2)));
        else
            sys = class(sys,'ncvar');
        end
        return
    case 7 % Fast version for internal use
        sys.basis = varargin{5};
        sys.lmi_variables=varargin{4};
        sys.dim(1) = varargin{1};
        sys.dim(2) = varargin{2};
        sys.typeflag = varargin{6};
        sys.savedata = [];
        sys.extra = varargin{7};
        sys.extra.expanded = [];
        sys.extra.createTime = definecreationtime;
        sys.conicinfo = varargin{7};
        % Find zero-variables
        constants = find(sys.lmi_variables==0);
        if ~isempty(constants);
            sys.lmi_variables(constants)=[];
            sys.basis(:,1) = sys.basis(:,1) + sum(sys.basis(:,1+constants),2);
            sys.basis(:,1+constants)=[];
        end
        if isempty(sys.lmi_variables)
            sys = full(reshape(sys.basis(:,1),sys.dim(1),sys.dim(2)));
        else
            sys = class(sys,'sdpvar');
        end
        return

    otherwise
        error('Wrong number of arguments in sdpvar creation');
end

if isempty(n) | isempty(m)
    error('Size must be integer valued')
end;
if ~((abs((n-ceil(n)))+ abs((m-ceil(m))))==0)
    error('Size must be integer valued')
end

nonCommutingTable = yalmip('nonCommutingTable');
[monomtable,variabletype] = yalmip('monomtable');
lmi_variables = (1:nvar)+max(size(nonCommutingTable,1),size(monomtable,1));

for blk = 1:length(n)
    switch matrix_type

        case 'full'
            basis{blk} = [spalloc(n(blk)*m(blk),1,0) speye(n(blk)*m(blk))];%speye(nvar)];

        case 'full complex'
            basis = [spalloc(n*m,1,0) speye(nvar/2) speye(nvar/2)*sqrt(-1)];

        case 'symm'
            if 0
                basis = spalloc(n^2,1+nvar,n^2);
                l = 2;
                an_empty = spalloc(n,n,2);
                for i=1:n
                    temp = an_empty;
                    temp(i,i)=1;
                    basis(:,l)=temp(:);
                    l = l+1;
                    for j=i+1:n,
                        temp = an_empty;
                        temp(i,j)=1;
                        temp(j,i)=1;
                        basis(:,l)=temp(:);
                        l = l+1;
                    end
                end
            else
                % Hrm...fast but completely f*d up

                Y = reshape(1:n(blk)^2,n(blk),n(blk));
                Y = tril(Y);
                Y = (Y+Y')-diag(sparse(diag(Y)));
                [uu,oo,pp] = unique(Y(:));
                if 1
                    basis{blk} = sparse(1:n(blk)^2,pp+1,1);
                else
                    basis{blk} = lazybasis(n^2,1+(n*(n+1)/2),1:n(blk)^2,pp+1,ones(n(blk)^2,1));
                end

            end

        case 'symm complex'
            basis = spalloc(n^2,1+nvar,2);
            l = 2;
            an_empty = spalloc(n,n,2);
            for i=1:n
                temp = an_empty;
                temp(i,i)=1;
                basis(:,l)=temp(:);
                l = l+1;
                for j=i+1:n,
                    temp = an_empty;
                    temp(i,j)=1;
                    temp(j,i)=1;
                    basis(:,l)=temp(:);
                    l = l+1;
                end
            end
            for i=1:n
                temp = an_empty;
                temp(i,i)=sqrt(-1);
                basis(:,l)=temp(:);
                l = l+1;
                for j=i+1:n,
                    temp = an_empty;
                    temp(i,j)=sqrt(-1);
                    temp(j,i)=sqrt(-1);
                    basis(:,l)=temp(:);
                    l = l+1;
                end
            end

        case 'herm complex'
            basis = spalloc(n^2,1+nvar,2);
            l = 2;
            an_empty = spalloc(n,n,2);
            for i=1:n
                temp = an_empty;
                temp(i,i)=1;
                basis(:,l)=temp(:);
                l = l+1;
                for j=i+1:n,
                    temp = an_empty;
                    temp(i,j)=1;
                    temp(j,i)=1;
                    basis(:,l)=temp(:);
                    l = l+1;
                end
            end
            for i=1:n
                for j=i+1:n,
                    temp = an_empty;
                    temp(i,j)=sqrt(-1);
                    temp(j,i)=-sqrt(-1);
                    basis(:,l)=temp(:);
                    l = l+1;
                end
            end

        case 'skew'
            basis = spalloc(n^2,1+nvar,2);
            l = 2;
            an_empty = spalloc(n,n,2);
            for i=1:n
                for j=i+1:n,
                    temp = an_empty;
                    temp(i,j)=1;
                    temp(j,i)=-1;
                    basis(:,l)=temp(:);
                    l = l+1;
                end
            end

        case 'skew complex'
            basis = spalloc(n^2,1+nvar,2);
            l = 2;
            an_empty = spalloc(n,n,2);
            for i=1:n
                for j=i+1:n,
                    temp = an_empty;
                    temp(i,j)=1;
                    temp(j,i)=-1;
                    basis(:,l)=temp(:);
                    l = l+1;
                end
            end
            for i=1:n
                for j=i+1:n,
                    temp = an_empty;
                    temp(i,j)=sqrt(-1);
                    temp(j,i)=-sqrt(-1);
                    basis(:,l)=temp(:);
                    l = l+1;
                end
            end

        case 'toep'
            basis = spalloc(n^2,1+nvar,2);
            an_empty = spalloc(n,1,1);
            for i=1:n,
                v = an_empty;
                v(i)=1;
                temp = sparse(toeplitz(v));
                basis(:,i+1) = temp(:);
            end

            % Notice, complex Toeplitz not Hermitian
        case 'toep complex'
            basis = spalloc(n^2,1+nvar,2);
            an_empty = spalloc(n,1,1);
            for i=1:n,
                v = an_empty;
                v(i)=1;
                temp = sparse(toeplitz(v));
                basis(:,i+1) = temp(:);
            end
            for i=1:n,
                v = an_empty;
                v(i)=sqrt(-1);
                temp = sparse(toeplitz(v));
                basis(:,n+i+1) = temp(:);
            end

        case 'hank'
            basis = spalloc(n^2,1+nvar,2);
            an_empty = spalloc(n,1,1);
            for i=1:n,
                v = an_empty;
                v(i)=1;
                temp = sparse(hankel(v));
                basis(:,i+1) = temp(:);
            end

        case 'hank complex'
            basis = spalloc(n^2,1+nvar,2);
            an_empty = spalloc(n,1,1);
            for i=1:n,
                v = an_empty;
                v(i)=1;
                temp = sparse(hankel(v));
                basis(:,i+1) = temp(:);
            end
            for i=1:n,
                v = an_empty;
                v(i)=sqrt(-1);
                temp = sparse(hankel(v));
                basis(:,n+i+1) = temp(:);
            end

        case 'rhankel'
            basis = spalloc(n^2,1+nvar,2);
            an_empty = spalloc(2*n-1,1,1);
            for i=1:nvar,
                v = an_empty;
                v(i)=1;
                temp = sparse(hankel(v(1:n),[v(n);v(n+1:2*n-1)]));
                basis(:,i+1) = temp(:);
            end

        case 'rhankel complex'
            basis = spalloc(n^2,1+nvar,2);
            an_empty = spalloc(2*n-1,1,1);
            for i=1:nvar/2,
                v = an_empty;
                v(i)=1;
                temp = sparse(hankel(v(1:n),[v(n);v(n+1:2*n-1)]));
                basis(:,i+1) = temp(:);
            end
            for i=1:nvar/2,
                v = an_empty;
                v(i)=sqrt(-1);
                temp = sparse(hankel(v(1:n),[v(n);v(n+1:2*n-1)]));
                basis(:,nvar/2+i+1) = temp(:);
            end

        otherwise
            error('Bug! Report')
    end

end

% Update noncommuting table and monomtables
nonCommutingTable(lmi_variables,1) = nan;
nonCommutingTable(lmi_variables,2) = lmi_variables;
yalmip('nonCommutingTable',nonCommutingTable);
variabletype(lmi_variables) = 0;
monomtable(lmi_variables(end),lmi_variables(end)) = 0;
yalmip('setmonomtable',monomtable,variabletype);
%if strcmp(matrix_type,'NonHermitian')
%    yalmip('setNonHermitianNonCommuting',lmi_variables);
%end

% Create an object
if isa(basis,'cell')
    top = 1;
    for blk = 1:length(n)
        sys{blk}.basis=basis{blk};
        nn = size(sys{blk}.basis,2)-1;
        sys{blk}.lmi_variables = lmi_variables(top:top+nn-1);
        top = top + nn;
        sys{blk}.dim(1) = n(blk);
        sys{blk}.dim(2) = m(blk);
        sys{blk}.typeflag = 0;
        sys{blk}.savedata = [];
        sys{blk}.extra = [];
        sys{blk}.extra.expanded = [];
        sys{blk}.extra.createTime = definecreationtime;
        sys{blk}.conicinfo = conicinfo;
        sys{blk}.originalbasis = [];
        sys{blk}.leftfactors{1} = [];
        sys{blk}.rightfactors{1} = [];
        sys{blk}.midfactors{1} = [];
        sys{blk} = class(sys{blk},'ncvar');
    end
    if length(n)==1
        sys = sys{1};
    end
else
    sys.basis=basis;
    sys.lmi_variables = lmi_variables;
    sys.dim(1) = n;
    sys.dim(2) = m;
    sys.typeflag = 0;
    sys.savedata = [];
    sys.extra = [];
    sys.extra.expanded = [];
    sys.extra.createTime = definecreationtime;
    sys.conicinfo = conicinfo;
    sys.originalbasis = [];
    sys.leftfactors{1} = [];
    sys.rightfactors{1} = [];
    sys.midfactors{1} = [];
    sys = class(sys,'ncvar');
    if ~isreal(basis)
        % Add internal information about complex pairs
        complex_elements = find(any(imag(basis),2));
        complex_pairs = [];
        for i = 1:length(complex_elements)
            complex_pairs = [complex_pairs;lmi_variables(find(basis(complex_elements(i),:))-1)];
        end
        complex_pairs = uniquesafe(complex_pairs,'rows');
        yalmip('addcomplexpair',complex_pairs);
    end
end


function fem = update10nbr(fem_struct,jj)
% update10nbr takes a node with 10 elements connected to it
%  (which defines a region) and addes 3 nodes and 6 elements to
%  that region then regroups so that no node has more than six
%  elements connected to it.  The new nodes and elements are sprung.
%
%  NOTE:  fem_struct.nei must be a component.
%
%  Variables
%  fem_struct -- finite element structure from opnml.
%  jj -- the node number that has 10 elements connected to it.
%  fem -- the updated finite element structure.
%        Note:  Only fem.x,fem.y,fem.z,fem.e, and fem.nei
%               get updated.  fem.bnd does not need to be updated.
%
%  Usage -- fem = update10nbr(fem_struct,jj)
%
%  Name: update10nbr.m
%  Written by: Ben Holladay (SEAP Student, 2004)
%  Date: June 22,2004
%  Modified:  Aug. 30, 2004 -- Chris Massey
% Last Modifed:
%         Sept. 19, 2006 -- Chris Massey, NRL Code 7322, S.S.C. MS 39529
%                      % Add test to make sure the correct nodal neighbor
%                        connectivity existed for the requested update
%                        and added a test to create the nodal neighbor list
%                        if not present.
%         April 9, 2007 -- Chris Massey, NRL Code 7322
%                     Fixed an apparent bug in the code that updates
%                     nodal neighbor list.
%         Aug. 13, 2007 -- Chris Massey, NRL Code 7322
%                     Fixed a bug in the code that updates nodal neighbor
%                     list for nodes getting extra connectivity.
%         July 14, 2008 -- Chris Massey, NRL Code 7322
%               moved test for 4 neighbors to after the test for nei.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Checks if the fem_struct has an nei. If not one is generated.
try
    nei = fem_struct.nei;
catch
    fem_struct.nei = ele2nei(fem_struct.e,fem_struct.x,fem_struct.y);
    nei = fem_struct.nei;
end

tempnc = sum(~ismember((fem_struct.nei(jj,:)),0));
if tempnc ~= 10
    error(['There are ',num2str(tempnc),' nodal neighbors for node number ',...
           num2str(jj),'. This routine only works for 10 nodal neighbors.']);
end

%Sets the intial fem_struct variables.
x = fem_struct.x;
y = fem_struct.y;
z = fem_struct.z;
enodes = fem_struct.e;
vod = [1:10,1:10];

%Adds new nodes and elements and labels the bad node, neighboring nodes,
%and neighboring elements.
[nnodes,nc] = size(nei);
nelems = size(enodes,1);
newnode = nnodes + [1:3];
newelem = nelems + [1:6];
badnode = jj;
nbrnode = nei(jj,1:10);
[nbrelem,~] = find(enodes == badnode);
inrad = max(sqrt((x(nbrnode)-x(badnode)).^2 + (y(nbrnode)-y(badnode)).^2));

%First guess of the new coordinates and bathymetry.
Bx = sum(x([(nbrnode([1:4])),badnode]))./5;
By = sum(y([(nbrnode([1:4])),badnode]))./5;
Bz = sum(z([(nbrnode([1:4])),badnode]))./5;

Dx = sum(x([(nbrnode([6:9])),badnode]))./5;
Dy = sum(y([(nbrnode([6:9])),badnode]))./5;
Dz = sum(z([(nbrnode([6:9])),badnode]))./5;

Cx = sum([(x([(nbrnode([4:6])),badnode]))',Bx,Dx])./6;
Cy = sum([(y([(nbrnode([4:6])),badnode]))',By,Dy])./6;
Cz = sum([(z([(nbrnode([4:6])),badnode]))',Bz,Dz])./6;

Ax = sum([(x([nbrnode([9,10,1])]))',Bx,Dx,Cx])./6;
Ay = sum([(y([nbrnode([9,10,1])]))',By,Dy,Cy])./6;
Az = sum([(z([nbrnode([9,10,1])]))',Bz,Dz,Cz])./6;

x(badnode) = Ax;
y(badnode) = Ay;
z(badnode) = Az;
x(newnode(1)) = Bx;
y(newnode(1)) = By;
z(newnode(1)) = Bz;
x(newnode(2)) = Cx;
y(newnode(2)) = Cy;
z(newnode(2)) = Cz;
x(newnode(3)) = Dx;
y(newnode(3)) = Dy;
z(newnode(3)) = Dz;

%Updates the effected elements. J variables are used to represent which
%elements are connected each node.
i = 1;
while i <= 8
    j1 = ismember((enodes((nbrelem),:)),(nbrnode(vod(i))));
    j2 = ismember((enodes((nbrelem),:)),(nbrnode(vod(i+1))));
    spb = [1,1,1,2,2,3,3,3];
    j = sum([j1,j2],2);
    temp = nbrelem(find (j == 2));
    if ~isempty(temp)
        enodes(temp(1),:) = ([nbrnode(vod(i)),nbrnode(vod(i+1)),newnode(spb(i))]);
    end
    i = i + 1;
    clear j j1 j2 temp;
end

%Addes the 6 new elements.
enodes(newelem(1),:) = [nbrnode(4),newnode(2),newnode(1)];
enodes(newelem(2),:) = [badnode,newnode(1),newnode(2)];
enodes(newelem(3),:) = [nbrnode(1),newnode(1),badnode];
enodes(newelem(4),:) = [nbrnode(6),newnode(3),newnode(2)];
enodes(newelem(5),:) = [badnode,newnode(2),newnode(3)];
enodes(newelem(6),:) = [nbrnode(9),badnode,newnode(3)];

%Springs the region.
M(1,:) = [Ax,Ay,Az];
M(2,:) = [Bx,By,Bz];
M(3,:) = [Cx,Cy,Cz];
M(4,:) = [Dx,Dy,Dz];

imax = 20;
stoptol = 10e-8 * inrad;
i = 1;
tol = stoptol + 10;
while i <= imax && tol > stoptol
    M2(2,1) = sum([(x([(nbrnode([1:4])),badnode]))',M(3,1)]);
    M2(2,2) = sum([(y([(nbrnode([1:4])),badnode]))',M(3,2)]);
    M2(2,3) = sum([(z([(nbrnode([1:4])),badnode]))',M(3,3)]);

    M2(4,1) = sum([(x([(nbrnode([6:9])),badnode]))',M(3,1)]);
    M2(4,2) = sum([(y([(nbrnode([6:9])),badnode]))',M(3,2)]);
    M2(4,3) = sum([(z([(nbrnode([6:9])),badnode]))',M(3,3)]);

    M2(3,1) = sum([(x([(nbrnode([4:6])),badnode]))',M(2,1),M(4,1)]);
    M2(3,2) = sum([(y([(nbrnode([4:6])),badnode]))',M(2,2),M(4,2)]);
    M2(3,3) = sum([(z([(nbrnode([4:6])),badnode]))',M(2,3),M(4,3)]);

    M2(1,1) = sum([(x([nbrnode([9,10,1])]))',M(2,1),M(3,1),M(4,1)]);
    M2(1,2) = sum([(y([nbrnode([9,10,1])]))',M(2,2),M(3,2),M(4,2)]);
    M2(1,3) = sum([(z([nbrnode([9,10,1])]))',M(2,3),M(3,3),M(4,3)]);
    M2 = M2 ./ 6;

    tol = max(sqrt((M2(:,1)-M(:,1)).^2 + (M2(:,2)-M(:,2)).^2));

    M = M2;
    x([badnode,([newnode(1:3)])]) = ([M(:,1)]);
    y([badnode,([newnode(1:3)])]) = ([M(:,2)]);
    z([badnode,([newnode(1:3)])]) = ([M(:,3)]);
    i = i+1;
end

%Creates the output structure.
fem = fem_struct;
fem.e = enodes;
fem.x = x;
fem.y = y;
fem.z = z;
fem.nei = nei;

%Update the connectivity for the bad node and adds the connections for
%the new nodes.
fem.nei(badnode,1:nc) = ([newnode([1:3]),nbrnode([9,10,1]),zeros(1,nc-6)]);
fem.nei(newnode(1),1:nc) = ([badnode,nbrnode(1:4),newnode(2),zeros(1,nc-6)]);
fem.nei(newnode(2),1:nc) = ([badnode,newnode(1),nbrnode(4:6),newnode(3),zeros(1,nc-6)]);
fem.nei(newnode(3),1:nc) = ([badnode,newnode(2),nbrnode(6:9),zeros(1,nc-6)]);

%Updates the nei for neighbor nodes. These nodes get an additional
%connector.
spb = ([1,4,6,9]);
for i = 1:4
    addconn = ([badnode,newnode([1:3]),badnode]);
    tmp = nei(nbrnode(spb(i)),:);
	ij = find(tmp == badnode);
    ik = min((find(tmp == 0)) + 1);
    if isempty(ik)  % TCM 08/13/2007 -- Added this check in case ik was empty.
        ik= size(nei,2)+1;
    end
    fem.nei(nbrnode(spb(i)),1:ik) = ([tmp(1:(ij-1)),addconn(i+1),addconn(i),tmp((ij+1):(ik-1))]);
end

%Updates the nei for neighbor nodes. These nodes get different connector.
spn = ([2,3,5,7,8]);
for i = 1:5
    addnode = ([newnode(1),newnode(1),newnode(2),newnode(3),newnode(3)]);
    tmp = nei(nbrnode(spn(i)),:);
    ij = find(tmp == badnode);
    % TCM 04/09/2007 -- Begin
    tmp2 = ([tmp(1:(ij-1)),addnode(i),tmp((ij+1):end)]);
    fem.nei(nbrnode(spn(i)),:) = 0;  %Zero out the list
    fem.nei(nbrnode(spn(i)),1:length(tmp2)) = tmp2; %Fill in the list
    %fem.nei(nbrnode(spn(i)),1:end) = ([tmp(1:(ij-1)),addnode(i),tmp((ij+1):end)]);
    % TCM 04/09/2007 -- End
end

%Determine the triqual for the final form to see if has very low quality
%elements.
tmp1 = (nbrelem);
tempL3=(fem.x(fem.e(tmp1,1))-fem.x(fem.e(tmp1,2))).^2+(fem.y(fem.e(tmp1,1))-...
   fem.y(fem.e(tmp1,2))).^2;
tempL1=(fem.x(fem.e(tmp1,2))-fem.x(fem.e(tmp1,3))).^2+(fem.y(fem.e(tmp1,2))-...
   fem.y(fem.e(tmp1,3))).^2;
tempL2=(fem.x(fem.e(tmp1,3))-fem.x(fem.e(tmp1,1))).^2+(fem.y(fem.e(tmp1,3))-...
   fem.y(fem.e(tmp1,1))).^2;
tempLs = ([tempL1 + tempL2 + tempL3]);
xnodes = fem.x(fem.e(tmp1,:));
ynodes = fem.y(fem.e(tmp1,:));
temparea = 0.5*(xnodes(:,1).*(ynodes(:,2)-ynodes(:,3))+xnodes(:,2).*...
   (ynodes(:,3)-ynodes(:,1))+xnodes(:,3).*(ynodes(:,1)-ynodes(:,2)));
fem.ar(tmp1) = temparea;
tempq = (4 * sqrt(3) * temparea) ./ tempLs;
clear tempL3 tempL2 tempL1 tempLs xnodes ynodes temparea;

fem1 = fem;
%Identify very low quality elements and attempts to use a line swap to fix.
poor = find(tempq < .1);
nflag = 1;good = 1;
if ~isempty(poor)
   for it = 1:length(poor)
      je = tmp1(poor(it));
      nodes = fem.e(je,:);
      nflag = 0;

      %Determine the angles to see if an edge can be fliped
      a2 = (x(enodes(je,3))-x(enodes(je,2))).^2+(y(enodes(je,3))-y(enodes(je,2))).^2;
      b2 = (x(enodes(je,1))-x(enodes(je,3))).^2+(y(enodes(je,1))-y(enodes(je,3))).^2;
      c2 = (x(enodes(je,2))-x(enodes(je,1))).^2+(y(enodes(je,2))-y(enodes(je,1))).^2;
      A = (180/pi)*acos((b2+c2-a2)./(2*sqrt(b2).*sqrt(c2)));
      B = (180/pi)*acos((c2+a2-b2)./(2*sqrt(c2).*sqrt(a2)));
      C = (180/pi)*acos((a2+b2-c2)./(2*sqrt(a2).*sqrt(b2)));
      [test,ind] = max([A,B,C]);
      if test > 160
         %Find the element numbers to flip the edge.
         nogood = nodes(ind);
         swap = setdiff(nodes,nogood);
         [temp1,tc] = find(enodes == swap(1) | enodes == swap(2));
         [b,j,k] = unique(temp1);
         temp2 = setdiff(1:length(temp1),j);
         swap = temp1(temp2);
         try
            fem1 = line_swap(fem,swap(1),swap(2));
         catch
            fem1 = fem;
         end

         %Spring the new mesh after the line swap.
         for itt = 1:2
             temp = find(fem1.nei(newnode(1),:) ~= 0);
             tempnei = fem1.nei(newnode(1),temp);
             fem1.x(newnode(1)) = mean(fem1.x(tempnei));
             fem1.y(newnode(1)) = mean(fem1.y(tempnei));
             fem1.z(newnode(1)) = mean(fem1.z(tempnei));
             temp = find(fem1.nei(newnode(2),:) ~= 0);
             tempnei = fem1.nei(newnode(2),temp);
             fem1.x(newnode(2)) = mean(fem1.x(tempnei));
             fem1.y(newnode(2)) = mean(fem1.y(tempnei));
             fem1.z(newnode(2)) = mean(fem1.z(tempnei));
             temp = find(fem1.nei(newnode(3),:) ~= 0);
             tempnei = fem1.nei(newnode(3),temp);
             fem1.x(newnode(3)) = mean(fem1.x(tempnei));
             fem1.y(newnode(3)) = mean(fem1.y(tempnei));
             fem1.z(newnode(3)) = mean(fem1.z(tempnei));
             temp = find(fem1.nei(badnode,:) ~= 0);
             tempnei = fem1.nei(badnode,temp);
             fem1.x(badnode) = mean(fem1.x(tempnei));
             fem1.y(badnode) = mean(fem1.y(tempnei));
             fem1.z(badnode) = mean(fem1.z(tempnei));
         end

         %Use triqual to determine if the new mesh is better quality.
         tmp1 = (nbrelem);
         tempL3=(fem1.x(fem1.e(tmp1,1))-fem1.x(fem1.e(tmp1,2))).^2+(fem1.y(fem1.e(tmp1,1))-...
            fem1.y(fem1.e(tmp1,2))).^2;
         tempL1=(fem1.x(fem1.e(tmp1,2))-fem1.x(fem1.e(tmp1,3))).^2+(fem1.y(fem1.e(tmp1,2))-...
            fem1.y(fem1.e(tmp1,3))).^2;
         tempL2=(fem1.x(fem1.e(tmp1,3))-fem1.x(fem1.e(tmp1,1))).^2+(fem1.y(fem1.e(tmp1,3))-...
            fem1.y(fem1.e(tmp1,1))).^2;
         tempLs = ([tempL1 + tempL2 + tempL3]);
         xnodes = fem1.x(fem1.e(tmp1,:));
         ynodes = fem1.y(fem1.e(tmp1,:));
         temparea = 0.5*(xnodes(:,1).*(ynodes(:,2)-ynodes(:,3))+xnodes(:,2).*...
            (ynodes(:,3)-ynodes(:,1))+xnodes(:,3).*(ynodes(:,1)-ynodes(:,2)));
         fem1.ar(tmp1) = temparea;
         tempq = (4 * sqrt(3) * temparea) ./ tempLs;
         clear tempL3 tempL2 tempL1 tempLs xnodes ynodes temparea;
         if min(tempq) > .4
            fem = fem1;
            nflag = 1;
         else
            nflag = 0;
            good = 6;
         end
      end
   end
end

% If problems with new mesh, then retriangulate

%Correct output if line swap failed to run
if sum(nflag) == 0
   fem = patch_update(fem_struct,fem1,jj);
   good = 6;
end

% Correct output if invalid elements were created.
if sum(fem.ar < 0) > 0
   fem = patch_update(fem_struct,fem1,jj);
   good = 6;
end

%Display message if invalid elements where created.
if good == 6
   disp('The nodally updated mesh contained invalid or badly conditioned');
   disp('elements, therefore the patch was retriangulated which should');
   disp('reduce the connecitivity but is not guaranteed to do so.');
end
if sum(fem.ar < 0) > 0
   disp(' ');
   disp('returning original mesh');
   fem = fem_struct;
end

return


function [EI,Pth,Qthth] = lme_mass_RgEI1(X,Zcols,W,CBhat,L,phi,ni,G,GDa,GDb)
% [EI,Pth,Qthth] = lme_mass_RgEI1(X,Zcols,W,CBhat,L,phi,ni,G,GDa,GDb)
%
% Expected information matrix of the restricted log-likelihood of a whole
% region. This is less computationally efficient than lme_mass_RgEI
% but more numerically stable.
%
% Input
% X: Ordered design Matrix (according to time for each subject).
% Zcols: Vector with the indices of the colums of X that will be considered
% as random effects.
% W: Inverses of the estimated temporal covariance matrices for each
% subject stacked in W.
% CBhat: Asymptotic covariance matrix of the fixed effects.
% L: Cholesky factor of the covariance matrix of the random effects (D).
% phi: Within-subject standard deviation of the errors.
% ni: Vector whose entries are the number of repeated measures for each
% subject in the study (ordered according to X).
% G: Spatial covariance matrix.
% GDa: Derivative of the spatial covariance matrix for the first spatial
% parameter.
% GDb: Derivative of the spatial covariance matrix for the second spatial
% parameter (empty for spatial models with a single parameter).
%
% Output
% EI: Expected information matrix.
% Pth,Qthth: Matrices that are useful for inferences on the fixed effects.
%
% $Revision: 1.2 $  $Date: 2015/01/06 17:14:55 $
% Original Author: Jorge Luis Bernal Rusiel
% CVS Revision Info:
%    $Author: mreuter $
%    $Date: 2015/01/06 17:14:55 $
%    $Revision: 1.2 $
% References: Bernal-Rusiel J.L., Greve D.N., Reuter M., Fischl B., Sabuncu
% M.R., 2012. Statistical Analysis of Longitudinal Neuroimage Data with Linear
% Mixed Effects Models, NeuroImage, doi:10.1016/j.neuroimage.2012.10.065.
%
m = length(ni);
n = sum(ni);
nv = size(G,1);
q = length(Zcols);
nth = q*(q+1)/2+1;
p = size(X,2);
Pth = zeros(nth,p,p);
Qthth = zeros(nth,nth,p,p);
Der = zeros(nth,n,max(ni));
%Computation of the first order derivatives of the temporal covariance matrix
jk = 0;
for k=1:q
    for j=1:k
        jk = jk + 1;
        posi = 1;
        for i=1:m
            posf = posi+ni(i)-1;
            Zi = X(posi:posf, Zcols);
            Zki = Zi(:,k);
            Mjki = Zki*L(j,:)*Zi';
            Mjki = Mjki + Mjki';
            Der(jk,posi:posf,1:ni(i)) = Mjki;
            posi = posf+1;
        end;
    end;
end;
posi = 1;
for i=1:m
    posf = posi+ni(i)-1;
    Der(nth,posi:posf,1:ni(i)) = 2*phi*eye(ni(i));
    posi = posf+1;
end;
%Computation of Pis,Qijs and the expected information matrix EI.
for j=1:nth
    posi = 1; Pj = 0;
    Bj = squeeze(Der(j,:,:));
    for i=1:m
        posf = posi+ni(i)-1;
        Wi = W(posi:posf,1:ni(i));
        Pj = Pj - X(posi:posf,:)'*Wi*Bj(posi:posf,1:ni(i))*Wi*X(posi:posf,:);
        posi = posf+1;
    end;
    Pth(j,:,:) = Pj;
end;
EI = zeros(nth+2,nth+2);
%Expected information among Lijs (including phi)
for k=1:nth
    Bk = squeeze(Der(k,:,:));
    Pk = squeeze(Pth(k,:,:));
    for j=1:k
        Bj = squeeze(Der(j,:,:));
        Pj = squeeze(Pth(j,:,:));
        posi = 1; Qkj = 0;
        traceBkj = 0;
        for i=1:m
            posf = posi+ni(i)-1;
            Wi = W(posi:posf,1:ni(i));
            Bkji = Wi*Bk(posi:posf,1:ni(i))*Wi*Bj(posi:posf,1:ni(i));
            traceBkj = traceBkj + trace(Bkji);
            Bkji = Bkji*Wi;
            Qkji = X(posi:posf,:)'*Bkji*X(posi:posf,:);
            Qkj = Qkj + Qkji;
            posi = posf+1;
        end;
        Qthth(k,j,:,:) = Qkj;
        Qthth(j,k,:,:) = Qkj;
        EI(k,j) = nv*(traceBkj - trace(CBhat*(2*Qkj-Pk*CBhat*Pj)));
        EI(j,k) = EI(k,j);
    end;
end;
%Expected information between Lijs (including phi) and a (first spatial parameter)
Mauxa = G\GDa;
trMauxa = trace(Mauxa);
for k=1:nth
    Bk = squeeze(Der(k,:,:));
    Pk = squeeze(Pth(k,:,:));
    posi = 1;
    traceBk = 0;
    for i=1:m
        posf = posi+ni(i)-1;
        Wi = W(posi:posf,1:ni(i));
        Bki = Wi*Bk(posi:posf,1:ni(i));
        traceBk = traceBk + trace(Bki);
        posi = posf+1;
    end;
    EI(k,nth+1) = trMauxa*(traceBk + trace(CBhat*Pk));
    EI(nth+1,k) = EI(k,nth+1);
end
%Expected information between a and a
EI(nth+1,nth+1) = (n-p)*trace(Mauxa*Mauxa);
if ~isempty(GDb)
    %Expected information between Lijs (including phi) and b (second spatial parameter)
    Mauxb = G\GDb;
    trMauxb = trace(Mauxb);
    for k=1:nth
        Bk = squeeze(Der(k,:,:));
        Pk = squeeze(Pth(k,:,:));
        posi = 1;
        traceBk = 0;
        for i=1:m
            posf = posi+ni(i)-1;
            Wi = W(posi:posf,1:ni(i));
            Bki = Wi*Bk(posi:posf,1:ni(i));
            traceBk = traceBk + trace(Bki);
            posi = posf+1;
        end;
        EI(k,nth+2) = trMauxb*(traceBk + trace(CBhat*Pk));
        EI(nth+2,k) = EI(k,nth+2);
    end
    %Expected information between b and b
    EI(nth+2,nth+2) = (n-p)*trace(Mauxb*Mauxb);
    %Expected information between a and b
    EI(nth+1,nth+2) = (n-p)*trace(Mauxa*Mauxb);
    EI(nth+2,nth+1) = EI(nth+1,nth+2);
else
    EI = EI(1:nth+1,1:nth+1);
end;
EI = 0.5*EI;


function U = som_umat(sMap, varargin)

%SOM_UMAT Compute unified distance matrix of self-organizing map.
%
% U = som_umat(sMap, [argID, value, ...])
%
%  U = som_umat(sMap);
%  U = som_umat(M,sTopol,'median','mask',[1 1 0 1]);
%
%  Input and output arguments ([]'s are optional):
%   sMap     (struct) map struct or
%            (matrix) the codebook matrix of the map
%   [argID,  (string) See below. The values which are unambiguous can
%    value]  (varies) be given without the preceeding argID.
%
%   U        (matrix) u-matrix of the self-organizing map
%
% Here are the valid argument IDs and corresponding values. The values which
% are unambiguous (marked with '*') can be given without the preceeding argID.
%   'mask'       (vector) size dim x 1, weighting factors for different
%                         components (same as BMU search mask)
%   'msize'      (vector) map grid size
%   'topol'     *(struct) topology struct
%   'som_topol','sTopol' = 'topol'
%   'lattice'   *(string) map lattice, 'hexa' or 'rect'
%   'mode'      *(string) 'min','mean','median','max', default is 'median'
%
% NOTE! the U-matrix is always calculated for 'sheet'-shaped map and
% the map grid must be at most 2-dimensional.
%
% For more help, try 'type som_umat' or check out online documentation.
% See also SOM_SHOW, SOM_CPLANE.

%%%%%%%%%%%%% DETAILED DESCRIPTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% som_umat
%
% PURPOSE
%
% Computes the unified distance matrix of a SOM.
%
% SYNTAX
%
%  U = som_umat(sM)
%  U = som_umat(...,'argID',value,...)
%  U = som_umat(...,value,...)
%
% DESCRIPTION
%
% Compute and return the unified distance matrix of a SOM.
% For example a case of 5x1 -sized map:
%            m(1) m(2) m(3) m(4) m(5)
% where m(i) denotes one map unit. The u-matrix is a 9x1 vector:
%    u(1) u(1,2) u(2) u(2,3) u(3) u(3,4) u(4) u(4,5) u(5)
% where u(i,j) is the distance between map units m(i) and m(j)
% and u(k) is the mean (or minimum, maximum or median) of the
% surrounding values, e.g. u(3) = (u(2,3) + u(3,4))/2.
%
% Note that the u-matrix is always calculated for 'sheet'-shaped map and
% the map grid must be at most 2-dimensional.
%
% REFERENCES
%
% Ultsch, A., Siemon, H.P., "Kohonen's Self-Organizing Feature Maps
%   for Exploratory Data Analysis", in Proc. of INNC'90,
%   International Neural Network Conference, Dordrecht,
%   Netherlands, 1990, pp. 305-308.
% Kohonen, T., "Self-Organizing Map", 2nd ed., Springer-Verlag,
%    Berlin, 1995, pp. 117-119.
% Iivarinen, J., Kohonen, T., Kangas, J., Kaski, S., "Visualizing
%   the Clusters on the Self-Organizing Map", in proceedings of
%   Conference on Artificial Intelligence Research in Finland,
%   Helsinki, Finland, 1994, pp. 122-126.
% Kraaijveld, M.A., Mao, J., Jain, A.K., "A Nonlinear Projection
%   Method Based on Kohonen's Topology Preserving Maps", IEEE
%   Transactions on Neural Networks, vol. 6, no. 3, 1995, pp. 548-559.
%
% REQUIRED INPUT ARGUMENTS
%
%  sM (struct) SOM Toolbox struct or the codebook matrix of the map.
%     (matrix) The matrix may be 3-dimensional in which case the first
%              two dimensions are taken for the map grid dimensions (msize).
%
% OPTIONAL INPUT ARGUMENTS
%
%  argID (string) Argument identifier string (see below).
%  value (varies) Value for the argument (see below).
%
%  The optional arguments are given as 'argID',value -pairs. If the
%  value is unambiguous, it can be given without the preceeding argID.
%  If an argument is given value multiple times, the last one is used.
%
%  Below is the list of valid arguments:
%   'mask'      (vector) mask to be used in calculating
%                        the interunit distances, size [dim  1]. Default is
%                        the one in sM (field sM.mask) or a vector of
%                        ones if only a codebook matrix was given.
%   'topol'     (struct) topology of the map. Default is the one
%                        in sM (field sM.topol).
%   'sTopol','som_topol' (struct) = 'topol'
%   'msize'     (vector) map grid dimensions
%   'lattice'   (string) map lattice 'rect' or 'hexa'
%   'mode'      (string) 'min', 'mean', 'median' or 'max'
%                        Map unit value computation method. In fact,
%                        eval-function is used to evaluate this, so
%                        you can give other computation methods as well.
%                        Default is 'median'.
%
% OUTPUT ARGUMENTS
%
%  U   (matrix) the unified distance matrix of the SOM
%               size 2*n1-1 x 2*n2-1, where n1 = msize(1) and n2 = msize(2)
%
% EXAMPLES
%
%  U = som_umat(sM);
%  U = som_umat(sM.codebook,sM.topol,'median','mask',[1 1 0 1]);
%  U = som_umat(rand(10,10,4),'hexa','rect');
%
% SEE ALSO
%
%  som_show    show the selected component planes and the u-matrix
%  som_cplane  draw a 2D unified distance matrix

% Copyright (c) 1997-2000 by the SOM toolbox programming team.
% http://www.cis.hut.fi/projects/somtoolbox/

% Version 1.0beta juuso 260997
% Version 2.0beta juuso 151199, 151299, 200900

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% check arguments

error(nargchk(1, Inf, nargin));  % check no. of input arguments is correct

% sMap
if isstruct(sMap),
  M = sMap.codebook;
  sTopol = sMap.topol;
  mask = sMap.mask;
elseif isnumeric(sMap),
  M = sMap;
  si = size(M);
  dim = si(end);
  if length(si)>2, msize = si(1:end-1);
  else msize = [si(1) 1];
  end
  munits = prod(msize);
  sTopol = som_set('som_topol','msize',msize,'lattice','rect','shape','sheet');
  mask = ones(dim,1);
  M = reshape(M,[munits,dim]);
end
mode = 'median';

% varargin
i=1;
while i<=length(varargin),
  argok = 1;
  if ischar(varargin{i}),
    switch varargin{i},
      % argument IDs
     case 'mask',       i=i+1; mask = varargin{i};
     case 'msize',      i=i+1; sTopol.msize = varargin{i};
     case 'lattice',    i=i+1; sTopol.lattice = varargin{i};
     case {'topol','som_topol','sTopol'}, i=i+1; sTopol = varargin{i};
     case 'mode',       i=i+1; mode = varargin{i};
      % unambiguous values
     case {'hexa','rect'}, sTopol.lattice = varargin{i};
     case {'min','mean','median','max'}, mode = varargin{i};
     otherwise argok=0;
    end
  elseif isstruct(varargin{i}) && isfield(varargin{i},'type'),
    switch varargin{i}(1).type,
     case 'som_topol', sTopol = varargin{i};
     case 'som_map',   sTopol = varargin{i}.topol;
     otherwise argok=0;
    end
  else
    argok = 0;
  end
  if ~argok,
    disp(['(som_umat) Ignoring invalid argument #' num2str(i+1)]);
  end
  i = i+1;
end

% check
[munits dim] = size(M);
if prod(sTopol.msize)~=munits,
  error('Map grid size does not match the number of map units.')
end
if length(sTopol.msize)>2,
  error('Can only handle 1- and 2-dimensional map grids.')
end
if prod(sTopol.msize)==1,
  warning('Only one codebook vector.'); U = []; return;
end
if ~strcmp(sTopol.shape,'sheet'),
  disp(['The ' sTopol.shape ' shape of the map ignored. Using sheet instead.']);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% initialize variables

y = sTopol.msize(1);
x = sTopol.msize(2);
lattice = sTopol.lattice;
shape = sTopol.shape;
M = reshape(M,[y x dim]);

ux = 2 * x - 1;
uy = 2 * y - 1;
U  = zeros(uy, ux);

calc = sprintf('%s(a)',mode);

if size(mask,2)>1, mask = mask'; end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% u-matrix computation

% distances between map units

if strcmp(lattice, 'rect'), % rectangular lattice

  for j=1:y, for i=1:x,
      if i<x,
	dx = (M(j,i,:) - M(j,i+1,:)).^2; % horizontal
	U(2*j-1,2*i) = sqrt(mask'*dx(:));
      end
      if j<y,
	dy = (M(j,i,:) - M(j+1,i,:)).^2; % vertical
	U(2*j,2*i-1) = sqrt(mask'*dy(:));
      end
      if j<y && i<x,
	dz1 = (M(j,i,:) - M(j+1,i+1,:)).^2; % diagonals
	dz2 = (M(j+1,i,:) - M(j,i+1,:)).^2;
	U(2*j,2*i) = (sqrt(mask'*dz1(:))+sqrt(mask'*dz2(:)))/(2 * sqrt(2));
      end
    end
  end

elseif strcmp(lattice, 'hexa') % hexagonal lattice

  for j=1:y,
    for i=1:x,
      if i<x,
	dx = (M(j,i,:) - M(j,i+1,:)).^2; % horizontal
	U(2*j-1,2*i) = sqrt(mask'*dx(:));
      end

      if j<y, % diagonals
	dy = (M(j,i,:) - M(j+1,i,:)).^2;
	U(2*j,2*i-1) = sqrt(mask'*dy(:));

	if rem(j,2)==0 && i<x,
	  dz= (M(j,i,:) - M(j+1,i+1,:)).^2;
	  U(2*j,2*i) = sqrt(mask'*dz(:));
	elseif rem(j,2)==1 && i>1,
	  dz = (M(j,i,:) - M(j+1,i-1,:)).^2;
	  U(2*j,2*i-2) = sqrt(mask'*dz(:));
	end
      end
    end
  end

end

% values on the units

if (uy == 1 || ux == 1),
  % in 1-D case, mean is equal to median

  ma = max([ux uy]);
  for i = 1:2:ma,
    if i>1 && i<ma,
      a = [U(i-1) U(i+1)];
      U(i) = eval(calc);
    elseif i==1, U(i) = U(i+1);
    else U(i) = U(i-1); % i==ma
    end
  end

elseif strcmp(lattice, 'rect')

  for j=1:2:uy,
    for i=1:2:ux,
      if i>1 && j>1 && i<ux && j<uy,    % middle part of the map
	a = [U(j,i-1) U(j,i+1) U(j-1,i) U(j+1,i)];
      elseif j==1 && i>1 && i<ux,        % upper edge
	a = [U(j,i-1) U(j,i+1) U(j+1,i)];
      elseif j==uy && i>1 && i<ux,       % lower edge
	a = [U(j,i-1) U(j,i+1) U(j-1,i)];
      elseif i==1 && j>1 && j<uy,        % left edge
	a = [U(j,i+1) U(j-1,i) U(j+1,i)];
      elseif i==ux && j>1 && j<uy,       % right edge
	a = [U(j,i-1) U(j-1,i) U(j+1,i)];
      elseif i==1 && j==1,              % top left corner
	a = [U(j,i+1) U(j+1,i)];
      elseif i==ux && j==1,             % top right corner
	a = [U(j,i-1) U(j+1,i)];
      elseif i==1 && j==uy,             % bottom left corner
	a = [U(j,i+1) U(j-1,i)];
      elseif i==ux && j==uy,            % bottom right corner
	a = [U(j,i-1) U(j-1,i)];
      else
	a = 0;
      end
      U(j,i) = eval(calc);
    end
  end

elseif strcmp(lattice, 'hexa')

  for j=1:2:uy,
    for i=1:2:ux,
      if i>1 && j>1 && i<ux && j<uy,      % middle part of the map
	a = [U(j,i-1) U(j,i+1)];
	if rem(j-1,4)==0, a = [a, U(j-1,i-1) U(j-1,i) U(j+1,i-1) U(j+1,i)];
	else a = [a, U(j-1,i) U(j-1,i+1) U(j+1,i) U(j+1,i+1)]; end
      elseif j==1 && i>1 && i<ux,        % upper edge
	a = [U(j,i-1) U(j,i+1) U(j+1,i-1) U(j+1,i)];
      elseif j==uy && i>1 && i<ux,       % lower edge
	a = [U(j,i-1) U(j,i+1)];
	if rem(j-1,4)==0, a = [a, U(j-1,i-1) U(j-1,i)];
	else a = [a, U(j-1,i) U(j-1,i+1)]; end
      elseif i==1 && j>1 && j<uy,        % left edge
	a = U(j,i+1);
	if rem(j-1,4)==0, a = [a, U(j-1,i) U(j+1,i)];
	else a = [a, U(j-1,i) U(j-1,i+1) U(j+1,i) U(j+1,i+1)]; end
      elseif i==ux && j>1 && j<uy,       % right edge
	a = U(j,i-1);
	if rem(j-1,4)==0, a=[a, U(j-1,i) U(j-1,i-1) U(j+1,i) U(j+1,i-1)];
	else a = [a, U(j-1,i) U(j+1,i)]; end
      elseif i==1 && j==1,              % top left corner
	a = [U(j,i+1) U(j+1,i)];
      elseif i==ux && j==1,             % top right corner
	a = [U(j,i-1) U(j+1,i-1) U(j+1,i)];
      elseif i==1 && j==uy,             % bottom left corner
	if rem(j-1,4)==0, a = [U(j,i+1) U(j-1,i)];
	else a = [U(j,i+1) U(j-1,i) U(j-1,i+1)]; end
      elseif i==ux && j==uy,            % bottom right corner
	if rem(j-1,4)==0, a = [U(j,i-1) U(j-1,i) U(j-1,i-1)];
	else a = [U(j,i-1) U(j-1,i)]; end
      else
	a=0;
      end
      U(j,i) = eval(calc);
    end
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% normalization between [0,1]

% U = U - min(min(U));
% ma = max(max(U)); if ma > 0, U = U / ma; end

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