%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
    traj_feas = true;
    % timesteps = params.model.TimeStep;
    size_u = size(u);
    timesteps = size_u(end);
    s_max = max(max(abs(x(1,:))),max(abs(x(3,:))));
    y_max = max(abs(x(2,:)));
    u_max = max(abs(u));
    J_u = 0;
    % fprintf("timestep: %s\n\n\n",timesteps);
%     fprintf("u shape: %s\n\n\n",size(u));
    % display(size(x))
    for k = 1:timesteps
        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));
    if s_max > params.constraints.MaxAbsPositionXZ
        traj_feas = false;
    end
    if y_max > params.constraints.MaxAbsPositionY
        traj_feas = false;
    end 
    if u_max > params.constraints.MaxAbsThrust
        traj_feas = false;
    end
    if df_max > params.constraints.MaxFinalPosDiff
        traj_feas = false;
    end
    if vf_max > params.constraints.MaxFinalVelDiff
        traj_feas = false;
    end
end