%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich.
%
% All rights reserved.
%
% Please see the LICENSE file that has been included as part of this package.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = traj_constraints(x,u,params)
    % YOUR CODE HERE

    s_max = max(max(abs(x([1,3],:))));
    y_max = max(abs(x(2,:)));
    u_max = max(max(abs(u)));

    J_u = 0;
    for k=1:size(u,2)
        J_u = J_u + u(:,k)' * u(:,k);
    end

    df_max = sqrt(sum(x(1:3,end).^2));
    vf_max = sqrt(sum(x(4:6,end).^2));

    constr = [
        s_max  <= params.constraints.MaxAbsPositionXZ, ...
        y_max  <= params.constraints.MaxAbsPositionY, ...
        u_max  <= params.constraints.MaxAbsThrust, ...
        df_max <= params.constraints.MaxFinalPosDiff, ...
        vf_max <= params.constraints.MaxFinalVelDiff
    ];
    
    % disp([s_max, y_max, df_max, vf_max])
    % disp([params.constraints.MaxAbsPositionXZ, ...
    %       params.constraints.MaxAbsPositionY, ...
    %       params.constraints.MaxFinalPosDiff, ...
    %       params.constraints.MaxFinalVelDiff])
    % disp(constr')
    traj_feas = all(constr);
end