%PGraph Graph class
%
%   g = PGraph()        create a 2D, planar embedded, directed graph
%   g = PGraph(n)       create an n-d, embedded, directed graph
%
% Provides support for graphs that:
%   - are directed
%   - are embedded in a coordinate system (2D or 3D)
%   - have multiple unconnected components
%   - have symmetric cost edges (A to B is same cost as B to A)
%   - have no loops (edges from A to A)
%
% Graph representation:
%   - vertices are represented by integer vertex ids (vid)
%   - edges are represented by integer edge ids (eid)
%   - each vertex can have arbitrary associated data
%   - each edge can have arbitrary associated data
%
% Methods::
%
% Constructing the graph::
%   g.add_node(coord)      add vertex
%   g.add_edge(v1, v2)     add edge fbetween vertices
%   g.setcost(e, c)        set cost for edge
%   g.setedata(e, u)       set user data for edge
%   g.setvdata(v, u)       set user data for vertex
%
% Modifying the graph::
%   g.clear()              remove all vertices and edges from the graph
%   g.delete_edge(e)       remove edge
%   g.delete_node(v)       remove vertex
%   g.setcoord(v)          set coordinate of vertex
%
% Information from graph::
%   g.about()                   summary information about node
%   g.component(v)              component id for vertex
%   g.componentnodes(c)         vertices in component
%   g.connectivity()            number of edges for all vertices
%   g.connectivity_in()         number of incoming edges for all vertices
%   g.connectivity_out()        number of outgoing edges for all vertices
%   g.coord(v)                  coordinate of vertex
%   g.cost(e)                   cost of edge
%   g.distance_metric(v1,v2)    distance between nodes
%   g.edata(e)                  get edge user data
%   g.edgedir(v1,v2)            direction of edge
%   g.edges(v)                  list of edges for vertex
%   g.edges_in(v)               list of edges into vertex
%   g.edges_out(v)              list of edges from vertex
%   g.lookup(name)              vertex from name
%   g.name(v)                   name of vertex
%   g.neighbours(v)             neighbours of vertex
%   g.neighbours_d(v)           neighbours of vertex and edge directions
%   g.neighbours_in(v)          neighbours with edges in
%   g.neighbours_out(v)         neighbours with edges out
%   g.samecomponent(v1,v2)      test if vertices in same component
%   g.vdata(v)                  vertex user data
%   g.vertices(e)               vertices for edge
%
% Display::
%
%   g.char()                   convert graph to string
%   g.display()                display summary of graph
%   g.highlight_node(v)        highlight vertex
%   g.highlight_edge(e)        highlight edge
%   g.highlight_component(c)   highlight all nodes in component
%   g.highlight_path(p)        highlight nodes and edge along path
%   g.pick(coord)              vertex closest to coord
%   g.plot()                   plot graph
%
%
% Matrix representations::
%   g.adjacency()          adjacency matrix
%   g.degree()             degree matrix
%   g.incidence()          incidence matrix
%   g.laplacian()          Laplacian  matrix
%
% Planning paths through the graph::
%   g.Astar(s, g)          shortest path from s to g
%   g.goal(v)              set goal vertex, and plan paths
%   g.path(v)              list of vertices from v to goal
%
% Graph and world points::
%   g.closest(coord)       vertex closest to coord
%   g.coord(v)             coordinate of vertex v
%   g.distance(v1, v2)     distance between v1 and v2
%   g.distances(coord)     return sorted distances from coord to all vertices
%
% Object properties (read only)::
%   g.n            number of vertices
%   g.ne           number of edges
%   g.nc           number of components
%
% Example::
%         g = PGraph();
%         g.add_node([1 2]');  % add node 1
%         g.add_node([3 4]');  % add node 1
%         g.add_node([1 3]');  % add node 1
%         g.add_edge(1, 2);    % add edge 1-2
%         g.add_edge(2, 3);    % add edge 2-3
%         g.add_edge(1, 3);    % add edge 1-3
%         g.plot()
%
% Notes::
% - Support for edge direction is quite simple.
% - The method distance_metric() could be redefined in a subclass.

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math

% Peter Corke 8/2009.

% TODO:
%       be able to delete nodes, must update connectivity

% update to use map.container class

classdef PGraph < matlab.mixin.Copyable

    properties (SetAccess=private, GetAccess=public)
        vertexlist      % vertex coordinates, columnwise, vertex number is the column number
        edgelist        % 2xNe matrix, each column is vertex index of edge start and end
        edgelen         % length (cost) of this edge

        labels          % label of each vertex (1xN)
        maxlabel        % set of all labels (1xNc)

        goaldist        % distance from goal, after planning

        vertexdata      % per vertex data, cell array
        edgedata        % per edge data, cell array
        ndims           % number of coordinate dimensions, height of vertices matrix
        verbose
        measure         % distance measure: 'Euclidean', 'SE2'
        dweight         % distance weighting for SE2 measure
        ncvalid
        names
    end

    properties (Dependent)
        n               % number of nodes/vertices
        ne              % number of edges
        nc              % number of components
    end

    methods

        function g = PGraph(ndims, varargin)
            %PGraph.PGraph Graph class constructor
            %
            % G=PGraph(D, OPTIONS) is a graph object embedded in D dimensions.
            %
            % Options::
            %  'distance',M   Use the distance metric M for path planning which is either
            %                 'Euclidean' (default) or 'SE2'.
            %  'verbose'      Specify verbose operation
            %
            % Notes::
            % - Number of dimensions is not limited to 2 or 3.
            % - The distance metric 'SE2' is the sum of the squares of the difference
            %   in position and angle modulo 2pi.
            % - To use a different distance metric create a subclass of PGraph and
            %   override the method distance_metric().

            if nargin < 1
                ndims = 2;  % planar by default
            elseif isa(ndims, 'PGraph')
                % do a deep copy
                g = ndims.copy();
                return
            end
            g.ndims = ndims;
            opt.distance = 'Euclidean';
            opt.dweight = 1;
            opt = tb_optparse(opt, varargin);

            g.clear();
            g.verbose = opt.verbose;
            g.measure = opt.distance;
            g.dweight = opt.dweight;
            g.vertexdata = {};
            g.edgedata = {};
            g.ncvalid = false;  % mark connectivity as suspect
            g.names = strings(0,0);
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%% GRAPH MAINTENANCE
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function v = add_node(g, coord, varargin)
            %PGraph.add_node Add a node
            %
            % V = G.add_node(X, OPTIONS) adds a node/vertex with coordinate X (Dx1) and
            % returns the integer node id V.
            %
            % Options:
            % 'name',N    Assign a string name N to this vertex
            % 'from',V    Create a directed edge from vertex V with cost equal to the distance between the vertices.
            % 'cost',C    If an edge is created use cost C
            %
            % Notes::
            % - Distance is computed according to the metric specified in the
            %   constructor.
            %
            % See also PGraph.add_edge, PGraph.data, PGraph.getdata.

            if length(coord) ~= g.ndims
                error('coordinate length different to graph coordinate dimensions');
            end

            opt.from = [];
            opt.name = [];
            opt.cost = NaN;

            opt = tb_optparse(opt, varargin);

            % append the coordinate as a column in the vertex matrix
            g.vertexlist = [g.vertexlist coord(:)];
            v = g.n;

            if g.verbose
                fprintf('add node (%d) = ', v);
                fprintf('%f ', coord);
                fprintf('\n');
            end

            % optionally add an edge
            if ~isempty(opt.from)
                if isnan(opt.cost)
                    opt.cost = g.distance(v, opt.from);
                end
                g.add_edge(opt.from, v, opt.cost);
            end

            if ~isempty(opt.name)
                g.names(v) = opt.name;
            end
            g.ncvalid = false;  % mark connectivity as suspect
        end

        function e = add_edge(g, v1, v2, d)
            %PGraph.add_edge Add an edge
            %
            % E = G.add_edge(V1, V2) adds a directed edge from vertex id V1 to vertex id V2, and
            % returns the edge id E.  The edge cost is the distance between the vertices.
            %
            % E = G.add_edge(V1, V2, C) as above but the edge cost is C.
            %
            % Notes::
            % - If V2 is a vector add edges from V1 to all elements of V2
            % - Distance is computed according to the metric specified in the
            %   constructor.
            %
            % See also PGraph.add_node, PGraph.edgedir.
            if g.verbose
                fprintf('add edge %d -> %d\n', v1, v2);
            end
            e = [];
            for vv=v2(:)'
                g.edgelist = [g.edgelist [v1; vv]];
                e = [e numcols(g.edgelist)];
                if (nargin < 4) || isempty(d)
                    d = g.distance(v1, vv);
                end
                g.edgelen = [g.edgelen d];
            end
            g.ncvalid = false;  % mark connectivity as suspect

        end

        function delete_node(g, vv)

            for v=sort(vv(:)', 2, 'descend')
                % remove all its edges
                el = g.edges(v);
                g.delete_edge(el);

                % remove the column from the vertex table
                g.vertexlist(:,v) = [];

                % now renumber all the edges that might have changed
                k = find(g.edgelist > v);
                g.edgelist(k) = g.edgelist(k) - 1;

                g.ncvalid = false;  % mark connectivity as suspect
            end
        end

        function delete_edge(g, e)
            g.edgelist(:,e) = [NaN; NaN];
            g.ncvalid = false;  % mark connectivity as suspect

        end

        function clear(g)
            %PGraph.clear Clear the graph
            %
            % G.clear() removes all vertices, edges and components.

            % set the orientation of the edge and vertex tables
            g.labels = zeros(1, 0);
            g.edgelist = zeros(2, 0);
            g.edgelen = zeros(1, 0);
            g.vertexlist = zeros(g.ndims, 0);
            g.ncvalid = false;  % mark connectivity as suspect
            g.maxlabel = 0;
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%% GRAPH STRUCTURE
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        % which edges contain v
        %  elist = g.edges(v)
        function e = edges(g, v)
            %PGraph.edges Find edges given vertex
            %
            % E = G.edges(V) is a vector containing the id of all edges connected to vertex id V.
            %
            % See also PGraph.edgedir.

            e = [find(g.edgelist(1,:) == v) find(g.edgelist(2,:) == v)];
        end

        function e = edges_in(g, v)
            %PGraph.edges Find edges given vertex
            %
            % E = G.edges(V) is a vector containing the id of all edges connected to vertex id V.
            %
            % See also PGraph.edgedir.
            e = find(g.edgelist(2,:) == v);
        end

        function e = edges_out(g, v)
            %PGraph.edges Find edges given vertex
            %
            % E = G.edges(V) is a vector containing the id of all edges connected to vertex id V.
            %
            % See also PGraph.edgedir.
            e = find(g.edgelist(1,:) == v);
        end

        function dir = edgedir(g, v1, v2)
            %PGraph.edgedir Find edge direction
            %
            % D = G.edgedir(V1, V2) is the direction of the edge from vertex id V1
            % to vertex id V2.
            %
            % If we add an edge from vertex 3 to vertex 4
            %         g.add_edge(3, 4)
            % then
            %         g.edgedir(3, 4)
            % is positive, and
            %         g.edgedir(4, 3)
            % is negative.
            %
            % See also PGraph.add_node, PGraph.add_edge.
            n = g.edges(v1);
            if any(ismember( g.edgelist(2, n), v2))
                dir = 1;
            elseif any(ismember( g.edgelist(1, n), v2))
                dir = -1;
            else
                dir = 0;
            end
        end

        function v = vertices(g, e)
            %PGraph.vertices Find vertices given edge
            %
            % V = G.vertices(E) return the id of the vertices that define edge E.
            v = g.edgelist(:,e);
        end

        function [n,c] = neighbours(g, v)
            %PGraph.neighbours Neighbours of a vertex
            %
            % N = G.neighbours(V) is a vector of ids for all vertices which are
            % directly connected neighbours of vertex V.
            %
            % [N,C] = G.neighbours(V) as above but also returns a vector C whose elements
            % are the edge costs of the paths corresponding to the vertex ids in N.
            e = g.edges(v);
            n = g.edgelist(:,e);
            n = n(:)';
            n(n==v) = [];   % remove references to self
            if nargout > 1
                c = g.cost(e);
            end
        end

        function [n,c] = neighbours_out(g, v)
            %PGraph.neighbours Outgoing neighbours of a vertex
            %
            % N = G.neighbours(V) is a vector of ids for all vertices which are
            % directly connected neighbours of vertex V.
            %
            % [N,C] = G.neighbours(V) as above but also returns a vector C whose elements
            % are the edge costs of the paths corresponding to the vertex ids in N.
            e = g.edges_out(v);
            n = g.edgelist(:,e);
            n = n(:)';
            n(n==v) = [];   % remove references to self
            if nargout > 1
                c = g.cost(e);
            end
        end

        function [n,c] = neighbours_in(g, v)
            %PGraph.neighbours Incoming neighbours of a vertex
            %
            % N = G.neighbours(V) is a vector of ids for all vertices which are
            % directly connected neighbours of vertex V.
            %
            % [N,C] = G.neighbours(V) as above but also returns a vector C whose elements
            % are the edge costs of the paths corresponding to the vertex ids in N.
            e = g.edges_in(v);
            n = g.edgelist(:,e);
            n = n(:)';
            n(n==v) = [];   % remove references to self
            if nargout > 1
                c = g.cost(e);
            end
        end

        function [n,c] = neighbours_d(g, v)
            %PGraph.neighbours_d Directed neighbours of a vertex
            %
            % N = G.neighbours_d(V) is a vector of ids for all vertices which are
            % directly connected neighbours of vertex V.  Elements are positive
            % if there is a link from V to the node (outgoing), and negative if the link
            % is from the node to V (incoming).
            %
            % [N,C] = G.neighbours_d(V) as above but also returns a vector C whose elements
            % are the edge costs of the paths corresponding to the vertex ids in N.
            e = g.edges(v);
            n = [-g.edgelist(1,e) g.edgelist(2,e)];
            n(abs(n)==v) = [];   % remove references to self
            if nargout > 1
                c = g.cost(e);
            end
        end

        function c = connectivity(g,nn)
            %PGraph.connectivity Node connectivity
            %
            % C = G.connectivity() is a vector (Nx1) with the number of edges per
            % vertex.
            %
            % The average vertex connectivity is
            %         mean(g.connectivity())
            %
            % and the minimum vertex connectivity is
            %         min(g.connectivity())

            if nargin == 1
                for k=1:g.n
                    c(k) = length(g.edges(k));
                end
            elseif nargin == 2
                c = [];
                for n=nn
                    c = [c length(g.edges(n))];
                end
            end
        end

        function c = connectivity_in(g,n )
            %PGraph.connectivity Graph connectivity
            %
            % C = G.connectivity() is a vector (Nx1) with the number of incoming edges per
            % vertex.
            %
            % The average vertex connectivity is
            %         mean(g.connectivity())
            %
            % and the minimum vertex connectivity is
            %         min(g.connectivity())

            if nargin == 1
                for k=1:g.n
                    c(k) = length(g.edges_in(k));
                end
            elseif nargin == 2
                c = length(g.edges_in(n));
            end
        end

        function c = connectivity_out(g,n )
            %PGraph.connectivity Graph connectivity
            %
            % C = G.connectivity() is a vector (Nx1) with the number of outgoing edges per
            % vertex.
            %
            % The average vertex connectivity is
            %         mean(g.connectivity())
            %
            % and the minimum vertex connectivity is
            %         min(g.connectivity())

            if nargin == 1
                for k=1:g.n
                    c(k) = length(g.edges_out(k));
                end
            elseif nargin == 2
                c = length(g.edges_out(n));
            end
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%% NODE PROPERTIES
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function p = coord(g, v)
            %PGraph.coord Coordinate of node
            %
            % X = G.coord(V) is the coordinate vector (Dx1) of vertex id V.

            if nargin < 2
                p = g.vertexlist;
            else
                p = g.vertexlist(:,v);
            end
        end

        function p = name(g, v)
            %PGraph.coord Name of node
            %
            % X = G.name(V) is the name (string) of vertex id V.

            if nargin < 2
                p = [g.names];
            else
                p = g.names(v);
            end
        end

        function p = lookup(g, name)
            p  = find( [g.names] == name );
        end

        function p = setcoord(g, v, c)
            %PGraph.coord Coordinate of node
            %
            % X = G.coord(V) is the coordinate vector (Dx1) of vertex id V.

            if nargin < 3
                if ~all(size(v) == size(g.vertexlist))
                    error('SMTB:PGraph:badarg', 'value must be size of vertex table');
                end
                g.vertexlist = v;
            else
                g.vertexlist(:,v) = c;
            end
                end

        function u = vdata(g, v)
            %PGraph.data Get user data for node
            %
            % U = G.data(V) gets the user data of vertex V which can be of any
            % type such as a number, struct, object or cell array.
            %
            % See also PGraph.setdata.
            u = g.vertexdata{v};
        end

        function u = setvdata(g, v, u)
            %PGraph.setdata Set user data for node
            %
            % G.setdata(V, U) sets the user data of vertex V to U which can be of any
            % type such as a number, struct, object or cell array.
            %
            % See also PGraph.data.

            g.vertexdata{v} = u;
        end

        function d = distance(g, v1, v2)
            %PGraph.distance Distance between vertices
            %
            % D = G.distance(V1, V2) is the geometric distance between
            % the vertices V1 and V2.
            %
            % See also PGraph.distances.

            d = g.distance_metric( g.vertexlist(:,v1), g.vertexlist(:,v2));

        end

        function [d,k] = distances(g, p)
            %PGraph.distances Distances from point to vertices
            %
            % D = G.distances(X) is a vector (1xN) of geometric distance from the point
            % X (Dx1) to every other vertex sorted into increasing order.
            %
            % [D,W] = G.distances(P) as above but also returns W (1xN) with the
            % corresponding vertex id.
            %
            % Notes::
            % - Distance is computed according to the metric specified in the
            %   constructor.
            %
            % See also PGraph.closest.

            d = g.distance_metric(p(:), g.vertexlist);
            [d,k] = sort(d, 'ascend');
        end

        function [c,dn] = closest(g, p, tol)
            %PGraph.closest Find closest vertex
            %
            % V = G.closest(X) is the vertex geometrically closest to coordinate X.
            %
            % [V,D] = G.closest(X) as above but also returns the distance D.
            %
            % See also PGraph.distances.
            d = g.distance_metric(p(:), g.vertexlist);
            [mn,c] = min(d);

            if nargin > 2 && mn > tol
                c = []; dn = [];
            end

            if nargout > 1
                dn = mn;
            end

        end

        function about(g, vv)
            if nargin < 2
                disp('pick a node using the mouse');
                vv = g.pick()
            end

            if ~g.ncvalid
                g.graphcolor();
            end

            for v=vv
                fprintf('Node %d #%d@ (', v, g.labels(v)); fprintf('%g ', g.coord(v)); fprintf(')\n');
                fprintf('  neighbours: ');
                fprintf('%d ', g.neighbours_in(v)); fprintf(' >-o-> ');
                fprintf('%d ', g.neighbours_out(v)); fprintf('\n');

                fprintf('  edges: ');
                fprintf('%d ', g.edges_in(v)); fprintf(' >-o-> ');
                fprintf('%d ', g.edges_out(v)); fprintf('\n');
            end
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%% EDGE PROPERTIES
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function d = cost(g, e)
            %PGraph.cost Cost of edge
            %
            % C = G.cost(E) is the cost of edge id E.
            d = g.edgelen(e);
        end

        function d = setcost(g, e, c)
            %PGraph.cost Set cost of edge
            %
            % G.setcost(E, C) set cost of edge id E to C.
            g.edgelen(e) = c;
        end

        function u = edata(g, e)
            %PGraph.data Get user data for node
            %
            % U = G.data(V) gets the user data of vertex V which can be of any
            % type such as a number, struct, object or cell array.
            %
            % See also PGraph.setdata.
            u = g.edgedata{e};
        end

        function u = setedata(g, e, u)
            %PGraph.setdata Set user data for node
            %
            % G.setdata(V, U) sets the user data of vertex V to U which can be of any
            % type such as a number, struct, object or cell array.
            %
            % See also PGraph.data.

            g.edgedata{e} = u;
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%% GRAPH INFORMATION
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function n = get.n(g)
            %Pgraph.n Number of vertices
            %
            % G.n is the number of vertices in the graph.
            %
            % See also PGraph.ne.
            n = numcols(g.vertexlist);
        end

        function ne = get.ne(g)
            %Pgraph.ne Number of edges
            %
            % G.ne is the number of edges in the graph.
            %
            % See also PGraph.n.
            ne = numcols(g.edgelist);
        end

        function ne = get.nc(g)
            %Pgraph.nc Number of components
            %
            % G.nc is the number of components in the graph.
            %
            % See also PGraph.component.

            if ~g.ncvalid
                g.graphcolor();
            end
            ne = g.maxlabel;
        end

        function display(g)
            %PGraph.display Display graph
            %
            % G.display() displays a compact human readable representation of the
            % state of the graph including the number of vertices, edges and components.
            %
            % See also PGraph.char.
            loose = strcmp( get(0, 'FormatSpacing'), 'loose');
            if loose
                disp(' ');
            end
            disp([inputname(1), ' = '])
            disp( char(g) );
        end % display()

        function s = char(g)
            %PGraph.char Convert graph to string
            %
            % S = G.char() is a compact human readable representation of the
            % state of the graph including the number of vertices, edges and components.

            s = '';
            s = char(s, sprintf('  %d dimensions', g.ndims));
            s = char(s, sprintf('  %d vertices', g.n));
            s = char(s, sprintf('  %d edges', numcols(g.edgelist)));
            s = char(s, sprintf('  %d components', g.nc));
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%% GRAPH COMPONENTS
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function graphcolor(g)
            % color the graph

            g.labels = repmat(NaN, 1, g.n);

            function colorComponent(g, v, l)
                g.labels(v) = l;
                for n = g.neighbours(v)
                    if isnan(g.labels(n))
                        colorComponent(g, n, l);
                    end
                end
            end

            for label = 1:g.n
                % find first vertex with no label
                v = find(isnan(g.labels));
                if isempty(v)
                    g.maxlabel = label-1;
                    break;
                end
                v = v(1);

                colorComponent(g, v, label);
            end
            g.ncvalid = true;

        end

        function c = component(g, v)
            %PGraph.component Graph component
            %
            % C = G.component(V) is the id of the graph component that contains vertex
            % V.
            c = g.labels(v);
        end

        function v = componentnodes(g, c)
            %PGraph.component Graph component
            %
            % C = G.component(V) are the ids of all vertices in the graph component V.
            v = find(g.labels == c);
        end

        function c = samecomponent(g, v1, v2)
            %PGraph.component Graph component
            %
            % C = G.component(V) is the id of the graph component that contains vertex
            % V.

            if ~g.ncvalid
                % lazy graph coloring
                g.graphcolor();
            end
            c = g.labels(v1) == g.labels(v2);
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%% GRAPHICS
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function plot(g, varargin)
            %PGraph.plot Plot the graph
            %
            % G.plot(OPT) plots the graph in the current figure.  Nodes
            % are shown as colored circles.
            %
            % Options::
            %  'labels'              Display vertex id (default false)
            %  'edges'               Display edges (default true)
            %  'edgelabels'          Display edge id (default false)
            %  'NodeSize',S          Size of vertex circle (default 8)
            %  'NodeFaceColor',C     Node circle color (default blue)
            %  'NodeEdgeColor',C     Node circle edge color (default blue)
            %  'NodeLabelSize',S     Node label text sizer (default 16)
            %  'NodeLabelColor',C    Node label text color (default blue)
            %  'EdgeColor',C         Edge color (default black)
            %  'EdgeLabelSize',S     Edge label text size (default black)
            %  'EdgeLabelColor',C    Edge label text color (default black)
            %  'componentcolor'      Node color is a function of graph component
            %  'only',N              Only show these nodes

            % show vertices
            holdon = ishold;
            hold on

            % parse options
            opt.componentcolor = false;
            opt.labels = false;
            opt.edges = true;
            opt.edgelabels = false;
            opt.NodeSize = 8;
            opt.NodeFaceColor = 'b';
            opt.NodeEdgeColor = 'b';
            opt.NodeLabelSize = 16;
            opt.NodeLabelColor = 'b';
            opt.EdgeColor = 'k';
            opt.EdgeLabelSize = 8;
            opt.EdgeLabelColor = 'k';
            opt.EdgeWidth = 0.5;
            opt.dims = g.ndims;
            opt.only = [1:g.n];

            [opt,args] = tb_optparse(opt, varargin);

            % set default color if none specified
            if ~isempty(args)
                mcolor = args{1};
            else
                mcolor = 'b';
            end

            if opt.componentcolor

                colororder = get(gca, 'ColorOrder');

                % step through each component
                for c=1:g.nc
                    vertices = g.componentnodes(c);
                    if length(vertices) == 1
                        % singleton is grey
                        color = 0.8*[1 1 1];
                    else
                        % otherwise use next color from the axis color order
                        color = colororder(mod(c+1,numrows(colororder)-1)+1,:);
                    end

                                        % plot the edges
                    if opt.edges
                        coords = [];

                        for v = vertices
                            p0 = g.coord(v); p0 = p0(1:opt.dims);
                            for n = g.neighbours(v)
                                pn = g.coord(n); pn = pn(1:opt.dims);
                                coords = [coords; p0'; pn'; NaN*ones(1,g.ndims)];
                            end
                        end
                        if ~isempty(coords)
                            if opt.dims == 3
                                plot3(coords(1,:), coords(2,:), coords(3,:), 'Color', color, 'LineWidth', opt.EdgeWidth);
                            else
                                plot(coords(:,1), coords(:,2), 'Color', color, 'LineWidth', opt.EdgeWidth);
                            end
                        end
                    end

                    % plot the nodes
                    args = {'LineStyle', 'None', ...
                        'Marker', 'o', ...
                        'MarkerFaceColor', color, ...
                        'MarkerSize', opt.NodeSize, ...
                        'MarkerEdgeColor', color };

                    for v = vertices
                        if ~ismember(v, opt.only)
                            continue;
                        end
                        p = g.coord(v);
                        if opt.dims == 3
                            plot3(p(1), p(2), p(3), args{:});
                        else
                            plot(p(1), p(2), args{:});
                        end
                    end

                    if length(vertices) == 1
                        continue; % no edges to plot
                    end

                end
            else

                %% user selected colors

                % show edges
                if opt.edges
                for e=g.edgelist
                    v1 = g.vertexlist(:,e(1));
                    v2 = g.vertexlist(:,e(2));
                    if opt.dims == 3
                        plot3([v1(1) v2(1)], [v1(2) v2(2)], [v1(3) v2(3)], ...
                            'Color', opt.EdgeColor, 'LineWidth', opt.EdgeWidth);
                    else
                        plot([v1(1) v2(1)], [v1(2) v2(2)], ...
                            'Color', opt.EdgeColor, 'LineWidth', opt.EdgeWidth);
                    end
                end
                end

                % show the vertices as filled circles
                for i=1:g.n
                    % for each node
                        if ~ismember(i, opt.only)
                            continue;
                        end
                    args = {'LineStyle', 'None', ...
                        'Marker', 'o', ...
                        'MarkerFaceColor', opt.NodeFaceColor, ...
                        'MarkerSize', opt.NodeSize, ...
                        'MarkerEdgeColor', opt.NodeEdgeColor };
                    if opt.dims == 3
                        plot3(g.vertexlist(1,i), g.vertexlist(2,i), g.vertexlist(3,i), args{:});
                    else
                        plot(g.vertexlist(1,i), g.vertexlist(2,i), args{:});
                    end
                end
            end

            % show the edge labels
            if opt.edgelabels
                for i=1:g.ne
                    e = g.edgelist(:,i);
                    v1 = g.vertexlist(:,e(1));
                    v2 = g.vertexlist(:,e(2));

                    text('String', sprintf('  %g', g.cost(i)), ...
                        'Position', (v1 + v2)/2, ...
                        'HorizontalAlignment', 'left', ...
                        'VerticalAlignment', 'middle', ...
                        'FontUnits', 'pixels', ...
                        'FontSize', opt.EdgeLabelSize, ...
                        'Color', opt.EdgeLabelColor);
                end
            end
            % show the labels
            if opt.labels
                for i=1:g.n
                    text('String', sprintf('  %d', i), ...
                        'Position', g.vertexlist(:,i), ...
                        'HorizontalAlignment', 'left', ...
                        'VerticalAlignment', 'middle', ...
                        'FontUnits', 'pixels', ...
                        'FontSize', opt.NodeLabelSize, ...
                        'Color', opt.NodeLabelColor);
                end
            end
            if ~holdon
                hold off
            end
            grid on
        end

        function v = pick(g)
            %PGraph.pick Graphically select a vertex
            %
            % V = G.pick() is the id of the vertex closest to the point clicked
            % by the user on a plot of the graph.
            %
            % See also PGraph.plot.
            [x,y] = ginput(1);
            d = colnorm( bsxfun(@minus,[x; y], g.vertexlist(1:2,:)) );

            [~,v] = min(d);
        end

        function highlight_node(g, verts, varargin)
            %PGraph.highlight_node Highlight a node
            %
            % G.highlight_node(V, OPTIONS) highlights the vertex V with a yellow marker.
            % If V is a list of vertices then all are highlighted.
            %
            % Options::
            %  'NodeSize',S          Size of vertex circle (default 12)
            %  'NodeFaceColor',C     Node circle color (default yellow)
            %  'NodeEdgeColor',C     Node circle edge color (default blue)
            %
            % See also PGraph.highlight_edge, PGraph.highlight_path, PGraph.highlight_component.

            hold on

            % parse options
            opt.NodeSize = 12;
            opt.NodeFaceColor = 'y';
            opt.NodeEdgeColor = 'b';

            [opt,args] = tb_optparse(opt, varargin);
            markerprops = {'LineStyle', 'None', ...
                'Marker', 'o', ...
                'MarkerFaceColor', opt.NodeFaceColor, ...
                'MarkerSize', opt.NodeSize, ...
                'MarkerEdgeColor', opt.NodeEdgeColor };

            for v=verts
                if g.ndims == 3
                    plot3(g.vertexlist(1,v), g.vertexlist(2,v), g.vertexlist(3,v), ...
                        markerprops{:});
                else
                    plot(g.vertexlist(1,v), g.vertexlist(2,v), markerprops{:});
                end
            end
        end

        function highlight_component(g, c, varargin)
            %PGraph.highlight_component Highlight a graph component
            %
            % G.highlight_component(C, OPTIONS) highlights the vertices that belong to
            % graph component C.
            %
            % Options::
            %  'NodeSize',S          Size of vertex circle (default 12)
            %  'NodeFaceColor',C     Node circle color (default yellow)
            %  'NodeEdgeColor',C     Node circle edge color (default blue)
            %
            % See also PGraph.highlight_node, PGraph.highlight_edge, PGraph.highlight_component.
            nodes = find(g.labels == g.labelset(c));
            for v=nodes
                g.highlight_node(v, varargin{:});
            end
        end

        function highlight_edge(g, e, varargin)
            %PGraph.highlight_node Highlight a node
            %
            % G.highlight_edge(V1, V2) highlights the edge between vertices V1 and V2.
            %
            % G.highlight_edge(E) highlights the edge with id E.
            %
            % Options::
            % 'EdgeColor',C         Edge edge color (default black)
            % 'EdgeThickness',T     Edge thickness (default 1.5)
            %
            % See also PGraph.highlight_node, PGraph.highlight_path, PGraph.highlight_component.

            % parse options
            opt.EdgeColor = 'k';
            opt.EdgeThickness = 1.5;

            [opt,args] = tb_optparse(opt, varargin);

            hold on
            if (length(args) > 0) && isnumeric(args{1})
                % highlight_edge(V1, V2)
                v1 = e;
                v2 = args{1};

                v1 = g.vertexlist(:,v1);
                v2 = g.vertexlist(:,v2);
            else
                % highlight_edge(E)
                e = g.edgelist(:,e);
                v1 = g.vertexlist(:,e(1));
                v2 = g.vertexlist(:,e(2));
            end

            % create the line properties for the edges
            lineprops = {
                'Color', opt.EdgeColor, ...
                'LineWidth', opt.EdgeThickness };

            if g.ndims == 3
                plot3([v1(1) v2(1)], [v1(2) v2(2)], [v1(3) v2(3)], lineprops{:});
            else
                plot([v1(1) v2(1)], [v1(2) v2(2)], lineprops{:});
            end
        end

        function highlight_path(g, path, varargin)
            %PGraph.highlight_path Highlight path
            %
            % G.highlight_path(P, OPTIONS) highlights the path defined by vector P
            % which is a list of vertex ids comprising the path.
            %
            % Options::
            %  'NodeSize',S          Size of vertex circle (default 12)
            %  'NodeFaceColor',C     Node circle color (default yellow)
            %  'NodeEdgeColor',C     Node circle edge color (default blue)
            %  'EdgeColor',C         Node circle edge color (default black)
            %  'EdgeThickness',T     Edge thickness (default 1.5)
            %
            % See also PGraph.highlight_node, PGraph.highlight_edge, PGraph.highlight_component.
            g.highlight_node(path, varargin{:});

            % highlight the edges
            for i=1:numel(path)-1
                v1 = path(i);
                v2 = path(i+1);
                g.highlight_edge(v1, v2, varargin{:});
            end
        end

        function dotfile(g, varargin)
            %Pgraph.dotfile Create a GraphViz dot file
            %
            % G.dotfile(filename, OPTIONS) creates the specified file which contains the
            % GraphViz code to represent the embedded graph.
            %
            % G.dotfile(OPTIONS) as above but outputs the code to the console.
            %
            % Options::
            %  'directed'    create a directed graph
            %
            % Notes::
            % - An undirected graph is default
            % - Use neato rather than dot to get the embedded layout

            opt.directed = false;

            [opt,args] = tb_optparse(opt, varargin);

            if length(args) == 0
                fp = 1;
            else
                fp = fopen(args{1}, 'w');
            end

            if opt.directed
                fprintf(fp, 'digraph {\n');
            else
                fprintf(fp, 'graph {\n');
            end

            % add the nodes including name and position
            for i=1:g.n
                fprintf(fp, '"%s" [pos="%d,%d"]\n', g.names{i}, g.vertexlist(:,i));
            end

            % add the edges
            for i=1:g.ne
                edge = g.edgelist(:,i);
                if opt.directed
                    fprintf(fp, '"%s" -> "%s"\n', g.names{edge(1)}, g.names{edge(2)});
                else
                    fprintf(fp, '"%s" -- "%s"\n', g.names{edge(1)}, g.names{edge(2)});
                end
            end
            fprintf(fp, '}\n');
            if length(args) > 1
                fclose(fp);
            end
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%% MATRIX REPRESENTATIONS
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function L = laplacian(g)
            %Pgraph.laplacian Laplacian matrix of graph
            %
            % L = G.laplacian() is the Laplacian matrix (NxN) of the graph.
            %
            % Notes::
            % - L is always positive-semidefinite.
            % - L has at least one zero eigenvalue.
            % - The number of zero eigenvalues is the number of connected components
            %   in the graph.
            %
            % See also PGraph.adjacency, PGraph.incidence, PGraph.degree.

            L = g.degree() - (g.adjacency() > 0);
        end

        function D = degree(g)
            %Pgraph.degree Degree matrix of graph
            %
            % D = G.degree() is a diagonal matrix (NxN) where element D(i,i) is the number
            % of edges connected to vertex id i.
            %
            % See also PGraph.adjacency, PGraph.incidence, PGraph.laplacian.

            D = diag( g.connectivity() );
        end

        function A = adjacency(g)
            %Pgraph.adjacency Adjacency matrix of graph
            %
            % A = G.adjacency() is a matrix (NxN) where element A(i,j) is the cost
            % of moving from vertex i to vertex j.
            %
            % Notes::
            % - Matrix is symmetric.
            % - Eigenvalues of A are real and are known as the spectrum of the graph.
            % - The element A(I,J) can be considered the number of walks of one
            %   edge from vertex I to vertex J (either zero or one).  The element (I,J)
            %   of A^N are the number of walks of length N from vertex I to vertex J.
            %
            % See also PGraph.degree, PGraph.incidence, PGraph.laplacian.

            A = zeros(g.n, g.n);
            for i=1:g.n
                [n,c] = g.neighbours(i);
                for j=1:numel(n)
                    A(i,n(j)) = c(j);
                    A(n(j),i) = c(j);
                end
            end
        end

        function I = incidence(g)
            %Pgraph.degree Incidence matrix of graph
            %
            % IN = G.incidence() is a matrix (NxNE) where element IN(i,j) is
            % non-zero if vertex id i is connected to edge id j.
            %
            % See also PGraph.adjacency, PGraph.degree, PGraph.laplacian.
            I = zeros(g.n, g.ne);
            for i=1:g.n
                for n=g.edges(i)
                    I(i,n) = 1;
                end
            end
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%% PATH PLANNING
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function [path,cost] = Astar(g, vstart, vgoal, varargin)
            %PGraph.Astar path finding
            %
            % PATH = G.Astar(V1, V2) is the lowest cost path from vertex V1 to
            % vertex V2.  PATH is a list of vertices starting with V1 and ending
            % V2.
            %
            % [PATH,C] = G.Astar(V1, V2) as above but also returns the total cost
            % of traversing PATH.
            %
            % Notes::
            % - Uses the efficient A* search algorithm.
            % - The heuristic is the distance function selected in the constructor, it
            %   must be  admissible, meaning that it never overestimates the actual
            %   cost to get to the nearest goal node.
            %
            % References::
            % - Correction to "A Formal Basis for the Heuristic Determination of Minimum Cost Paths".
            %   Hart, P. E.; Nilsson, N. J.; Raphael, B.
            %   SIGART Newsletter 37: 28-29, 1972.
            %
            % See also PGraph.goal, PGraph.path.

            opt.directed = false;

            opt = tb_optparse(opt, varargin);

            % The set of vertices already evaluated.
            closedSet = [];

            % The set of tentative vertices to be evaluated, initially containing the start node
            openSet = [vstart];
            came_from(vstart) = 0;    % The map of navigated vertices.

            g_score(vstart) = 0;    % Cost from start along best known path.
            h_score(vstart) = g.distance(vstart, vgoal);
            % Estimated total cost from start to goal through y.
            f_score(vstart) = g_score(vstart) + h_score(vstart);

            while ~isempty(openSet)
                % current := the node in openSet having the lowest f_score[] value
                [~,k] = min(f_score(openSet));
                vcurrent = openSet(k);

                if vcurrent == vgoal
                    % we have arrived!
                    path = [];
                    p = vgoal;
                    while true
                        path = [p path];
                        p = came_from(p);
                        if p == 0
                            break;
                        end
                    end
                    if nargout > 1
                        cost = f_score(vgoal);
                    end
                    return
                end

                %remove current from openSet
                openSet = setdiff(openSet, vcurrent);
                %add current to closedSet
                closedSet = union(closedSet, vcurrent);

                if opt.directed
                    [neighbours,costs] = g.neighbours_out(vcurrent);

                else
                    [neighbours,costs] = g.neighbours(vcurrent);
                end

                for k=1:length(neighbours)

                    neighbour = neighbours(k);

                    if ismember(neighbour, closedSet)
                        continue;
                    end
                    tentative_g_score = g_score(vcurrent) + costs(k);

                    if ~ismember(neighbour, openSet)
                        %add neighbor to openSet
                        openSet = union(openSet, neighbour);
                        h_score(neighbour) = g.distance(neighbour, vgoal);
                        tentative_is_better = true;
                    elseif tentative_g_score < g_score(neighbour)
                        tentative_is_better = true;
                    else
                        tentative_is_better = false;
                    end
                    if tentative_is_better
                        % we found an edge that belongs to the path

                        % came_from is an array that records the path taken
                        %  - length g.n
                        %  -  came_from(A) = B means a path segment from A -> B
%                         if came_from(neighbour) > 0
%                             disp('problem')
%                         end
                        came_from(neighbour) = vcurrent;
                        g_score(neighbour) = tentative_g_score;
                        f_score(neighbour) = g_score(neighbour) + h_score(neighbour);
                    end
                end
            end
            path = [];
            cost = Inf;
        end

        function d = distance_metric(g, x1, x2)

            % distance between coordinates x1 and x2 using the relevant metric
            % x2 can be multiple points represented by multiple columns
            if isa(g.measure, 'function_handle')
                d = g.measure(x1(:), x2(:));
                if ~isscalar(d) && d <= 0
                    error('SMTB:PGraph:badresult', 'distance function must return a positive scalar');
                end
            else switch g.measure
                    case 'Euclidean'
                        d = colnorm( bsxfun(@minus, x1, x2) );

                    case 'SE2'
                        d = bsxfun(@minus, x1, x2) * g.dweight;
                        d(3,:) = angdiff(x1(3), x2(3,:));
                        d = colnorm( d );

                    case 'Lattice'
                        d = bsxfun(@minus, x1, x2) * g.dweight;
                        d(3,:) = angdiff(x1(3)*pi/2, x2(3,:)*pi/2);
                        d = colnorm( d );
                    otherwise
                        error('unknown distance measure', g.measure);
                end
            end
        end
    end %  methods

end % classdef


function [ y ] = back_trans( x,waveS,waveletFilterName )
%FFT3D Summary of this function goes here
%   Detailed explanation goes here

if isreal(x{1,1})
    y = cell(1,size(x,2));
    for j = 1:size(x,2)
        y{1,j} = waverec2(x{1,j},waveS,waveletFilterName);
    end;
else
    y = cell(1,2*size(x,2));
    for j = 1:size(x,2)
        y{1,j} = waverec2(real(x{1,j}),waveS,waveletFilterName);
        y{1,j+size(x,2)} = waverec2(imag(x{1,j}),waveS,waveletFilterName);
    end;
end;
end


function test_ft_rejectcomponent

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_rejectcomponent

fs = 500;
nchan = 32;
start_time = -1; % seconds
end_time = 2.5; % seconds
nsamples = (end_time - start_time) * fs + 1;

data = [];
data.time{1} = linspace(start_time, end_time, nsamples);
data.trial{1} = randn(nchan,nsamples);
data.label = cellstr(num2str((1:nchan).'));

cfg = [];
comp = ft_componentanalysis(cfg, data);

cfg = [];
cfg.component = [1 4 14];
dataout = ft_rejectcomponent(cfg,comp,data);


function cx = zscal ( n, ca, cx, incx )

%*****************************************************************************80
%
%% ZSCAL scales a complex vector by a constant.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 May 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, complex CA, the multiplier.
%
%    Input, complex CX(*), the vector to be scaled.
%
%    Input, integer INCX, the increment between successive entries of CX.
%
%    Output, complex CX(*), the scaled vector.
%
  cx(1:incx:1+(n-1)*incx) = ca * cx(1:incx:1+(n-1)*incx);

  return
end


function d = vl_numder(func, x, varargin)
% VL_NUMDER  Numerical derivative
%   D = VL_NUMDER(FUNC, X) computes the numerical derivative of the
%   function FUNC at point X. X is a real array and is passed as first
%   argument of FUNC.
%
%   D = VL_NUMDER(FUNC, X, ARG1, ARG2, ...) passes ARG1, ARG2, ...  as
%   additional arguments to the function FUNC.
%
%   See also: VL_NUMDER2(), VL_HELP().

% TODO: uniform sacaling of axis is not a good idea

dx = 1e-7 ;
N  = numel(x) ;
fx = feval(func, x, varargin{:}) ;
d  = zeros(length(fx(:)),N) ;

for n=1:N
  e   = zeros(size(x)) ; e(n) = 1 ;
  fxn = feval(func, x+dx*e, varargin{:}) ;
  di  = (fxn - fx) / dx ;
  d(:,n) = di(:) ;
end


function tf = cvx_use_sparse( sz, nz, isr )

%CVX_USE_SPARSE   Sparse/dense matrix efficiency test.
%   CVX_USE_SPARSE(SZ,NZ,ISR) is an internal function used by CVX to determine
%   if a matrix of size SZ with NZ nonzeros would require less memory to store
%   in sparse form or dense form. The matrix is assumed to be real if ISR=1 or
%   ISR is omitted, and complex otherwise.

if nargin == 1,
    ss = size( sz );
    if length( ss ) > 2,
        tf = false;
        return
    end
    isr = isreal( sz );
    if issparse( sz ),
        nz = nzmax( sz );
    else
        nz = nnz( sz );
    end
    sz = ss;
elseif length( sz ) > 2,
    tf = false;
    return
elseif nargin < 3,
    isr = true;
end
if isr,
    tf = 1 + ( 1 - 2 * sz( 1 ) ) * sz( 2 ) + 3 * nz < 0;
else
    tf = 1 + ( 1 - 4 * sz( 1 ) ) * sz( 2 ) + 5 * nz < 0;
end

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


% THIS SOFTWARE AND ANY ACCOMPANYING DOCUMENTATION IS RELEASED "AS IS."  THE U.S. GOVERNMENT MAKES NO WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, CONCERNING THIS SOFTWARE AND ANY ACCOMPANYING DOCUMENTATION, INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE.  IN NO EVENT WILL THE U.S. GOVERNMENT BE LIABLE FOR ANY DAMAGES, INCLUDING ANY LOST PROFITS, LOST SAVINGS OR OTHER INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE, OR INABILITY TO USE, THIS SOFTWARE OR ANY ACCOMPANYING DOCUMENTATION, EVEN IF INFORMED IN ADVANCE OF THE POSSIBILITY OF SUCH DAMAGES.
%
% file: prop_diff_ci.m

% 030110 tdr created
% 031223 tdr added test for NaN in inputs
% 040203 tdr corrected error found by Steve Walls in check_inputs (was using n in place of n1 when ensuring that we had integers)

function metrics = prop_diff_ci(x1,n1,x2,n2,alpha,method,verbose)

if (nargin ~= 7 & nargin ~= 6 & nargin ~= 5),
    error('Requires 5, 6, or 7 input arguments (x1,n1,x2,n2,alpha), (x1,n1,x2,n2,alpha,method), or (x1,n1,x2,n2,alpha,method,verbose); default method = 3');
end
if nargin == 5, method = 3; verbose = 0; end;
if nargin == 6, verbose = 0; end;
inputsOkay = check_inputs(x1,n1,x2,n2,alpha,method,verbose);
if ~inputsOkay
    metrics = [NaN, NaN, NaN];
    return
end;

tic;

% call selected interval estimator method
switch method
case 1, % one-sided CI - note provides both upper and lower one-sided CIs
    ci = get_prop_diff_ci1(x1,n1,x2,n2,alpha,method,verbose);
case 2, % minimum length CI
    error('Method 2 Not Available - choose Method 1, 3, 4, 5 or 6');
case 3, % symmetrical about estimate, expect when outside range
    ci = get_prop_diff_ci3(x1,n1,x2,n2,alpha,method,verbose);
case 4, % equal tail masses, note - may not enclose estimate
    ci = get_prop_diff_ci1(x1,n1,x2,n2,alpha/2,method,verbose);
case 5, % Using Fagan's application of "exact" method
    ci = get_ci_fagan(x1,n1,x2,n2,alpha,method,verbose);
case 6, % Normal Approximation
    ci = get_ci_rss(x1,n1,x2,n2,alpha,method,verbose);
case 7, % Law of Large Numbers Approximation
    error('Method 7 Not Available - choose Method 1, 3, 4, 5 or 6');
otherwise
   error('Not a valid method: 1 (1-sided), 2 (min length), 3 (symm.), 4 (equal tail), 5 (MatLab-binofit), 6 (Normal Approx)');
end;
rt = toc;

if verbose
    p1_hat = x1/n1; p2_hat = x2/n2; delta_p_hat = p1_hat - p2_hat;
    length = ci(3)-ci(2);
    lower_tail = 1 - prop_diff(x1,n1,x2,n2,ci(2));
    upper_tail = prop_diff(x1,n1,x2,n2,ci(3));
    if (method == 1)
        actual_alpha = (lower_tail + upper_tail)/2;
    else
        actual_alpha = lower_tail + upper_tail;
    end;
    if (abs(actual_alpha - alpha) > 0.00005) % close_enough is 0.00001 throughout, but spec is 0.0005.
        warning('Interval not to spec: abs(desired_alpha - actual_alpha) < 0.00005)');
        warning_stats = [alpha actual_alpha alpha-actual_alpha]
    end;
    disp('p_diff_hat, Lower CI Bound, Upper CI Bound, x1, n1, x2, n2, Desired alpha, Method, Length, Lower Tail, Upper Tail, Actual alpha, Delta alpha, Run Time')
    metrics = [ci x1 n1 x2 n2 alpha method length lower_tail upper_tail actual_alpha (alpha - actual_alpha) rt];
    disp(sprintf('%8.6g, %8.6g, %8.6g, %d, %d, %d, %d, %8.6g, %d, %8.6g, %8.6g, %8.6g, %8.6g, %8.6g, %8.2g',metrics));
else
    metrics = ci;
end;

% ------------------------------------------------------------
% Difference CI based on root sum of squares of individual CI's, see
% Fagan'99 in Computers in Biology and Medicine.

function  ci = get_ci_fagan(x1,n1,x2,n2,alpha,method,verbose);
    ci1 = prop_ci(x1,n1,alpha,method);
    ci2 = prop_ci(x2,n2,alpha,method);
    diff = ci1(1) - ci2(1);
    correction_factor = 0.18 / max(n1,n2); % see Fagan'99, p.85
    rss_lower = diff - sqrt((ci1(2)-ci1(1))^2 + (ci2(3)-ci2(1))^2);
    rss_upper = diff + sqrt((ci1(3)-ci1(1))^2 + (ci2(2)-ci2(1))^2);
    diff_lower = max((ci1(2)-ci2(3)), (rss_lower - correction_factor));
    diff_upper = min((ci1(3)-ci2(2)), (rss_upper + correction_factor));
    ci = [diff diff_lower diff_upper];

% ------------------------------------------------------------
% Difference CI based on root sum of squares of individual CI's

function  ci = get_ci_rss(x1,n1,x2,n2,alpha,method,verbose);
    ci1 = prop_ci(x1,n1,alpha,method);
    ci2 = prop_ci(x2,n2,alpha,method);
    diff = ci1(1) - ci2(1);
    rss_lower = diff - sqrt((ci1(2)-ci1(1))^2 + (ci2(3)-ci2(1))^2);
    rss_upper = diff + sqrt((ci1(3)-ci1(1))^2 + (ci2(2)-ci2(1))^2);
    ci = [diff rss_lower rss_upper];

% ------------------------------------------------------------
% start checking inputs
function inputsOkay = check_inputs(x1,n1,x2,n2,alpha,method,verbose)

inputsOkay = 1;

% inputs must all be scalars
if (max(max([size(x1); size(n1); size(x2); size(n2); size(alpha)])) ~= 1)
    error('Non-scalar input');
end;

if isnan(x1) | isnan(n1) | isnan(x2) | isnan(n2) | isnan(alpha)
    warning('NaN input')
    inputsOkay = 0;
    return
end;

% x and n must be integers
if (round(x1) ~= x1)
    x1 = round(x1);
    warning('Non-integer input x1');
end;
if (round(x2) ~= x2)
    x2 = round(x2);
    warning('Non-integer input x2');
end;
if (round(n1) ~= n1)
    n1 = round(n1);
    warning('Non-integer input n1');
end;
if (round(n2) ~= n2)
    n2 = round(n2);
    warning('Non-integer input n2');
end;

% n must be > 0
if (n1 <= 0 | n2 <= 0),
   inputsOkay = 0;
   warning('n <= 0');
   return;
end;

% n should be < 10^6
if (n1 > 10^6 | n2 > 10^6),
   warning('n > 10^6, Results may not be accurate.');
end;

% x must be >= 0 and <=n
if ( x1 < 0 | x1 > n1 | x2 < 0 | x2 > n2),
   inputsOkay = 0;
   warning('x < 0 or x > n');
   return;
end;

% alpha must be > 0.0 and < 1.0
if ( alpha < 0 | alpha > 1),
   inputsOkay = 0;
   warning('alpha < 0 or alpha > 1');
   return;
end;

% verbose must be 0 or 1
if ( verbose ~= 0 & verbose ~= 1),
   inputsOkay = 0;
   warning('verbose not zero or one');
   return;
end;
% end checking inputs
% ------------------------------------------------------------


% David Krause, david.krause@ece.queensu.ca
% Queen's University
% July 4, 2006
% Create a two axes plot

% Clean up!
close all
clear all
clc

% Some plot data
x_bottom = [0, 1, 2, 3, 4] * 1e-10;
x_top = [-4, -2, -1, 0, 1] * 1e9;
y_left = [0.5, 0.6, 0.3, -0.1, -0.2] * 1e9;
y_right = [0, 4, -5, 5, 6] * 1e4;

% Create the figure
figure(1);
axes;
plot(x_bottom, y_left, 'k.-');
xlimits = get(gca,'XLim');
ylimits = get(gca,'YLim');
xinc = (xlimits(2)-xlimits(1))/5;
yinc = (ylimits(2)-ylimits(1))/5;
set(gca,'XTick',[xlimits(1):xinc:xlimits(2)],...
        'YTick',[ylimits(1):yinc:ylimits(2)])
xlabel('Time (s)');
ylabel('Intensity (W/m)');
text(0.7e-10, 0e9, '$\bullet$ Black Line');
text(0.7e-10, -0.1e9, '$\circ$ Blue Line');

axes;
plot(x_top, y_right, 'bo-');
set(gca, 'color', 'none', 'YAxisLocation', 'right', ...
                          'XAxisLocation', 'top');
xlimits = get(gca,'XLim');
ylimits = get(gca,'YLim');
xinc = (xlimits(2)-xlimits(1))/5;
yinc = (ylimits(2)-ylimits(1))/5;
set(gca,'XTick',[xlimits(1):xinc:xlimits(2)],...
        'YTick',[ylimits(1):yinc:ylimits(2)]);
xlabel('Time (s)');
ylabel('Widgets ($\mu$ W)');

% Create a structure with the output parameters
st.filename = 'the_figure.tex';
st.comments = 'Hello world';
st.figure_box = [25, 15, 100, 90]; % In mm, the size of the plot area (not including the labels)
st.x_tick_weight = 0.25; % The weight (line thickness) of the ticks
st.x_tick_length = 2; % The length of the ticks
st.x_label_y_offset = 8; % How far (in mm), to offset the xlabel
st.x_ticklabel_y_offset = 3;

st.y_tick_weight = 0.25;
st.y_tick_length = 2;
st.y_label_x_offset = 18;
st.y_ticklabel_x_offset = 2;
st.y_ticklabel_pow10 = 6;

st.plot_line_thickness = 0.35;

% Convert to .tex
figure2latex(1, st);


function ll = fgplvmPointLogLikelihood(model, x, y)

% FGPLVMPOINTLOGLIKELIHOOD Log-likelihood of a point for the GP-LVM.
% FORMAT
% DESC returns the log likelihood of a latent point and an observed
% data point for the posterior prediction of the GP-LVM model.
% ARG model : the model for which the point prediction will be
% made.
% ARG x : the latent point for which the posterior distribution
% will be evaluated.
% ARG y : the observed data point for which the posterior
% distribution will be evaluated.
%
% SEEALSO : gpPointLogLikelihood, fgplvmCreate, fgplvmOptimisePoint, fgplvmPointObjective
%
% COPYRIGHT : Neil D. Lawrence, 2005, 2006

% FGPLVM

ll = gpPointLogLikelihood(model, x, y);
% check if there is a prior over latent space
if isfield(model, 'prior')
  for i = 1:size(x, 1)
    ll = ll + priorLogProb(model.prior, x(i, :));
  end
end


function [anx,any,anz,dx,dy,dz,ierr] = focal_pt2nd(wpx,wpy,wpz,wtx,wty,wtz)

    % compute Cartesian component of P and T versors from outward normal and slip vectors
    %
    %     usage:
    %     call pt2nd(px,py,pz,tx,ty,tz,anx,any,anz,dx,dy,dz,ierr)
    %
    %     arguments:
    %     px,py,pz       components of P (maximum dilatation) axis vector
    %                    in the Aki-Richards Cartesian coordinate system (INPUT)
    %     tx,ty,tz       components of T (maximum tension) axis vector
    %                    in the Aki-Richards Cartesian coordinate system (INPUT)
    %     anx,any,anz    components of fault plane outward normal versor in the
    %                    Aki-Richards Cartesian coordinate system (OUTPUT)
    %     dx,dy,dz       components of slip versor in the Aki-Richards
    %                    Cartesian coordinate system (OUTPUT)
    %     ierr           error indicator (OUTPUT)
    %
    %     errors:
    %     1              input vectors not perpendicular among each other
    %
    %      call fpsset
    amistr=-360.;
    amastr=360.;
    amidip=0.;
    amadip=90.;
    amirak=-360.;
    amarak=360.;
    amitre=-360.;
    amatre=360.;
    amiplu=0.;
    amaplu=90.;
    orttol=2.;
    ovrtol=0.001;
    tentol=0.0001;
    dtor=0.017453292519943296;
    c360=360.;
    c90=90.;
    c0=0.;
    c1=1.;
    c2=2.;
    c3=3.;

    %%c
    anx=c0;
    any=c0;
    anz=c0;
    dx=c0;
    dy=c0;
    dz=c0;
    ierr=0;
    [ang] = focal_angle(wpx,wpy,wpz,wtx,wty,wtz);
    if (abs(ang - c90) > orttol)
        disp(['PT2ND: input vectors not perpendicular, angle=' num2str(ang)]);
        ierr = 1;
    end
    [pnorm,px,py,pz] = focal_norm(wpx,wpy,wpz);

    if (pz < c0)
        [px,py,pz] = focal_invert(px,py,pz);
    end
    [tnorm,tx,ty,tz] = focal_norm(wtx,wty,wtz);
    if (tz < c0)
        [tx,ty,tz] = focal_invert(tx,ty,tz);
    end
    anx = tx + px;
    any = ty + py;
    anz = tz + pz;
    [amn,anx,any,anz] = focal_norm(anx,any,anz);
    % c
    dx = tx - px;
    dy = ty - py;
    dz = tz - pz;
    [amn,dx,dy,dz] = focal_norm(dx,dy,dz);
    if(anz > c0)
        [anx,any,anz] = focal_invert(anx,any,anz);
        [dx,dy,dz] = focal_invert(dx,dy,dz);
    end
end


classdef quadcopter < agent
    properties
        % GLOBAL.position   - The position of the object in the global axes
        % GLOBAL.velocity   - The velocity of the object in the global axes
        % GLOBAL.quaternion - The quaternion representing the earth to rotated body

        % DYNAMICS - All the models parameters are held in the DYNAMICS
        %            container field.
    end
    %% ////////////////////////// MAIN METHODS /////////////////////////////
    methods
        % Constructor
        function [this] = quadcopter(varargin)
            % Call the super class
            this@agent(varargin);

            % Create the dynamics of a quadcopter
            this.DYNAMICS = this.CreateDYNAMICS();

            % //////////////// Check for user overrides ///////////////////
            this = this.ApplyUserOverrides(varargin); % Recursive overrides
            % /////////////////////////////////////////////////////////////
        end
        % Setup
        function [this] = setup(this,localXYZVelocity,localXYZrotations)
            % This function calculates the intial state for a quadrotor
            % object.

            % For a state vector defined as x_k:
            % [x,y,z,dx,dy,dz,R11,R12,R13,R21,R22,R23,R31,R32,R33,wx,wy,wz]

            % THIS MODEL OPERATES IN THE GLOBAL FRAME
            p0 = this.GetGLOBAL('position');   % True global position
            v0 = this.GetGLOBAL('velocity');   % True global velocity
            % GET THE ROTATION MATRIX fixed local axes -> rotated global pose
            R0 = OMAS_geometry.eulersToRotationMatrix(localXYZrotations);
            % Build an initial (global) state vector
            vecR0   = reshape(R0',9,1);        % Defines local vector to global vector
            omega0  = zeros(3,1);
            x0      = [p0;v0;vecR0;omega0];
            % ASSIGN THE LOCAL FRD STATE
            this.SetGLOBAL('priorState',x0);
            this.localState = x0;
        end
        % Main
        function [this] = main(this,ENV,varargin)

            % //////////// CHECK FOR NEW INFORMATION UPDATE ///////////////
            % Update the agent with the environmental data
            [this,~,~] = this.GetAgentUpdate(ENV,varargin{1});
            % /////////////////////////////////////////////////////////////

            % The state reference
            desiredPosition = [0;0;0];

            % Call the controller loop
            this = this.Controller(ENV,desiredPosition);

            % /////////////// RECORD THE AGENT-SIDE DATA //////////////////
            this.DATA.inputNames = {'$\dot{x}$ (m/s)','$\dot{y}$ (m/s)','$\dot{z}$ (m/s)',...
                '$p$ (rad/s)','$q$ (rad/s)','$r$ (rad/s)'};
            this.DATA.inputs(1:length(this.DATA.inputNames),ENV.currentStep) = [this.localState(4:6);this.localState(16:end)];          % Record the control inputs
        end
    end
    %% //////////////////////// AUXILLARY METHODS //////////////////////////
    methods
        % The quadcopter controller
        function [this] = Controller(this,ENV,y_k)

            % Input sanity checks
            assert(IsColumn(y_k,3),"Expecting a numeric velocity vector.");
            assert(1 == size(this.localState,2),"The length of the objects state update must match the its local state.");

            % ///////////// UPDATE THE LOCAL STATE VECTOR /////////////////
            x_k_plus = this.UpdateLocalState(ENV,this.localState,y_k);
            % Update the global properties
            this = this.GlobalUpdate(x_k_plus);
        end
        % Get the state update (using ode45)
        function [x_k_plus] = UpdateLocalState(this,ENV,x_k,y_desired)
            % This function computes the state update for the current agent
            % using the ode45 function.

            % Integrate across the time delta "dt"
            opts = odeset('RelTol',1e-2,'AbsTol',ENV.dt*1E-1);

            [~,Xset] = ode45(@(t,X) this.ClosedLoopDynamics_position(X,y_desired),[0 ENV.dt],x_k,opts);
%             [~,Xset] = ode45(@(t,X) this.ClosedLoopDynamics_velocity(X,y_desired),[0 ENV.dt],x_k,opts);

            % Assign the integral state
            x_k_plus = Xset(end,:)';
        end
        % Global update from the new state
        function [this] = GlobalUpdate(this,x_k_plus)
            % This function updates the global structure from the new state
            % definition: [p_k;v_k;vecR_k;omega_k]

            % Extract the rotation matrix components
            R_k_plus = reshape(x_k_plus(7:15),3,3);
            % Define the new quaternion from R
            q_k_plus = OMAS_geometry.rotationMatrixToQuaternion(R_k_plus');

            % //////////////// UPDATE GLOBAL PROPERTIES ///////////////////
            [this] = this.GlobalUpdate_direct(...
                x_k_plus(1:3),...   % The global position is within the state
                x_k_plus(4:6),...   % The global velocity is within the state
                q_k_plus);          % Append the global quaternion

            % Ensure the local state is re-assigned
            this.localState = x_k_plus;
        end
    end
    % Dynamic functions
    methods
        % Quadcopter Dynamics (Closed-loop)
        function [dxdt] = ClosedLoopDynamics_velocity(this,x_k,v_desired)

            % Extract the current state properties
            p_k     = x_k(1:3);
            v_k     = x_k(4:6);
            R_k     = reshape(x_k(7:15),3,3); % The rotation matrix components
            eta_k   = OMAS_geometry.rotationMatrixToEulers(R_k);
            omega_k = x_k(16:18);

            % The state reference
            y_k = [p_k;v_k;eta_k;omega_k];

            % Extract the parameters from the state
            psi_desired = pi/2;
            y_desired = [zeros(3,1);v_desired;[0;0;psi_desired];zeros(3,1)];

            % Extract DYNAMIC parameters
            g  = this.DYNAMICS.g;
            m  = this.DYNAMICS.m;

            % State matrix
            A = this.DYNAMICS.A;
            A(4:6,7:9) = g*[ sin(psi_desired), cos(psi_desired), 0;
                            -cos(psi_desired), sin(psi_desired), 0;
                                            0,                0, 0];
            % Input matrix
            B = this.DYNAMICS.B;

            % LQR control gain
            [K,~,~] = lqr(A,B,eye(12),eye(4));
            K(:,1:3) = 0;
            % Calculate the error
            e_k  = y_k - y_desired;
            du_k = -K*e_k;
            % Calculate the inputs
            u_k = [m*g;0;0;0] + du_k;

            % Provide the inputs to the open-loop dynamics
            [dxdt] = this.OpenLoopDynamics(x_k,u_k);
        end
        % Quadcopter Dynamics (Closed-loop)
        function [dxdt] = ClosedLoopDynamics_position(this,x_k,p_desired)

            % Extract the current state properties
            p_k     = x_k(1:3);
            v_k     = x_k(4:6);
            R_k     = reshape(x_k(7:15),3,3); % The rotation matrix components
            eta_k   = OMAS_geometry.rotationMatrixToEulers(R_k);
            omega_k = x_k(16:18);

            % The state reference
            y_k = [p_k;v_k;eta_k;omega_k];

            % Extract the parameters from the state
            psi_desired = 0;
            y_desired = [p_desired(1:3);zeros(3,1);[0;0;psi_desired];zeros(3,1)];

            % Extract DYNAMIC parameters
            e3 = this.DYNAMICS.e3;
            g  = this.DYNAMICS.g;
            m  = this.DYNAMICS.m;
            I  = this.DYNAMICS.I;

            % State matrix
            A = this.DYNAMICS.A;
            A(1:3,4:6) = eye(3);
            A(4:6,7:9) = g*[ sin(psi_desired), cos(psi_desired), 0;
                -cos(psi_desired), sin(psi_desired), 0;
                0,                0, 0];
            A(7:9,10:12)=eye(3);

            % Input matrix
            B = this.DYNAMICS.B;
            B(4:6,1) = e3/m;
            B(10:12,2:4) = inv(I);

            % LQR control gain
            [K,~,~] = lqr(A,B,eye(12),eye(4));
            % Calculate the error
            e_k  = y_k - y_desired;
            du_k = -K*e_k;
            % Calculate the inputs
            u_k = [m*g;0;0;0] + du_k;

            % Provide the inputs to the open-loop dynamics
            [dxdt] = this.OpenLoopDynamics(x_k,u_k);
        end
        % Quadcopter Dynamics (Open-loop)
        function [dxdt] = OpenLoopDynamics(this,x_k,u_k)

            % Extract the state parameters
            v_k = x_k(4:6);
            R_k = reshape(x_k(7:15),3,3);
            w_k = x_k(16:18);
            % Extract the inputs
            f   = u_k(1);
            tau = u_k(2:4);

            % Get the constants
            m  = this.DYNAMICS.m;
            g  = this.DYNAMICS.g;
            I  = this.DYNAMICS.I;
            e3 = this.DYNAMICS.e3;

            % nonlinear dynamics based on rotation dynamics
            p_dot   = v_k;
            v_dot   = f/m*R_k*e3-g*e3;
            R_dot    = R_k*skew(w_k);
            vecR_dot = reshape(R_dot,9,1);
            w_dot    = inv(I)*(tau-skew(w_k)*I*w_k);

            % THE STATE DIFFERENCE
            dxdt = [p_dot;v_dot;vecR_dot;w_dot];
        end
    end
    % Dynamics container
    methods
        % Get the (generic) quadcopter dynamic properties
        function [DYNAMICS] = CreateDYNAMICS(this)
            % Simple quadcopter, use the default properties
            DYNAMICS = this.CreateDYNAMICS_default();
        end
        % Create (quadcopter) default dynamic structure
        function [DYNAMICS] = CreateDYNAMICS_default(this)
            % Define an infinite "throw" range by default
            DYNAMICS = struct();
            % Define kinematic constraints
            DYNAMICS.maxLinearVelocity      = ones(3,1)*this.v_max;	% Limits on the agents linear velocity
            DYNAMICS.maxLinearAcceleration  = inf(3,1);            	% Limits on the agents linear acceleration
            DYNAMICS.maxAngularVelocity     = ones(3,1)*this.w_max;	% Limits on the agents angular velocity
            DYNAMICS.maxAngularAcceleration = inf(3,1);            	% Limits on the agents angular acceleration
            % Dynamical properties
            DYNAMICS.e3 = [0;0;1];                                   % Local z-axis
            DYNAMICS.g  = 300978377846937375/30680772461461504;      % Gravitational constant
            DYNAMICS.m  = 1;                                         % Quadcopter mass
            DYNAMICS.I  = 2*diag([4.856, 4.856, 8.801])*10^(-3);     % Quadcopter inertia
            % Control properties
            % Plant matrix
            DYNAMICS.A = zeros(12);
            DYNAMICS.A(4:6,7:9)   = eye(3)*DYNAMICS.g;
            DYNAMICS.A(1:3,4:6)   = eye(3);
            DYNAMICS.A(7:9,10:12) = eye(3);
            % Input matrix
            DYNAMICS.B = zeros(12,4);
            DYNAMICS.B(4:6,1)     = DYNAMICS.e3/DYNAMICS.m;
            DYNAMICS.B(10:12,2:4) = inv(DYNAMICS.I);
            % Observation matrix
            DYNAMICS.C = eye(12);
            % Feed-forward matrix
            DYNAMICS.D = eye(4);
        end
    end
end


function E = foe_energy_a(x, im, basis, W)
%FOE_ENERGY_A Compute value of FoE energy function for a specific a
%
%   E = foe_energy_a(x, im, W)
%
% Computes the enegery value of the Field of Experts model defined by
% parameters W and a at point x. The energy is returned in E.
%
% This function is only used to check the gradient FOE_ENERGY_GRAD_A.
%
%
% (C) Laurens van der Maaten, 2009
% Delft University of Technology

    % Compute energy
    a = x;
    E = foe_energy(im, basis, W, a);


function [Anew Bnew] = padwithnan(A, B, dim)
% returns the two input arrays, with the smaller padded to the size of the
% larger in the particular dimension(s) with NaNs
%
% :Usage:
% ::
%
%     [Anew Bnew] = padwithnan(A, B, dim)
%

    if(nargin ~= 3)
        error('padwithnan: incorrect number of arguments');
    end

    diff = size(A,dim) - size(B,dim);

    if diff > 0
        Bnew = pwn_expand(B, dim, diff);
        Anew = A;
    elseif diff < 0
        Anew = pwn_expand(A, dim, diff);
        Bnew = B;
    else
        Anew = A;
        Bnew = B;
    end
end

function outarray = pwn_expand(inarray, dim, diff)
    sznandims = size(inarray);
    sznandims(dim) = abs(diff);
    outarray = cat(dim, inarray, NaN(sznandims));
end


function calcAllFeatureSets_STS(pathWORK,pathMINE,fSetNameType,outcome,setSize,nonTextStruct,textCells,textCellsName,paramAll,freedomMat,baseline,alpha,delta,nBoot)
% -------------------------------------------------------------------------
% function calcAllFeatureSets_STS(pathWORK,pathMINE,fSetNameType,outcome,setSize,nonTextStruct,textCells,textCellsName,paramAll,freedomMat,baseline,alpha,delta,nBoot)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes feature set reduction for a given feature set type
% and for all experiments with different degrees of freedom. See ref. [1]
% for more details.
% -------------------------------------------------------------------------
% REFERENCE:
% [1] Vallieres, M. et al. (2015). A radiomics model from joint FDG-PET and
%     MRI texture features for the prediction of lung metastases in soft-tissue
%     sarcomas of the extremities. Physics in Medicine and Biology, 60(14),
%     5471-5496. doi:10.1088/0031-9155/60/14/5471
% -------------------------------------------------------------------------
% INPUTS:
% - pathWORK: Full path to the STS WORKSPACE directory.
% - pathMINE: Full path to the MINE.jar executable. The executable can be
%             downloaded at: <http://www.exploredata.net/>.
% - fSetNameType: String specifying the name of the type of feature set
%                 (e.g., 'PET', 'SEPARATE', 'FUSED', etc.)
% - outcome: Column vector of size [nInst X 1] specifying the outcome status
%            (1 or 0) for all instances.
% - setSize: Size of the output feature set (typically set to 25 in ref. [1]).
% - nonTextStruct: Structure data for non-texture features. This structure
%                  is of the same format as the one saved as output to
%                  computeAllNonTextureFeatures_STS.m, for example.
% - textCells: Cell vector of organized texture cells for all the
%              different scans. Format: textCells = {cell1, cell2, etc.},
%              where cell1, cell2, etc. are the files saved as output to
%              organizeSeparateTextures_STS.m or organizeFusedTextures_STS.m,
%              for example.
% - textCellsName: Cell vector corresponding to the name of the
%                  corresponding cells in textCells.
%                  (e.g., textCellsName = {'PET','T1','T2FS'} for SEPARATE
%                  scans,  textCellsName = {'PET_T1','PET_T2FS'} for FUSED
%                  scans, as defined in ref. [1])
% - paramAll: Cell vector incorporating all texture extraction parameters
%             tested in textCells. See EXAMPLE below for more details.
% - freedomMat:  Matrix of row vectors of 1's and 0's to specify the degree
%                of freedom on texture extraction parameters for all
%                experiments. For example, for an nth experiment where
%                extraction parameters 1, 2 and 4 in paramAll are allowed
%                to vary, use freedomMat(n,:) = [1,1,0,1].
% - baseline: Vector of numerical values specifying the baseline texture
%             extraction parameters for each entry in paramAll. See EXAMPLE
%             below for more details.
% - alpha: Numerical values specifying the coefficient of the first part of
%          the Gain equation, as defined in ref. [1].
% - delta: Numerical values specifying the coefficient of the second part
%          of the Gain equation, as defined in ref. [1] (third part is set
%          to 0 in this function).
% - nBoot: Number of bootstrap samples to use.
%
% See <https://github.com/mvallieres/radiomics/tree/master/STUDIES/STSstudy/Functions>
% to find computeAllNonTextureFeatures_STS.m, organizeSeparateTextures_STS.m
% and organizeFusedTextures_STS.m. See masterScript_STS.m for a complete
% example of how to utilize the current function.
% -------------------------------------------------------------------------
% OUTPUTS: Feature sets are saved in a folder named 'FSET' in the STS
%          WORKSPACE.
% -------------------------------------------------------------------------
% EXAMPLE:
% MRIinv_cell = {'NoInv','Inv'};
% MRIweight_mat = [1/4,1/3,1/2,2/3,3/4];
% R_mat = [1/2,2/3,1,3/2,2];
% scale_cell = {'pixelW',1,2,3,4,5};
% algo_cell = {'Equal','Lloyd'};
% Ng_mat = [8,16,32,64];
%
% FOR FUSED SCANS
% paramAll = {MRIinv_cell,MRIweight_mat,R_mat,scale_cell,algo_cell,Ng_mat};
% freedomMat = [1 1 1 1 ; 0 0 0 0 ; 0 1 0 1]; (example)
% baseline = [1 3 3 1 2 3];
%
% FOR SEPARATE SCANS
% paramAll = {R_mat,scale_cell,algo_cell,Ng_mat};
% freedomMat = [1 1 1 1 1 1 ; 0 0 0 0 0 0 ;  0 1 0 1 0 1]; (example)
% baseline = [3 1 2 3];
%
% NOTE: paramAll must always be of the same format and size for SEPARATE
% and FUSED SCANS, with the same ordering of different extraction
% parameters.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: May 2015
%--------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
%
%    This package is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    This package is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this package.  If not, see <http://www.gnu.org/licenses/>.
% -------------------------------------------------------------------------

startpath = pwd;
cd([pathWORK,'/FSET'])

nParamType = size(freedomMat,2);
nFreedom = size(freedomMat,1);
tStart = tic;
for i = 1:nFreedom
    nameSave=['FSET_',fSetNameType,'_'];
    for j = 1:nParamType
        nameSave = [nameSave,num2str(freedomMat(i,j))];
    end
    fprintf(['COMPUTING ',nameSave,' ... '])
    tic
    [fSet] = featureSetReduction(pathMINE,fSetNameType,outcome,setSize,nonTextStruct,textCells,textCellsName,paramAll,freedomMat(i,:),baseline,alpha,delta,nBoot);
    toc
    save(nameSave,'fSet')
end
time = toc(tStart);
fprintf('TOTAL TIME: %.2f seconds\n',time)

cd(startpath)
end


function c = linkage(c,varargin);

% c = LINKAGE(c) Creates a hierarchical cluster tree for all waveforms in
% the correlation object. It reads the CORR field (must be filled) and
% fills in the LINK field. This iuse is identical to c =
% LINKAGE(c,'average').
%
% c = LINKAGE(c,...) is just a wrapper program which calls the linkage
% function in the matlab stats toolbox.  Usage is the same as the stats
% toolbox version except that the first argument and the returned value are
% correlation objects. In most cases these will be the same object. See
% HELP LINKAGE for details and usage. All options and user controls are the
% same.
%
% ** IMPORTANT NOTE **: There is one significant difference from the
% built-in linkage routine. The native linkage command operates on
% *dissimilarity* information. That is, the opposite of correlation values.
% When linkage is called on a correlation object, it expects max
% correlation values as input. Temporary dissimiliarity information is
% created internally and passed to the built in linkage routine.
%
% Common uses include:
%   c = LINKAGE(c)           % returns clusters of similar waveforms
% 	c = LINKAGE(c,'average') % same as first use
% 	c = LINKAGE(c,'single')  % useful for evolving waveforms

% Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
% $Date$
% $Revision$

if nargin <= 0
    error('Not enough inputs');
end

if ~isa(c,'correlation')
    error('First input must be a correlation object');
end;

if get(c,'TRACES')<2
    error('correlationLinkageTooFewTraces','Correlation object must contain at least two traces to use the LINKAGE function');
end;

% if isempty(get(c,'LAG'))
%     error('LAG field must be filled in input object');
%     error('See correlation/linkage function');
% end;

K = 1.001 - c.C;			% create dissimilarity matrix
K = K - diag(diag(K));			% remove diagonal (required format)
Y = squareform(K);          % transform to "pdist" vector format

if nargin == 1
    c.link = linkage(Y,'average');
else
    c.link = linkage(Y,varargin{:});
end


% segmentiris - peforms automatic segmentation of the iris region
% from an eye image. Also isolates noise areas such as occluding
% eyelids and eyelashes.
%
% Usage:
% [circleiris, circlepupil, imagewithnoise] = segmentiris(image)
%
% Arguments:
%	eyeimage		- the input eye image
%
% Output:
%	circleiris	    - centre coordinates and radius
%			          of the detected iris boundary
%	circlepupil	    - centre coordinates and radius
%			          of the detected pupil boundary
%	imagewithnoise	- original eye image, but with
%			          location of noise marked with
%			          NaN values
%
% Author:
% Libor Masek
% masekl01@csse.uwa.edu.au
% School of Computer Science & Software Engineering
% The University of Western Australia
% November 2003

function [circleiris, circlepupil, imagewithnoise] = segmentiris(eyeimage)

% define range of pupil & iris radii

%CASIA
lpupilradius = 28;
upupilradius = 75;
lirisradius = 80;
uirisradius = 150;

%    %LIONS
%    lpupilradius = 32;
%    upupilradius = 85;
%    lirisradius = 145;
%    uirisradius = 169;

% define scaling factor to speed up Hough transform
scaling = 0.4;

reflecthres = 240;

% find the iris boundary by Daugman's intefrodifferential operaror

        [rowp, colp, rp] = SearchInnerBoundary(double(eyeimage));

        [row, col, r] = SearchOuterBoundary(double(eyeimage), rowp, colp, rp);
        circleiris = [row, col, r];
        rowd = double(row);
        cold = double(col);
        rd = double(r);

        irl = round(rowd-rd);
        iru = round(rowd+rd);
        icl = round(cold-rd);
        icu = round(cold+rd);

        imgsize = size(eyeimage);

        if irl < 1
            irl = 1;
        end

        if icl < 1
            icl = 1;
        end

        if iru > imgsize(1)
            iru = imgsize(1);
        end

        if icu > imgsize(2)
            icu = imgsize(2);
        end

        % to find the inner pupil, use just the region within the previously
        % detected iris boundary
        imagepupil = double(eyeimage( irl:iru,icl:icu));

        rowp = double(rowp)-irl;
        colp = double(colp)-icl;
        r = double(rp);

        row = double(irl) + rowp;
        col = double(icl) + colp;

        row = round(row);
        col = round(col);

        circlepupil = [row col r];

% set up array for recording noise regions
% noise pixels will have NaN values
imagewithnoise = double(eyeimage);

%find top eyelid
topeyelid = uint8(imagepupil(1:(rowp-r),:));
lines = findline(topeyelid);

if size(lines,1) > 0
    [xl yl] = linecoords(lines, size(topeyelid));
    yl = double(yl) + irl-1;
    xl = double(xl) + icl-1;

    yla = max(yl);

    y2 = 1:yla;

    ind3 = sub2ind(size(eyeimage),yl,xl);
    imagewithnoise(uint32(ind3)) = NaN;
    imagewithnoise(round(y2), round(xl)) = NaN;
end

%find bottom eyelid
bottomeyelid = uint8(imagepupil((rowp+r):size(imagepupil,1),:));
lines = findline(bottomeyelid);

if size(lines,1) > 0

    [xl yl] = linecoords(lines, size(bottomeyelid));
    yl = double(yl)+ irl+rowp+r-2;
    xl = double(xl) + icl-1;

    yla = min(yl);

    y2 = yla:size(eyeimage,1);

    ind4 = sub2ind(size(eyeimage),yl,xl);
    imagewithnoise(uint32(ind4)) = NaN;
    imagewithnoise(round(y2), round(xl)) = NaN;

end

% %For CASIA, eliminate eyelashes by thresholding
ref = eyeimage < 80;
coords = find(ref==1);
imagewithnoise(coords) = NaN;


function calculateDDM(basePlanCFile,ddmTemplatePlanCFileName,...
    registeredPlanCFilesDir,vfDir,saveFileLocationWithDDM)

% Linux
basePlanCFile = '/lab/deasylab1/Data/RTOG0617/CERR_files_tcia/rider_template/RIDER-1225316081_First_resampled_1x1x3.mat';
ddmTemplatePlanCFileName = '0617-548359_09-09-2000-30377';
registeredPlanCFilesDir = '/lab/deasylab1/Data/RTOG0617/CERR_files_tcia/dose_mapping_original_plans'; % RTOG0617
vfDir = '/lab/deasylab1/Data/RTOG0617/registrations_pericardium/ddm/RTOG0617_to_RIDER_1225316081_First_template'; %RIDER RIDER-1225316081
saveFileLocationWithDDM = '/lab/deasylab1/Data/RTOG0617/registrations_pericardium/ddm/RTOG0617_to_RIDER_1225316081_First_template_ddm.mat';

% Windows
% basePlanCFile = 'L:/Data/RTOG0617/CERR_files_tcia/rider_template/RIDER-1225316081_First_resampled_1x1x3.mat';
% ddmTemplatePlanCFileName = '0617-548359_09-09-2000-30377';
% registeredPlanCFilesDir = 'L:/Data/RTOG0617/CERR_files_tcia/dose_mapping_original_plans'; % RTOG0617
% vfDir = 'L:/Data/RTOG0617/registrations_pericardium/ddm/RTOG0617_to_RIDER_1225316081_First_template'; %RIDER RIDER-1225316081
% saveFileLocationWithDDM = 'L:/Data/RTOG0617/registrations_pericardium/ddm/RTOG0617_to_RIDER_1225316081_First_template_ddm.mat';
%
% basePlanCFile = strrep(basePlanCFile,'/','\');
% ddmTemplatePlanCFileName = strrep(ddmTemplatePlanCFileName,'/','\');
% registeredPlanCFilesDir = strrep(registeredPlanCFilesDir,'/','\');
% vfDir = strrep(vfDir,'/','\');
% saveFileLocationWithDDM = strrep(saveFileLocationWithDDM,'/','\');

[~,basePlanCFileName] = fileparts(basePlanCFile);

% registeredPlanCFilesDir = 'L:\Data\RTOG0617\registrations_pericardium\ddm\RTOG0617_to_RIDER_1225316081_First_template'

% registeredPlanCFilesDir = '/lab/deasylab2/Ishita/Stenosis_ultracentral/00216080_unzipped';
% baseTemplateFile = 'Baseline';
% ddmTemplateFile = '6month';

% baseTemplateFile = fullfile(registeredPlanCFilesDir,[basePlanCFile,'.mat']) ;
% ddmTemplateFile = fullfile(registeredPlanCFilesDir,[ddmTemplatePlanCFile,'.mat']) ;

dirS = dir(registeredPlanCFilesDir);
dirS([dirS.isdir]) = [];
fileNamC = {dirS.name};

% x,y,z coordinates for DDM calculation
planC = loadPlanC(basePlanCFile,tempdir);
planC = updatePlanFields(planC);
planC = quality_assure_planC(basePlanCFile,planC);
scanNum = 1;
indexS = planC{end};
siz = getUniformScanSize(planC{indexS.scan}(scanNum));
[xBaseV,yBaseV,zBaseV] = getUniformScanXYZVals(planC{indexS.scan}(scanNum));
[xBase3M,yBase3M,zBase3M] = meshgrid(xBaseV,yBaseV,zBaseV);
% xBaseV = xBase3M(:);
% yBaseV = yBase3M(:);
% zBaseV = zBase3M(:);

% filesC = fullfile(registeredPlanCFilesDir,fileNamC);

% if ~exist('vfDir','var')
%     vfDir = fullfile(registeredPlanCFilesDir,'registered');
% end

numFiles = length(fileNamC);
xDeformM = [];
yDeformM = [];
zDeformM = [];
for iFile = 1:numFiles
    fName = fileNamC{iFile};
    fullFilePath = fullfile(registeredPlanCFilesDir,fName);
    planC = loadPlanC(fullFilePath,tempdir);
    indexS = planC{end};
    scanNum = 1;
    [xUnifV,yUnifV,zUnifV] = getUniformScanXYZVals(planC{indexS.scan}(scanNum));

    if strcmp(basePlanCFileName,strtok(fName,'.'))
        continue;
    end
    vfBaseName = [basePlanCFileName,'~',strtok(fName,'.'),'_vf.mat'];
    vfBaseFile = fullfile(vfDir,vfBaseName);
    if ~exist(vfBaseFile,'file')
        continue;
    end

    load(vfBaseFile)
    xBaseDeform3M = xBase3M + vf(:,:,:,1);
    yBaseDeform3M = yBase3M + vf(:,:,:,2);
    zBaseDeform3M = zBase3M + vf(:,:,:,3);

    if strcmp(strtok(fName,'.'),ddmTemplatePlanCFileName)
        xDeformV = xBaseDeform3M(:);
        yDeformV = yBaseDeform3M(:);
        zDeformV = zBaseDeform3M(:);
    else
        vfDdmName = [strtok(fName,'.'),'~',ddmTemplatePlanCFileName,'_vf.mat'];
        vfDdmFile = fullfile(vfDir,vfDdmName);
        if ~exist(vfDdmFile,'file')
            continue;
        end
        load(vfDdmFile)

        xFieldV = [xUnifV(1)-1e-6,xUnifV(2)-xUnifV(1),xUnifV(end)+1e-6];
        yFieldV = [yUnifV(end)-1e-6,yUnifV(1)-yUnifV(2),yUnifV(1)+1e-6];

        xDeformV = xBaseDeform3M(:) + finterp3(xBaseDeform3M(:),yBaseDeform3M(:),zBaseDeform3M(:),...
            flip(vf(:,:,:,1),1),xFieldV,yFieldV,zUnifV);
        yDeformV = yBaseDeform3M(:) + finterp3(xBaseDeform3M(:),yBaseDeform3M(:),zBaseDeform3M(:),...
            flip(vf(:,:,:,2),1),xFieldV,yFieldV,zUnifV);
        zDeformV = zBaseDeform3M(:) + finterp3(xBaseDeform3M(:),yBaseDeform3M(:),zBaseDeform3M(:),...
            flip(vf(:,:,:,3),1),xFieldV,yFieldV,zUnifV);
    end

    xDeformM(:,end+1) = xDeformV;
    yDeformM(:,end+1) = yDeformV;
    zDeformM(:,end+1) = zDeformV;

end

% % Pairwise distance
% numDeforms = size(xDeformM,2);
% pairsM = nchoosek(1:numDeforms,2);
% numPairs = size(pairsM,1);
% distM = zeros(size(xDeformM,1),numPairs);
% for i = 1:numPairs
%     xSquareV = diff(xDeformM(:,pairsM(i,:)),1,2).^2;
%     ySquareV = diff(yDeformM(:,pairsM(i,:)),1,2).^2;
%     zSquareV = diff(zDeformM(:,pairsM(i,:)),1,2).^2;
%     distSquareV = xSquareV + ySquareV + zSquareV;
%     distM(:,i) = distSquareV.^0.5;
% end
% medianDistV = nanmedian(distM,2);

xMeanV = median(xDeformM,2,'omitnan');
yMeanV = median(yDeformM,2,'omitnan');
zMeanV = median(zDeformM,2,'omitnan');
xSquareV = bsxfun(@minus,xDeformM,xMeanV);
ySquareV = bsxfun(@minus,yDeformM,yMeanV);
zSquareV = bsxfun(@minus,zDeformM,zMeanV);

medianDistV = median((xSquareV.^2 + ySquareV.^2 + zSquareV.^2).^0.5,2,...
    'omitnan')/sqrt(2);

dist3M = reshape(medianDistV,siz);

% Add as dose to base planC
planC = loadPlanC(basePlanCFile,tempdir);
planC = updatePlanFields(planC);
planC = quality_assure_planC(basePlanCFile,planC);
indexS = planC{end};
scanType = 'UniformCT';
assocScanUID = planC{indexS.scan}(scanNum).scanUID;
planC = dose2CERR(single(dist3M),'','DDM','',...
'',scanType,'','No',assocScanUID,planC);
planC = save_planC(planC,[],'passed',saveFileLocationWithDDM);