% Define a function named 'time_varying_LQR' which takes input 'in'
function u = time_varying_LQR(in)

    % Define constants for the cart-pole system
    m = 0.1; % Mass of the pendulum
    L = 1; % Length of the pendulum
    g = 9.81; % Acceleration due to gravity
    M = 1; % Mass of the cart
    d1 = 1; % Damping coefficient of the cart
    d2 = 0.5; % Damping coefficient of the pendulum

    % Extract state variables and control parameters from input
    dot_x = in(2); % Velocity of the cart
    theta = in(3); % Angle of the pendulum
    dot_theta = in(4); % Angular velocity of the pendulum
    x = in(1:4); % State vector
    
    Q = diag(in(5:8)); % State cost matrix
    R = in(9); % Control effort cost
    u_ctrl = in(10); % Current control input (not used in the calculation)
    Ts = in(11); % Sampling time

    % Calculate nonlinear elements of the system dynamics for linearization
    % These are elements of the Jacobian matrix derived from the nonlinear dynamics
    f_23 = (g*m*sin(theta)^2 - g*m*cos(theta)^2 + L*dot_theta^2*m*cos(theta))/(- m*cos(theta)^2 + M + m) - (2*m*cos(theta)*sin(theta)*(L*m*sin(theta)*dot_theta^2 + u_ctrl - g*m*cos(theta)*sin(theta)))/(- m*cos(theta)^2 + M + m)^2;
    f_24 = (2*L*dot_theta*m*sin(theta))/(- m*cos(theta)^2 + M + m);
    f_42 =  (d1*cos(theta))/L;
    f_43 =  -(cos(theta)*((g*m*sin(theta)^2 - g*m*cos(theta)^2 + L*dot_theta^2*m*cos(theta))/(- m*cos(theta)^2 + M + m) - (2*m*cos(theta)*sin(theta)*(L*m*sin(theta)*dot_theta^2 + u_ctrl - g*m*cos(theta)*sin(theta)))/(- m*cos(theta)^2 + M + m)^2) + sin(theta)*(d1*dot_x - (L*m*sin(theta)*dot_theta^2 + u_ctrl - g*m*cos(theta)*sin(theta))/(- m*cos(theta)^2 + M + m)) - g*cos(theta))/L;
    f_44 =  - d2 - (2*dot_theta*m*cos(theta)*sin(theta))/(- m*cos(theta)^2 + M + m);

    % Construct the linearized system dynamics matrix A
    A = [0,1,0,0 ; 0,-d1,f_23, f_24; 0,0,0,1; 0, f_42, f_43, f_44 ];

    % Construct the input matrix B, linearized around the current state
    B = [0;
        1/(- m*cos(theta)^2 + M + m);
        0 ;
        -cos(theta)/(L*(- m*cos(theta)^2 + M + m))  ]; 

    % Discretize the system for the given sampling time
    Ad = expm(A*Ts); % Discretize the state matrix A
    Bd = pinv(A)*(Ad-eye(4))*B; % Discretize the input matrix B
    
    % Solve the Discrete-time Algebraic Riccati Equation (DARE) for state-feedback gains
    [X,K_lqr_d, L] = idare(Ad,Bd,Q, R);

    % Calculate the control input based on the state-feedback law
    u = -K_lqr_d*x; % Negative feedback of the state times the LQR gain

end
% End of the time_varying_LQR function