function BasicSystemExample(waitTime)
%   An example animation of a simple 2D system with a single position and
% velocity X=[p;v] described by the dynamics
%       p(n+1) = p(n) + v(n)
%       v(n+1) = v(n
%  Incoming measurements are of position only, with a uniformly distributed 
% independent white noise added in so that m(n) = p(n) + u(n). so the set 
% of possiblities from the measurements is all velocities but positions
% only between m(n) - u <= x <= m(n) + u. This region (blue on the plots)
% is show and then every point inside is advanced using the dynamics
% equations (white/silver). 
%   Then, when a new measurement comes in, we get another blue set of
% possiblities. Since we know both the white and the blue region in that
% time step are true, we can create a better estimate of the true state
% (the black x) by taking the intersection of the blue and silver sets.
%   We do this for each new measure: define the possible position and
% velocity from the new measurement (blue), evolve the old set of
% possibilities(silver), and take these two sets intersection as the best 
% result (red).
%   Note that on the first iteration we have no previous estimate, so we
% must use the entire blue region as our estimate. This make sense because
% we can not estimate velocity with only one position.
%
%   This example based on an illustration by Dr. James Clayton Kerce in the
%   Georgia Institute of Technology (GaTech) College of Mathematics special
%   topics course MA8813 in Fall 2008.
%
%   Copyright (c) 2008, Stephen E. Conover
%   All rights reserved.
%   This software is licensed under the New BSD License (see end of file).

%%%%%%% OPTIONS YOU CAN SET %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
defaultWaitTime = 1;    % Time to pause between each update of the plot
x0 = [0; 5];            % Start the target at a given position and a given velocity X = [pos;vel]
numIterations = 7;     % Number of iterations to calculate
errorMagnitude = 1;     % The measurement error u~uniform[-errorMagnitude, errorMagnitude]
useRandomError = false;  % Add a random component to the measurement error
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~exist('waitTime','var')
    waitTime = defaultWaitTime;
end

% Forcing function:
H = [1 1;       %  x(n+1) = x(n) + v(n)
     0 1];      %  v(n+1) = v(n)

rand('state', 5);       % Start the random number generator at the same
% point every time, so we get the same results run after run.

% Go ahead and calculate where the target will be after each iteration:
x = NaN(2,numIterations);
x(:,1) = x0;
for ind = 2:numIterations;
    x(:, ind) = H*x(:, ind-1);
end

% Now calcuate what the measurements will be:
% We will measurement position only (not velocity) and  the error on
% position measurements will be uniformly distributed over the region
% [-e,e].
e = errorMagnitude;
if useRandomError
    uErr = e*ones(numIterations,1).*rand(numIterations,1);
else
    uErr = e*ones(numIterations,1);
end
u = -uErr + (2*uErr) .* rand(numIterations, 1);
% So measurements m(n) = x(n) + u
m = x(1,:)'+u;

% Now loop through and calculate what happens after each of the
% measurements we computed above:

prevBestGuess = [];
evolvedPrevBestGuess = [];

% setup the plot:
initializePlot(1, x0, numIterations);

for ind = 1:size(m,1)

    % Get the vertical set of possiblities described by the measurement:
    PM = getMeasurementSet(m(ind), uErr(ind),x(:,ind));

    % If we have no previous best guess, then just use the current PM as
    % best guess. Otherwise, evolve the old best guess and use the
    % intersection of that and the PM as the new best guess:
    if isempty(prevBestGuess)
        bestGuess = PM;
    else
        evolvedPrevBestGuess = H*prevBestGuess;
        bestGuess = getIntersection(evolvedPrevBestGuess, PM(1,1), PM(1,2));
    end

    % Plot the results of this iteration:
    plotInteration(evolvedPrevBestGuess, PM, bestGuess, m(ind), x(:, ind), waitTime);
    
    % prepare for the next iteration:
    prevBestGuess = bestGuess;
end

% All done!

end


function h = initializePlot(figID, x0, numIterations)

% Setup a plot with the correct axis and labels:

h=figure(figID);
cla;

bestAxis = [sort([x0(1)+sign(-x0(2))*abs(x0(2)) , numIterations*x0(2)]), sort([0-sign(x0(2))*abs(x0(2))*0.1, x0(2)*1.3])];
axis(bestAxis);

xlabel('X (meters)', 'fontsize', 13)
ylabel('V (meters/sec)', 'fontsize', 13);

title('The Evolution of a Simple System', 'fontsize', 16, 'FontWeight', 'bold');

hold on; % must hold on now to keep the first plot from destroying the axis

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function h = plotInteration(evolvedPrevBestGuess, measurementPolygon, bestGuess, m, x, waitTime)
% Plot stuff about this iteration:

h = [];

% Transparency options for the sets:
alpha = .4;
edgeAlpha = .5;

% Coloring options:
colorMeasurement = 'b';
edgeColorMeasurement = 'b';
intersectionColor = 'r';
intersectionEdgecolor = 'r';
evolvedColor = [180 180 180]/255;'grey';
evolvedEdgecolor = [180 180 180]/255;'grey';

h(end+1) = plot(x(1), x(2), 'kx', 'markersize', 15);
pause(waitTime);
h(end+1) = plot(m, 0, 'bo', 'markersize', 15);

if ~isempty(measurementPolygon)
    h(end+1) = patch(measurementPolygon(1,:), measurementPolygon(2,:),colorMeasurement,'FaceAlpha',alpha,'edgecolor',edgeColorMeasurement);
end

pause(waitTime);

if ~isempty(evolvedPrevBestGuess)
    h(end+1) = patch(evolvedPrevBestGuess(1,:), evolvedPrevBestGuess(2,:),evolvedColor,'FaceAlpha',alpha,'edgecolor',evolvedEdgecolor, 'edgeAlpha',edgeAlpha);
end

pause(waitTime);

if ~isempty(bestGuess)
    h(end+1) = patch(bestGuess(1,:), bestGuess(2,:),intersectionColor,'FaceAlpha',alpha,'edgecolor',intersectionEdgecolor);
end

pause(waitTime);

end

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

function PM = getMeasurementSet(m, e, x)
% The measurement tells us that the position is within
% (m-e) <= x <= (m+e) but we do not know anything about velocity.
% So let the set of posible states be a rectangle ~infinite in height
% and bounded on the left and right as described:

% Defined from the top left corner P=[x;y]:
vBound = abs(x(2))*100;          % a vertical bound to simulate infinity
PM = [m-e, m+e, m+e, m-e;
    vBound, vBound, -vBound, -vBound];

end


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

function [ Intersection ] = getIntersection( P, L, R )
% This function returns a polygon that represents the intersection of
%  1. Another polygon P (this represents the best state estimates)
%  2. An infinite set bounded by the vertical lines L on the left and R on
%       the right. (this represents the region described by a new
%       measurement)
%
% Inputs:
%    P  - Polygon with row 1 as x and row 2 as y
%    L  - Left bound
%    R  - Right bound

% We add the first point onto the end for the algorithm to work cleanly as
% we assume that we are givena  single closed polygon:
P = [P, P(:,1)];

% We will grown the result dynamically for convenience:
Intersection = [;];

% We look at ever line segment in the polygon and determine how it does or
% does not intersect the vertical region defined by L and R. There are
% three types of lines of interest:
%   1. Lines that move IN to the vertical region.
%   2. Lines that move OUT of the vertical region.
%   3. Lines the move ACROSS the vertical region.
% All other points may be discarded. If we find one of the cases, we must
% add a point on the intersection of the line segment in P and the vertical
% bar. A bit of switching is required here to account for everything.
% Note that we always work with the current end point and the previous,
% but we only save the previous. With some inspection you will find that we
% should only save the previous  point, p1 because we only know what to do
% with the current point, p2, when we know the next line segment. This is
% also the reason that we must append the first point of the polygon P onto
% the end of the set. This accounts for the line segment from the last
% point in P to the first point in P.
for ind = 2:size(P,2)

    p1 = P(:,ind-1);    % the starting point of the line segment
    p2 = P(:, ind);     % the ending point of the line segment

    % Define some variables for convenience and clarity:
    startsIn = p1(1) > L && p1(1) < R;  % The line starts in the region
    endsIn =  p2(1) > L && p2(1) < R;   % The line ends in the region

    allIn = startsIn && endsIn;         % Both points are inside the region

    movesOut = startsIn && ~endsIn;     % The line starts in but ends out
    movesIn = ~startsIn && endsIn;      % The line starts out but moves in

    % The line crosses over the region entirely, from one side to the
    % other:
    crosses = (p1(1) < L && p2(1) > R) || (p2(1) < L && p1(1) > R);

    % Equation for a line is y = m*x+b:
    m = (p2(2) - p1(2))/(p2(1) - p1(1));    % Slope of segment
    b = P(2, ind) - m *P(1, ind);           % Offset of segment

    % Jump through each case:
    if allIn
        % If both the points are in, then keep p1. BOTH these
        % points will be kept, but we wait until next round to save p2.
        % This rounds p2 will be next rounds p1.
        Intersection = [Intersection p1];
    elseif movesOut
        % The system is moving from inside the vertical region to outside
        % of it.

        % Did we move out from the left or the right?
        % We will either find the intercept of the LEFT value or the RIGHT
        % value:
        if p2(1) < L
            xi = L;
        else % p2(1) > R
            xi = R;
        end

        % Calculate the y intercept:
        yi = m*xi+b;

        % Now save both p1 (which is inside) and a new point which is the
        % point where this line intersects either the R or L bar:
        Intersection = [Intersection p1 [xi;yi]];
    elseif movesIn
        % In the case, the line is moving IN to the vertical region.

        % Did we move out from the left or the right?
        if p1(1) < L
            % From the left:
            xi = L;
        else
            % from the right:
            xi = R;
        end

        % Calculate the y intercept:
        yi = m*xi+b;

        % We throw out the p1, because it's outside the region, but we keep
        % a new point which is where the line crosses. p2 is also a good
        % point, but we will pick that up next iteration of the loop when
        % p2 becomes p1:
        Intersection = [Intersection [xi;yi]];

    elseif crosses
        % The line crosses over the vertical region, but p1 nor p2 are
        % inside:

        % Cross left or cross right?
        % The direction matters because we need to know which order to put
        % in the points in--left intercept first, or right?
        if (p2(1) - p1(1)) > 0
            xi = [L,R];
        else
            xi = [R,L];
        end

        % Calculate both y intercepts at once:
        yi = m*xi+b;

        % and add them, in the correct order, to the polygon we are making:
        Intersection = [Intersection [xi;yi]];
    else
        % Otherwise, both p1 and p2 are outside, and we don't need to do
        % anything.
    end

end

% At this point we have processed every line sigment and have created a
% polygone that represents the intersection of the two regions.

end

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

% Copyright (c) 2008, Stephen E. Conover
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in the
%       documentation and/or other materials provided with the distribution.
%     * Neither the name of Stephen Conover nor the
%       names of his contributors may be used to endorse or promote products
%       derived from this software without specific prior written permission.
%
% THIS SOFTWARE IS PROVIDED BY STEPHEN E. CONOVER ``AS IS'' AND ANY
% EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
% DISCLAIMED. IN NO EVENT SHALL STEPHEN. E. CONOVER BE LIABLE FOR ANY
% DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
% (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
% LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
% ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
% (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.