function herusticsolver_cleaned()
%% Inputs
global eps Comp

eps = 0.01; %% error limit for final point reachability
set(0,'defaulttextInterpreter','latex') %latex axis labels
%% Desired constraint
ts = 0;

xs = 0;
xgmin = 4;
xgmax = 4;

ys = 1.5;
ygmin = 2.8;
ygmax = 3.2;

zs = 1;
zgmin = 1.7;
zgmax = 2.3;

%%% First pose
vxs = 3; vys = 3; vzs = 1;
axs = 1; ays =0.5; azs = 0.1;

%%% Second pose
% vxs =3; vys = 3; vzs = 3;
% axs = 1; ays =0.5; azs = 1;

%%% Third pose
% vxs =3; vys = 3; vzs = -2;%1;
% axs = 1; ays =0.5; azs = 0.1;

%%% Trial
% vxs =3; vys = 2; vzs = 1;
% axs = 1; ays =0.05; azs = 0.1;

% % Dynamic bounds
Vxmin = -5; Vymin = -5; Vzmin = -5;
Vxmax = 5; Vymax = 5; Vzmax = 5;
Axmin = -10; Aymin = -10; Azmin = -10;
Axmax = 10; Aymax = 10; Azmax = 10;
Jxmin = -50; Jymin = -50; Jzmin = -50;
Jxmax = 50; Jymax = 50; Jzmax = 50;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %% % %% Complex Obstacle scenario
% % % Case 1: Simple obstacle
 Xl = [3.75; 3.75];
 Xu = [4.25;4.25];
 
 Yl = [2.3; 3.2];
 Yu = [2.8; 3.7];
 
 Zl = [zgmin; zgmin];
 Zu = [zgmax; zgmax];
 
 N = size(Xl,1);
 to_init = -Inf.*ones(N,1);
 to_end = Inf.*ones(N,1);
%%%===========================
%%% Case2 obstacle data
% load('obs10_1438sol20x20.mat') %% Case3

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Algo 1: First step: Dynamic constraints solution sets:
% % Fx is flag array indicating respective obstacle avoidance
% % Rx obstacle-wise feasible sets: if obstacle not avoided (Fxx = 0) means only flyability set values
tic
Rx = SS_1D_19feb2021_v2(xs,vxs,axs,xgmin,xgmax,Vxmin,Vxmax,Axmin,Axmax,Jxmin,Jxmax,ts,[to_init to_end Xl Xu]);
t_xSS = toc;
tic
Ry = SS_1D_19feb2021_v2(ys,vys,ays,ygmin,ygmax,Vymin,Vymax,Aymin,Aymax,Jymin,Jymax,ts,[to_init to_end Yl Yu]);
t_ySS = toc;
tic
Rz = SS_1D_19feb2021_v2(zs,vzs,azs,zgmin,zgmax,Vzmin,Vzmax,Azmin,Azmax,Jzmin,Jzmax,ts,[to_init to_end Zl Zu]);
t_zSS = toc;
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Algo 2: Second step proposed obstacle avoidance approach
%%%First step check
% tic
%%Selection of three solution pairs
Srnd = [];
Tgoal = [];
Sol = {};
n = 1;
Comp = [];

% % Find pairs which avoid all obstacles (Merge with decoupled axis approach above)
if any(~cell2mat(Rx(:,4))) || any(~cell2mat(Ry(:,4))) || any(~cell2mat(Rz(:,4)))
    k1x = find(~cell2mat(Rx(:,4)))
    k1y = find(~cell2mat(Ry(:,4)))
    k1z = find(~cell2mat(Rz(:,4)))
    
    if ~isempty(k1x)
        Sol{n,1} =  Rx(k1x,1);
        Sol{n,2} =  Ry(:,1);
        Sol{n,3} =  Rz(:,1);
        
        Comp = [Comp; toc size(Sol{n,1},1)*size(Sol{n,2},1)*size(Sol{n,3},1) 1.1];
        
        n = n+1;
        
        Rx(k1x,:)=[];
    end
    if ~isempty(k1y)
        Sol{n,1} =  Rx(:,1);
        Sol{n,2} =  Ry(k1y,1);
        Sol{n,3} =  Rz(:,1);
        
        Comp = [Comp; toc size(Sol{n,1},1)*size(Sol{n,2},1)*size(Sol{n,3},1) 1.2];
        
        n = n+1;
        Ry(k1y,:)=[];
        
        
    end
    if ~isempty(k1z)
        Sol{n,1} =  Rx(:,1);
        Sol{n,2} =  Ry(:,1);
        Sol{n,3} =  Rz(k1z,1);
        
        Comp = [Comp; toc size(Sol{n,1},1)*size(Sol{n,2},1)*size(Sol{n,3},1) 1.3];
        
        n = n+1;
        
        Rz(k1z,:)=[];
 end
    t_SS1step = toc;
  
