% AstarMOO A*-MOO navigation class
%
% A concrete subclass of the Navigation class that implements the A*
% navigation algorithm for multiobjective optimization (MOO) - i.e.
% optimizes over several objectives/criteria.
%
% Methods:
%     plan              Compute the cost map given a goal and map
%     path              Compute a path to the goal
%     visualize         Display the obstacle map (deprecated)
%     plot              Display the obstacle map
%     costmap_modify    Modify the costmap
%     costmap_get       Return the current costmap
%     costmap_set       Set the current costmap
%     distancemap_get   Set the current distance map
%     heuristic_get     Get the current heuristic map
%     display           Print the parameters in human readable form
%     char              Convert to string
%
% Properties:
% TBD
%
% Example::
%
%         load map1           % load map
%         goal = [50;30];
%         start = [20;10];
%         as = AstarMOO(map);    % create Navigation object
%         as.plan(goal,2);       % setup costmap for specified goal
%         as.path(start);        % plan solution path star-goal, animate
%         P = as.path(start);    % plan solution path star-goal, return path
%
% Example 2:
%         goal = [100;100];
%         start = [1;1];
%         as = AstarMOO(0);   % create Navigation object with random occupancy grid
%         as.addCost(1,L);    % add 1st add'l cost layer L
%         as.plan(goal,3);    % setup costmap for specified goal
%         as.path(start);     % plan solution path start-goal, animate
%         P = as.path(start);    % plan solution path start-goal, return path
%
% Notes::
% - Obstacles are represented by Inf in the costmap.
%
% References::
% - A Pareto Optimal D* Search Algorithm for Multiobjective Path Planning, A. Lavin.
% - A Pareto Front-Based Multiobjective Path Planning Algorithm, A. Lavin.
% - Robotics, Vision & Control, Sec 5.2.2, Peter Corke, Springer, 2011.
%
% Author::
% Alexander Lavin
%
% See also Navigation, Astar, AstarPO.

% Copyright (C) 1993-2015, by Peter I. Corke, Alexander Lavin
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
%
% The RTB implementation of this algorithm is done by Alexander Lavin.
% http://alexanderlavin.com

% Implementation notes:
%
% All the state is kept in the structure called a
% X is an index into the array of states.
% state pointers are kept as matlab array index rather than row,col format

