% number of particles
N = 1000;

% number of simulation steps
T = 40;

% number of seconds to wait until next step
t = 0.01;

% number of landmarks to provide (1,2,3,4 or 5)
noLandmarks = 5;

% flag to indicate whether you want to save the animation - 'true' or
% 'false'
saveAnimation = 'false';

myParticleFilterRun(N, T, t, noLandmarks, saveAnimation);

function [] = myParticleFilterRun(N, T, t, noLandmarks, saveAnimation)

    %% Configuration
    %
    % set the World and place landmarks
    if noLandmarks == 1
        % 1 landmark
        landmarks  = [[50;50]];
    elseif noLandmarks == 2
        % 2 landmarks
        landmarks  = [[50;75], [50; 25]];
    elseif noLandmarks == 3
        % 3 landmarks
        landmarks  = [[50;75], [25; 25], [75; 25]];
    elseif noLandmarks == 4
        % 4 landmarks
        landmarks  = [[20.0; 20.0], [80.0; 80.0], [20.0; 80.0], [80.0; 20.0]];
    else
        % 5 landmarks
        landmarks  = [[10.0; 20.0], [50.0; 10.0], [50.0; 30.0], [10.0; 70.0], [70.0; 70.0]];
    end
    worldSize = 100.0;
    
    % number of particles
    %N = 1000;
    
    % number of simulation steps
    %T = 10;
    
    %% Initialize robot and particles
    %
    
    % create the robot...
    myRobot = createRobot(worldSize);
    myRobot = setRobot(myRobot, worldSize, 10, 10, 0.3);
    
    % ...and let it make it's first move!
    myRobot = robotMove(myRobot, worldSize, 0.1, 5.0);
    
    % Sense it's position (NOTE: we have no Noise yet, so sensing and
    % movement before should be 'perfect')
    Z = robotSense(myRobot, landmarks);

    % create particles
    for i = 1:N
        p = createRobot(worldSize);
        p = setNoise(p, 0.05, 0.05, 5.0);
        particlesList(i) = p;
    end
    
    disp( sprintf('Initial Error is: %.6f', robotEvaluate(myRobot, particlesList, worldSize) ) );
    
    % additionally add some noise to robot    
    myRobot = setNoise(myRobot, 0.05, 0.05, 2.0);
    
    % Plot initial configuration
    figure(1);
    hold on;
    axis([0 worldSize 0 worldSize]);
    set(gcf, 'PaperUnits', 'inches', 'PaperPosition', [0 0 1.906666667 0.9]);
    plot(landmarks(1,:), landmarks(2,:), 'bh', 'MarkerFaceColor', 'b', 'MarkerSize', 20);
    plot(myRobot.x, myRobot.y, 'g^', 'MarkerFaceColor', 'g', 'MarkerSize', 15);
    for i = 1:N
        plot(particlesList(i).x, particlesList(i).y, 'r.', 'MarkerFaceColor', 'r');
    end
    drawnow;
    if saveAnimation
        saveas(gcf,sprintf('img/particleStep_%02d', 1),'png');
    end
    hold off;
    pause(1);
    
    %% Particle Filtering on Robot Localization
    %
    
    for t = 1:T
        % preliminary randomly set how much the robot should turn and how
        % far it should drive (random walk!)
        currentTurn = rand(1) * pi/4 - pi/8;  % random turn between [-pi/8;pi/8]
        currentStep = rand(1) * 8;            % random step between [0;8]
        
        % 1st Step - Robot moves
        myRobot = robotMove(myRobot, worldSize, currentTurn, currentStep);
        
        % 2nd Step - Robot senses
        Z = robotSense(myRobot, landmarks);
        
        % 3rd Step - Move all Particles (let them do the same movement as the robot)
        for i = 1:N
            particlesList(i) = robotMove(particlesList(i), worldSize, currentTurn, currentStep);
        end
        
        % 4th Step - Calculate Measurement Probabilitys (for Importance
        % Weight of each Particle)
        W = [];
        for i = 1:N
            W(i) = measurementProbability(particlesList(i), landmarks, Z);
        end
        
        % 5th Step - Resample Particles according to Measurement
        % Probabilitys
        index = int32( rand(1) * N );
        if index == 0
            index = 1;
        end
        beta = 0.0;
        mw = max(W);
        for i = 1:N
            beta = beta + rand(1) * 2.0 * mw;
            while beta > W(index)
                beta = beta - W(index);
                index = mod( index, N ) + 1;
            end
            newParticlesList(i) = particlesList(index);
        end
        particlesList = newParticlesList;        
        
        % 6th Step - Plot Everything
        clf;
        hold on;
        axis([0 worldSize 0 worldSize]);
        set(gcf, 'PaperUnits', 'inches', 'PaperPosition', [0 0 1.906666667 0.9]);
        plot(landmarks(1,:), landmarks(2,:), 'bh', 'MarkerFaceColor', 'b', 'MarkerSize', 20);
        plot(myRobot.x, myRobot.y, 'g^', 'MarkerFaceColor', 'g', 'MarkerSize', 15);
        for i = 1:N
            plot(particlesList(i).x, particlesList(i).y, 'r.', 'MarkerFaceColor', 'r');
        end
        drawnow;
        if saveAnimation
            saveas(gcf,sprintf('img/particleStep_%02d', t+1),'png');
        end
        hold off;
        
        % 7th Step - Print Error
        disp( sprintf('Error is: %.6f', robotEvaluate(myRobot, particlesList, worldSize) ) );
        pause(t);
    end
    
    
    input('Press Any Key to Exit...!');

