function [u_r,v_r,psi_desired,err_s,err_e] = fcn(North, East, u, v,ret,psi,ret_off)
%#codegen

persistent sett index WP WP_arc WP_psi WP_index alfa R_p epsilon s e chi_p chi_r  vq_x vq_y
persistent vq_x_flipped vq_y_flipped prev_alfa alfa_index prev_psi psi_index
u_max = 4.3;
v_max = 3.2;
delta_s = 15;
delta_e = 15;
delta = 20;

% Check if first iteration and if return is not activated
if isempty(WP) || ret_off
    WP = zeros(2,100);
    WP_arc = zeros(1,100);
    WP_psi = zeros(1,100);
    WP_index = 1;
    WP(1,WP_index) = North;
    WP(2,WP_index) = East;
    WP_arc(1,WP_index) = 0;                 % Preallocate arrays
    WP_psi(1,WP_index) = psi;
    index = 1;
    sett =1;
    vq_x_flipped = zeros(1,200);
    vq_y_flipped = zeros(1,200);
    prev_alfa = zeros(1,10000);
    alfa_index = 1;
    prev_psi = zeros(1,10000);
    psi_index = 1;
end

if ret% If return button is pressed, generate the return path and plot it to visualize
    if WP_index > 1
        if sett == 1            
            close all
            xq  = 0:1:200;          
            vq_x = interp1(WP_arc(1,1:WP_index),WP(1,1:WP_index),xq);
            vq_y = interp1(WP_arc(1,1:WP_index),WP(2,1:WP_index),xq);
            vq_x_pick = ~isnan(vq_x);
            vq_x_fix = vq_x(vq_x_pick);
            vq_x_flipped = fliplr(vq_x_fix);
            vq_y_pick = ~isnan(vq_y);
            vq_y_fix = vq_y(vq_y_pick);
            vq_y_flipped = fliplr(vq_y_fix);
            figure(1)
            plot(vq_y,vq_x,WP(2,1:WP_index),WP(1,1:WP_index),'o');           
            axis equal;
            grid;
            sett = 0;
        end

%% NOT NORMAL MODE  Tar hensyn til at ROVen kan kjøre i sirkel.. Funker ikke optimalt
%       if WP(1,WP_index)-WP(1,WP_index-1) > WP(2,WP_index)-WP(2,WP_index-1)
%         if WP(1,WP_index)-WP(1,WP_index-1) < 0 && WP(2,WP_index)-WP(2,WP_index-1) < 0 && abs(psi) > (pi/2);
%             alfa = atan2(WP(2,WP_index)-WP(2,WP_index-1),WP(1,WP_index)-WP(1,WP_index-1));
%             alfa_index = alfa_index +1;
%         elseif WP(1,WP_index)-WP(1,WP_index-1) < 0 && WP(2,WP_index)-WP(2,WP_index-1) > 0 && abs(psi) > (pi/2);
%             alfa = atan2((WP(2,WP_index)-WP(2,WP_index-1)),WP(1,WP_index)-WP(1,WP_index-1))-2*pi;
%             alfa_index = alfa_index +1;
%         elseif WP(1,WP_index)-WP(1,WP_index-1) < 0 && WP(2,WP_index)-WP(2,WP_index-1) < 0 && abs(psi) <= (pi/2);
%             alfa = atan2(WP(2,WP_index-1)-WP(2,WP_index),WP(1,WP_index-1)-WP(1,WP_index));
%             alfa_index = alfa_index +1;
%         elseif WP(1,WP_index)-WP(1,WP_index-1) < 0 && WP(2,WP_index)-WP(2,WP_index-1) >= 0 && abs(psi) <= (pi/2);
%             alfa = atan2(WP(2,WP_index-1)-WP(2,WP_index),WP(1,WP_index-1)-WP(1,WP_index));
%             alfa_index = alfa_index +1;
%         else
%             alfa = atan2(WP(2,WP_index)-WP(2,WP_index-1),WP(1,WP_index)-WP(1,WP_index-1));
%             alfa_index = alfa_index +1;
%         end
%       else
%         alfa = pi/2 - atan2(WP(2,WP_index)-WP(2,WP_index-1),WP(1,WP_index)-WP(1,WP_index-1));
%         alfa_index = alfa_index +1;
%       end
%% NORMAL MODE  Fungerer for straight paths og s-shaped paths
        if WP(1,WP_index)-WP(1,WP_index-1) < 0 && abs(psi) > (pi/2);
            alfa = atan2(WP(2,WP_index)-WP(2,WP_index-1),WP(1,WP_index)-WP(1,WP_index-1));
        elseif WP(1,WP_index)-WP(1,WP_index-1) < 0 && abs(psi) <= (pi/2);
            alfa = atan2(WP(2,WP_index-1)-WP(2,WP_index),WP(1,WP_index-1)-WP(1,WP_index));
        else
            alfa = atan2(WP(2,WP_index)-WP(2,WP_index-1),WP(1,WP_index)-WP(1,WP_index-1));
        end
