function  [opt, fall] = htopy(fu, svars, prex0, upperS, numchecks, box, Hchoice, ODEchoice)
% htopy2 computes an approximate solution to the system of equations fu(x)=0.
% It uses an homotopy represented by H(x,t)=0, with known solution for x at t=0, and 
% with H(x,1)=fu(x)=0, so any x satisfying (approx) H(x,1) is an (approx) solution to
% fu(x), the system of interest.
% This implementation continously deforms H(x,0) into a sequence of H(x,t) by making 
% x and t functions of another variable 's' (thus, x becomes x(s) and t becomes t(s)),
% and solves a sequence of systems of ODE in order to 'track' the path defined by H(x,t).
% For more explanations about this procedure and formulas involved, see Judd 1998, pages 
% 179 to 187)
%
% OUTPUT:
%
% fall: fall = 1 only if the function succeeds. Success means: 1) a candidate solution was found 
% (i.e. a t close enough to 1 is found), AND 2) the candidate solution lies inside the
% bounds for x specified by the user (in 'box'). In any other case the function fails
% and fall = 0. This is useful to instruct other functions not to use the value in opt
% if fall is not 1.
%
% opt: if the function succeeds, opt is a vector of values x
% that makes fu(x) approx. zero. If fails because the candidate solution lies 
% outside the box, then it returns and empty array []. If it fails because no 
% candidate solution was found it ret)urns the values of x and t for the last
% iteration.
%
% INPUTS:
%
% fu: a cell array of strings representing a system of equations
% (it can be just a single equation) in the form f(x)=0
%
% svars : a cell array of strings containig the names of the variables included
% in fu (in particular, do not include t or s here)
%
% prex0 : a vector with the initial conditions for variables in svars
%
% upperS : this scalar is the upper bound used in the ODE part for s. You can set upperS
% relatively large and insert many check points between 0 and upperS. The function
% will exit as soon as one candidate solution has been found in any of the 
% intervals defined by the checkpoints [0,s1,s2....upperS]. That can happen well
% before s is close to upperS.
% 
% numchecks: A scalar. Is the number of intermediate checks s1,s2,s3... between 0
% and upperS. The fucntion will divide [0,upperS] in (numchecks+1) intervals of equal
% length
%
% box: an nx2 matrix. Each rows represent one variable in svars. The first column 
% holds the lower bound for that variable and the second column contains the upper bound.
%
% Hchoice: a string. If it takes the value 'FP' then the (1-t)(x-x0) is
% done. If it takes the value 'N', it implements a Newton type homotopy (see page 179) 
%
% ODEchoice: a string. If it takes the value 'ode45' then the solver ode45
% (runge-kutta, recommended choice for nonstiff problems) is used.  If it takes the
%  value 'ode15s', a BDF-type solver is used. This appropiate for stiff
%  problem and when precision is less of a concern



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%
%%% Step 0: set tolerance parameters and default values
%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% this is our max acceptable deviation from H(x,t) = 0, we make this check
%%% at the end of each subinterval [s_{i} , s_{i+1}]
Htol = 1e-4;

%%% we accept t as close enough to 1 if t \in (1-epsilont , 1+epsilont)
epsilont=1e-1;

%% set the failure flag to 0, do not change to success unless on sol is found
fall=0;

%% set a void value for the solution as the default in case of failure 
opt=[];




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%
%%%  Step 1: symbolize relevant variables and set the homotopy H
%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% simbolize s and t and some otehr symbols we may need
syms t s pi e a;

%%% augment the vector of initial conditions -prex0-, by adding the initial condition for t 
%%% (zero) at the end. The augmented vector is x0
x0= [prex0, 0];

numvar = size(svars,2);
numfun = size(fu,2);

%% check your system is square

if numfun ~= numvar
    'the number of variables is not equal to the number of equations'
    return;
end

%% initialize these to be symbolic but empty
allvars = sym([]);
allfun = sym([]);

for i=1:numvar
    allvars = [allvars, sym(svars{i})];
    allfun = [allfun, sym(fu{i})];
end
%% now allvars contains all variables in symbolic form
%% and allfun has all equations in fu but with symbolic status

%%% Define your Homotopy
if Hchoice=='FP'
    % this is the fixed point homotopy
    H = (1-t)*(allvars-prex0) + t*allfun
elseif Hchoice=='N'
    % Newton's homotopy
    f0=allfun;
    allfun
    x0
    for a=1:size(allvars,2)
    f0=subs(f0,{allvars(a)},{x0(a)});
    end
    f0
    H=allfun-(1-t)*f0
else 
   display('Error: no Homotopy was selected or you typed an incorrect name')
   return;
