%  * **********************************************************************
%  *
%  * Copyright (c) 2014 Regents of the University of California. All rights reserved.
%  *
%  * Redistribution and use in source and binary forms, with or without
%  * modification, are permitted provided that the following conditions
%  * are met:
%  *
%  * 1. Redistributions of source code must retain the above copyright
%  *    notice, this list of conditions and the following disclaimer.
%  *
%  * 2. 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.
%  *
%  * 3. The names of its contributors may not be used to endorse or promote
%  *    products derived from this software without specific prior written
%  *    permission.
%  *
%  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
%  * "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 THE COPYRIGHT OWNER OR
%  * CONTRIBUTORS 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.
%  *
%  * **********************************************************************

% makeMap - generates a Cartesian coordinate map with at least one obstacle.
%
% @param xDim - X dimension of the map.
% @param yDim - Y dimension of the map.
% @param makeRoad - if true, makes straight roads with low cost.
% @param twoObstacles - if true, makes a second obstacle with high cost.
% @param minorObstacles - if true, makes a random distribution of locations with moderate cost.
function map = makeMap (xDim, yDim, makeRoad, twoObstacles, minorObstacles)

ROAD = 1;
OPEN_SPACE = 3;
MAJOR_OBSTACLE = 25;
MINOR_OBSTACLE = 5;
NUMGOALS = 4;

map = OPEN_SPACE*ones(xDim,yDim);

if minorObstacles
    for i=1:xDim*2
        map(randi(xDim),randi(yDim)) = MINOR_OBSTACLE;
    end
end

% make the first obstacle
xPos = randi(round(xDim/6));
xSize = randi(round(xDim/8))+randi(round(xDim/2));
yPos = randi(round(xDim/6));
ySize = randi(round(yDim/8))+randi(round(yDim/2));
for i = xPos:xPos+xSize
    for j = yPos:yPos+ySize;
        if i <= xDim && j <= yDim
            map(i,j) = MAJOR_OBSTACLE;
        end
    end
end

if twoObstacles
    xPos = xDim-randi(xDim)+1;
    xSize = randi(round(xDim/4))+randi(round(xDim/2));
    yPos = yDim-randi(yDim)+1;
    ySize = randi(round(yDim/4))+randi(round(yDim/2));
    
    for i = xPos:xPos+xSize
        for j = yPos:yPos+ySize;
            if i <= xDim && j <= yDim
                map(i,j) = MAJOR_OBSTACLE;
            end
        end
    end
end

% make outer road
if makeRoad
    innerEdge = 3;
    for i = innerEdge:xDim-innerEdge
        for j = innerEdge:yDim-innerEdge
            if i == innerEdge || i == xDim-innerEdge || j == innerEdge || j == yDim-innerEdge
                map(i,j) = ROAD;
            end
        end
    end
    
    for i = 1:NUMGOALS
        if mod(i,2) == 0
            goal(i).x = randi([innerEdge+2 xDim-innerEdge-2],1);
            if rand > 0.5
                goal(i).y = innerEdge;
            else
                goal(i).y = yDim-innerEdge;
            end
        else
            if rand > 0.5
                goal(i).x = innerEdge;
            else
                goal(i).x = xDim-innerEdge;
            end
            goal(i).y = randi([innerEdge+2 yDim-innerEdge-2],1);
        end
        map(goal(i).x,goal(i).y) = 3;
    end
    
    for i = 1:NUMGOALS
        for j = 1:NUMGOALS
            if goal(i).x ~= goal(j).x && goal(i).y ~= goal(j).y && j > i
                if goal(i).x < goal(j).x && goal(i).y < goal(j).y
                    map(goal(i).x:goal(j).x, goal(i).y) = ROAD;
                    map(goal(j).x, goal(i).y:goal(j).y) = ROAD;
                elseif goal(i).x < goal(j).x && goal(i).y > goal(j).y
                    map(goal(i).x:goal(j).x, goal(i).y) = ROAD;
                    map(goal(j).x, goal(j).y:goal(i).y) = ROAD;
                elseif goal(i).x > goal(j).x && goal(i).y < goal(j).y
                    map(goal(j).x:goal(i).x, goal(i).y) = ROAD;
                    map(goal(j).x, goal(i).y:goal(j).y) = ROAD;
                else
                    map(goal(j).x:goal(i).x, goal(i).y) = ROAD;
                    map(goal(j).x, goal(j).y:goal(i).y) = ROAD;
                end
            end
        end
    end
end

imagesc(map); colorbar;

end


