function  simulateParking(parkFromRight,fis,trainingData,xf,yf,theta)
% Simulate autonomous parking for for fuzzy inference system starting from
% the specified initial condition.

car = setInitialConfiguration(xf,yf,theta);
[occGridX,occGridY] = createSimEnvironment();
simparking(parkFromRight,fis,trainingData,car,occGridX,occGridY)

end

function car = setInitialConfiguration(xf,yf,theta)

% Car width and height
car.width = 5;  % On x-axis
car.height = 3; % On y-axis

% Offset from front/rear edge to wheel center
car.offset = 1; 

% Length of the wheelbase
car.l = car.width - 2*car.offset; 

% Current orientation of the car
car.theta = theta;

% Current steering angle
car.phi = 0; 

% Front wheel center
car.xf = xf; 
car.yf = yf;

% Rear wheel center
car.xr = car.xf - car.l*cosd(theta);
car.yr = car.yf - car.l*sind(theta);

% Update coordinates of the bounding box-corners.
car = updateBoundingCorners(car);

% Set steering limits (in degrees).
car.leftSteeringLimit = 30; % in degrees
car.rightSteeringLimit = -30; % in degrees

% Set parking speed limit (in ft/sec).
car.forwardSpeedLimit = 5;

% Setting it higher values, which is equivalent to moving back for longer
% period of time to making space for safe turning.
car.backwardSpeedLimit = -3*car.forwardSpeedLimit;

end

function car = updateBoundingCorners(car)

% Update coordinates of the corners of the car.

car.xfLeft = car.xf + car.offset*cosd(car.theta) - 0.5*car.height*sind(car.theta);
car.yfLeft = car.yf + car.offset*sind(car.theta) + 0.5*car.height*cosd(car.theta);

car.xfRight = car.xf + car.offset*cosd(car.theta) + 0.5*car.height*sind(car.theta);
car.yfRight = car.yf + car.offset*sind(car.theta) - 0.5*car.height*cosd(car.theta);

car.xrLeft = car.xr - car.offset*cosd(car.theta) - 0.5*car.height*sind(car.theta);
car.yrLeft = car.yr - car.offset*sind(car.theta) + 0.5*car.height*cosd(car.theta);

car.xrRight = car.xr - car.offset*cosd(car.theta) + 0.5*car.height*sind(car.theta);
car.yrRight = car.yr - car.offset*sind(car.theta) - 0.5*car.height*cosd(car.theta);

end

function [occGridX,occGridY] = createSimEnvironment

% Set parking spot specification.
parkingSpot.width = 6;
parkingSpot.height = 7;
parkingSpot.bottomLeft.x = 20;
parkingSpot.bottomLeft.y = 8;

% Set road specification.
lane.width = 45;
lane.height = 7;
lane.bottomLeft.x = 1;
lane.bottomLeft.y = 1;

% Set simulation environment specification.
environment.width = lane.width + 2;
environment.height = parkingSpot.height + lane.height + 2;
environment.topLeft.x = 1;
environment.topLeft.y = 1;
environment.bottomLeft.x = 0;
environment.bottomLeft.y = 0;
environment.scale = 1;

% Draw simulation environment.
axis([0 environment.width 0 environment.height]);
xlabel('X (ft)')
ylabel('Y (ft)')
daspect([1 1 1])
hold on

% Draw road -------------------------------------
x = lane.bottomLeft.x;
y = lane.bottomLeft.y;
w = lane.width;
h = lane.height;

line([x x+w],[y y],'Color','k')
line([x x],[y y+h],'Color','k')
line([x+w x+w],[y y+h],'Color','k')
line([x x+w],[y+h y+h],'Color','k')

% Find occupied grid locations for road.
gridX = x:x+w;
gridY = y:y+h;

laneOccGridX = gridX;
laneOccGridY = repmat(y,[1 length(gridX)]);

laneOccGridY = [laneOccGridY gridY];
laneOccGridX = [laneOccGridX repmat(x,[1 length(gridY)])];

laneOccGridY = [laneOccGridY gridY];
laneOccGridX = [laneOccGridX repmat(x+w,[1 length(gridY)])];

laneOccGridX = [laneOccGridX gridX];
laneOccGridY = [laneOccGridY repmat(y+h,[1 length(gridX)])];

laneId = sub2ind([environment.height environment.width],laneOccGridY,laneOccGridX);
    
% Draw the parking spot -----------------------------------
x = parkingSpot.bottomLeft.x;
y = parkingSpot.bottomLeft.y;
w = parkingSpot.width;
h = parkingSpot.height;