end
    



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%
%%%  Step 2: Construct the system of ODEs (see eq. 5.9.4 , page 182)
%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%% Step 2.1: obtaing the RHS of the system

%% append symbolic t to end of list of symbolic x
allvars = [allvars, t];

%% compute the symbolic Jacobian of H w.r.t. x and t
J=jacobian(H,allvars);

%% form the Right Hand Side of the ODE system
allRHS = [];
for i = 1:numvar+1
     subJ = J;
     %% exclude the i-th column of the Jacobian before taking Determinant
     subJ(:,i)=[];
     
     %% concatenate each eq vertically, do the sign switch
     allRHS=[allRHS; (-1)^i * (det(subJ))];           
 end

%% display the RHS of the ODE system 

if mod(numvar,2)==0 
    allRHS=(-1)*allRHS;
end


%% now we change the notation of the symbolic expressions that appear 
%% in allRHS. The reason is to get an expression that, once transformed
%% into a string (see step 2.2) will be usable as an inline function 
%% accepted as input by fsolve (see step 2.3). 

%% the first step to change the notation of allRHS is to create a row 
%% vector y=[y(1), y(2), ..., y(numvar+1)], where numvar was defined in
%% step zero and corresponds to allvars-1.
yy=sym([]);
for j=1:size(allvars,2) 
    sj = num2str(j);
    name = strcat('y(',sj,')');
    yy(j)=name;
end
y=yy;

%% the second step in changing the notation is to sobstitute the original 
%% variables given in allvars with the y(i) ones we constructed above. 
[m1,n1]=size(allRHS);
B=jacobian(allRHS,allvars);
 for j=1:m1
     for i=1:m1
        if B(j,i)==0
            allRHSs(j)=allRHS(j);
        else 
        allRHSs(j)=subs(allRHS(j),{allvars(i)},{y(i)});
        end
      allRHS(j)=allRHSs(j);             
     end
 end
 allRHS

%%%%%%% Step 2.2
%%%%%%% change the symbolic object to string object, so you can use inline functions

newstring = '[';
for i=1:size(allRHS,1)
for j=1:1
newstring = [newstring char(allRHS(i,j)) ' '];
end
newstring = [newstring ' ;  '];


end
newstring = newstring(1:end-3); %remove trailing punctuation
newstring = strcat(newstring, ']');


%%%%%%% Step 2.3 :
%%%%%%% create an inline function to make RHS usable with Matlab's ODE-functions

f=inline(newstring,'s','y');





%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%
%%%  Step 3: Do the path-tracking via ODE-solving
%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% these points define a uniform grid on [0,upperS] 
checkpoints = linspace(0,1,numchecks+1)*upperS;

%% container for solutions passed by the ODE solver
pastedvalues = [];
pastedtime =[];

    
%% Htest will be used to asses value of H at the checkpoints. For now
%% it is a symbolic object but it will be converted to a number once we 
%% have computed the values of t and the svars at which we want to compute
%% the value of H.
Htest = H;