end







%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Robot Class Functions from CS373 - Course from UDACITY %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function robot = createRobot(worldSize)

    robot.x = rand(1) * worldSize;
    robot.y = rand(1) * worldSize;
    robot.orientation = rand(1) * 2.0 * pi;
    robot.forwardNoise = 0.0;
    robot.turnNoise    = 0.0;
    robot.senseNoise   = 0.0;
    
end

function robot = setRobot(robot, worldSize, newX, newY, newOrientation)

    if newX < 0 || newX >= worldSize
        error('X coordinate out of bound');
    end
    if newY < 0 or newY >= worldSize
        error('Y coordinate out of bound');
    end
    if newOrientation < 0 or newOrientation >= 2 * pi
        error('Orientation must be in [0..2pi]');
    end
    
    robot.x = newX;
    robot.y = newY;
    robot.orientation = newOrientation;
    
end

function robot = setNoise(robot, newForwardNoise, newTurnNoise, newSensorNoise)

    % makes it possible to change the noise parameters
    % this is often useful in particle filters
    robot.forwardNoise = newForwardNoise;
    robot.turnNoise    = newTurnNoise;
    robot.senseNoise   = newSensorNoise;

end

function distList = robotSense(robot, landmarks)

    distList = [];
    
    for i = 1:size(landmarks,2)
        dist = sqrt((robot.x - landmarks(1,i)) ^ 2 + (robot.y - landmarks(2,i)) ^ 2);
        dist = dist + randn(1) * robot.senseNoise;
        distList(i) = dist;
    end

end

function robot = robotMove(robot, worldSize, turn, forward)

    if forward < 0
        error('Robot cant move backwards');
    end

    % turn, and add randomness to the turning command
    newOrientation = robot.orientation + turn + randn(1) * robot.turnNoise;
    newOrientation = mod(newOrientation, 2 * pi);

    % move, and add randomness to the motion command
    dist = forward + randn(1) * robot.forwardNoise;
    newX = robot.x + cos(newOrientation) * dist;
    newY = robot.y + sin(newOrientation) * dist;
    % cyclic truncate
    newX = mod(newX, worldSize);    
    newY = mod(newY, worldSize);

    % set particle
    newRobot = createRobot(worldSize);
    newRobot = setRobot(newRobot, worldSize, newX, newY, newOrientation);
    newRobot = setNoise(newRobot, robot.forwardNoise, robot.turnNoise, robot.senseNoise);
    
    robot = newRobot;
    
end

function w = gaussianProbability(mu, sigma, x)

    % calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
    w = exp(- ((mu - x) .^ 2) ./ (sigma .^ 2) ./ 2.0) ./ sqrt(2.0 .* pi .* (sigma .^ 2));

end

function prob = measurementProbability(robot, landmarks, measurement)

    % calculates how likely a measurement should be
    % for every measurement-point (landmark) we first calculate the
    % distance of the particle to this landmark and then it's gaussian
    % probability
        
    prob = 1.0;
    for i = 1:size(landmarks,2)
        dist = sqrt((robot.x - landmarks(1,i)) ^ 2 + (robot.y - landmarks(2,i)) ^ 2);
        prob = prob * gaussianProbability(dist, robot.senseNoise, measurement(i));
    end

end

function [] = robotDisplay(robot)

    % display current position and orientation        
    disp(sprintf('[x=%.6f y=%.6f orient=%.6f]', robot.x, robot.y, robot.orientation));

end



function sumError = robotEvaluate(robot, particleList, worldSize)

    sumError = 0.0;
    % calculate mean error
    for i = 1:length(particleList)
        dx = mod(particleList(i).x - robot.x + (worldSize/2.0), worldSize) - (worldSize/2.0);
        dy = mod(particleList(i).y - robot.y + (worldSize/2.0), worldSize) - (worldSize/2.0);
        err = sqrt(dx * dx + dy * dy);
        sumError = sumError + err;
    end
    
    sumError = sumError / length(particleList);

end