line([x x+w],[y y],'Color','w')
line([x x],[y y+h],'Color','k')
line([x+w x+w],[y y+h],'Color','k')
line([x x+w],[y+h y+h],'Color','k')

% Find occupied grid locations for the parking spot.
gridX = x:x+w;
gridY = y:y+h;

parkOccGridX = gridX;
parkOccGridY = repmat(y,[1 length(gridX)]);

parkOccGridY = [parkOccGridY gridY];
parkOccGridX = [parkOccGridX repmat(x,[1 length(gridY)])];

parkOccGridY = [parkOccGridY gridY];
parkOccGridX = [parkOccGridX repmat(x+w,[1 length(gridY)])];

parkOccGridX = [parkOccGridX gridX];
parkOccGridY = [parkOccGridY repmat(y+h,[1 length(gridX)])];

parkId = sub2ind([environment.height environment.width],parkOccGridY,parkOccGridX);
[~,cmnLnId,cmnPrkId] = intersect(laneId,parkId);
laneId(cmnLnId) = [];
parkId(cmnPrkId) = [];
[ly,lx] = ind2sub([environment.height environment.width],laneId);
[py,px] = ind2sub([environment.height environment.width],parkId);

% Combine occupancy grid locations for the road and the parking spot.
occGridX = [lx px];
occGridY = [ly py];

end

function simparking(parkFromRight,fis,data,car,occGridX,occGridY)

% Get car boundary (offset values from the center), which is subtracted
% from the occupied grid distance to find obstacle distance at runtime.
[carOffset,cornerIds] = getCarBoundary(car.width,car.height);

% Set orientation of the parking spot, which can also be calculated from
% its coordinates.
parkingTheta = 90;

% Draw the initial position of the car.
carpoly = polyshape(...
    [car.xfRight car.xfLeft car.xrLeft car.xrRight], ...
    [car.yfRight car.yfLeft car.yrLeft car.yrRight]);

plot(carpoly,'EdgeColor','black','FaceColor','white', ...
    'FaceAlpha',1,'EdgeAlpha',1)

% Set sampling period.
delT = 0.1; % sec

% Set options for FIS evaluation.
eoptions = evalfisOptions;
eoptions.EmptyOutputFuzzySetMessage ='none';
eoptions.NoRuleFiredMessage = 'none';
eoptions.OutOfRangeInputValueMessage = 'none';

% Run simulation loop
continueBackup = false;
preOutputs = [];
while true
    % Set input values for the fuzzy system.
    alignment = limitAngleToPlusMinus180Degree(parkingTheta-car.theta);        
    [frontMinR,leftMinR,rearMinR,rightMinR] = ...
        getMinimumClearance(car,occGridX,occGridY,carOffset,cornerIds);
    inVals = [...
        alignment ...
        frontMinR ...
        leftMinR ...
        rearMinR ...
        rightMinR];
        
    % Stop simulation if the car is too close to occupany grids.
    if any([frontMinR,leftMinR,rearMinR,rightMinR]<0.25)
        disp('Stopping - boundary too close ...!')
        break
    end
    
    if abs(alignment)<=2 && frontMinR<=1 %abs(v)<=0.05
        disp('Parked')
        break
    end
        
    % Normalize inputs
    if parkFromRight
        inVals = [inVals(1) inVals(2) inVals(3) inVals(4) inVals(5)];
    else
        inVals = [-inVals(1) inVals(2) inVals(5) inVals(4) inVals(3)];
    end
    inVals = (inVals - data.xMinVal)./(data.xMaxVal-data.xMinVal);
    
    % Apply controller.
    inVals = min(1,max(0,inVals));
    
    % Evaluate FIS
    outputs = evalfis(fis,inVals,eoptions);
    
    % Denormalize output values.
    outputs = outputs(1:2).*(data.yMaxVal-data.yMinVal) + data.yMinVal;
    
    if ~parkFromRight
        outputs(1) = -outputs(1);
    end
    
    % Get the output values. The fuzzy system produces higher magnitude for
    % backward speed, which is clipped to max forward speed value with
    % longer duration for backward motion. This also creates smooth car
    % trajectory for visualization.
    if continueBackup
        phi = preOutputs(1);
        v = preOutputs(2);
        if v < -car.forwardSpeedLimit
            preOutputs = [phi v+car.forwardSpeedLimit];
            v = -car.forwardSpeedLimit;
        else
            continueBackup = false;
        end
    else
        phi = min(car.leftSteeringLimit,max(car.rightSteeringLimit,outputs(1)));
        v = min(car.forwardSpeedLimit,max(car.backwardSpeedLimit,outputs(2)));
        if v < -car.forwardSpeedLimit
            preOutputs = [phi v+car.forwardSpeedLimit];
            v = -car.forwardSpeedLimit;
            continueBackup = true;
        end
    end
        
    % Update robot states.
    car = updateCarStates(car,phi,v,delT);
    
    % Draw updated car position.
    carpoly = polyshape(...
        [car.xfRight car.xfLeft car.xrLeft car.xrRight], ...
        [car.yfRight car.yfLeft car.yrLeft car.yrRight]);
    
    plot(carpoly,'EdgeColor','black','FaceColor','white', ...
        'FaceAlpha',1,'EdgeAlpha',1)
    
    % Pause for animation.
    pause(0.25)