end

%%%Second step: ordering
[~,Sind] = sort([size(Rx,1) size(Ry,1) size(Rz,1)]);
if Sind == [1 2 3]
    Ra = Rx;
    Rb = Ry;
    Rc = Rz;
elseif Sind == [1 3 2]
    Ra = Rx;
    Rb = Rz;
    Rc = Ry;
elseif Sind == [2 3 1]
    Ra = Ry;
    Rb = Rz;
    Rc = Rx;
elseif Sind == [2 1 3]
    Ra = Ry;
    Rb = Rx;
    Rc = Rz;
elseif Sind == [3 1 2]
    Ra = Rz;
    Rb = Rx;
    Rc = Ry;
elseif Sind == [3 2 1]
    Ra = Rz;
    Rb = Ry;
    Rc = Rx;
else
    disp('Bug detected in the ordering step')
end
if ~isempty(Sol)
    Sol = Sol(:,Sind);
end


%% Heauristic Obstacle avoidance approach
Sol = heuristic(Ra,Rb,Rc,Sol,n);
toc

%%%%%%%%%%%%%%%%
% tic
if ~isempty(Sol)
    sz = 0;
    for l = 1:size(Sol,1)
        S{l,Sind(1)} = Sol{l,1};  %% Changelog: Reorder of Solution set S based on sorting 
        S{l,Sind(2)} = Sol{l,2};
        S{l,Sind(3)} = Sol{l,3};
        
        sz = sz + (size(S{l,1},1) * size(S{l,2},1) * size(S{l,3},1));
    end
    fprintf('***%d Solution found***\n',sz)
    
    % % Pick random samples for trajectory generation
    Srnd = Inf.*ones(1,6);
    Tgoal = [];
    if sz > 10
        numitr = 10;
    else
        numitr = sz;
    end
    itr = 0;
    while itr < numitr
        Irnd = randi(size(S,1));
        irnd = randi(size(S{Irnd,1},1));
        jrnd = randi(size(S{Irnd,2},1));
        krnd = randi(size(S{Irnd,3},1));
        
        Srnd1 = cell2mat([S{Irnd,1}(irnd,:) S{Irnd,2}(jrnd,:) S{Irnd,3}(krnd,:)]);
        Tgoal1 = Srnd1(:,[3 6 9]);
        Srnd1 = Srnd1(:,[1:2 4:5 7:8]);
        if ismember(Srnd1,Srnd,'rows')
            continue
        else
            itr = itr +1;
            Srnd(itr,:) = Srnd1;
            Tgoal(itr) = max(Tgoal1);
        end
    end
else
    disp('***No solution found***')
    return
end
toc

%% Trajectory generation
if isempty(Srnd)
    disp('No solution')
else

    set(0,'defaulttextInterpreter','latex') %latex axis labels
    t_toctraj = [];
    
    X_traj = [];
    Y_traj = [];
    Z_traj = [];
    T_traj = [];
    
    %     errx = [];
    %     erry = [];
    %     errz = [];
    
    for i = 1:size(Srnd,1)%:ceil(size(Srnd,1)/100)
        xit = Srnd(i,1);
        xgt = Srnd(i,2);
        yit = Srnd(i,3);
        ygt = Srnd(i,4);
        zit = Srnd(i,5);
        zgt = Srnd(i,6);
        tg = Tgoal(i);
        
        %% X, Y and Z trajectories
        tic
        [tx,x,vx,ax,jx] = fourpl(xs,vxs,axs,xgt,xit,ts,tg, false);
        t_xtraj = toc;
        %[x1,dx1,ddx1,tx1] = fourpl(xit,Xf,Xi,Vxd,Axd);
        
        tic
        [ty,y,vy,ay,jy] = fourpl(ys,vys,ays,ygt,yit,ts,tg, true);
        t_ytraj = toc;
        
        tic
        [tz,z,vz,az,jz] = fourpl(zs,vzs,azs,zgt,zit,ts,tg, false);
        t_ztraj = toc;
        
        t_toctraj = [t_toctraj; t_xtraj t_ytraj t_ztraj];
        X_traj = [X_traj; x];
        Y_traj = [Y_traj; y];
        Z_traj = [Z_traj; z];
        T_traj = [T_traj; tx];
    

    end

end