classdef mpc_follow_functions
    methods(Static)
        function [u_max, u_min] = formulate_input_con(vel_max,vel_min,w_max,w_min,np,nu)
            u_max = zeros(nu*np,1);
            u_min = zeros(nu*np,1);
            j = 1;
            while j <= nu*np
                u_max(j,1) = vel_max;
                u_max(j+1,1) = w_max;
                u_min(j,1) = vel_min;
                u_min(j+1,1) = w_min;
                j = j + nu;
            end
        end
        %the v_ref will be kept the same for all trajectory 
        function [r_val,r_phi,v_ref] = obtain_reference(x_path,y_path,theta_path,x_robot,y_robot,np,nx,k)
            r_val = zeros(np*nx,1);
            r_phi = zeros(np,1);
            r_val(1:3:end)=x_path(k:(k+np-1))';
            r_val(2:3:end)=y_path(k:(k+np-1))';
            r_val(3:3:end)=theta_path(k:(k+np-1))';
            r_phi(:)=theta_path(k:(k+np-1));
            v_ref = 0.09;
%             I = find(x_path >= x_robot + 0.1);
%             if isempty(I) == 1
%                index_ref = length(x_path);
%             else
%                index_ref = I(1); 
%             end
%             if index_ref + np > length(x_path)
%                 index_ref = length(x_path) - np;
%             end
%             r_val = zeros(np*nx,1);
%             r_phi = zeros(np,1);
%             i = index_ref;
%             j = 1;
%             k = 1;
%             while j <= np*nx
%                 r_val(j,1) = x_path(i);
%                 r_val(j+1,1) = y_path(i);
%                 r_val(j+2,1) = theta_path(i);
%                 r_phi(k,1) = theta_path(i);
%                 k = k + 1;
%                 i = i + 1;
%                 j = j + nx;
%             end
%             v_ref = 0.1;
        end
        function [H,f] = grad_hess(Q,R,x_state,Ad,Bd,Cd,r_phi,r_val,v_ref,np,nx,nu,Ts)
            rr = x_state - r_val(1:3,1);    
            [A,B] = mpc_follow_functions.cal_AB(Ad,Bd,r_phi,v_ref,np,nx,nu,Ts);
            H = 2*(B'*Q*B+R);
            f = 2*(B'*Q*A*(rr));
        end
        function [A,B] = cal_AB(Ad,Bd,r_phi,v_ref,np,nx,nu,Ts)
            %for calculating the A matrix 
            A_val = mpc_follow_functions.formulate_A(r_phi,v_ref,Ts,1);
            row_A = A_val;
            A = [row_A];
            for i = 2:np
                A_val = mpc_follow_functions.formulate_A(r_phi,v_ref,Ts,i);
                row_A = A_val*row_A;
                A = [A; row_A];
            end

            %for calculating the B matrix 
            for i = 1:np
                for j = np:-1:1
                    if j == np
                        if i == np
                            B_val = mpc_follow_functions.formulate_B(r_phi,Ts,np);
                            row = B_val;
                        else
                            row = zeros(size(Bd));
                        end
                    else
                        if i < j
                            row = [zeros(size(Bd)) row];
                        else
                            B_val = mpc_follow_functions.formulate_B(r_phi,Ts,j);
                            element = B_val;
                            k = j;
                            for z = 1:i-j
                                A_val = mpc_follow_functions.formulate_A(r_phi,v_ref,Ts,k);
                                element = A_val*element;
                                k = k + 1;
                            end
                            row = [element row];
                        end
                    end
                end
                if i == 1
                    B = row;
                else
                    B = [B;row];
                end
            end
        end

        %calculate the A matrix in respect to the reference at k = ref_val
        function [A_val] = formulate_A(r_phi,v_ref,Ts,ref_val)
            v_d = v_ref;
            phi_d = r_phi(ref_val);
            A_val = [1 0 -v_d*sin(phi_d)*Ts; 0 1 v_d*cos(phi_d)*Ts; 0 0 1];
        end

        %calculate the B matrix in respect to the reference at k = ref_val
        function [B_val] = formulate_B(r_phi,Ts,ref_val)
            phi_d = r_phi(ref_val);
            B_val = [cos(phi_d)*Ts 0; sin(phi_d)*Ts 0; 0 Ts];
        end
        %to plot the current position of the robot
        function [vx,vy] = pmark(x,y,theta,s)

            R = [cos(theta) -sin(theta);sin(theta) cos(theta)];

            v(:,1)= [x;y] +s*R*[1; 0];
            v(:,2)= [x;y] +s*R*[-1.5; -1];
            v(:,3)= [x;y] +s*R*[-1; 0];
            v(:,4)= [x;y] +s*R*[-1.5; 1];

            vx=v(1,:);
            vy=v(2,:);
        end
    end
end