end

% Current simulation is complete.
hold off

end

function [range,cornerIds] = getCarBoundary(w,h)
% Calculate offset values from the car center to its boundary.

halfW = w/2;
halfH = h/2;
diagAngle = atan2d(halfH,halfW);

range = zeros(1,360);
x = zeros(1,360);
y = zeros(1,360);
cornerIds = round([360-diagAngle diagAngle 180-diagAngle 180+diagAngle]) + 1;

for theta = 0:359
    
    index = theta + 1;
    
    if (theta>=0 && theta<=diagAngle) || ...
            (theta>=(360-diagAngle) && theta<=359)
        x(index) = halfW;
        y(index) = tand(theta)*x(index);
    end

    if theta<=(180-diagAngle) && theta>diagAngle
        y(index) = halfH;
        x(index) = y(index)*tand(90-theta);
    end
    
    if theta>(180-diagAngle) && theta<=(180+diagAngle)
        x(index) = -halfW;
        y(index) = tand(theta)*x(index);
    end

    if theta>(180+diagAngle) && theta<(360-diagAngle)
        y(index) = - halfH;
        x(index) = y(index)*tand(270-theta);        
    end
    
    range(index) = getDistance(x(index),y(index));
end

end

function d = getDistance(delX,delY)
% Get distance using point difference.

d = sqrt(delX*delX + delY*delY);
end

function theta = limitAngleToPlusMinus180Degree(theta)
% Limit angle value in [-180 180] deg.

signOfTheta = sign(theta);
theta = signOfTheta*mod(abs(theta),360);

if theta > 180
    theta = 360 - theta;
elseif theta < -180
    theta = 360 + theta;
end

end


function [frontMinR,leftMinR,rearMinR,rightMinR] = ...
    getMinimumClearance(car,occGridX,occGridY,carOffset,cor)
% Get minimum distance values in front, on left and right, and at the rear
% of the car.

scan = 100*ones(1,360);
refX = (car.xf+car.xr)/2;
refY = (car.yf+car.yr)/2;
refTheta = car.theta;
if refTheta < 0
    refTheta = refTheta + 359;
end
for occGridIndex = 1:length(occGridX)
    delX = occGridX(occGridIndex) - refX;
    delY = occGridY(occGridIndex) - refY;
    gridTheta = atan2d(delY,delX);
    gridR = getDistance(delX,delY);
    if gridTheta < 0
        gridTheta = gridTheta + 359;
    end
    gridTheta = gridTheta - refTheta;
    if gridTheta < 0
        gridTheta = gridTheta + 359;
    end
    thetaIndex = round(gridTheta) + 1;
    scan(thetaIndex) = min(scan(thetaIndex),max(0,gridR-carOffset(thetaIndex)));
end
frontMinR = min(scan([1:cor(2) cor(1):360]));
leftMinR = min(scan(cor(2)+1:cor(3)));
rearMinR = min(scan(cor(3)+1:cor(4)));
rightMinR = min(scan(cor(4)+1:cor(1)-1));

end

function car = updateCarStates(car,phi,v,delT)

% Update car states using the kinematic model.

car.phi = limitAngleToPlusMinus180Degree(phi);

car.xf    = car.xf + v*cosd(car.theta+car.phi)*delT;
car.yf    = car.yf + v*sind(car.theta+car.phi)*delT;

car.theta = limitAngleToPlusMinus180Degree(car.theta + rad2deg(v*sind(car.phi)*(1/car.l)*delT));

car.xr    = car.xf - car.l*cosd(car.theta);
car.yr    = car.yf - car.l*sind(car.theta);

car = updateBoundingCorners(car);

end