classdef AstarMOO < Navigation

    properties (SetAccess=private, GetAccess=private)

        % essential world info
        costmap   % world cost map: obstacle = Inf
        G         % index of goal point
        N         % number of objectives

        % info kept per cell (state):
        b            % backpointer (0 means not set)
        t            % tag: NEW OPEN CLOSED

        cost_g       % path distance summation
        cost_h       % path heuristic (state to goal) cost
        cost_01      % add'l cost layer 01 (unused)
        cost_02      % add'l cost layer 02 (unused)
        cost_03      % add'l cost layer 03 (unused)
                     % add more cost layers if needed...

        priority
        tie

        % list of open states: 2xN matrix
        %   each open point is a column, row 1 = index of cell, row 2 = k
        openlist
        niter

        changed

        openlist_maxlen     % keep track of maximum length
        quiet

        % tag state values
        NEW = 0;
        OPEN = 1;
        CLOSED = 2;
    end

    methods % start of public methods
        % AstarMOO class constructor:
        function as = AstarMOO(world, varargin)
            %AstarMOO.AstarMOO A*-MOO constructor
            %
            % AS = AstarMOO(MAP, OPTIONS) is a A* navigation object, and MAP is an
            % occupancy grid, a representation of a planar world as a
            % matrix whose elements are 0 (free space) or 1 (occupied).
            % The occupancy grid is coverted to a costmap with a unit cost
            % for traversing a cell.
            %
            % Options::
            % 'goal',G      Specify the goal point (2x1)
            % 'metric',M    Specify the distance metric as 'euclidean' (default)
            %               or 'cityblock'.
            % 'inflate',K   Inflate all obstacles by K cells.
            % 'quiet'       Don't display the progress spinner
            %
            % Other options are supported by the Navigation superclass.
            %
            % Notes::
            % - If MAP == 0 a random map is created.
            %
            % See also Navigation.Navigation.

            % invoke the superclass constructor
            as = as@Navigation(world, varargin{:}); % includes the occgrid

            % options
            opt.quiet = false;
            opt = tb_optparse(opt, varargin);
            as.quiet = opt.quiet;

            as.occgrid2costmap(as.occgrid);

            % initialize the A* state variables
            as.reset();
            if ~isempty(as.goal)
                as.goal_change();
            end
            as.changed = false;
        end

        function reset(as)
            %AstarMOO.reset Reset the planner
            %
            % AS.reset() resets the A* planner.  The next instantiation
            % of AS.plan() will perform a global replan.

            % build the matrices required to hold the state of each cell for A*
            as.b = zeros(size(as.occgrid), 'uint32');     % backpointers
            as.t = zeros(size(as.occgrid), 'uint8');      % tags, all NEW=0
            as.cost_g = Inf*ones(size(as.costmap));       % path cost estimate
            as.openlist = zeros(2,0);                     % the open list, one column per point

            as.openlist_maxlen = -Inf;
        end

        %AstarMOO.goal_change Changes to costlayers due to new goal
        %position
        function goal_change(as)

            if isempty(as.b)
                return;
            end
            goal = as.goal;

            % keep goal in index rather than row,col format
            as.G = sub2ind(size(as.occgrid), goal(2), goal(1));
            as.INSERT(as.G, as.projectCost(as.G), 'goalset');
            as.cost_g(as.G) = 0;

            % new goal changes cost layers:
            as.calcHeuristic(as.occgrid, as.goal);
        end

        function s = char(as)
            %AstarMOO.char Convert navigation object to string
            %
            % AS.char() is a string representing the state of the Astar
            % object in human-readable form.
            %
            % See also AstarMOO.display, Navigation.char.

            % most of the work is done by the superclass
            s = char@Navigation(as);
        end

        function plot(as, varargin)
            %AstarMOO.plot Visualize navigation environment
            %
            % AS.plot() displays the occupancy grid and the goal distance
            % in a new figure.  The goal distance is shown by intensity which
            % increases with distance from the goal.  Obstacles are overlaid
            % and shown in red.
            %
            % AS.plot(P) as above but also overlays a path given by the set
            % of points P (Mx2).
            %
            % See also Navigation.plot.

            plot@Navigation(as, 'distance', as.cost_h, varargin{:});
        end

       % invoked by Navigation.step
        function n = next(as, current)
            % Backpropagate from goal to start
            % Return [col;row] of previous step
            if as.changed
                error('Cost map has changed, replan');
            end
            X = sub2ind(size(as.occgrid), current(2), current(1));
            X = as.b(X); % set X as the backpointer of X
            if X == 0
                n = []; % goal (no further backpointer)
            else
                [r,c] = ind2sub(size(as.costmap), X);
                n = [c;r];
            end
        end

        function plan(as, goal, N)
            %AstarMOO.plan Prep the grid for planning.
            %
            % AS.plan() updates AS with a costmap of distance to the
            % goal from every non-obstacle point in the map.  The goal is
            % as specified to the constructor.
            %
            % Inputs:
            %   goal: goal state coordinates
            %   N: number of optimization objectives; standard A* is 2
            %   (i.e. distance and heuristic)

            as.N = N; % number of optimization objectives
            as.openlist = zeros(as.N+1,0);

            % Setup cost layers. If a
            % cost layer is goal-dependent, it's setup function needs to
            % also be called in AS.goal_change(). If more cost layers are
            % needed, add similar to AS.cost_01.

            % initializations first:
            as.cost_g = zeros(size(as.occgrid));
            as.cost_h = zeros(size(as.occgrid)); % filled after setting goal below
            % if add'l costs haven't been added with addCost()
            if isempty(as.cost_01)
                as.cost_01 = zeros(size(as.occgrid));
            end
            if isempty(as.cost_02)
                as.cost_02 = zeros(size(as.occgrid));
            end
            if isempty(as.cost_03)
                as.cost_03 = zeros(size(as.occgrid));
            end

            if nargin > 1
                as.goal = goal; % invokes superclass method set.goal()
            end
            if isempty(as.goal)
                error('must specify a goal point');
            end

            % Setup cost layers AS.cost_g and AS.cost_h.
            % assign values to the distance cost layer, set as AS.costmap
            as.occgrid2costmap(as.occgrid);
            % assign values to the heuristic cost layer, set as AS.cost_h
            as.calcHeuristic(as.occgrid, as.goal);
            % Additional cost layers are added by the user with the
            % AS.addCost() method

            % Cost priority/tiebreaker: cost_g (distance to node)
            as.priority = as.cost_g;
            as.tie = 1; % first cost: cost_g
        end

        function P = path(as, start)
        %AstarMOO.path Find a path between two points
        %
        % AS.path(START) finds and displays a path from START to GOAL
        % which is overlaid on the occupancy grid.
        %
        % P = AS.path(START) returns the path (2xM) from START to GOAL.
            if nargin < 1
                error('must specify start state');
            end

            % invoke the superclass path function, which iterates on our
            % next method
            start = [start; 1]; % Specifies backpropogation for NAV.path()
            if nargout == 0
                path@Navigation(as, start);
            else
                P = path@Navigation(as, start);
            end
        end

        % Handler invoked by Navigation.path() to start the navigation process
        % Calculate the solution path
        % Line comments LN reference A* pseudocode in Lavin's "A Pareto
        % Front-Based Multiobjective Path Planning Algorithm" where n is
        % the line number.
        function navigate_init(as, start)
            as.openlist = zeros(as.N+1,0); % make sure openlist is empty
            % begin with the start node
            start = sub2ind(size(as.costmap), start(2), start(1));
            as.openlist(1,1) = start;
            as.t(start) = as.OPEN;
            as.niter = 0; flag = 0; % init while loop
            % Plan the A* path
            while ~isempty(as.openlist) % L4
                % Normalize costs on the open list, choose expansion state:
                queue = normc(as.openlist(2:size(as.openlist,1),:)');
                [~,ind]=min(sum(queue,2));
                X = as.openlist(1,ind); % L5; minimum state on the open list
                as.DELETE(X); % L6; remove state from the openlist

                as.niter = as.niter + 1;

                if ~as.quiet && mod(as.niter, 20) == 0 % i.e. spinner every 20 iterations
                    as.spinner();
                end

                % Populate the openlist:
                for Y=as.neighbors(X) % L7,8; check node's 8 neighbors
                    if(Y==as.G) % L9; check for goal
                        as.b(Y) = X;
                        as.updateCosts(Y,X,as.N)
                        flag = 1; % flag for goal
                        break;
                    end
                    if as.t(Y)==as.NEW && as.costmap(Y)~=Inf % hasn't been checked and isn't an obstacle
                        as.b(Y) = X;
                        as.updateCosts(Y,X,as.N); % as.N = 2 -> base case w/ only distance cost parameters (g and h)
                        % project node's costs into objective space:
                        objspace = as.projectCost(Y,X);
                        as.INSERT(Y, objspace, '');
                    end
                end
                if as.verbose
                    disp(' ')
                end
                if flag==1 % goal found
                    break;
                end
            end
            if ~as.quiet
                fprintf('\r');
            end
            as.changed = false;
        end

        function layer = cost_get(as)
        %AstarMOO.cost_get Get the specified cost layer
            layer = as.cost_02;
        end

        function c = heuristic_get(as)
        %AstarMOO.heuristic_get Get the current heuristic map
        %
        % C = AS.heuristic_get() is the current heuristic map.  This map is the same size
        % as the occupancy grid and the value of each element is the shortest distance
        % from the corresponding point in the map to the current goal.  It is computed
        % by Astar.plan.
        %
        % See also Astar.plan.
            c = as.cost_h;
        end

        function c = costmap_get(as)
        %AstarMOO.costmap_get Get the current costmap
        %
        % C = AS.costmap_get() is the current costmap.  The cost map is the same size
        % as the occupancy grid and the value of each element represents the cost
        % of traversing the cell.  It is autogenerated by the class constructor from
        % the occupancy grid such that:
        % - free cell (occupancy 0) has a cost of 1
        % - occupied cell (occupancy >0) has a cost of Inf
        %
        % See also Astar.costmap_set, Astar.costmap_modify.

            c = as.costmap;
        end

        function costmap_set(as, costmap)
        %AstarMOO.costmap_set Set the current costmap
        %
        % AS.costmap_set(C) sets the current costmap.  The cost map is the same size
        % as the occupancy grid and the value of each element represents the cost
        % of traversing the cell.  A high value indicates that the cell is more costly
        % (difficult) to traverese.  A value of Inf indicates an obstacle.
        %
        % Notes::
        % - After the cost map is changed the path should be replanned by
        %   calling AS.plan().
        %
        % See also Astar.costmap_get, Astar.costmap_modify.
            if ~all(size(costmap) == size(as.occgrid))
                error('costmap must be same size as occupancy grid');
            end
            as.costmap = costmap;
            as.changed = true;
            end

        function costmap_modify(as, point, newcost)
        %AstarMOO.costmap_modify Modify cost map
        %
        % AS.costmap_modify(P, NEW) modifies the cost map at P=[X,Y] to
        % have the value NEW.  If P (2xM) and NEW (1xM) then the cost of
        % the points defined by the columns of P are set to the corresponding
        % elements of NEW.
        %
        % Notes::
        % - After one or more point costs have been updated the path
        %   should be replanned by calling AS.plan().
        %
        % See also AstarMOO.costmap_set, AstarMOO.costmap_get.

            if numel(point) == 2
                % for case of single point ensure it is a column vector
                point = point(:);
            end
            if numcols(point) ~= numcols(newcost)
                error('number of columns in point must match columns in newcost');
            end
            for i=1:numcols(point)
                X = sub2ind(size(as.costmap), point(2,i), point(1,i));
                as.costmap(X) = newcost(i);
            end
            if as.t(X) == as.CLOSED
                as.INSERT(X, as.h(X), 'modifycost');
            end
            as.changed = true;
        end

        function addCost(as, layer, values)
        %AstarMOO.addCost Add an additional cost layer
        %
        % AS.addCost(LAYER, VALUES) adds the matrix specified by values as a
        % cost layer.  The layer number is given by LAYER, and VALUES has the same
        % size as the original occupancy grid.

            if size(values)~=size(as.occgrid)
                display('Layer size does not match the environment')
                return
            end
            if max(max(values))~=1 || min(min(values))~=0
                display('Layer values are not normalized [0:1]')
                return
            end

            if layer==1
                as.cost_01 = values;
            elseif layer==2
                as.cost_02 = values;
            elseif layer==3
                as.cost_03 = values;
            else
                display('Layer index out of range')
            end
            % If more cost layers needed, add additional elseif statements
            % as above.
        end

    end % end of public methods

    methods (Access=protected) % start of private methods

        function occgrid2costmap(as, og, cost)
            if nargin < 3
                cost = 1; % cost to traverse each cell
            end
            as.costmap = og;
            as.costmap(as.costmap==1) = Inf; % occupied cells -> infinite path cost
            as.costmap(as.costmap==0) = cost; % unoccupied cells -> path cost
        end

        function calcHeuristic(as, grid, goal)
            as.cost_h=zeros(size(grid));
            for ii=1:size(grid,1)
                for jj=1:size(grid,2)
                    as.cost_h(ii,jj)=sqrt((ii-goal(1))^2+(jj-goal(2))^2);
                end
            end
        end

             % NOTE: Only for costs that accumulate (i.e. sum) over the
            % path, and for dynamic costs.
            % E.g. the heuristic parameter AS.cost_h only needs updating
            % when the goal state changes; it's values are stored for each
            % cell.
            %
            % Location moving from state b to a.

        function k_new = updateCosts(as, a, b, obj)

            if nargout > 0
                k_new = as.cost_g(b) + as.dc(b,a);
                return
            end
            if obj == 0                        % just return what the new priority cost would be (k_new)
                return
            end
            if obj > 1                         % base case
                as.cost_g(a) = as.cost_g(b) + as.dc(b,a);
                % (no heuristic update needed)
            end
            if obj > 2                         % w/ cost_01: elevation
                % (no elevation update needed)
            end
            if obj > 3                         % w/ cost_02: solar
                sV = [cos(as.niter/100);sin(as.niter/100)]; % rotates 1rad per 100 steps
                as.cost_02(a) = dot(sV,as.vc(b,a));
            end
            if obj > 4                         % w/ cost_03: risk
                % (no risk update needed)
            end
        end

        % Returns the projection of state a into objective space. If
        % specified, location is moving from b to a.
        function pt = projectCost(as, a, b)
            switch nargin
                case 2
                    pt = [as.cost_g(a);
                          as.cost_h(a);
                          as.cost_01(a);
                          as.cost_02(a);
                          as.cost_03(a);
                          ];
                case 3
                    pt = [as.cost_g(b) + as.dc(a,b);
                          as.cost_h(a);
                          as.cost_01(a);
                          as.cost_02(a);
                          as.cost_03(a);
                          ];
                otherwise
                    return
            end
        end

        % X is new node with costs in array pt

        function INSERT(as, X, pt, where)

            if nargin>2
                as.message('insert (%s) %d = %f\n', where, X, pt);
            end

            i = find(as.openlist(1,:) == X);
            if length(i) > 1
                error('A*:INSERT: state in open list %d times', X);
            end

            if as.t(X) == as.NEW
                % add a new column to the open list
                as.openlist = [as.openlist [X; pt]];
            elseif as.t(X) == as.OPEN
                % L13: If a node with same position as successor is in the
                % OPEN list & has a lower f than successor, then skip this
                % successor.
                if pt(as.tie) < as.priority(X) % break tie with the sum of path costs
                    % add a new column to the open list
                    as.openlist = [as.openlist [X; pt]];
                end
            elseif as.t(X) == as.CLOSED
                % L14: If a node with same position as successor is in the
                % CLOSED list & has a lower f than successor, then skip this
                % successor.
                if pt(as.tie) < as.priority(X) % break tie with the sum of path costs
                    % add a new column to the open list
                    as.openlist = [as.openlist [X; pt]];
                end
            end

            % keep track of the max length of the openlist:
            if numcols(as.openlist) > as.openlist_maxlen
                as.openlist_maxlen = numcols(as.openlist);
            end

            as.t(X) = as.OPEN; % tag X as open
        end

        function DELETE(as, X)
            as.message('delete %d\n', X);
            i = find(as.openlist(1,:) == X);
            if length(i) ~= 1
                error('D*:DELETE: state %d doesnt exist', X);
            end
            as.openlist(:,i) = []; % remove the column
            as.t(X) = as.CLOSED;
        end

        % return the distance cost of moving from state X to state Y
        function cost = dc(as, X, Y)
            [r,c] = ind2sub(size(as.costmap), [X; Y]);
            dist = sqrt(sum(diff([r c]).^2));
            dcost = (as.costmap(X) + as.costmap(Y))/2;

            cost = dist * dcost;
        end

        % return the robot unit vector; direction of moving from state X to state Y
        function vector = vc(as, X, Y)
            [Xi,Xj] = ind2sub(size(as.occgrid),X);
            [Yi,Yj] = ind2sub(size(as.occgrid),Y);
            vector = [Yi-Xi;Yj-Xj];
            vector = vector/norm(vector);
%             slope = vector(2) / vector(1);
%             theta = dot([0,1],[vector])/(norm([0,1])*norm(vector));
        end

        % return index of neighbor states (max 8) as a row vector
        function Y = neighbors(as, X)
            dims = size(as.costmap);
            [r,c] = ind2sub(dims, X);

            % list of 8-way neighbors:
            Y = [r-1 r-1 r-1 r r  r+1 r+1 r+1; c-1 c c+1 c-1 c+1 c-1 c c+1];
            % only use neighbors w/in grid bounds...
            k = (min(Y)>0) & (Y(1,:)<=dims(1)) & (Y(2,:)<=dims(2));
            Y = Y(:,k);
            Y = sub2ind(dims, Y(1,:)', Y(2,:)')';
        end

    end % end of private methods
end


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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the target point positions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function targets = update_Target_Point_Positions(dt,current_time,targets)

%IDs = targets(:,1);                 % Stores Lag-Pt IDs in col vector
%xPts= targets(:,2);                 % Original x-Values of x-Target Pts.
%yPts= targets(:,3);                 % Original y-Values of y-Target Pts.
%kStiffs = targets(:,4);             % Stores Target Stiffnesses
%N_target = length(targets(:,1));    % Gives total number of target pts!

tR1 = 0.25;                          % Period A->B (and B->A) for RIGHT
tR2 = 0.25;                          % Period B->C (abd C->B) for RIGHT
periodR = (tR1+tR2);                % Time it takes to move to the right

off_Left = 0;%0.10*periodR;             % OFFSET FOR LEFT ARM
tL1 = 0.25;                          % Period A->B (and B->A) for LEFT
tL2 = 0.25;                          % Period B->C (abd C->B) for LEFT
periodL = (tL1+tL2);        % Time it takes to move to the LEFT

tR = rem(current_time,periodR);      % "Time" (moded out) according to right arm

tL = rem(current_time,periodL);      % "Time" (moded out) according to left arm

% Cubic Interpolation Information
p1 = 0.125;
p2 = 0.875;

% a COEFFICIENTS
a0 = 0;
a1 = 0;
a2 = 0;
a3 = 9.1429;

% b COEFFICIENTS
b0 = 0.0238;
b1 = -0.5714;
b2 = 4.5714;
b3 = -3.0476;

% c COEFFICIENTS
c0 = -8.1429;
c1 = 27.4286;
c2 = -27.4286;
c3 = 9.1429;

%
% JELLYFISH STATES:
%   XY(:,1) = 1st X-positions
%   XY(2:150,2) = 2nd X-positions (RIGHT)
%   XY(151:end,2) = 2nd X-positions (LEFT)
%
%   XY(:,3) = 1st Y-positions
%   XY(2:150,4) = 2nd Y-positions (RIGHT)
%   XY(151:end,4) = 2nd Y-positions (LEFT)
%
XY = read_In_Phase_Positions('XY_2Pos.txt');

% Coming from give_Me_Jelly:
NArm = 174;

% 1st and 2nd position of RIGHT arm
xR1 = XY(2:NArm+1,1); yR1 = XY(2:NArm+1,3);
xR2 = XY(2:NArm+1,2); yR2 = XY(2:NArm+1,4);

% 1st and 2nd position of LEFT arm
xL1 = XY(NArm+2:end,1); yL1 = XY(NArm+2:end,3);
xL2 = XY(NArm+2:end,2); yL2 = XY(NArm+2:end,4);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% START THE INTERPOLATING BETWEEN STATES!
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%
%
% RIGHT ARM!
%
%%%%%%%%%%%%%

if tR <= tR1+off_Left % STATE A -> STATE B

    % Scaling time for appropriate use in interp. function so tRTilde\in[0,1]
    tRTilde = ( tR / tR1 );

    % Evaluate Piecewise Cubic Interpolation Poly
    if tRTilde<=p1
        gFUNC = a0 + a1*tRTilde + a2*tRTilde^2 + a3*tRTilde^3;
    elseif tRTilde<=p2
        gFUNC = b0 + b1*tRTilde + b2*tRTilde^2 + b3*tRTilde^3;
    else
        gFUNC = c0 + c1*tRTilde + c2*tRTilde^2 + c3*tRTilde^3;
    end

    % MOVING X-POSITIONS
    targets(2:NArm+1,2) = xR1 + gFUNC*( xR2 - xR1 );

    % MOVING Y-POSITIONS
    targets(2:NArm+1,3) = yR1 + gFUNC*( yR2 - yR1 );

elseif tR <= (tR1+tR2) % STATE B -> A

    % Scaling time for appropriate use in interp. function so tRTilde\in[0,1]
    tRTilde = (tR-tR1)/( tR2 );

    % Evaluate Piecewise Cubic Interpolation Poly
    if tRTilde<=p1
        gFUNC = a0 + a1*tRTilde + a2*tRTilde^2 + a3*tRTilde^3;
    elseif tRTilde<=p2
        gFUNC = b0 + b1*tRTilde + b2*tRTilde^2 + b3*tRTilde^3;
    else
        gFUNC = c0 + c1*tRTilde + c2*tRTilde^2 + c3*tRTilde^3;
    end

    % MOVING X-POSITIONS
    targets(2:NArm+1,2) = xR2 + gFUNC*( xR1 - xR2 );

    % MOVING Y-POSITIONS
    targets(2:NArm+1,3) = yR2 + gFUNC*( yR1 - yR2 );

end

%%%%%%%%%%%%%
%
% LEFT ARM!
%
%%%%%%%%%%%%%

if current_time >= off_Left

    if tL <= tL1 % STATE A -> STATE B

        % Scaling time for appropriate use in interp. function so tRTilde\in[0,1]
        tLTilde = (tL/tL1);

        % Evaluate Piecewise Cubic Interpolation Poly
        if tLTilde<=p1
            gFUNC = a0 + a1*tLTilde + a2*tLTilde^2 + a3*tLTilde^3;
        elseif tRTilde<=p2
            gFUNC = b0 + b1*tLTilde + b2*tLTilde^2 + b3*tLTilde^3;
        else
            gFUNC = c0 + c1*tLTilde + c2*tLTilde^2 + c3*tLTilde^3;
        end

        % MOVING X-POSITIONS
        targets(NArm+2:end,2) = xL1 + gFUNC*( xL2 - xL1 );

        % MOVING Y-POSITIONS
        targets(NArm+2:end,3) = yL1 + gFUNC*( yL2 - yL1 );

    elseif tL <= (tL1+tL2) % STATE B -> A

        % Scaling time for appropriate use in interp. function so tRTilde\in[0,1]
        tLTilde = (tL-tL1)/(tL2);

        % Evaluate Piecewise Cubic Interpolation Poly
        if tLTilde<=p1
            gFUNC = a0 + a1*tLTilde + a2*tLTilde^2 + a3*tLTilde^3;
        elseif tLTilde<=p2
            gFUNC = b0 + b1*tLTilde + b2*tLTilde^2 + b3*tLTilde^3;
        else
            gFUNC = c0 + c1*tLTilde + c2*tLTilde^2 + c3*tLTilde^3;
        end

        % MOVING X-POSITIONS
        targets(NArm+2:end,2) = xL2 + gFUNC*( xL1 - xL2 );

        % MOVING Y-POSITIONS
        targets(NArm+2:end,3) = yL2 + gFUNC*( yL1 - yL2 );

    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Reads in the # of vertex pts and all the vertex pts from the
%           .vertex file.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function PTS = read_In_Phase_Positions(struct_name)

filename = struct_name;  %Name of file to read in
fileID = fopen(filename);

% Read in the file, use 'CollectOutput' to gather all similar data together
% and 'CommentStyle' to to end and be able to skip lines in file.
C = textscan(fileID,'%f %f %f %f','CollectOutput',1);

fclose(fileID);     %Close the data file.

vertices = C{1};    %Stores all read in data in vertices (N+1,2) array

PTS = vertices(1:end,1:4);


classdef prtRegressRvm < prtRegress
    % prtRegressRvm  Relevance vector machine regression object
    %
    %   REGRESS = prtRegressRvm returns a prtRegressRvm object
    %
    %    REGRESS = prtRegressRVM(PROPERTY1, VALUE1, ...) constructs a
    %    prtRegressRvm object REGRESS with properties as specified by
    %    PROPERTY/VALUE pairs.
    %
    %    A prtRegressRvm object inherits all properties from the prtRegress
    %    class. In addition, it has the following properties:
    %
    %   SetAccess = public:
    %    kernels            - A cell array of prtKernel objects specifying
    %                         the kernels to use
    %    verbosePlot        - Flag indicating whether or not to plot during
    %                         training
    %    verboseText        - Flag indicating whether or not to display
    %                         a message during training
    %
    %   SetAccess = private/protected:
    %    learningConverged  - Flag indicating if the training converged
    %    learningResults    - Struct with information about the convergence
    %    beta               - The weights on each of the kernel elements;
    %                         learned during training
    %    Sigma              - The learned covariance
    %    sparseBeta         - The weights on the retained kernel elements;
    %                         learned durning training
    %    sparseKernels      - The retained kernels
    %
    %   This code is based on:
    %       Michael E Tipping, Sparse bayesian learning and the relevance
    %   vector machine, The Journal of Machine Learning Research, Vol 1.
    %
    %   Also see http://en.wikipedia.org/wiki/Relevance_vector_machine
    %
    %   A prtRegressionRvm object inherits the PLOT method from the
    %   prtRegress object, and the TRAIN, RUN, CROSSVALIDATE and KFOLDS
    %   methods from the prtAction object.
    %
    %   Example:
    %
    %   dataSet = prtDataGenNoisySinc;           % Load a prtDataRegress
    %   dataSet.plot;                    % Display data
    %   reg = prtRegressRvm;             % Create a prtRegressRvm object
    %   reg = reg.train(dataSet);        % Train the prtRegressRvm object
    %   reg.plot();                      % Plot the resulting curve
    %   dataSetOut = reg.run(dataSet);   % Run the regressor on the data
    %   hold on;
    %   plot(dataSet.getX,dataSetOut.getX,'c.') % Plot, overlaying the
    %                                           % fitted points with the
    %                                           % curve and original data
    %   legend('Regression curve','Original Points','Kernel Locations Used',0)
    %
    %
    %   See also prtRegress, prtRegressGP, prtRegressLslr

    properties (SetAccess=private)
        name = 'Relevance Vector Machine'  % Relevance Vector Machine
        nameAbbreviation = 'RVM'           % RVM
    end

    properties
        kernels = prtKernelDc & prtKernelRbfNdimensionScale;

        verbosePlot = false;   % Whether or not to plot during training
        verboseText = false;   % Whether or not to plot during training
    end

    properties (Hidden = true)
        learningMaxIterations = 1000;  % Maximum number of iteratoins
        learningConvergedTolerance = 1e-6;
        learningRelevantTolerance = 1e-3;
    end
    properties (SetAccess = 'protected',GetAccess = 'public')
        learningConverged = [];% Whether or not the training converged

        beta = [];      % Estimated in training
        Sigma = [];     % Estimated in training
        sigma2 = [];    % Estimated in training

        sparseBeta = [];% Estimated in training
        sparseKernels = {};% Estimated in training
    end
    methods
         % Allow for string, value pairs
        function Obj = prtRegressRvm(varargin)
            Obj = prtUtilAssignStringValuePairs(Obj,varargin{:});
        end

        function Obj = set.kernels(Obj,val)
            if ~isa(val,'prtKernel')
                error('prt:prtRegressRvm:kernels','kernels must be a prtKernel');
            end

            Obj.kernels = val;
        end

        function Obj = set.verbosePlot(Obj,val)
            if ~prtUtilIsLogicalScalar(val)
                error('prt:prtRegressRvm:verbosePlot','verbosePlot must be a logical value or a positive integer');
            end
            Obj.verbosePlot = val;
        end

        function Obj = set.verboseText(Obj,val)
            if ~prtUtilIsLogicalScalar(val)
                error('prt:prtRegressRvm:verboseText','verboseText must be a logical value or a positive integer');
            end
            Obj.verboseText = val;
        end
    end

    methods (Access = protected, Hidden = true)

        function Obj = trainAction(Obj,DataSet)
            %Rvm = trainAction(Rvm,DataSet) (Private; see prtClass\train)
            %   Implements Jefferey's prior based training of a relevance
            %   vector machine.  The Rvm output from this function contains
            %   fields "sparseBeta" and "sparseKernels"

            warningState = warning;

            if DataSet.nTargetDimensions ~= 1
                error('prt:prtRegressRvm:tooManyTargets','prtRegressRvm can only operate on single target data.');
            end

            y = DataSet.getTargets(1:DataSet.nObservations,1);

            localKernels = Obj.kernels.train(DataSet);
            gram = localKernels.run_OutputDoubleArray(DataSet);
            nBasis = size(gram,2);

            if Obj.verboseText
                fprintf('RVM training with %d possible vectors.\n', nBasis);
            end

            Obj.beta = zeros(nBasis,1);

            relevantIndices = true(nBasis,1); % Everybody!

            alpha = ones(nBasis,1); % Initialize

            Obj.sigma2 = var(y); % A descent guess to start with

            for iteration = 1:Obj.learningMaxIterations
                % Given currenet relevant stuff find the weight mean and
                % covariance
                if ~any(relevantIndices)
                    break;
                end
                cPhi = gram(:,relevantIndices);
                A = diag(alpha(relevantIndices));

                sigma2Inv = (Obj.sigma2^-1);

                SigmaInv = A + sigma2Inv*(cPhi'*cPhi);
                Obj.Sigma = inv(SigmaInv);
                mu = sigma2Inv*(SigmaInv\(cPhi'*y));

                % Find the current prediction
                yHat = cPhi*mu;

                % Update A
                logAlphaOld = log(alpha(relevantIndices));

                cG = 1 - alpha(relevantIndices).*diag(Obj.Sigma);
                alpha(relevantIndices) = cG./(mu.^2);

                % Update sigma2
                Obj.sigma2 = norm(y-yHat)./(length(yHat) - sum(cG));

                Obj.beta = zeros(nBasis,1);
                Obj.beta(relevantIndices) = mu;

                %check tolerance for basis removal
                TOL = abs(log(alpha(relevantIndices))-logAlphaOld);
                TOL(isnan(TOL)) = 0; % inf-inf = nan

                if Obj.verboseText
                    fprintf('\t Iteration %d: %d RV''s, Convergence tolerance: %g \n',iteration, sum(relevantIndices), max(TOL));
                end

                if all(TOL < Obj.learningConvergedTolerance) && iteration > 1
                    Obj.learningConverged = true;

                    if Obj.verboseText
                        fprintf('Convergence reached. Exiting...\n\n');
                    end

                    break;
                end
                % We didn't break so we can contiue on
                % Select relevant stuff
                newRelevantIndices = alpha < 1./Obj.learningRelevantTolerance;

                if ~mod(iteration,Obj.verbosePlot)
                    if DataSet.nFeatures == 1
                        Obj.verboseIterationPlot(DataSet,relevantIndices);
                    elseif iteration == 1
                        warning('prt:prtRegressRvm','Learning iteration plot can only be produced for training Datasets with 1 feature.');
                    end
                end

                relevantIndices = newRelevantIndices;
            end

            if Obj.verboseText && iteration == Obj.learningMaxIterations
                fprintf('Exiting...Convergence not reached before the maximum allowed iterations was reached.\n\n');
            end

            % Make sparse represenation
            Obj.sparseBeta = Obj.beta(relevantIndices,1);
            Obj.sparseKernels = localKernels.retainKernelDimensions(relevantIndices);

            % Very bad training
            if isempty(Obj.sparseBeta)
                warning('prt:prtClassRvm:NoRelevantFeatures','No relevant features were found during training.');
            end

            % Reset warning
            warning(warningState);

        end

        function DataSetOut = runAction(Obj,DataSet)

            if isempty(Obj.sparseBeta)
                DataSetOut = DataSet.setObservations(zeros(DataSet.nObservations,Obj.dataSetSummary.nTargetDimensions));
                return
            end

            memChunkSize = 1000; % Should this be moved somewhere?
            n = DataSet.nObservations;

            DataSetOut = prtDataSetRegress(zeros(n,1));
            for i = 1:memChunkSize:n;
                cI = i:min(i+memChunkSize,n);
                cDataSet = prtDataSetRegress(DataSet.X(cI,:));
                gram = Obj.sparseKernels.run_OutputDoubleArray(cDataSet);

                DataSetOut = DataSetOut.setObservations(gram*Obj.sparseBeta, cI);
            end
        end
    end

    methods
        function varargout = plot(Obj)

            HandleStructure = plot@prtRegress(Obj);

            holdState = get(gca,'nextPlot');

            % Plot the kernels
            hold on
            %This only plots the x-coordinate... which is weird, but it's
            %all we can do right now b/c kernels don't know about
            %targets...
            Obj.sparseKernels.plot();
            set(gca, 'nextPlot', holdState);

            varargout = {};
            if nargout > 0
                varargout = {HandleStructure};
            end
        end
    end

    methods (Access=protected,Hidden=true)
        function Obj = verboseIterationPlot(Obj,DataSet,relevantIndices)
            DsSummary = DataSet.summarize;

            [linGrid, gridSize] = prtPlotUtilGenerateGrid(DsSummary.lowerBounds, DsSummary.upperBounds, Obj.plotOptions);

            trainedKernel = train(Obj.kernels, DataSet);
            trainedKernel = trainedKernel.retainKernelDimensions(relevantIndices);
            cPhi = trainedKernel.run_OutputDoubleArray(prtDataSetClass(linGrid));

            yHat = reshape(cPhi*Obj.beta(relevantIndices),gridSize);

            colors = Obj.plotOptions.colorsFunction(Obj.dataSetSummary.nTargetDimensions);
            lineWidth = Obj.plotOptions.lineWidth;
            plot(linGrid,yHat,'color',colors(1,:),'lineWidth',lineWidth);
            hold on
            plot(DataSet);
            plot(trainedKernel);
            hold off
            drawnow;
        end
    end
end


function g = matRad_objectiveGradient(optiProb,apertureInfoVec,dij,cst)
% matRad IPOPT callback: gradient function for direct aperture optimization
%
% call
%   g = matRad_objectiveGradient(optiProb,apertureInfoVec,dij,cst)
%
% input
%   optiProb:           option struct defining the type of optimization
%   apertureInfoVect:   aperture info in form of vector
%   dij:                matRad dij struct as generated by bixel-based dose calculation
%   cst:                matRad cst struct
%
% output
%   g: gradient
%
% References
%   [1] http://dx.doi.org/10.1118/1.4914863
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2015 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% update apertureInfo, bixel weight vector an mapping of leafes to bixels
if ~isequal(apertureInfoVec,optiProb.apertureInfo.apertureVector)
    optiProb.apertureInfo = optiProb.matRad_daoVec2ApertureInfo(optiProb.apertureInfo,apertureInfoVec);
end
apertureInfo = optiProb.apertureInfo;

% bixel based gradient calculation
bixelG = matRad_objectiveGradient@matRad_OptimizationProblem(optiProb,apertureInfo.bixelWeights,dij,cst);

% allocate gradient vector for aperture weights and leaf positions
g = NaN * ones(size(apertureInfoVec,1),1);

% 1. calculate aperatureGrad
% loop over all beams
offset = 0;
for i = 1:numel(apertureInfo.beam)

    % get used bixels in beam
    ix = ~isnan(apertureInfo.beam(i).bixelIndMap);

    % loop over all shapes and add up the gradients x openingFrac for this shape
    for j = 1:apertureInfo.beam(i).numOfShapes
        g(j+offset) = apertureInfo.beam(i).shape(j).shapeMap(ix)' ...
                        * bixelG(apertureInfo.beam(i).bixelIndMap(ix));
    end

    % increment offset
    offset = offset + apertureInfo.beam(i).numOfShapes;

end

% 2. find corresponding bixel to the leaf Positions and aperture
% weights to calculate the gradient
g(apertureInfo.totalNumOfShapes+1:end) = ...
        apertureInfoVec(apertureInfo.mappingMx(apertureInfo.totalNumOfShapes+1:end,2)) ...
     .* bixelG(apertureInfo.bixelIndices(apertureInfo.totalNumOfShapes+1:end)) / apertureInfo.bixelWidth;

% correct the sign for the left leaf positions
g(apertureInfo.totalNumOfShapes+1:apertureInfo.totalNumOfShapes+apertureInfo.totalNumOfLeafPairs) = ...
    -g(apertureInfo.totalNumOfShapes+1:apertureInfo.totalNumOfShapes+apertureInfo.totalNumOfLeafPairs);


function DCM = VBA_spm_dcm_test(DCM)
% tests whether DCM parameters are zero using Savage-Dickey ratios
% function DCM = spm_dcm_test(DCM)
% IN:
%   - DCM: the DCM structure
% OUT:
%   - DCM: the DCM structure, augmented with field .sdr, which contains the
%   Savage-Dickey ratios in the same format as DCM.Ep

n = size(DCM.Ep.A,1);
nu = size(DCM.Ep.C,2);

E = VBA_spm_vec(DCM.Ep);
V = DCM.Cp;
E0 = VBA_spm_vec(DCM.M.pE);
V0 = DCM.M.pC;

sdr.A = zeros(n,n);
for i=1:n
    for j=1:n
        % 0- identify param index in DCM.Cp
        tmp = DCM.Vp;
        tmp.A(i,j) = 0;
        ind = find(full(VBA_spm_vec(tmp))~=full(VBA_spm_vec(DCM.Vp)));
        % 1- define reduced model
        E0r = E0;
        E0r(ind) = 0;
        V0r = V0;
        V0r(ind,:) = 0;
        V0r(:,ind) = 0;
        % 2- call Savage-Dickey ratios
        if isempty(ind) % this param was fixed
            if isequal(E0,E0r)
                dF = Inf;
            else
                dF = -Inf;
            end
        else
            dF = VBA_spm_log_evidence(E,V,E0,V0,E0r,V0r);
        end
        % 3- store Bayes factor
        sdr.A(i,j) = 1./(1+exp(dF));
    end
end

sdr.B = zeros(n,n,nu);
for i=1:n
    for j=1:n
        for k=1:nu
            tmp = DCM.Vp;
            tmp.B(i,j,k) = 0;
            ind = find(full(VBA_spm_vec(tmp))~=full(VBA_spm_vec(DCM.Vp)));
            E0r = E0;
            E0r(ind) = 0;
            V0r = V0;
            V0r(ind,:) = 0;
            V0r(:,ind) = 0;
            if isempty(ind) % this param was fixed
                if isequal(E0,E0r)
                    dF = Inf;
                else
                    dF = -Inf;
                end
            else
                dF = VBA_spm_log_evidence(E,V,E0,V0,E0r,V0r);
            end
            sdr.B(i,j,k) = 1./(1+exp(dF));
        end
    end
end

sdr.C = zeros(n,nu);
for i=1:n
    for j=1:nu
        tmp = DCM.Vp;
        tmp.C(i,j) = 0;
        ind = find(full(VBA_spm_vec(tmp))~=full(VBA_spm_vec(DCM.Vp)));
        E0r = E0;
        E0r(ind) = 0;
        V0r = V0;
        V0r(ind,:) = 0;
        V0r(:,ind) = 0;
        if isempty(ind) % this param was fixed
            if isequal(E0,E0r)
                dF = Inf;
            else
                dF = -Inf;
            end
        else
            dF = VBA_spm_log_evidence(E,V,E0,V0,E0r,V0r);
        end
        sdr.C(i,j) = 1./(1+exp(dF));
    end
end

nD = size(DCM.Ep.D,3);
if nD > 0
    for i=1:n
        for j=1:n
            for k=1:n
                tmp = DCM.Vp;
                tmp.D(i,j,k) = 0;
                ind = find(full(VBA_spm_vec(tmp))~=full(VBA_spm_vec(DCM.Vp)));
                E0r = E0;
                E0r(ind) = 0;
                V0r = V0;
                V0r(ind,:) = 0;
                V0r(:,ind) = 0;
                if isempty(ind) % this param was fixed
                    if isequal(E0,E0r)
                        dF = Inf;
                    else
                        dF = -Inf;
                    end
                else
                    dF = VBA_spm_log_evidence(E,V,E0,V0,E0r,V0r);
                end
                sdr.D(i,j,k) = 1./(1+exp(dF));
            end
        end
    end
end

DCM.sdr = sdr;


function [g1, g2] = ggwhiteXggwhiteKernGradient(ggwhiteKern1, ggwhiteKern2, ...
    x, x2, covGrad)

% GGWHITEXGGWHITEKERNGRADIENT Compute a cross gradient between two GG WHITE kernels.
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel between two
%	gg white kernels for the multiple output kernel.
% RETURN g1 : gradient of the parameters of the first kernel, for ordering
%	   see ggwhiteKernExtractParam.
% RETURN g2 : gradient of the parameters of the second kernel, for ordering
%	   see ggwhiteKernExtractParam.
% ARG ggwhiteKern1 : the kernel structure associated with the first GG white
%	   kernel.
% ARG ggwhiteKern2 : the kernel structure associated with the second GG white
%	   kernel.
% ARG x : inputs for which kernel is to be computed.
% ARG covgrad : gradient of the objective function with respect to the
%	   elements of the cross kernel matrix.
%
% FORMAT
% DESC computes cross kernel terms between two GG WHITE kernels for the multiple
%	output kernel.
% RETURN g1 : gradient of the parameters of the first kernel, for ordering
%	   see ggwhiteKernExtractParam.
% RETURN g2 : gradient of the parameters of the second kernel, for ordering
%	   see ggwhiteKernExtractParam.
% ARG ggwhiteKern1 : the kernel structure associated with the first GG WHITE
%	   kernel.
% ARG ggwhiteKern2 : the kernel structure associated with the second GG WHITE
%	   kernel.
% ARG x : row inputs for which kernel is to be computed.
% ARG x2 : column inputs for which kernel is to be computed.
% ARG covgrad : gradient of the objective function with respect to the
%	   elements of the cross kernel matrix.
%
% SEEALSO : multiKernParamInit, multiKernCompute, ggwhiteKernParamInit,
% ggwhiteKernExtractParam
%
% COPYRIGHT : Mauricio A. Alvarez and Neil D. Lawrence, 2008
%
% MODIFICATIONS : Mauricio A. Alvarez, 2009.

% KERN

if nargin < 5
    covGrad = x2;
    x2 = x;
end

[K, Pinv, Lqrinv, Lsrinv, Kbase, factorNoise, factorVar1, factorVar2, dist] = ...
    ggwhiteXggwhiteKernCompute(ggwhiteKern1, ggwhiteKern2, x, x2);
P = 1./Pinv;

if ggwhiteKern1.isArd
    matGradLqr = zeros(ggwhiteKern1.inputDimension,1);
    matGradLsr = zeros(ggwhiteKern1.inputDimension,1);
    for i=1:ggwhiteKern1.inputDimension
        X = repmat(x(:,i),1, size(x2,1));
        X2 = repmat(x2(:,i)',size(x,1),1);
        X_X2 = (X - X2).*(X - X2);
        matGradLqr(i) = sum(sum(0.5*covGrad.*K.*...
            (-0.5*Lqrinv(i) + Lqrinv(i)*P(i)*Lqrinv(i) - Lqrinv(i)*P(i)*X_X2*P(i)*Lqrinv(i) )));
        matGradLsr(i) = sum(sum(0.5*covGrad.*K.*...
            (-0.5*Lsrinv(i) + Lsrinv(i)*P(i)*Lsrinv(i) - Lsrinv(i)*P(i)*X_X2*P(i)*Lsrinv(i) )));
    end
else
    matGradLqr = sum(sum(0.5*covGrad.*K.*( -0.5*ggwhiteKern1.inputDimension*Lqrinv...
        + ((P*Lqrinv)^2)*(ggwhiteKern1.inputDimension*Pinv - dist))    ));
    matGradLsr = sum(sum(0.5*covGrad.*K.*( -0.5*ggwhiteKern1.inputDimension*Lsrinv...
        + ((P*Lsrinv)^2)*(ggwhiteKern1.inputDimension*Pinv - dist))   ));
end
grad_sigma2Noise = factorNoise*sum(sum(covGrad.*Kbase));
grad_variance1 = factorVar1*sum(sum(covGrad.*Kbase));
grad_variance2 = factorVar2*sum(sum(covGrad.*Kbase));
% only pass the gradient with respect to the inverse width to one
% of the gradient vectors ... otherwise it is counted twice.
g1 = [matGradLqr(:)' grad_sigma2Noise grad_variance1];
%g1 = [matGradLqr(:)' 0 grad_variance1];
g2 = [matGradLsr(:)' 0 grad_variance2];


function [g, model] = vargplvmPointLogLikeGradient(model, vardistx, y)
% VARGPLVMPOINTLOGLIKEGRADIENT Log-likelihood gradient for of some points of the GP-LVM.
% FORMAT
% DESC returns the gradient of the log likelihood with respect to
% the latent positions, where the log likelihood is conditioned on
% the training set. The function works even when we are interested only in
% one point. See vargplvmPointLogLikelihood for more details.
% ARG model : the model for which the gradient computation is being
% done.
% ARG x : the latent positions where the gradient is being computed.
% ARG y : the positions in data space for which the computation is
% being done.
% RETURN g : the gradient of the log likelihood, conditioned on the
% training data, with respect to the latent position.
%
% SEEALSO : vargplvmPointLogLikelihood, vargplvmOptimisePoint, vagplvmSequenceLogLikeGradient
%
% COPYRIGHT : Michalis K. Titsias and Neil D. Lawrence, 2009, 2011
% COPYRIGHT : Andreas C. Damianou, 2011
% VARGPLVM

if isfield(model, 'dynamics') && ~isempty(model.dynamics)
    dynUsed = 1;
else
    dynUsed = 0;
end

indexMissing = find(isnan(y(1,:)));
indexPresent = setdiff(1:model.d, indexMissing);
y = y(:,indexPresent);

if dynUsed
    % use a special function
    [g, model] = dynPointLogLikeGradient(model, vardistx, y, indexPresent, indexMissing);
    % g = g(:)'; %%% RE-OPT-CODE-REM
    return;
end

% normalize y exactly as model.m is normalized
my = y - repmat(model.bias(indexPresent),size(y,1),1);
my = my./repmat(model.scale(indexPresent),size(y,1),1);

[gPsi0, gPsi1, gPsi2] = vargpCovGrads(model, vardistx, my, indexPresent);

[gKern1, gVarmeans1, gVarcovs1] = kernVardistPsi1Gradient(model.kern, vardistx, model.X_u, gPsi1');
[gKern2, gVarmeans2, gVarcovs2] = kernVardistPsi2Gradient(model.kern, vardistx, model.X_u, gPsi2);
[gKern0, gVarmeans0, gVarcovs0] = kernVardistPsi0Gradient(model.kern, vardistx, gPsi0);

gVarcovs0 = (gVarcovs0(:).*vardistx.covars(:))';
gVarcovs1 = (gVarcovs1(:).*vardistx.covars(:))';
gVarcovs2 = (gVarcovs2(:).*vardistx.covars(:))';

% KL divergence terms
gVarmeansKL = - vardistx.means(:)';
% !!! the covars are optimized in the log space
gVarcovsKL = 0.5 - 0.5*vardistx.covars(:)';

gVarmeans = gVarmeans0 + gVarmeans1 + gVarmeans2 + gVarmeansKL;
gVarcovs = gVarcovs0 + gVarcovs1 + gVarcovs2 + gVarcovsKL;

g = [gVarmeans gVarcovs];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% --------------------------------------------------------
function [gPsi0, gPsi1, gPsi2] = vargpCovGrads(model, vardistx, my, indexPresent)
%
%

d = prod(size(indexPresent));

% change the data (by including the new point and taking only the present indices)
model.m = model.m(:,indexPresent);
model.m = [my; model.m];

pointPsi1 = kernVardistPsi1Compute(model.kern, vardistx, model.X_u);
pointPsi2 = kernVardistPsi2Compute(model.kern, vardistx, model.X_u);

model.Psi1 = [pointPsi1; model.Psi1];

model.C = model.invLm * (model.Psi2 + pointPsi2) * model.invLmT;
model.TrC = sum(diag(model.C)); % Tr(C)
model.At = (1/model.beta) * eye(size(model.C,1)) + model.C;
model.Lat = jitChol(model.At)';
model.invLat = model.Lat\eye(size(model.Lat,1));
model.invLatT = model.invLat';
model.logDetAt = 2*(sum(log(diag(model.Lat)))); % log |At|
model.P1 = model.invLat * model.invLm; % M x M
model.P = model.P1 * (model.Psi1' * model.m);
model.TrPP = sum(sum(model.P .* model.P));
model.B = model.P1' * model.P;
P1TP1 = (model.P1' * model.P1);
Tb = (1/model.beta) * d * P1TP1;
Tb = Tb + (model.B * model.B');
model.T1 = d * model.invK_uu - Tb;

gPsi2 = (model.beta/2) * model.T1;

gPsi0 = -0.5 * model.beta * d;

gPsi1 = model.beta*(P1TP1*model.Psi1'*model.m*my');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% --------------------------------------------------------
function [gPointDyn, model] = dynPointLogLikeGradient(model, vardistx, y, indexPresent, indexMissing)
jitter = 1e-6;

%%% RE-OPT-CODE-NEW_
if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise
    % In this case the inducing points and theta_t are optimised and
    % since updateStats is not called, any computations which need X_u
    % or theta_t must be done here.
    model = vargplvmDynamicsUpdateStats(model);
    model.K_uu = kernCompute(model.kern, model.X_u);
    model.K_uu = model.K_uu ...
        + sparseDiag(repmat(jitter, size(model.K_uu, 1), 1));
    model.Lm = jitChol(model.K_uu)';
    model.invLm = model.Lm\eye(model.k);
    model.invLmT = model.invLm';
    model.invK_uu = model.invLmT * model.invLm;
end %%% _RE-OPT-CODE-NEW

%%%%%%%%% Only the missing parts and from the training data only
mOrig = model.m;
vardistDynTemp = model.dynamics.vardist;

N = model.N;
Nstar = size(y,1);
Kt = zeros(N+Nstar, N+Nstar);

%model.y = model.y(:,indexMissing);
model.m = model.m(:,indexMissing);
model.d = prod(size(indexMissing));
% Augment the time vector to include the timestamp of the new point
model.dynamics.t = [model.dynamics.t; model.dynamics.t_star];
% Augment the reparametrized variational parameters mubar and lambda
model.dynamics.vardist.means = [model.dynamics.vardist.means; vardistx.means];
model.dynamics.vardist.covars = [model.dynamics.vardist.covars; vardistx.covars];
model.dynamics.N = model.dynamics.N + Nstar;
model.vardist.numData = model.dynamics.N;
model.dynamics.vardist.numData = model.dynamics.N;
model.vardist.nParams = 2*prod(size(model.dynamics.vardist.means));
model.dynamics.vardist.nParams = 2*prod(size(model.dynamics.vardist.means));
%model.dynamics.seq = model.dynamics.N; %%%%%% Chec
% Faster computation of the Kt matrix
% (all this could have been avoided as Kt is constant... we need to do these
% precomputations outside of this function)
% construct Kt
Kt(1:N,1:N) = model.dynamics.Kt;
% new diagonal block
Kt(N+1:end, N+1:end) = kernCompute(model.dynamics.kern, model.dynamics.t_star);
% cross block
if ~isfield(model.dynamics, 'seq') || isempty(model.dynamics.seq)
    Kt(1:N, N+1:end) = kernCompute(model.dynamics.kern, model.dynamics.t(1:N), model.dynamics.t_star);
    Kt(N+1:end, 1:N) = Kt(1:N, N+1:end)';
end
model.dynamics.Kt = Kt;
model.dynamics.fixedKt = 1;
model = vargpTimeDynamicsUpdateStats(model);

vardist2 = model.vardist;
vardist2.means = model.vardist.means(1:end-Nstar,:);
vardist2.covars = model.vardist.covars(1:end-Nstar,:);
vardist2.nParams = 2*prod(size(vardist2.means));
vardist2.numData = size(vardist2.means,1);
model.Psi0 = kernVardistPsi0Compute(model.kern, vardist2);
model.Psi1 = kernVardistPsi1Compute(model.kern, vardist2, model.X_u);
model.Psi2 = kernVardistPsi2Compute(model.kern, vardist2, model.X_u);

model.C = model.invLm * model.Psi2 * model.invLmT;
%model.TrC = sum(diag(model.C)); % Tr(C)
model.At = (1/model.beta) * eye(size(model.C,1)) + model.C; % At = beta^{-1} I + C
model.Lat = jitChol(model.At)';
model.invLat = model.Lat\eye(size(model.Lat,1));
%model.invLatT = model.invLat';
%model.logDetAt = 2*(sum(log(diag(model.Lat)))); % log |At|
model.P1 = model.invLat * model.invLm; % M x M
model.P = model.P1 * (model.Psi1' * model.m);
%model.TrPP = sum(sum(model.P .* model.P));
model.B = model.P1' * model.P;
%model.invK_uu = model.invLmT * model.invLm;
Tb = (1/model.beta) * model.d * (model.P1' * model.P1);
Tb = Tb + (model.B * model.B');
model.T1 = model.d * model.invK_uu - Tb;

modelTmp = model;
modelTmp.vardist = vardist2;
modelTmp.dynamics.vardist = vardistDynTemp;
modelTmp.dynamics.Kt = modelTmp.dynamics.Kt(1:end-Nstar,1:end-Nstar);
modelTmp.dynamics.N = N;
modelTmp.dynamics.t = modelTmp.dynamics.t(1:end-Nstar);
modelTmp.dynamics.vardist.numData = modelTmp.dynamics.N; %%%% new...
modelTmp.vardist.numData = modelTmp.N;%%%% new...
%modelTmp.TrYY = sum(sum(model.m .* model.m));

% GRADIENT OF THE LL1 TERM mu,S from the training data

% To understand the following code it's helpful to see
% vargpTimeDynamicsPriorReparamGrads.m; LL1 term now is not computed
% together with KL so we cannot use that function here; however, it does
% contain theta_t so these computations must be done but only for the part
% corresponding to modelTmp. Thus, the following code actually is a copy of
% vargpTimeDynamicsPriorReparamGrads but is applied only on the appropriate
% part of Kt.
% i.e: gPointDyn1Tmp(:,1:model.dynamics.q) -> gVarmeansLik
%      gPointDyn1Tmp(:,model.dynamics.q+q) -> gVarcovsLik(:,q)

%gPointDyn1 = zeros(Nstar,modelTmp.dynamics.q*2);
gPointDyn1 = zeros(N+Nstar,modelTmp.dynamics.q*2);
if ~isempty(indexMissing)
    if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise %%% RE-OPT-CODE-NEW
        % gDynKern will be empty in that case (includeKL is 0) %%% RE-OPT-CODE-NEW
        [gPointDyn1Tmp, gDynKern, gInd1]= vargplvmLogLikeGradientsVar1(modelTmp, 0); %%% RE-OPT-CODE-NEW
        sumTrGradKL = 0; %%% RE-OPT-CODE-NEW
    else %%% RE-OPT-CODE-NEW
        gPointDyn1Tmp = vargplvmLogLikeGradientsVar1(modelTmp, 0);
    end %%% RE-OPT-CODE-NEW
    gPointDyn1Tmp = reshape(gPointDyn1Tmp, modelTmp.dynamics.vardist.numData, modelTmp.dynamics.q*2);

    % means (Note: Kt(1:N,:)' == Kt(:,1:N)  )
    gPointDyn1(:,1:modelTmp.dynamics.q) = model.dynamics.Kt(1:N,:)'*gPointDyn1Tmp(:,1:model.dynamics.q);

    % covars
    %gcovTmp = zeros(Nstar,modelTmp.dynamics.q);
    gcovTmp = zeros(N+Nstar,modelTmp.dynamics.q);
    sumTrGradKL1 = zeros(N+Nstar,N);
    sumTrGradKL2 = zeros(N+Nstar, N+Nstar);
    for q=1:model.dynamics.q
        LambdaH_q = model.dynamics.vardist.covars(:,q).^0.5;
        Bt_q = eye(model.dynamics.N) + LambdaH_q*LambdaH_q'.*model.dynamics.Kt;
        % Invert Bt_q
        Lbt_q = jitChol(Bt_q)';
        G1 = Lbt_q \ diag(LambdaH_q);
        G = G1*model.dynamics.Kt;
        % Find Sq
        Sq = model.dynamics.Kt - G'*G;
        Sq = - (Sq .* Sq);

        % only the cross matrix is needed as in barmu case
        %gcovTmp(:,q) = Sq(1:N,N+1:end)'*gPointDyn1Tmp(:,model.dynamics.q+q);

        % gPointDyn1Tmp contains [means covars]. model.dynamics.q+q gives
        % us only the covars.
        gcovTmp(:,q) = Sq(1:N,:)'*gPointDyn1Tmp(:,model.dynamics.q+q);

        %%% RE-OPT-CODE-NEW_
        if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise
            % Find the coefficient for the grad. wrt theta_t (params of Kt)
            G1T=G1';
            Bhat=G1T*G1;
            BhatKt=G1T*G;

            IBK = eye(model.dynamics.N) - BhatKt;

            diagVarcovs = repmat(gPointDyn1Tmp(:,model.dynamics.q+q)', model.dynamics.N,1);
            sumTrGradKL2 = sumTrGradKL2  +  (IBK(:,1:N).*diagVarcovs)*IBK(:,1:N)';

           %%%
         %   tmp = [gPointDyn1Tmp(:,model.dynamics.q+q)' zeros(1,Nstar)]; %tmp
         %   tmp2=IBK*diag(tmp)*IBK'; %tmp
         %   sumTrGradKL2 = sumTrGradKL2  +  tmp2; % tmp
           %%%

            sumTrGradKL1 = sumTrGradKL1 + model.dynamics.vardist.means(:,q)*gPointDyn1Tmp(:,q)';
            %if ~isempty(gInd)
            %    trGradKL = trGradKL + dynModel.vardist.means(:,q) * gInd(:,q)';
            %end
        end
        %%% _RE-OPT-CODE-NEW
    end

    gPointDyn1(:,(modelTmp.dynamics.q+1):(modelTmp.dynamics.q*2)) = gcovTmp;

    if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise                   %%% RE-OPT-CODE-NEW
        gDynKern1 = kernGradient(model.dynamics.kern, model.dynamics.t, model.dynamics.t(1:N),  sumTrGradKL1)  ; %%% RE-OPT-CODE-NEW
        gDynKern1 = gDynKern1 + kernGradient(model.dynamics.kern, model.dynamics.t, sumTrGradKL2); %%% RE-OPT-CODE-NEW
    end                                                                                %%% RE-OPT-CODE-NEW

end

% GRADIENT FOR LL2 plus KL TERMS
%
model.m = mOrig;
% normalize y exactly as model.m is normalized
my = y - repmat(model.bias(indexPresent), Nstar, 1);
my = my./repmat(model.scale(indexPresent), Nstar, 1);
model.m = model.m(:,indexPresent);
model.m = [model.m; my];
model.N = model.N + Nstar;
d = prod(size(indexPresent));
model.d = d;
model.dynamics.nParams = model.dynamics.nParams + 2*prod(size(vardistx.means));
model.nParams = model.nParams + 2*prod(size(vardistx.means));

vardist2 = model.vardist;
vardist2.means = model.vardist.means(end-Nstar+1:end,:);
vardist2.covars = model.vardist.covars(end-Nstar+1:end,:);
vardist2.nParams = 2*prod(size(vardist2.means));
vardist2.numData = size(vardist2.means,1);
pointPsi0 = kernVardistPsi0Compute(model.kern, vardist2);
pointPsi1 = kernVardistPsi1Compute(model.kern, vardist2, model.X_u);
pointPsi2 = kernVardistPsi2Compute(model.kern, vardist2, model.X_u);
model.Psi1 = [model.Psi1; pointPsi1];
model.Psi2 = model.Psi2 + pointPsi2;
model.Psi0 = model.Psi0 + pointPsi0;

model.C = model.invLm * model.Psi2 * model.invLmT;
%model.TrC = sum(diag(model.C)); % Tr(C)
model.At = (1/model.beta) * eye(size(model.C,1)) + model.C; % At = beta^{-1} I + C
model.Lat = jitChol(model.At)';
model.invLat = model.Lat\eye(size(model.Lat,1));
%model.invLatT = model.invLat';
%model.logDetAt = 2*(sum(log(diag(model.Lat)))); % log |At|
model.P1 = model.invLat * model.invLm; % M x M
model.P = model.P1 * (model.Psi1' * model.m);
%model.TrPP = sum(sum(model.P .* model.P));
model.B = model.P1' * model.P;
%model.invK_uu = model.invLmT * model.invLm;
Tb = (1/model.beta) * model.d * (model.P1' * model.P1);
Tb = Tb + (model.B * model.B');
model.T1 = model.d * model.invK_uu - Tb;

if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise %%% RE-OPT-CODE-NEW
    [gPointDyn2, gDynKern2, gInd2] = vargplvmLogLikeGradientsVar1(model,1); %%% RE-OPT-CODE-NEW
else %%% RE-OPT-CODE-NEW
    gPointDyn2 = vargplvmLogLikeGradientsVar1(model,1);
end %%% RE-OPT-CODE-NEW

gPointDyn2 = reshape(gPointDyn2, model.dynamics.vardist.numData, model.dynamics.q*2);
%gPointDyn2 = gPointDyn2(N+1:end,:);

gPointDyn = gPointDyn1 + gPointDyn2;

%%% RE-OPT-CODE-NEW_
gPointDyn = gPointDyn(:)';
if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise
    gDynKern = gDynKern1 + gDynKern2;
    gInd = gInd1 + gInd2;
    if strcmp(model.dynamics.kern.comp{1}.type,'rbf') || strcmp(model.dynamics.kern.comp{1}.type,'matern32')
        if ~isfield(model.dynamics, 'learnVariance') || ~model.dynamics.learnVariance
            gDynKern(2) = 0;
        end
    end
    gPointDyn = [gPointDyn 0*gDynKern gInd]; %%%%%%%%%% TEMP!!!!
end
%%% _RE-OPT-CODE-NEW

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% --------------------------------------------------------
%function gVar = vargplvmLogLikeGradientsVar1(model, includeKL) %%% RE-OPT-CODE-REM
function [gVar, gDynKern, gInd] = vargplvmLogLikeGradientsVar1(model, includeKL) %%% RE-OPT-CODE-NEW
% Like vargplvmLogLikeGradients but only for the variational parameters (ehm, not exactly). %%% RE-OPT-CODE-MOD

% Likelihood terms (coefficients)
gPsi1 = model.beta * model.m * model.B';
gPsi1 = gPsi1'; % because it is passed to "kernVardistPsi1Gradient" as gPsi1'...
gPsi2 = (model.beta/2) * model.T1;
gPsi0 = -0.5 * model.beta * model.d;

[gKern1, gVarmeans1, gVarcovs1, gInd1] = kernVardistPsi1Gradient(model.kern, model.vardist, model.X_u, gPsi1');
[gKern2, gVarmeans2, gVarcovs2, gInd2] = kernVardistPsi2Gradient(model.kern, model.vardist, model.X_u, gPsi2);
[gKern0, gVarmeans0, gVarcovs0] = kernVardistPsi0Gradient(model.kern, model.vardist, gPsi0);

gVarmeansLik = gVarmeans0 + gVarmeans1 + gVarmeans2;

gVarcovsLik = gVarcovs0 + gVarcovs1 + gVarcovs2;
if includeKL
    %[gVarmeans gVarcovs gDynKern] = modelPriorReparamGrads(model.dynamics, gVarmeansLik, gVarcovsLik);
    % TODO: This may have to be changed if we have model.fixInducing==1.
    [gVarmeans gVarcovs gDynKern] = modelPriorReparamGrads(model.dynamics, gVarmeansLik, gVarcovsLik,[]);
else
    dynModel = model.dynamics;
    gVarmeansLik = reshape(gVarmeansLik,dynModel.N,dynModel.q);
    gVarcovsLik = reshape(gVarcovsLik,dynModel.N,dynModel.q);
    gVarmeans = gVarmeansLik;
    gVarcovs = gVarcovsLik;

    gVarcovs = gVarcovs(:)';
    gVarmeans = gVarmeans(:)';
end

gVarcovs = (gVarcovs(:).*model.dynamics.vardist.covars(:))';
gVar = [gVarmeans gVarcovs];

%%% RE-OPT-CODE-NEW_
if isfield(model.dynamics, 'reoptimise') && model.dynamics.reoptimise
    % Compute Gradients with respect to X_u
    gK_uu = 0.5 * (model.T1 - (model.beta * model.d) * model.invLmT * model.C * model.invLm);
    gKX = kernGradX(model.kern, model.X_u, model.X_u);
    gKX = gKX*2;
    dgKX = kernDiagGradX(model.kern, model.X_u);
    for i = 1:model.k
        gKX(i, :, i) = dgKX(i, :);
    end
    gX_u = zeros(model.k, model.q);
    for i = 1:model.k
        for j = 1:model.q
            gX_u(i, j) = gKX(:, j, i)'*gK_uu(:, i);
        end
    end
    gInd = gInd1 + gInd2 + gX_u(:)';
    if ~includeKL
        gDynKern = [];
    end
end
%%% _RE-OPT-CODE-NEW


 function ob = Gblur2(mask, varargin)
%function ob = Gblur(mask, options)
%|
%| Construct Gblur object for image restoration.
%|
%| See Gblur_test.m for example usage.
%|
%| in
%|	mask	size(image)	logical array of object support.
%|
%| options
%|	'chat'		verbose printing of debug messages
%|	'psf'		point spread function (aka impulse response)
%|	'type'		type of blur:
%|				'conv,same'	usual case (default)
%|				'conv,per'	(periodic end conditions)
%|				'fft,same'	(periodic end conditions)
%|				todo: allow replicated end conditions!
%|				'imfilter,same'	(requires image toolbox)
%|				'imfilter,circ'	(requires image toolbox)
%|				'imfilter,mirror' (requires image toolbox)
%|					(do not use - adjoint fails)
%|				'sparse'	todo: return sparse matrix
%|	'imfilter_options'	options to 'imfilter' (if used)
%|
%| out
%|	ob [nd np]	np = sum(mask(:)), so it is already "masked"
%|			nd = prod(size(mask)) for 'conv,same' type
%|
%| Copyright 2005-4-22, Jeff Fessler, University of Michigan

if nargin == 1 && streq(mask, 'test'), Gblur_test, return, end
if nargin < 1, help(mfilename), error(mfilename), end

arg.mask = mask;

% option defaults
arg.chat = 0;
arg.psf = 1; % identity matrix
arg.type = 'conv,same';
arg.class = 'fatrix2';
arg.imfilter_options = {};

% options specified by name/value pairs
arg = vararg_pair(arg, varargin);

if ndims(arg.psf) ~= ndims(mask), error 'psf dim mismatch', end
if any(size(arg.psf) > size(mask)), error 'psf too large', end

if streq(arg.type, 'imfilter,circ') && exist('imfilter') ~= 2
	warn 'no imfilter (no image processing toolbox) so using fft'
	arg.type = 'fft,same';
end

if streq(arg.type, 'imfilter,mirror') && exist('imfilter') ~= 2
	fail 'no imfilter (no image processing toolbox)'
end

if streq(arg.type, 'imfilter,same') && exist('imfilter') ~= 2
	warn 'no imfilter (no image processing toolbox) so using slower convn'
	arg.type = 'conv,same';
end

arg.psf = Gblur_mask_psf_odd(arg.psf);

psf_flip = conj(flipdims2(arg.psf, 'odd', 1));
does_many = false;

switch arg.type

case 'conv,same'
	arg.odim = size(mask);
	arg.psf_flip = psf_flip;
	handle_forw = @(arg,x) convn(x, arg.psf, 'same');
	handle_back = @(arg,y) fatrix2_maskit(arg.mask, ...
			convn(y, arg.psf_flip, 'same'));
	does_many = true;

case 'conv,per'
	arg.odim = size(mask);
	arg.psf_flip = psf_flip;
	handle_forw = @(arg,x) ir_conv(x, arg.psf, 'per', true);
	handle_back = @(arg,y) fatrix2_maskit(arg.mask, ...
			ir_conv(y, arg.psf_flip, 'per', true));

case 'fft,same'
	arg.odim = size(mask);
	[arg, handle_forw, handle_back] = Gblur_setup_fft_same(arg);

case 'imfilter,circ'
	arg.odim = size(mask);
	arg.psf_flip = psf_flip;
	handle_forw = @(arg,x) imfilter(x, arg.psf, 'conv', 'same', ...
		'circular', arg.imfilter_options{:});
	handle_back = @(arg,y) fatrix2_maskit(arg.mask, ...
			imfilter(y, arg.psf_flip, 'conv', 'same', ...
				'circular', arg.imfilter_options{:}));

case 'imfilter,mirror'
	arg.odim = size(mask);
	arg.psf_flip = psf_flip;
	handle_forw = @(arg,x) imfilter(x, arg.psf, 'conv', 'same', ...
		'symmetric', arg.imfilter_options{:});
	handle_back = @(arg,y) fatrix2_maskit(arg.mask, ...
			imfilter(y, arg.psf_flip, 'conv', 'same', ...
				'symmetric', arg.imfilter_options{:}));

case 'imfilter,same'
	arg.odim = size(mask);
	arg.psf_flip = psf_flip;
	handle_forw = @(arg,x) imfilter(x, arg.psf, 'conv', 'same', ...
		arg.imfilter_options{:});
	handle_back = @(arg,y) fatrix2_maskit(arg.mask, ...
			imfilter(y, arg.psf_flip, 'conv', 'same', ...
				arg.imfilter_options{:}));

case 'sparse'
	ob = Gblur_sparse(arg.psf, arg.idim);
	ob = ob(:,arg.mask(:));
	return

otherwise
	error 'unknown blur type'
end

switch arg.class

case 'Fatrix'

	arg.nd = prod(arg.odim);
	arg.np = sum(mask(:));
	dim = [arg.nd arg.np]; % trick: make it masked by default!

	ob = Fatrix(dim, arg, 'caller', 'Gblur', ...
		'abs', @Gblur_abs, ...
		'forw', @Gblur_forw_Fatrix, 'back', @Gblur_back_Fatrix);

case 'fatrix2'

	ob = fatrix2('idim', arg.odim, 'arg', arg, 'does_many', does_many, ...
		'abs', @Gblur_abs, 'forw', handle_forw, 'back', handle_back);

otherwise
	fail('bug')
end

% Gblur_mask_psf_odd(psf)
% having an odd-sized psf facilites adjoint
function psf = Gblur_mask_psf_odd(psf)
nd = ndims(psf);
for id=1:nd
	sz = size(psf, id);
	if ~mod(sz, 2) % even
		warn('size(psf, %d) = %d is even; appending 0', id, sz)
		tmp = size(psf);
		tmp(id) = 1;
		z = zeros(tmp);
		%psf = cat(id, psf, z);
		psf = cat(id, z, psf);
	end
end

% Gblur_sparse()
% a_ij = b[ n(i) - n(j), m(i) - m(j) ]
% n(i) = (i-1) mod N
% m(i) = floor((i-1) / N)
function sp = Gblur_sparse(psf, idim)
if numel(idim) ~= 2
	fail 'not done'
end
fail 'todo: not done'
%sp = sparse(i, j, s, length(ii), size(ob.arg.G, 2));

% Gblur_abs(): |A| for abs(A)
function ob = Gblur_abs(ob)
ob.arg.psf = abs(ob.arg.psf);
if isfield(ob.arg, 'psf_flip')
	ob.arg.psf_flip = abs(ob.arg.psf_flip);
end
if isfield(ob.arg, 'psf_fft') % fixed 2012-04-08
	ob.arg.psf_fft = Gblur_setup_fft_same_fft(ob.arg.psf, ob.mask);
end

% Gblur_setup_fft_same()
function [arg, handle_forw, handle_back] = Gblur_setup_fft_same(arg)

arg.psf_fft = Gblur_setup_fft_same_fft(arg.psf, arg.mask);

handle_forw = @(arg,x) ifftn(fftn(x) .* arg.psf_fft);
handle_back = @(arg,y) fatrix2_maskit(arg.mask, ...
			ifftn(fftn(y) .* conj(arg.psf_fft)));

% Gblur_setup_fft_same_fft()
function psf_fft = Gblur_setup_fft_same_fft(psf, mask)

% % put psf in center of array
% tmp = zeros(size(mask));
% switch ndims(psf)
% case 2
% 	[n1 n2] = size(mask);
% 	[p1 p2] = size(psf);
% %	h1 = (size(psf,1)-1)/2;
% %	h2 = (size(psf,2)-1)/2;
% %	i1 = floor(n1/2) + 1 + [-h1:h1]
% %	i2 = floor(n2/2) + 1 + [-h2:h2]
% 	i1 = floor(n1/2) + 1 + (1:p1) - floor((p1+1)/2);
% 	i2 = floor(n2/2) + 1 + (1:p2) - floor((p2+1)/2);
% 	tmp(i1,i2) = psf;
% case 3
% 	if any(~rem(size(psf),2)), error 'psf size must be odd', end
% 	h1 = (size(psf,1)-1)/2;
% 	h2 = (size(psf,2)-1)/2;
% 	h3 = (size(psf,3)-1)/2;
% 	i1 = floor(size(mask,1)/2) + 1 + [-h1:h1];
% 	i2 = floor(size(mask,2)/2) + 1 + [-h2:h2];
% 	i3 = floor(size(mask,3)/2) + 1 + [-h3:h3];
% 	tmp(i1,i2,i3) = psf;
% otherwise
% 	error 'only 2d & 3d done'
% end
% psf_fft = fftn(fftshift(tmp));

% pad psf with 0, and make size(psf)==size(mask)
sz_mask = size(mask);
sz_psf = ones(1,ndims(mask));
for ii=1:length(sz_psf), sz_psf(ii) = size(psf,ii); end
if any(sz_mask>sz_psf)
    psf = padarray(psf, sz_mask-sz_psf, 'post');
end
center = ceil(sz_psf/2-1/2) + 1; % or center = floor(sz_psf/2) + 1;
psf_fft = fftn( circshift(psf, 1-center) ); % by yusheng

% Gblur_forw_Fatrix(): y = A * x
function y = Gblur_forw_Fatrix(arg, x)

[x ei] = embed_in(x, arg.mask, arg.np);

switch arg.type
case 'conv,same'
	y = convn(x, arg.psf, 'same');
case 'conv,per'
	y = ir_conv(x, arg.psf, 'per', true);
case 'fft,same'
	if ndims(x) > ndims(arg.mask);
		y = fft_conv_multi(x, arg.psf_fft);
	else
		y = ifftn(fftn(x) .* arg.psf_fft);
	end
case 'imfilter,circ'
	y = imfilter(x, arg.psf, 'conv', 'same', ...
		'circular', arg.imfilter_options{:});
case 'imfilter,same'
	y = imfilter(x, arg.psf, 'conv', 'same', arg.imfilter_options{:});
otherwise
	error 'bug'
end

y = ei.shape(y);

% Gblur_back_Fatrix(): x = A' * y
function x = Gblur_back_Fatrix(arg, y)

[y eo] = embed_out(y, arg.odim);

switch arg.type
case 'conv,same'
	x = convn(y, arg.psf_flip, 'same');
case 'conv,per'
	x = ir_conv(y, arg.psf_flip, 'per', true);
case 'fft,same'
	if ndims(y) > ndims(arg.mask);
		x = fft_conv_multi(y, conj(arg.psf_fft));
	else
		x = ifftn(fftn(y) .* conj(arg.psf_fft));
	end
case 'imfilter,circ'
	x = imfilter(y, arg.psf_flip, 'conv', 'same', ...
		'circular', arg.imfilter_options{:});
case 'imfilter,same'
	x = imfilter(y, arg.psf_flip, 'conv', 'same', arg.imfilter_options{:});
otherwise
	error 'bug'
end

x = eo.shape(x, arg.mask, arg.np);

% fft_conv_multi()
function y = fft_conv_multi(x, H)
dim = size(x);
y = zeros(size(x));
y = [];
for ii=1:dim(end)
	tmp = ifftn(fftn(stackpick(x, ii)) .* H);
	y = cat(length(dim), y, tmp); % slow: keeps enlarging y.
end


% LOTKA-VOLTERRA system

clear all, close all, clc
figpath = '../FIGURES/'; mkdir(figpath)
datapath = '../DATA/';
addpath('../utils');

%% Load Models
ModelCollection = {'DMDc','SINDYc', 'NARX'};
Nmodels = length(ModelCollection);
InputSignalType = 'sphs'; % prbs; chirp; noise; sine2; sphs; mixed
TrainAlg = 'trainbr'; %trainlm,trainbr
dep_trainlength = 1;
dep_noise = 0;

% Ntrain_vec = [5:15,20:5:95,100:100:1000];%,1500:500:3000];
% eta_vec = 0.0;
% Nr = 1;

Ntrain_vec = [5:15,20:5:95,100:100:1000,1250,1500,2000,3000];%,1500:500:3000];
eta_vec = 0.05;
Nr = 1;

N_LENGTHS = length(Ntrain_vec);

%% Load data
ResultsALL(N_LENGTHS,1:Nmodels) = struct('x', [], 'u', [], 't', [], 'xref', [], 'J', [], 'eval_time', []);

ModelName = 'DMDc';
datapath1 = [datapath,'EX_LOTKA_Dependencies/',ModelName,'/'];
load(fullfile(datapath1,['EX_LOTKA_MPC_',ModelName,'_',InputSignalType,'_TrainLength','.mat']))
ResultsALL(:,1) = Results;

ModelName = 'SINDYc';
datapath1 = [datapath,'EX_LOTKA_Dependencies/',ModelName,'/'];
load(fullfile(datapath1,['EX_LOTKA_MPC_',ModelName,'_',InputSignalType,'_TrainLength','.mat']))
ResultsALL(:,2) = Results;

ModelName = 'NARX';
datapath1 = [datapath,'EX_LOTKA_Dependencies/',ModelName,'/',TrainAlg,'/'];
load(fullfile(datapath1,['EX_LOTKA_MPC_',ModelName,'_',InputSignalType,'_TrainLength','.mat']))
ResultsALL(:,3) = Results;

%% Correct cost
Q = [1,1]; R = 0.5; Ru = 0.5;
for iM = 1:Nmodels
    for iL = 1:N_LENGTHS
        ResultsALL(iL,iM).J = evalObjectiveFCN(ResultsALL(iL,iM).u,ResultsALL(iL,iM).x,ResultsALL(iL,iM).xref,diag(Q),R,Ru);
    end
end

%% Show trajectories
cb_hrzntl = 1;

cmap = jet(N_LENGTHS);
for jModel = 1:Nmodels
    figure, hold on, box on
    for iM = 1:N_LENGTHS
        plot(ResultsALL(iM,jModel).x(1,:),ResultsALL(iM,jModel).x(2,:),'-','Color',cmap(iM,:),'LineWidth',1)
    end
    plot(ResultsALL(iM,jModel).xref(1,1),ResultsALL(iM,jModel).xref(2,1),'ok','MarkerFaceColor','k')
    %     axis equal
    ylim([-20 100]), xlim([0 200])
    colormap(cmap),
    ch = colorbar;
    ch.Limits = [0 1];
    ch.Ticks = [0,0.25,0.5,0.75,1];
    ch.TickLabels = num2str( Ntrain_vec( [1, (N_LENGTHS-1)/4, (N_LENGTHS-1)/2+1, 3*(N_LENGTHS-1)/4+1, 4*(N_LENGTHS-1)/4+1] )' );
    if cb_hrzntl == 0
        ch.Position = [ch.Position(1),ch.Position(2)+0.14,ch.Position(3)-0.01,ch.Position(4)-0.1];
        set(gca,'Position',[0.08 0.2753 0.775 0.6497])
    elseif cb_hrzntl == 1
        ch.Location = 'southoutside';
        ch.Position = [0.2,0.1,0.75,ch.Position(4)];
        set(gca,'Position',[0.2 0.4 0.75 0.55])
    end
    xlabel('x1');
    ylabel('x2')
    set(gca,'LineWidth',1, 'FontSize',14)
    set(gcf,'Position',[100 100 300 200])
    set(gcf,'PaperPositionMode','auto')
    print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_State_',ModelCollection{jModel},'.eps']);
end

%% Show cost trajectory
cb_hrzntl = 1;

cmap = jet(N_LENGTHS);
for jModel = 1:Nmodels
    figure, hold on, box on
    for iM = 1:N_LENGTHS
        plot(ResultsALL(end,jModel).t,cumsum(ResultsALL(iM,jModel).J),'-','Color',cmap(iM,:),'LineWidth',1)
    end
    plot(ResultsALL(end,2).t,cumsum(ResultsALL(end,2).J),'--','Color','k','LineWidth',2)
    ylim([-10 10^6]), xlim([200 max(ResultsALL(end,jModel).t)])
    colormap(cmap),
    ch = colorbar;
    ch.Limits = [0 1];
    ch.Ticks = [0,0.25,0.5,0.75,1];
    ch.TickLabels = num2str( Ntrain_vec( [1, (N_LENGTHS-1)/4, (N_LENGTHS-1)/2+1, 3*(N_LENGTHS-1)/4+1, 4*(N_LENGTHS-1)/4+1] )' );
    if cb_hrzntl == 0
        ch.Position = [ch.Position(1),ch.Position(2)+0.14,ch.Position(3)-0.01,ch.Position(4)-0.1];
        set(gca,'Position',[0.08 0.2753 0.775 0.6497])
    elseif cb_hrzntl == 1
        ch.Location = 'southoutside';
        ch.Position = [0.2,0.1,0.75,ch.Position(4)];
        set(gca,'Position',[0.2 0.4 0.75 0.55])
    end
    ylim([10^3 10^6])
    set(gca,'yscale','log')
    xlabel('Time');
    ylabel('Cost')
    set(gca,'LineWidth',1, 'FontSize',14)
    set(gcf,'Position',[100 100 300 200])
    set(gcf,'PaperPositionMode','auto')
    print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_Cost_',ModelCollection{jModel},'.eps']);
end

%% Show execution time
clear ph
ccolors = [1 0 0; 0 1 0; 0.7,0.7,1];
lstyles = {'-', '--', '-.'};
symbols = {'o', 'd', 's'};

Tx = zeros(length(N_LENGTHS),Nmodels);
for iM = 1:Nmodels
    for iL = 1:N_LENGTHS
        Tx(iL,iM) = ResultsALL(iL,iM).eval_time;
    end
end
figure, hold on, box on
for iM = 1:Nmodels
    plot(Ntrain_vec,Tx(:,iM)./60,symbols{iM},'Color',ccolors(iM,:),'MarkerFaceColor',ccolors(iM,:))
end
xlim([4 1001])
ylim([0.5*10^-5 10^2])
set(gca,'ytick',[10.^([-5:2:2])])
set(gca,'XScale','log','YScale','log')
xlabel('Length of Training Data');
ylabel('Time [m]')
legend(ModelCollection,'Location','SouthEast')
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto'),
print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_ExecTime','.eps']);
legend('off')
print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_ExecTime','_noleg.eps']);

%% Add penalization
Q = 1*eye(2); %20
for iM = 1:Nmodels
    for iL = 1:N_LENGTHS
       if ResultsALL(iL,iM).t(end) == 0
           % Flag not converged / ended early
           ResultsALL(iL,iM).J(end) = 10^6;
       else
           % Flag if far from end state at end of time
           %ResultsALL(iL,iM).J(end) = ResultsALL(iL,iM).J(end) + abs(ResultsALL(iL,iM).xref(:,end) - ResultsALL(iL,iM).x(:,end))'*Q*abs(ResultsALL(iL,iM).xref(:,end) - ResultsALL(iL,iM).x(:,end));
       end
    end
end

%% Show Performance over training lengths
clear ph
ccolors = [1 0 0; 0 1 0; 0.7,0.7,1];
lstyles = {'-', '--', '-.'};
symbols = {'o', 'd', 's'};

Nstart = 20;
xx = Ntrain_vec(Nstart):2:1000;
yModel = zeros(length(xx),Nmodels);
JendAll = zeros(length(N_LENGTHS),Nmodels);
Jend = cell(Nmodels,1);
xJend = cell(Nmodels,1);

for iM = 1:Nmodels
%     Jend{iM} = zeros(length(N_LENGTHS),Nmodels);
    count = 0;
    for iL = 1:N_LENGTHS,
        tmp = cumsum(ResultsALL(iL,iM).J);
        if tmp(end)<10^10
            count = count + 1;
            xJend{iM}(count) = Ntrain_vec(iL);
            Jend{iM}(count) = tmp(end);
        end
        JendAll(iL,iM) = tmp(end);
    end
    yModel(:,iM) = spline(xJend{iM}(Nstart:end),Jend{iM}(Nstart:end),xx);
end

clear ph
figure, hold on, box on

if eta_vec == 0.05
    fillh1 = fill([4.5*10^0 2.85*10^3 2.85*10^3 4.5*10^0], [5.5*10^3 5.5*10^3 1.5*10^4 1.5*10^4],0.9*ones(1,3)); % /w noise, not performing well above
elseif  eta_vec == 0.0
    fillh1 = fill([4.5*10^0 2.85*10^3 2.85*10^3 4.5*10^0], [5.5*10^3 5.5*10^3 2.5*10^4 2.5*10^4],0.9*ones(1,3));
end
fillh1.EdgeColor = 0.9*ones(1,3); fillh1.FaceAlpha = 0.5;

[~,idx_best] = min(JendAll(:,2));
plot(Ntrain_vec,JendAll(idx_best,2)*ones(size(Ntrain_vec)),'-','Color',ccolors(2,:),'LineWidth',1)

for iM = 1:Nmodels
%     if iM <= Nmodels
%         plot(xx,yModel(:,iM),'-','Color',ccolors(iM,:), 'LineWidth',1, 'LineStyle',lstyles{iM}), hold on,
%     end
    ph(iM) = plot(Ntrain_vec,JendAll(:,iM),symbols{iM},'Color',ccolors(iM,:),'MarkerFaceColor',ccolors(iM,:),'MarkerSize',4);
end
if eta_vec == 0
    iM = 1; plot([6.5,6.5],[5*10^3 10^7],'Color',ccolors(iM,:),'LineStyle',lstyles{iM});
    iM = 2; plot([13.5,13.5],[5*10^3 10^7],'Color',ccolors(iM,:),'LineStyle',lstyles{iM});
    iM = 3; plot([100,100],[5*10^3 10^7],'Color',ccolors(iM,:),'LineStyle',lstyles{iM});
elseif eta_vec == 0.05
    iM = 1; plot([7.5,7.5],[5*10^3 10^7],'Color',ccolors(iM,:),'LineStyle',lstyles{iM});
    iM = 2; plot([70,70],[5*10^3 10^7],'Color',ccolors(iM,:),'LineStyle',lstyles{iM});
    iM = 3; plot([130,130],[5*10^3 10^7],'Color',ccolors(iM,:),'LineStyle',lstyles{iM});
end
xlim([4 3001])
% ylim([-100 5000])
% ylim([5*10^2 1.5*10^4])
% ylim([1*10^2 10^5])
ylim([5*10^3 10^7])
% set(gca,'YScale','log')
set(gca,'XScale','log','YScale','log', 'ytick', [10^3,10^4,10^5,10^6,10^7], 'xtick', [10^0,10^1,10^2,10^3]) %[10^3,5*10^3,10^4]
xlabel('Length of Training Data');
ylabel('Cost')
lh = legend(ph,ModelCollection,'Location','NorthEast');
lh.Position = [0.71 0.73 lh.Position(3)-0.05 lh.Position(4)];
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto'),
print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_Cost','.eps']);

lh.Location = 'NorthOutside';
lh.Orientation = 'horizontal';
lh.Box = 'off';
print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_Cost','_legout.eps']);
legend('off')
print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_Cost','_noleg.eps']);

%% Show best case for all models
% eta = 0.05: 8    38    21
% -->  Ntrain = 12        1250          65
% --> Jend = 9.0603e+03,  8.6549e+03, 2.0592e+04
% Jend for best eta==0: 8(12), 38(1250), 21(65)
% SINDY converged Ntrain = 200
% eta = 0.0: 12    25    37
% -->  Ntrain = 20          85        1000
% --> Jend = 9.0601e+03,  8.6595e+03, 8.9236e+03
% SINDY converged Ntrain = 200
BestModelIDX = zeros(1,Nmodels);
Jend = zeros(N_LENGTHS,Nmodels);
for iM = 1:Nmodels
   for iL = 1:N_LENGTHS
        tmp = cumsum(ResultsALL(iL,iM).J);
        Jend(iL,iM) = tmp(end);
   end
   [~,BestModelIDX(iM)] = min(Jend(:,iM));
end

% [val,idx1] = min(abs(Jend(:,1)-9.0601e+03))
% Ntrain_vec(idx1)
% [val,idx2] = min(abs(Jend(:,2)-8.6595e+03))
% Ntrain_vec(idx2)
% [val,idx3] = min(abs(Jend(:,3)-8.9236e+03))
% Ntrain_vec(idx3)

ccolors = [1 0 0; 0 1 0; 0.7,0.7,1];
lstyles = {'-', '--', '-.'};

figure,
hold on, box on
plot(ResultsALL(BestModelIDX(1),1).t,ResultsALL(1,1).xref(1,:),'-k')
for iM = 1:Nmodels
    plot(ResultsALL(BestModelIDX(iM),iM).t,ResultsALL(BestModelIDX(iM),iM).x(1,:),'Color',ccolors(iM,:),'LineStyle',lstyles{iM},'LineWidth',2)
end
ylim([65 110])
xlabel('Time'), ylabel('Prey')
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 100])
set(gcf,'PaperPositionMode','auto'),
print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_BestModels_x1','.eps']);

figure,
hold on, box on
plot(ResultsALL(BestModelIDX(1),1).t,ResultsALL(1,1).xref(2,:),'-k')
for iM = 1:Nmodels
    plot(ResultsALL(BestModelIDX(iM),iM).t,ResultsALL(BestModelIDX(iM),iM).x(2,:),'Color',ccolors(iM,:),'LineStyle',lstyles{iM},'LineWidth',2)
end
ylim([7 27])
xlabel('Time'), ylabel('Predator')
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 100])
set(gcf,'PaperPositionMode','auto'),
print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_BestModels_x2','.eps']);

figure,
hold on, box on
for iM = 1:Nmodels
    plot(ResultsALL(BestModelIDX(iM),iM).t,cumsum(ResultsALL(BestModelIDX(iM),iM).J),'Color',ccolors(iM,:),'LineStyle',lstyles{iM},'LineWidth',2)
end
if eta_vec == 0
    ylim([7*10^3 9.5*10^3])
elseif eta_vec == 0.05
    ylim([7*10^3 1*10^4])
end
set(gca,'yscale','linear','ytick',[7*10^3,8*10^3,9*10^3],'yticklabels',{'70','80','90'})
xlabel('Time'), ylabel('Cost')
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 100])
set(gcf,'PaperPositionMode','auto'),
print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_BestModels_J','.eps']);

%% Show State Performance over training lengths
Q = 1*eye(2); %20
Jstate = zeros(length(ResultsALL(iL,iM).J),N_LENGTHS,Nmodels);
for iM = 1:Nmodels
    for iL = 1:N_LENGTHS
        for it = 1:length(ResultsALL(iL,iM).xref(1,:))
            Jstate(it,iL,iM) = abs(ResultsALL(iL,iM).xref(:,it) - ResultsALL(iL,iM).x(:,it))'*Q*abs(ResultsALL(iL,iM).xref(:,it) - ResultsALL(iL,iM).x(:,it));
        end
    end
end

clear ph
ccolors = [1 0 0; 0 1 0; 0.7,0.7,1];
lstyles = {'-', '--', '-.'};
symbols = {'o', 'd', 's'};

Nstart = 20;
xx = Ntrain_vec(Nstart):2:1000;
yModel = zeros(length(xx),Nmodels);
JendAll = zeros(length(N_LENGTHS),Nmodels);
Jend = cell(Nmodels,1);
xJend = cell(Nmodels,1);

for iM = 1:Nmodels
%     Jend{iM} = zeros(length(N_LENGTHS),Nmodels);
    count = 0;
    for iL = 1:N_LENGTHS,
        tmp = cumsum(Jstate(:,iL,iM));
        if tmp(end)<10^10
            count = count + 1;
            xJend{iM}(count) = Ntrain_vec(iL);
            Jend{iM}(count) = tmp(end);
        end
        JendAll(iL,iM) = tmp(end);
    end
    yModel(:,iM) = spline(xJend{iM}(Nstart:end),Jend{iM}(Nstart:end),xx);
end

figure, hold on, box on
for iM = 1:Nmodels
%     if iM < Nmodels
%         plot(xx,yModel(:,iM),'-','Color',ccolors(iM,:), 'LineWidth',1, 'LineStyle',lstyles{iM}), hold on,
%     end
    plot(Ntrain_vec,JendAll(:,iM),symbols{iM},'Color',ccolors(iM,:),'MarkerFaceColor',ccolors(iM,:),'MarkerSize',4)
end
xlim([4 1001])
ylim([5*10^3 5*10^6])
% set(gca,'YScale','log')
set(gca,'XScale','log','YScale','log', 'ytick', [10^4,10^5,10^6], 'xtick', [10^0,10^1,10^2,10^3])
xlabel('Length of Training Data');
ylabel('Cost')
lh = legend(ModelCollection,'Location','NorthEast');
lh.Position = [0.71 0.73 lh.Position(3)-0.05 lh.Position(4)];
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto'),
print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_CostState','.eps']);

lh.Location = 'NorthOutside';
lh.Orientation = 'horizontal';
lh.Box = 'off';
print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_CostState','_legout.eps']);
legend('off')
print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_CostState','_noleg.eps']);

return
%% Legend // Dummy plot
figure, hold on, box on
for iM = 1:Nmodels
    plot(Ntrain_vec,ones(size(Ntrain_vec)),'-','Marker',symbols{iM},'Color',ccolors(iM,:),'MarkerFaceColor',ccolors(iM,:))
end

% xlabel('Length of Training Data');
% ylabel('Cost')
axis off
lh = legend(ModelCollection,'Location','NorthEast');
lh.Location = 'NorthOutside';
lh.Orientation = 'horizontal';
lh.Box = 'off';
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 100])
set(gcf,'PaperPositionMode','auto'),
print('-depsc2', '-painters', '-loose', '-cmyk', [figpath,'EX_LOTKA_CTRLPERF_',InputSignalType,'_Eta',sprintf('%03g',100*eta_vec),'_TrainingLength_Legend','.eps']);

return
%% TESTING

% [~,idx_best_DMDc] = min(JendAll(:,1));
% % plot(Ntrain_vec,JendAll(idx_best,2)*ones(size(Ntrain_vec)),'-','Color',ccolors(2,:),'LineWidth',1)
% iM = 1;
% iL = idx_best_DMDc;

%
iM = 3;
iL = find(Ntrain_vec == 85); % DMDc

% iM = 1;
% iL = find(Ntrain_vec == 10); % DMDc

% iM = 3;
% iL = find(Ntrain_vec == 3000); % NARX: 85, 100, 14

figure, hold on
plot(ResultsALL(end,2).x(1,:),ResultsALL(end,2).x(2,:),'-k','LineWidth',1)
plot(ResultsALL(iL,iM).x(1,:),ResultsALL(iL,iM).x(2,:),'--r','LineWidth',1)

figure, hold on
plot(ResultsALL(end,2).t,ResultsALL(end,2).x(2,:),'-k','LineWidth',1)
plot(ResultsALL(end,iM).t,ResultsALL(iL,iM).x(2,:),'--r','LineWidth',1)
plot(ResultsALL(end,2).t,ResultsALL(end,2).x(1,:),'-k','LineWidth',1)
plot(ResultsALL(end,iM).t,ResultsALL(iL,iM).x(1,:),'--r','LineWidth',1)

figure, hold on
plot(ResultsALL(end,2).t,cumsum(ResultsALL(end,2).J),'-k','LineWidth',1)
plot(ResultsALL(end,iM).t,cumsum(ResultsALL(iL,iM).J),'--r','LineWidth',1)

figure, hold on
plot(ResultsALL(end,2).t,ResultsALL(end,2).u,'-k','LineWidth',1)
plot(ResultsALL(end,iM).t,ResultsALL(iL,iM).u,'--r','LineWidth',1)

%% TESTING
figure,hold on
for iM = 1:Nmodels
    Jend = cumsum(ResultsALL(iM,1).J);
    semilogx(Ntrain_vec(iM),Jend(end),'or')
    Jend = cumsum(ResultsALL(iM,2).J);
    semilogx(Ntrain_vec(iM),Jend(end),'og')
    Jend = cumsum(ResultsALL(iM,3).J);
    semilogx(Ntrain_vec(iM),Jend(end),'ob')
end

%%
figure,hold on
for iM = 1:Nmodels
    Jend = cumsum(ResultsALL(iM,1).J);
    plot3(Ntrain_vec(iM).*ones(size(Jend)),ResultsALL(iM,1).t,Jend,'-r')
end

%%
Jend = zeros(Nmodels,1);
for iM = 1:Nmodels
    tmp = cumsum(ResultsALL(iM,1).J); Jend(iM) = tmp(end);
end

figure,plot(Jend)

figure,plot(ResultsALL(3,1).x(1,:))
figure,plot(ResultsALL(3,1).u)


% Focussed Detector in 3D Example
%
% This example shows how k-Wave can be used to model the output of a
% focussed bowl detector in 3D where the directionality arises from
% spatially averaging across the detector surface. It builds on the
% Focussed Detector in 2D example.
%
% author: Ben Cox
% date: 29th October 2010
% last update: 18th September 2012
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

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

clear all;

% =========================================================================
% SIMULATION
% =========================================================================

% create the computational grid
Nx = 64;            % number of grid points in the x direction
Ny = 64;            % number of grid points in the y direction
Nz = 64;            % number of grid points in the z direction
dx = 100e-3/Nx;     % grid point spacing in the x direction [m]
dy = dx;            % grid point spacing in the y direction [m]
dz = dx;            % grid point spacing in the z direction [m]
kgrid = makeGrid(Nx, dx, Ny, dy, Nz, dz);

% define the properties of the propagation medium
medium.sound_speed = 1500;	% [m/s]

% create the time array
[kgrid.t_array, dt] = makeTime(kgrid, medium.sound_speed);
Nt = length(kgrid.t_array);

% create a concave sensor
radius = Nx/4-1;
height = 10;
ss = makeSphericalSection(radius, height);

% add it to a mask of the correct size
sensor.mask = zeros(Nx, Ny, Nz);
[a,b,c] = size(ss);
sphere_offset = 10;
sensor.mask(sphere_offset+(1:a),(Ny-b+1)/2+(1:b),(Nz-c+1)/2+(1:c)) = ss;

% define a time varying sinusoidal source
source_freq = 0.25e6;
source_mag = 1;
source.p = source_mag*sin(2*pi*source_freq*kgrid.t_array);
source.p = filterTimeSeries(kgrid, medium, source.p);

% place the first point source near the focus of the detector
source1 = zeros(Nx, Ny, Nz);
source1(sphere_offset+radius, Ny/2+1, Nz/2+1) = 1;

% place the second point source off axis
source2 = zeros(Nx, Ny, Nz);
source2(sphere_offset+radius, Ny/2+6, Nz/2+6) = 1;

% run the first simulation
source.p_mask = source1;
input_args = {'PMLSize', 10, 'DataCast', 'single', 'PlotSim', false};
sensor_data1 = kspaceFirstOrder3D(kgrid, medium, source, sensor, input_args{:});

% average the data recorded at each grid point to simulate the measured
% signal from a single element focussed detector
sensor_data1 = sum(sensor_data1, 1);

% run the second simulation
source.p_mask = source2;
sensor_data2 = kspaceFirstOrder3D(kgrid, medium, source, sensor, input_args{:});

% average the data recorded at each grid point to simulate the measured
% signal from a single element focussed detector
sensor_data2 = sum(sensor_data2, 1);

% =========================================================================
% VISUALISATION
% =========================================================================

% plot the detector and on-axis and off-axis point sources
voxelPlot(sensor.mask + source1 + source2);
view([99, 18]);

% plot the time series corresponding to the on-axis and off-axis sources
figure
[t_sc, t_scale, t_prefix] = scaleSI(kgrid.t_array(end));
plot(kgrid.t_array.*t_scale, sensor_data1, '-');
hold on
plot(kgrid.t_array.*t_scale, sensor_data2, 'r-');
xlabel(['Time [' t_prefix 's]']);
ylabel('Average Pressure Measured By Focussed Detector [Pa]');
legend('Source on axis', 'Source off axis');