%%
        R_p = [cos(alfa) -sin(alfa);sin(alfa) cos(alfa)];        
        epsilon = R_p'*[North - WP(1,WP_index);East - WP(2,WP_index)];
        s = epsilon(1);
        e = epsilon(2);
        chi_p = alfa;
        chi_r = atan(-e/delta);
        U = sqrt(u^2+v^2);
        beta = asin(v/U);
        psi_desired = chi_p + chi_r;%-beta;
        psi_index = psi_index + 1;
        prev_psi(1,psi_index) = psi_desired;
        if psi_index >= 2 % Korrigerer heading hvis nødvendig
            if prev_psi(1,psi_index)-prev_psi(1,psi_index-1) <= -pi
                psi_desired = 2*pi - psi_desired;
            elseif prev_psi(1,psi_index)-prev_psi(1,psi_index-1) > pi
                psi_desired = -2*pi + psi_desired;
            end
        end
        err_s = s;
        err_e = e;
        u = -u_max*(s/sqrt(s^2+delta_s^2));
        v_r = -v_max*(e/sqrt(e^2+delta_e^2));
        u_r = sign(u)*0.5; % Set constant speed to 0.5m/s in correct direction
        
        if abs(WP(2,WP_index)-East) < 0.5 && abs(WP(1,WP_index)-North) < 0.5
            WP_index = WP_index - 1;
            index = index + 1;
        end
    else
        alfa = 0;
        alfa_index = alfa_index +1;
        prev_alfa(1,alfa_index) = alfa;
        if alfa_index > 2
            if prev_alfa(1,alfa_index)-prev_alfa(1,alfa_index-1) > pi;
                alfa = alfa - 2*pi;
                alfa_index = alfa_index+1;
                prev_alfa(1,alfa_index)=alfa;
            end
        end
        R_p = [cos(alfa) -sin(alfa);sin(alfa) cos(alfa)];
        epsilon = R_p'*[North-WP(1,1);East-WP(2,1)];
        s = epsilon(1);
        e = epsilon(2);
        chi_p = alfa;
        chi_r = atan(-e/delta);
        psi_desired = chi_p + chi_r;
        psi_index = psi_index + 1;
        prev_psi(1,psi_index) = psi_desired;
        if psi_index > 2
            if prev_psi(1,psi_index)-prev_psi(1,psi_index-1) <= -pi
                psi_desired = 2*pi - psi_desired;
            elseif prev_psi(1,psi_index)-prev_psi(1,psi_index-1) > pi
                psi_desired = -2*pi + psi_desired;
            end
        end
        u_r = -u_max*(s/sqrt(s^2+delta_s^2));
        v_r = -v_max*(e/sqrt(e^2+delta_e^2));
        err_s = s;
        err_e = e;
    end
else
    if sqrt((WP(1,WP_index)-North)^2+(WP(2,WP_index)-East)^2) >= 3
        WP_index = WP_index + 1;
        WP(1,WP_index) = North;
        WP(2,WP_index) = East;
        WP_psi(1,WP_index) = psi;
    end
    
    u_r = u;
    v_r = v;
    psi_desired = psi;
    err_s = 0;
    err_e = 0;
end
end