function Pos = randomTrajectory(opt)
% randomTrajectory
%   opt - Structure with the following fields:
%         * dt              - Step width in sec/step.
%         * dTh             - Below this distance threshold change speed 
%                             and rotate away from the closest wall in cm.
%         * sMin            - Minimal speed in cm/sec.
%         * sPeak           - Target speed in cm/sec.
%         * PosInit         - Initial position with x,y coordinates in cm.
%         * PosXInterval    - Horizontal interval xMin and xMax in cm.
%         * PosYInterval    - Vertical interval yMin and yMax in cm.
%         * DirInit         - Initalization of direction, no UNIT.
%         * nStep           - Number of steps for this trajectory, no UNIT.
%         * rayBeta         - Beta parameter of Rayleigh distribution, in
%                             cm. This corresponds to the peak location of
%                             the distribution.
%         * normMu          - Mean value of normal distribution, in °/RAD.
%         * normSimga       - Standard deviation of normal distribution, in
%                             °/RAD.
%
% RETURN
%   Pos - Position vector with x,y coordinates, which has the dimensions:
%         nStep x 2.
%
% DESCRIPTION
%   This script generates a random trajectory in the 2D plane within a box
%   of the specified dimensions. The distribution of linear and rotational
%   speeds follows that of data. Linear speed is modeled through a Rayleigh
%   distribution and rotational speed is modeled through a normal
%   distribution.
%
%   Copyright (C) 2015  Florian Raudies, 05/02/2015, Palo Alto, CA.
%   License, GNU GPL, free software, without any warranty.
%

if isfield(opt,'dt'),           dt              = opt.dt;
else                            dt              = 0.02;     end % sec/step
if isfield(opt,'dTh'),          dTh             = opt.dTh; 
else                            dTh             = 15;       end % cm
if isfield(opt,'sMin'),         sMin            = opt.sMin;   
else                            sMin            = 5;        end % cm/sec
if isfield(opt,'sPeak'),        sPeak           = opt.sPeak; 
else                            sPeak           = 20;       end % cm/sec
if isfield(opt,'PosInit'),      PosInit         = opt.PosInit;
else                            PosInit         = [0 0];    end % cm
if isfield(opt,'PosXInterval'), PosXInterval    = opt.PosXInterval;
else                            PosXInterval    = [-50 50]; end % cm
if isfield(opt,'PosYInterval'), PosYInterval    = opt.PosYInterval;
else                            PosYInterval    = [-50 50]; end % cm
if isfield(opt,'DirInit'),      DirInit         = opt.DirInit;
else                            DirInit         = [0 1];    end % NONE
if isfield(opt,'nStep'),        nStep           = opt.nStep;
else                            nStep           = 5*10^4;   end % step
if isfield(opt,'rayBeta'),      rayBeta         = opt.rayBeta;
else                            rayBeta         = 13.25;    end % cm/sec
if isfield(opt,'normMu'),       normMu          = opt.normMu;
else                            normMu          = 0.0;      end % deg/sec
if isfield(opt,'normSigma'),    normSigma       = opt.normSigma;
else                            normSigma       = 15.0;     end % deg/sec


% Generate random turn speeds and linear speeds.
RandTurn    = normMu/180*pi + randn(nStep,1)*normSigma/180*pi;
RandSpeed   = raylrnd(rayBeta,[nStep 1]);
s           = sPeak;
eta         = 0.5;
Pos         = zeros(nStep,2);
Pos(1,:)    = PosInit';
Dir         = DirInit;

% Define borders. 
xMin = PosXInterval(1);
xMax = PosXInterval(2);
yMin = PosYInterval(1);
yMax = PosYInterval(2);

% Define the corners NorthEast, NorthWest, SouthEast, and SouthWest.
NE = [xMax yMax];
NW = [xMin yMax];
SE = [xMax yMin];
SW = [xMin yMin];

% Setup the walls. Points appear in this order to have always positive 
% distances.
Wall = [wall2D(NW, NE); ... % top wall
        wall2D(SW, NW); ... % left wall
        wall2D(SE, SW); ... % bottom wall
        wall2D(NE, SE)];    % right wall

for iStep = 2:nStep,
    [dWall aWall] = minDistAngleWallInFoV2D(Wall, Pos(iStep-1,:), Dir, pi);
    
    % Update speed and turn angle.
    if dWall<dTh && abs(aWall)<pi/2,
        s       = s - eta*(s-sMin);
        angle   = sign(aWall)*(pi/2-abs(aWall)) + RandTurn(iStep,:);        
    else
        s       = RandSpeed(iStep);
        angle   = RandTurn(iStep,:);
    end
    
    % Move.
    Pos(iStep,:) = Pos(iStep-1,:) + Dir*s*dt;
    
    % Turn.
    Dir = ([+cos(angle) -sin(angle); ...
            +sin(angle) +cos(angle)]*Dir')';
end


function W = wall2D(P1, P2)
    % Defines a wall by [nx ny d p1x p1y p2x p2y]
    Delta   = P2 - P1;
    Delta   = Delta/hypot(Delta(1), Delta(2));
    d       = - (P1(1)*Delta(2) - P1(2)*Delta(1));
    W       = [-Delta(2) Delta(1) d min(P1(1),P2(1)) min(P1(2),P2(2)) ...
                                    max(P1(1),P2(1)) max(P1(2),P2(2))];
                                
function [d a] = minDistAngleWallInFoV2D(Wall, Pos, Dir, foV)
    NxWall  = Wall(:,1);
    NyWall  = Wall(:,2);
    DWall   = Wall(:,3);
    XMin    = Wall(:,4);
    YMin    = Wall(:,5);
    XMax    = Wall(:,6);
    YMax    = Wall(:,7);
    px      = Pos(1);
    py      = Pos(2);
    nx      = Dir(1);
    ny      = Dir(2);
    Valid   = abs(NxWall*nx + NyWall*ny) > eps;
    NxWall  = NxWall(Valid);
    NyWall  = NyWall(Valid);
    DWall   = DWall(Valid);
    D       = (DWall - NxWall*px - NyWall*py)./(NxWall*nx + NyWall*ny);
    Sx      = px + D*nx;
    Sy      = py + D*ny;
    % To avoid numerical problems need to add/sub 10^-5.
    Valid   = XMin(Valid) <= (Sx+10^-5) & (Sx-10^-5) <= XMax(Valid) ...
            & YMin(Valid) <= (Sy+10^-5) & (Sy-10^-5) <= YMax(Valid);
    NxWall  = NxWall(Valid);
    NyWall  = NyWall(Valid);
    D       = D(Valid);
    A       = atan2(-NyWall*nx + NxWall*ny, NxWall*nx + NyWall*ny);
    Valid   = abs(A) <= foV/2;
    D       = D(Valid);
    A       = A(Valid);
    [d,mi]  = min(abs(D));
    a       = A(mi);