%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 [tuning_struct, i_opt] = lqr_tuning(x0,Q,params)
    % YOUR CODE HERE
    i_opt = nan;
    best_J_u = inf;

    tuning_struct = repmat(struct())
    for i=1:size(Q,2)
        ts = struct()

        ts.InitialCondition = x0(:,i);
        ts.Qdiag = Q(:,i);

        ctrl = LQR(diag(Q(:,i)), eye(params.model.nu), params);
        [u, ~] = ctrl.eval(x0(:,i))

        [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = ...
            traj_constraints(x0(:,i), u, params)

        ts.MaxAbsPositionXZ = s_max;
        ts.MaxAbsPositionY = y_max;
        ts.MaxAbsThrust = u_max;
        ts.InputCost = J_u;
        ts.MaxFinalPosDiff = df_max;
        ts.MaxFinalVelDiff = vf_max;
        ts.TrajFeasible = traj_feas;

        tuning_struct(i) = ts;

        if ts.TrajFeasible
            if ts.InputCost < best_J_u
                i_opt = i;
                best_J_u = ts.InputCost;
            end
        end
    end
end