function [] = ObstacleAvoidance_splines()
% Example script for getting started with FORCESPRO NLP solver.
% 
% --------------------------------------------------------------------------
% NOTE: This example shows how to define a nonlinear programming problem,
% where all derivative information is automatically generated by
% using the AD tool CasADi. In particular, this example illustrates the 
% use of splines inside the problem formulation.
% --------------------------------------------------------------------------
% 
% This example solves an optimization problem for a car with the simple
% continuous-time, nonlinear dynamics (bicycle model):
% 
%    dxPos/dt  = v*cos(theta + beta)
%    dyPos/dt  = v*sin(theta + beta)
%    dv/dt     = F/m
%    dtheta/dt = v/l_r*sin(beta)
%    ddelta/dt = phi
% 
%    with:
%    beta      = arctan(l_r/(l_f + l_r)*tan(delta))
% 
% where xPos,yPos are the position, v the velocity in heading angle theta 
% of the car, and delta is the steering angle relative to the heading 
% angle. The inputs are acceleration force F and steering rate phi. The 
% physical constants m, l_r and l_f denote the car's mass and the distance 
% from the car's center of gravity to the rear wheels and the front wheels.
% 
% The car starts from standstill with a certain heading angle, and the
% optimization problem is to move the car as close as possible to a certain
% end position while staying inside a nonconvex area. In addition, an 
% elliptic obstacle must be avoided. Using an interactive window, 
% the position of the obstacle can be changed by the user and the car's 
% trajectory is adapted automatically.
% 
% Quadratic costs for the acceleration force and steering are added to the
% objective to avoid excessive maneouvers.
% 
% There are bounds on all variables except theta.
% 
% Variables are collected stage-wise into 
% 
%     z = [F phi xPos yPos v theta delta].
% 
% See also FORCES_NLP
% 
% (c) Embotech AG, Zurich, Switzerland, 2013-2023.
    
    clc;
    close all;

    %% Generate code for path finder
    model = generatePathplanner();

    %% Calculate path for fixed obstacle
    % Set runtime parameters
    params = [-1.5; 1.0];
    problem.all_parameters = repmat(params,model.N,1);

    % Set initial guess to start solver from (here, middle of upper and lower
    % bound)
    x0i=[0.0,0.0,-1.5,1.5,1.,pi/4.,0.];
    x0=repmat(x0i',model.N,1);
    problem.x0=x0; 

    % Set initial conditions. This is usually changing from problem instance 
    % to problem instance:
    model.xinit = [-2., 0., 0., deg2rad(90), 0.]';
    problem.xinit = model.xinit; % xPos=-2, yPos=0, v=0 (standstill), 
    % heading angle=90deg, steering angle=0deg

    % Time to solve the NLP!
    [output,exitflag,info] = FORCESNLPsolver(problem); %#ok<ASGLU>

    % Make sure the solver has exited properly. 
    assert(exitflag == 1,'Some problem in FORCESPRO solver');

    % extract output
    u = zeros(2,model.N);
    x = zeros(5,model.N);
    for i=1:model.N
        temp = output.(['x',sprintf('%02d',i)]);
        x(:,i) = temp(3:7,1);
        u(:,i) = temp(1:2,1);
    end

    %% Plot results and make window interactive
    plotAndMakeInteractive(x,u,model,params);
end

function [ model ] = generatePathplanner()
% Formulates the optimization problem, generates a solver for the path 
% planning problem by calling the FORCESPRO code generation.
    
    %% Problem dimensions
    model.N = 50;            % horizon length
    model.nvar = 7;          % number of variables
    model.neq  = 5;          % number of equality constraints
    model.nh   = 3;          % number of inequality constraint functions
    model.npar = 2;          % number of runtime parameters

    %% Objective function 
    % In this example, we want to move the car as close as possible to the
    % point (0,3) while adding some penalties on the inputs F and s:
    model.objective = @(z) 100*abs(z(3)-0) + 100*abs(z(4)-3.) + ...
        0.1*z(1)^2 + 0.01*z(2)^2;

    % You can use standard Matlab handles to define these functions, i.e. 
    % you could also have this in a separate file. We use anonymous handles
    % here only for convenience.

    %% Dynamics, i.e. equality constraints 
    % We use an explicit RK4 integrator here to discretize continuous 
    % dynamics. The definition of @continuousDynamics is further below in 
    % this file
    integrator_stepsize = 0.1;
    model.eq = @(z) RK4( z(3:7), z(1:2), @continuousDynamics,...
        integrator_stepsize);

    % Indices on LHS of dynamical constraint - for efficiency reasons, make
    % sure the matrix E has structure [0 I] where I is the identity matrix.
    model.E = [zeros(5,2), eye(5)];

    %% Inequality constraints
    % upper/lower variable bounds lb <= z <= ub
    %            inputs             |             states
    %             F      phi           xPos  yPos   v    theta    delta
    model.lb = [ -5.0,  deg2rad(-40),  -3.,   0.,   0.,  -inf,   -0.48*pi]; 
    model.ub = [ +5.0,  deg2rad(+40),   0.,   3.,   2.,  +inf,    0.48*pi];

    % General (differentiable) nonlinear inequalities hl <= h(z,p) <= hu
    % using two splines for road limit and a circle for an additional 
    % obstacle
    [upperRoadLimit, lowerRoadLimit] = setupRoadLimitSplines();
    
    model.ineq = @(z,p)  [ z(4) - upperRoadLimit(z(3)); ...
                           lowerRoadLimit(z(3)) - z(4); ...
                           (z(3)-p(1))^2 + (z(4)-p(2))^2 ];

    % Upper/lower bounds for inequalities
    model.hu = [    0,    0, +inf  ]';
    model.hl = [ -inf, -inf, 0.6^2 ]';

    %% Initial conditions
    % Initial conditions on all states
    model.xinitidx = 3:7; % use this to specify on which variables initial
    % conditions are imposed
    model.bfgs_init = 3.0*eye(7); % set initialization of the 
    % hessian approximation
    
    %% Define solver options
    codeoptions = getOptions('FORCESNLPsolver');
    codeoptions.maxit = 400;    % Maximum number of iterations
    codeoptions.printlevel = 0; % Use printlevel = 2 to print progress (but 
    % not for timings)
    codeoptions.optlevel = 3;   % 0: no optimization, 1: optimize for size,
    % 2: optimize for speed, 3: optimize for size & speed
    codeoptions.cleanup = false;
    codeoptions.timing = 1;
    % change this to your server or leave uncommented for using the 
    % standard embotech server at https://www.embotech.com/codegen
    % codeoptions.server = 'https://yourforcesserver.com:1234';
    codeoptions.nlp.ad_tool = 'casadi';

    %% Generate FORCESPRO solver
    FORCES_NLP(model, codeoptions);

end

function [ rad ] = deg2rad(deg) % convert degrees into radians
    rad = deg/180*pi;
end

function [ deg ] = rad2deg(rad) % convert radians into degrees
    deg = rad/pi*180;
end

%% Callback for plot
function [] = calculatePath(~,~,model)
% Run solver with the new clicked obstacle position and update the plots

    % read in parameters
    pos=get(gca,'CurrentPoint');
    param1=pos(1,1);
    param2=pos(1,2);
    fprintf([newline 'You clicked X: %4.4f, Y: %4.4f'],param1,param2);
    
     if ((param1 - model.xinit(1))^2 + (param2 - model.xinit(2))^2 ...
             <= model.hl(3))
        fprintf([newline ...
            'The initial position cannot lie within obstacle. ' newline]);
     elseif (param1 < model.lb(3) || param1 > model.ub(3) || ...
        param2 < model.lb(4) || param2 > model.ub(4))
        fprintf([newline 'The obstacle position you clicked is out of '...
            'bounds' newline]);
     else
        % Set runtime parameters
        params = [param1; param2];
        problem.all_parameters = repmat(params,model.N,1);

        % Set initial guess to start solver from:
        x0i=[0.0,0.0,-1.5,1.5,1.,pi/4.,0.];
        x0=repmat(x0i',model.N,1);
        problem.x0=x0; 

        % Set initial conditions. This is usually changing from problem 
        % instance to problem instance
        problem.xinit = model.xinit;
      
        % Time to solve the NLP!
        [output,exitflag,info] = FORCESNLPsolver(problem);

        % Make sure the solver has exited properly. 
        if (exitflag < 0)
            fprintf([newline 'Some error in FORCESPRO solver. exitflag=%d'...
                newline],exitflag)
        elseif (exitflag == 0)
            fprintf([newline 'Error in FORCESPRO solver: Maximum number'...
                ' of iterations was reached. ' newline]);
        else
            fprintf([newline 'FORCESPRO took %d iterations and %f seconds '...
                'to solve the problem.' newline], info.it, info.solvetime);
            % extract the output
            temp = zeros(model.nvar,model.N);
            for i=1:model.N
                temp(:,i) = output.(['x',sprintf('%02d',i)]);
            end
            u = temp(1:2,:);
            x = temp(3:7,:);
            
            % Update the plot with the new results
            updatePlots(x,u,model,params);
                        
        end
     end   
end

function [ xDot ] = continuousDynamics(x,u)
% state x = [xPos,yPos,v,theta,delta], input u = [F, phi]
    
    % set physical constants
    l_r = 0.5; % distance rear wheels to center of gravity of the car
    l_f = 0.5; % distance front wheels to center of gravity of the car
    m = 1.0;   % mass of the car
    
    % set parameters
    beta = atan(l_r/(l_f + l_r) * tan(x(5)));
    
    % calculate dx/dt
    xDot = [ x(3) * cos(x(4) + beta); % dxPos/dt = v*cos(theta+beta)
             x(3) * sin(x(4) + beta); % dyPos/dt = v*cos(theta+beta)
             u(1)/m;                  % dv/dt = F/m
             x(3)/l_r * sin(beta);    % dtheta/dt = v/l_r*sin(beta)
             u(2)];                   % ddelta/dt = phi
end

function [upperLimitSpline, lowerLimitSpline] = setupRoadLimitSplines()
% Returns two cubic splines representing the upper and lower limit
% of the road.

    xPos   = [-3.0, -2.0, -1.0, 0.0];
    yUpper = [ 0.0,  1.8,  2.5, 3.0];
    yLower = [-5.0, -2.0,  0.3, 1.5];
    
    upperLimitSpline = ForcesInterpolationFit( xPos,yUpper );
    lowerLimitSpline = ForcesInterpolationFit( xPos,yLower );
end

function [] = updatePlots(x,u,model,params)   
% Deletes old data sets in the current plot and adds the new data sets 
% given by the arguments x, u and params to the plot.
% x: matrix consisting of a set of state column vectors
% u: matrix consisting of a set of input column vectors
% params: position of obstacle
% model: model struct required for the code generation of FORCESPRO

    gcf();
            
    % plot updated acceleration force
    subplot(5,2,8);          
    oldAcc = findobj(gca,'Color','blue');
    delete(oldAcc);
    stairs(u(1,:),'b-'); grid on; 

    % plot updated steering rate
    subplot(5,2,10)
    oldPhi = findobj(gca,'Color','blue');
    delete(oldPhi);
    stairs(rad2deg(u(2,:)),'b-'); grid on;

    % plot updated velocity
    subplot(5,2,2)
    oldVel = findobj(gca,'Color','blue');
    delete(oldVel);
    plot(x(3,:),'b-'); grid on;

    % plot updated heading angle
    subplot(5,2,4) 
    oldTheta = findobj(gca,'Color','blue');
    delete(oldTheta)
    plot(rad2deg(x(4,:)),'b-'); grid on; 

    % plot updated steering angle 
    subplot(5,2,6)
    oldDelta = findobj(gca,'Color','blue');
    delete(oldDelta);
    plot(rad2deg(x(5,:)), 'b-'); grid on

    % plot updated trajectory
    subplot(5,2,[1,3,5,7,9])
    oldPath = findobj(gca,'Color','blue','LineStyle','-');
    delete(oldPath);
    plot(x(1,:),x(2,:),'b-'); hold on;
    oldObstacle = findobj(gca,'LineStyle','-.');
    delete(oldObstacle);
    rectangle('Position',[params(1)-...
        sqrt(model.hl(3)) params(2)-sqrt(model.hl(3)) ...
        2*sqrt(model.hl(3)) 2*sqrt(model.hl(3))],...
        'Curvature',[1 1],'EdgeColor','r','LineStyle','-.');
    legend('init pos','trajectory','Location','northwest');   
end

function [] = plotAndMakeInteractive(x,u,model,params)
% Creates a plot, adds the data sets provided by the arguments x, u, and
% params to the plot and makes the plot interactive by connecting a
% callback function
% x: matrix consisting of a set of state column vectors
% u: matrix consisting of a set of input column vectors
% params: position of obstacle
% model: model struct required for the code generation of FORCESPRO

    figure(1); clf;

    % plot acceleration force
    subplot(5,2,8)
    title('Acceleration force'); hold on;
    plot([1 model.N], [model.ub(1) model.ub(1)]', 'r:');
    plot([1 model.N], [model.lb(1) model.lb(1)]', 'r:');
    stairs(u(1,:),'b-');

    % plot steering rate
    subplot(5,2,10);
    title('Steering rate'); hold on;
    plot([1 model.N], rad2deg([model.ub(2) model.ub(2)]'), 'r:');
    plot([1 model.N], rad2deg([model.lb(2) model.lb(2)]'), 'r:');
    stairs(rad2deg(u(2,:)),'b-');
    
    % plot velocity
    subplot(5,2,2)
    title('Velocity'); hold on;
    plot([1 model.N], [model.ub(5) model.ub(5)]', 'r:');
    plot([1 model.N], [model.lb(5) model.lb(5)]', 'r:');
    plot(x(3,:),'b-');

    % plot heading angle
    subplot(5,2,4)
    title('Heading angle'); hold on;
    xlim([1, model.N]);
    ylim([-90,180]);
    plot(rad2deg(x(4,:)),'b-');

    % plot steering angle
    subplot(5,2,6)
    title('Steering angle'); hold on;
    plot([1 model.N], rad2deg([model.ub(7) model.ub(7)])', 'r:');
    plot([1 model.N], rad2deg([model.lb(7) model.lb(7)])', 'r:');
    plot(rad2deg(x(5,:)),'b-');

    % plot trajectory in interactive window
    subplot(5,2,[1,3,7,9])
    plot(model.xinit(1),model.xinit(2),'bx','LineWidth',3); hold on;
    
    % plot road limits
    [upperRoadLimit, lowerRoadLimit] = setupRoadLimitSplines();
    [xStart, xEnd] = getRange(upperRoadLimit);
    xPosFine = linspace( xStart,xEnd,100 );
    upperRoadLimitFine = upperRoadLimit( xPosFine );
    lowerRoadLimitFine = lowerRoadLimit( xPosFine );
    
    plot( xPosFine,upperRoadLimitFine, 'r','LineStyle',':' );
    plot( xPosFine,lowerRoadLimitFine, 'r','LineStyle',':' );
    
    plot(x(1,:),x(2,:),'b-');
    rectangle('Position',[params(1,1)- sqrt(model.hl(3))...
        params(2,1)-sqrt(model.hl(3)) 2*sqrt(model.hl(3)) ...
        2*sqrt(model.hl(3))],'Curvature',[1 1],'EdgeColor','r',...
            'LineStyle','-.');
    legend('init pos','trajectory','Location','northwest');
    title(sprintf('Click into figure to place obstacle'));
    axis equal
    xlim([model.lb(3), model.ub(3)]);
    ylim([model.lb(4),model.ub(4)]);
    xlabel('x-coordinate'); ylabel('y-coordinate');
    set(gcf,'WindowButtonDownFcn',{@calculatePath,model})
end