k=0;
for j=1:numchecks
    j   
    if (ODEchoice=='ode45')
        [timepoints, varvalues] = ode45(f,[checkpoints(j),checkpoints(j+1)],x0);
    elseif (ODEchoice=='ode15')
        [timepoints, varvalues] = ode15s(f,[checkpoints(j),checkpoints(j+1)],x0);
    else H=h;
        display('Error: no ode-solver is being called or you typed an incorrect name')
        return;
    end
    
    %% store the last chunk of x's t and s
    pastedvalues = [pastedvalues; varvalues(2:end,:)];
    pastedtime = [pastedtime; timepoints(2:end)];
    
    %% update the initial conditions for x and t, to be used in the next iteration
    x0 = varvalues(end,:);
    
    %% find the value of t closest to 1 for this [s_i,s_{i+1}] interval 
    testvec =abs(varvalues(:,end) -1);
    mintestvec= min(testvec);
    k = k+size(testvec,1)-1;
    
    %% test if our 'best' t is close enough to 1, if it's not, just go on
    if (abs(mintestvec) < epsilont) 
        %% if t is accepted then report a candidate solution
        z = min(testvec);
        i=1;
        while (testvec(i) ~= z)
            i = i+1;
        end
        %% opt does not include tH=h;
        %% set opt equal to your candidate solution
        opt = varvalues(i,1:end-1);
        
        %% test if your candidate solution lies inside the box for x or not
        if (opt >= box(:,2)') | (opt <= box(:,1)')
            display ('Out of bounds')
            out_of_bound_guess=opt
            %% your solution is out of the permisible region
            %% exit the function, do not report fall=1
            return;
        end
        %% your solution passed the test, report success
        fall=1;

        %% in the case of success the program will create plots for each of
        %% the original variables. Each plot has t on the vertical axis and
        %% one of the independent variables on the orizontal axis.
        if m1 == 3
            X1=pastedvalues(1:(k-size(testvec,1)+i),1);
            Y1=pastedvalues(1:(k-size(testvec,1)+i),2);
            Z1=pastedvalues(1:(k-size(testvec,1)+i),3);
            plot3(X1,Y1,Z1)
            grid on
            xlabel('x')
            ylabel('q')
            zlabel('t')
            title('3D Plot of the solution')
        else
            for l=1:m1-1
             h(l)=subplot(m1-1,1,l); plot(pastedvalues(1:(k-size(testvec,1)+i),l)',pastedvalues(1:(k-size(testvec,1)+i),end)')   
             xlabel('independent variable')
             ylabel('t')
             axis([h(l)],'tight')
            end
        end
         return
    end     

    %% A cnadidate solution was not found on this subinterval. Before
    %% checking the next subinterval (that is, going to the next j) we
    %% assess if our new guess is such to satisfy the requirement that 
    %% H is equal to zero along all the path.
    %% Tah is, we check taht H(x0,t)=0, where x0 is the last value of
    %% varvalues.
    for a=1:size(allvars,2)
        Htest=subs(Htest,{allvars(a)},{x0(a)});
    end
    Htest
    if (abs(Htest)/(1+norm(x0)) > Htol) 
        %% if we are off-path then use a solver to correct x, fixing t
        display('Warning: off-the path. The Htol loop has been activated to correct the initial guess for the next subinterval');
        
       % Now the program uses fsolve to produce a new vector of initial 
       % guesses for the original independent variables, given of t which 
       % we get at the end of the subinterval. 
       % To produce the new guess we proceed in three parts:
       
       %% PART I: H is evaluated at the last value of t and then the
       %% notation is changed so to get an expression that, once 
       %% transformed into a string (see PART II) will be usable as an 
       %% inline function accepted as input by fsolve (see PART III).
       [m2,n2]=size(H);
       y(n2+1)=x0(n2+1);
       B2=jacobian(H,allvars);
       H2=H;
       for u=1:n2
            for v=1:m1
                if J(u,v)==0
                    newHs(u)=H2(u);
                else
                    newHs(u)=subs(H2(u),{allvars(v)},{y(v)});
                end
                H2(u)=newHs(u);
            end
        end
        newH=sym([]);
        r0=zeros(n2,1);
        for r=1:n2
            newH=[newH;H2(r)];   
            r0(r)=x0(r);
        end
        newH % shows H(y(1),...,y(numvar),t*). Where t* is the last value 
             % of t computed by the ODE solver for the subinterval at hand.
        
        % PART II: then we need to make newH usable with fsolve. We make it a
        % string.
        newstring2 = '[';
        for c=1:size(newH,1)
            for w=1:1
                newstring2 = [newstring2 char(newH(c,w)) ' '];
            end
            newstring2 = [newstring2 ' ;  '];
        end
        newstring2 = newstring2(1:end-3); %remove trailing punctuation
        newstring2 = strcat(newstring2, ']');
        
        %% PART III: we use the above string into an inline function
        %% that becomes the input of fsolve together with an initial 
        %% guess for the original independent variables given by the last
        %% values generated for them by the ODE solver.
        %% The new guess we generate through fsolve is called "newx".
        f2=inline(newstring2,'y')        
        newx=fsolve(f2,r0)
        
        %% Then we also implement a check to see how well we are on the path 
        %% with this new values of the independent variables and the value
        %% of that ends the subinterval. That is we evaluate H at
        %% (newx,t*). We call "newHtest" the value that we compute and that
        %% we will compare to Htol.
        x02=[(newx)',x0(n2+1)]
        newHtest=H;
        for a=1:size(allvars,2)
        newHtest=subs(newHtest,{allvars(a)},{x02(a)});
        end
        newHtest 
        if abs(Htest)<abs(newHtest)
            display('Matlab equation solver gave a worse x than the ODE solver')           
            return;
        elseif (abs(newHtest)/(1+norm(x0)) > Htol)
            display('Matlab equation solver gave a result such that H is far from zero. Check Htol')           
            return;
        else
            x0=x02
        end % so either we start the check on the next subinterval with an 
            % initial guess so that H is close enough to zero or we get one
            % of the two error messages above and the program stops.
    end %% this complete the loop for a single subinterval    
end %% all the subintervals in [0, upperS] have been examined

%% if you made it up to this point without exit the function, it means that you never found
%% a candidate solution 

display ('Warning: t did not get close enough to 1: increase upperS or refine stepsize for t')