function Rhom_tp = priv_dependentHomSol(obj,Rinit,Uconst)
% priv_dependentHomSol - computes the homogeneous solution when the parameters
%    of the system matrix and the input are dependent
%
% Syntax:
%    Rhom_tp = priv_dependentHomSol(obj,Rinit,Uconst)
%
% Inputs:
%    obj - linParamSys object 
%    Rinit - initial reachable set
%    Uconst - set of constant inputs
%
% Outputs:
%    Rhom_tp - homogeneous reachable set
%
% Example:
%    -
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: plus

% Authors:       Matthias Althoff
% Written:       02-June-2011
% Last update:   25-August-2011
%                26-April-2024 (TL, adaptations new matZonotope class)
% Last revision: ---

% ------------------------------ BEGIN CODE -------------------------------

%obtain required variables
Ac = obj.A.C;
Ag = obj.A.G;
c = center(Rinit);
r = obj.stepSize;
n = obj.nrOfDims;
Uc = Uconst.c;
UG = Uconst.G;
params = obj.A.numgens;%'obj.A.gens' does not work anymore once lin_error2dAB adds lagrange remainder to system matrix

%SECOND ORDER DEPENDENT SOLUTION
%zero parametric order
R_c = c + Uc*r;
for i=1:2
    R_c = R_c + Ac^i*r^i/factorial(i)*(c + Uc*r/(i+1));
end

%first parametric order
%auxiliary value
M = eye(n)*r + Ac*r^2/2 + Ac^2*r^3/6;
%loop
for i=1:params
    R_g(:,i) = (Ag(:,:,i)*r + Ac*Ag(:,:,i)*r^2/2 + Ag(:,:,i)*Ac*r^2/2) * c + ...
               (Ag(:,:,i)*r^2/2 + Ac*Ag(:,:,i)*r^3/6 + Ag(:,:,i)*Ac*r^3/6) * Uc + ...
               M * UG(:,i);
end

%second parametric order
%same index (i,i)
for i=1:params
    Rtmp = Ag(:,:,i)^2*r^2/2*c + ...
           Ag(:,:,i)*r^3/6*Uc + ...
           (Ac*Ag(:,:,i) + Ag(:,:,i)*Ac)*r^3/6*UG(:,i);
    R_g(:,end+1) = 0.5*Rtmp;
    R_c = R_c + 0.5*Rtmp;
end
%different index
if (params>=2)
    ind = combinator(params,2,'c');
    for i=1:length(ind(:,1))
        Atmp = Ag(:,:,ind(i,1))*Ag(:,:,ind(i,2)) + Ag(:,:,ind(i,2))*Ag(:,:,ind(i,1));
        R_g(:,end+1) = Atmp*r^2/2*c + ...
                       Atmp*r^3/6*Uc + ...
                       (Ac*Ag(:,:,ind(i,1)) + Ag(:,:,ind(i,1))*Ac)*r^3/6*UG(:,ind(i,2));
    end
end
%obatin zonotope
R_lowOrder = zonotope(R_c,R_g);

%HIGHER ORDER INDEPENDENT SOLUTION
%get state transition matrices
eZhigh = obj.mappingMatrixSet.highOrderZono;
eIhigh = obj.mappingMatrixSet.highOrderInt;
%get input transition matrices
eZhigh_input = obj.mappingMatrixSet.highOrderZonoInput;
eIhigh_input = obj.mappingMatrixSet.highOrderIntInput;

%remaining reachable set
R_rem_state = eZhigh*zonotope(c) + eIhigh*zonotope(c);
R_rem_input = eZhigh_input*Uconst + eIhigh_input*Uconst;
R_rem = R_rem_state + R_rem_input;

%STATE SOLUTION WITHOUT CENTER
Rinit_noCenter = Rinit + (-c);
R_hom_state = obj.mappingMatrixSet.zono*Rinit_noCenter + obj.mappingMatrixSet.int*Rinit_noCenter;

%FINAL SOLUTION
Rhom_tp = R_hom_state + R_lowOrder + R_rem;

% ------------------------------ END OF CODE ------------------------------
