% Read the file wharf.csv which is a 30x30 matrix of 1's and 0's
wharf_locations = csvread('wharf.csv');

% Store wharf_locations in variable t
t = wharf_locations;

% Store the size of the matrix into variables
[m,n] = size(wharf_locations);

% Asks user for x and y coordinates of start and goal locations
x = input('Please enter x coordinate for start location:');
y = input('Please enter y coordinate for start location:');
c = input('Please enter x coordinate for goal location:');
d = input('Please enter y coordinate for goal location:');

% If the starting location is forbidden (outside of walls or in an
% obstacle, display a message and ask for new input
while x<1 || x > m || y < 1 || y > m || wharf_locations(y,x) == 1
    disp('Starting location is forbidden, enter new coordinates:');
    x = input('Please enter x coordinate for start location:');
    y = input('Please enter y coordinate for start location:');
end

% If the goal location is forbidden (outside of walls or in an obstacle,
% display a message and ask for new input)
while c<1 || c>m || d<1 || d>m || wharf_locations(d,c) == 1
    disp('Goal location is forbidden, enter new coordaintes:');
    c = input('Please enter x coordinate for goal location:');
    d = input('Please enter y coordinate for goal location:');
end

% Set variable steps originally = to 0
steps = 0;

% Ask the user for random or smart strategy
strat = input('Please type "1" for random strategy or "5" for smart strategy:');

% If the user did not enter 1 or s, get them to try again
while strat ~= 1 && strat ~= 5
    disp('You must enter either 1 or 5, try again');
    strat = input('Please type "1" for random strategy or "5" for smart strategy:');
end

% Create a movie matrix
movieMatrix = t;

% Change all containers originally 1 to -10
movieMatrix(movieMatrix > 0)= -10;

% 10 represents the robot which is originally at start position
movieMatrix(y,x) = 10;

% -3 represents the coordinates of the goal
movieMatrix(d,c) = - 3;

% Set the count for number of steps on a tile to 0
count1 = 0;
count2 = 0;
count3 = 0;
count4 = 0;


% Asks the user for the maximum number of steps the strad takes
maxSteps = input('Enter maximum number of steps strad should take:');

% If user typed 1, proceed with random strategy, if user typed s, use smart
% strategy
if strat == 1

    % Set up while loop that allows the strad to move to a random adjacent tile
    % without entering forbidden territory
    while steps <= maxSteps

        % Do not allow strad to move to forbidden territory if in corner of
        % matrix boundaries
        if x == 1 && y == 1
            a = randi(2);
        elseif x == 1 && y == n
            a = randi(2);
            if a == 1
                a = 4;
            end
        elseif x == m && y == 1
            a = randi(2);
            if a == 2
                a = 3;
            end
        elseif x == m && y == m
            a = randi(2);
            if a == 1
                a = 3;
            else a = 4;
            end

        % Do not allow strad to move to forbidden territory if on edge of
        % matrix boundaries
        elseif x == 1
            a = randi(3);
            if a == 3
                a = 4;
            end
        elseif x == m
            a = randi(3);
            if a == 2
                a = 4;
            end
        elseif y == 1
            a = randi(3);
        elseif y == m
            a = randi(3);
            if a == 1
                a = 4;
            end

        % Do not allow strad to move to forbidden territory if next to
        % obstacle surrounding 3 sides of strad
        elseif t(y+1,x) == 1 && t(y-1,x) == 1 && t(y,x+1) == 1
            a = 3;
        elseif t(y+1,x) == 1 && t(y-1,x) == 1 && t(y,x-1) == 1
            a = 2;
        elseif t(y+1,x) == 1 && t(y,x-1) == 1 && t(y,x+1) == 1
            a = 4;
        elseif t(y-1,x) == 1 && t(y,x-1) == 1 && t(y,x+1) == 1
            a = 1;

        % Do not allow strad to move to forbidden territory if next to obstacle
        % surrounding 2 sides of strad
        elseif t(y+1,x) == 1 && t(y-1,x) == 1
            a = randi(2,3);
        elseif t(y+1,x) == 1 && t(y,x-1) == 1
            a = randi(2);
            if a == 1
                a = 4;
            end
        elseif t(y+1,x) == 1 && t(y,x+1) == 1
            a = randi(2);
            if a == 1
                a = 3;
            elseif a == 2
                a = 4;
            end
        elseif t(y-1,x) == 1 && t(y,x-1) == 1
            a = randi(2);
        elseif t(y-1,x) == 1 && t(y,x+1) == 1
            a = randi(2);
            if a == 2
                a = 3;
            end
        elseif t(y,x+1) == 1 && t(y,x-1) == 1
            a = randi(2);
            if a == 2
                a = 4;
            end

        % Do not allow strad to move to forbidden territory if next to obstacle
        % on one side of strad
        elseif t(y,x-1) == 1
            a = randi(3);
            if a == 3
                a = 4;
                disp(a);
            end
        elseif t(y-1,x) == 1
            a = randi(3);
        elseif t(y,x+1) == 1
            a = randi(3);
            if a == 2
                a = 4;
            end
        elseif t(y+1,x) == 1
            a = randi(3);
            if a == 1
                a = 4;
            end
        else a = randi(4);
        end

        % Strad moves right if a = 1, down if a = 2, up if a = 3 and left if
        % a = 4
        if a == 1
            y = y + 1;
        elseif a == 2
            x = x + 1;
        elseif a == 3
            x = x - 1;
        elseif a == 4
            y = y - 1;
        end


        if a == 1
            movieMatrix(y-1,x)= movieMatrix(y-1,x)+ 1;
        elseif a == 2
            movieMatrix(y,x-1)= movieMatrix(y,x-1)+1;
        elseif a == 3;
            movieMatrix(y,x+1)= movieMatrix(y,x+1)+1;
        else movieMatrix(y+1,x)= movieMatrix(y+1,x) +1;
        end

        % Display each step in x and y coordinates
        s = [x,y];
        r = [c,d];
        disp(s)
        movieMatrix(y,x) = 1;

        colormap jet;
        colorbar;
        imagesc(movieMatrix);

        % Increase the count on the number of steps
        steps = steps + 1;

        frames = getframe();
        if r == s;
            break
        end
    end

    % If the maximum steps is reached, display "Maximum steps reached", if a
    % valid path is found, display "Valid path found in x steps"
    if steps >= maxSteps
        disp('Maximum steps reached');
    else disp(['Valid path found in ', num2str(steps),' ', 'steps']);
    end

elseif strat == 5
    disp('There is currently no smart strategy available')
end