% --- NUMERICAL REPRODUCIBILITY NOTE ---
% This code is based on numerical experiments conducted using MATLAB 9.13.0.
clc
clear
clear global
row_index = 0;
global n l m p q lb_y ub_y c_1 c_2 A_1 b_1 H d_2 A_2 B_2 b_2 A_3 B_3 b_3 r G d_4 b_4
for k = 1:50 
    row_index = row_index + 1;
    rng(k,'twister'); 
    run('problem.m');    
    tic
    epsilon_Inf = 1e-5;
    epsilon_SQP = 1e-16;
    c0=zeros(n+4*m+p+q+1,1);
    c1=zeros(n+4*m+p+q+1,1);
	
    [x0,~,exit1]=linprog([],A_1, b_1);
    if exit1<=0
       c0(1:n)=zeros(n,1);  
    else
       c0(1:n)=x0;  
    end
    options = optimoptions('fmincon','Algorithm','sqp','TolFun',epsilon_SQP);
    [y0,~,exit2,~,lambda2]=fmincon('fun_lower',zeros(m,1),full(B_2), full(b_2-A_2*c0(1:n)), full(B_3), full(b_3-A_3*c0(1:n)),lb_y,ub_y,'constraints_lower',options);
	if exit2<=0
       c0(n+1:n+4*m+p+q+1)=zeros(4*m+p+q+1,1);  
    else
       c0(n+1:n+m)=y0;
       c0(n+m+1:n+2*m)=y0;
       c0(n+2*m+1:n+2*m+p)=lambda2.ineqlin;
       c0(n+2*m+p+1:n+3*m+p)=lambda2.upper;
       c0(n+3*m+p+1:n+4*m+p)=lambda2.lower;
       c0(n+4*m+p+1:n+4*m+p+q)=lambda2.eqlin;
	   c0(n+4*m+p+q+1)=lambda2.ineqnonlin;
    end
	
	
    [x,ObjVal,exit_F]=fmincon('fun',c0,[],[],[],[],[],[],'constraints_TMDP',options);
    loweropti=0.5*x(n+1:n+m)'*H*x(n+1:n+m)+d_2'*x(n+1:n+m);
    [~,h,exit3]=fmincon('fun_lower',zeros(m,1),full(B_2), full(b_2-A_2*x(1:n)), full(B_3), full(b_3-A_3*x(1:n)),lb_y,ub_y,'constraints_lower',options);
    if exit3<=0
       a=100;
    else
       a=abs(loweropti-h);
    end
    uppercon=max(0,A_1*x(1:n)-b_1);
    lowerconin=max(0,A_2*x(1:n)+B_2*x(n+1:n+m)-b_2);
    lowerconeq=abs(A_3*x(1:n)+B_3*x(n+1:n+m)-b_3);
    lowerconqua=max(0,0.5*x(n+1:n+m)'*G*x(n+1:n+m)+(d_4)'*x(n+1:n+m)-b_4);
    upperbound_y=max(0,x(n+1:n+m)-ub_y);
    lowerbound_y=max(0,lb_y-x(n+1:n+m));
    Infeasibility_b=norm(uppercon)+norm(lowerconin)+norm(lowerconeq)+norm(lowerconqua)+norm(upperbound_y)+norm(lowerbound_y)+a;
	Inf_Lobj_b=a;
    Inf_Con_b=norm(uppercon)+norm(lowerconin)+norm(lowerconeq)+norm(lowerconqua)+norm(upperbound_y)+norm(lowerbound_y);
	
    if Infeasibility_b<=epsilon_Inf
       Infeasibility_a=Infeasibility_b;
       Inf_Lobj_a=Inf_Lobj_b;
       Inf_Con_a=Inf_Con_b;
    else
       r=x(1:n);
       [x1,~,exit4]=fmincon('fun_proj',zeros(n,1),full(A_1), full(b_1),[],[],[],[],[],options);
       if exit4<=0                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    
          Infeasibility_a=Infeasibility_b;
          Inf_Lobj_a=Inf_Lobj_b;
          Inf_Con_a=Inf_Con_b;
	   else
	      c1(1:n)=x1;
          [y1,~,exit5]=fmincon('fun_lower',zeros(m,1),full(B_2), full(b_2-A_2*c1(1:n)), full(B_3), full(b_3-A_3*c1(1:n)),lb_y,ub_y,'constraints_lower',options);
          if exit5<=0                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  
             Infeasibility_a=Infeasibility_b;
             Inf_Lobj_a=Inf_Lobj_b;
             Inf_Con_a=Inf_Con_b;
          else
             a=0;
             x(1:n)=c1(1:n);
             x(n+1:n+m)=y1;
             ObjVal=c_1'*x(1:n)+c_2'*x(n+1:n+m);
             uppercon=max(0,A_1*x(1:n)-b_1);
             lowerconin=max(0,A_2*x(1:n)+B_2*x(n+1:n+m)-b_2);
             lowerconeq=abs(A_3*x(1:n)+B_3*x(n+1:n+m)-b_3);
             lowerconqua=max(0,0.5*x(n+1:n+m)'*G*x(n+1:n+m)+(d_4)'*x(n+1:n+m)-b_4);
             upperbound_y=max(0,x(n+1:n+m)-ub_y);
             lowerbound_y=max(0,lb_y-x(n+1:n+m));
			 Infeasibility_a=norm(uppercon)+norm(lowerconin)+norm(lowerconeq)+norm(lowerconqua)+norm(upperbound_y)+norm(lowerbound_y)+a;
             Inf_Lobj_a=a;
             Inf_Con_a=norm(uppercon)+norm(lowerconin)+norm(lowerconeq)+norm(lowerconqua)+norm(upperbound_y)+norm(lowerbound_y);
          end  
       end	   
    end     
    t=toc;    
    result(row_index,:)=[k,ObjVal,Infeasibility_b,Infeasibility_a,Inf_Lobj_b,Inf_Lobj_a,Inf_Con_b,Inf_Con_a,t];
end
date = datestr(now);
date = strrep(date,':','-');
save(['TMDP, ',date],'result');