Clusters=ClusterGenerate_4(TestPoint,J,J_Grad,Y_Grad,Options)

function Clusters=ClusterGenerate_4(TestPoint,J,J_Grad,Y_Grad,Options)
%% Expand intervals as possible

% Set up outputs
X_Tips=[TestPoint,TestPoint];
Y_Tips=Options.Y([TestPoint,TestPoint]);
J_Tips=Options.J([TestPoint,TestPoint]);
Clusters.x=TestPoint;
Clusters.y=Options.Y(TestPoint);
Clusters.j=Options.J(TestPoint);
if isfield(Options,'Verbose')
    PrintOut=Options.Verbose;
else
    PrintOut=false;
end
OutputStr=repmat(' %g,',1,size(TestPoint,1));
OutputStr(end)=[];

% Check for lower extensions
Finished=false;
indy=0;

if PrintOut
fprintf(['\nForming Cluster about',OutputStr,'\n'],TestPoint);
end
while ~Finished
    indy=indy+1;
    Grad_Out1=Options.Grad_Y(TestPoint);
    tempF2=max(sum(Grad_Out1.^2,1).^.5,Options.MinGrad_Out);
    Grad_Out2=Grad_Out1./repmat(tempF2,size(TestPoint,1),1);
    Delta_X=-Options.Gamma_Out*Grad_Out2;
    % No need to perserve step direction exactly, so use simple method.
    % This does artifically reduce step length, corrected
    % This doesn't step to border and then along border, so isn't most efficient
    TestPoint_n=TestPoint+Delta_X;
    Adj_Delta_X=any((TestPoint_n<Options.X_range(:,1))|(TestPoint_n>Options.X_range(:,2)));
    if Adj_Delta_X
%         Delta_X=PullStepInside(TestPoint_n,Delta_X,Options.X_range);
        Delta_X((TestPoint_n<Options.X_range(:,1))|...
                (TestPoint_n>Options.X_range(:,2)))=0;
        Delta_X=Options.Gamma_Out*Delta_X/max(norm(Delta_X),Options.MinGrad_Out);
    end
    TestPoint=TestPoint+Delta_X;
    if PrintOut
        fprintf(['New Test Point:',OutputStr,'\n'],TestPoint);
    end
    TestPoint=max(min(TestPoint,Options.X_range(:,2)),Options.X_range(:,1));
    Settled=false;
    Options2=Options;
    for kim=1:Options.MaxTestSteps
        Delta_X=PSO_OptFun_CostDescent_Armijo_1(TestPoint,Options2.CostStepOpt);
        TestPoint_n=TestPoint+Delta_X;
%         disp(TestPoint_n.')
        Adj_Delta_X=any(TestPoint_n<Options.X_range(:,1))|...
            any(TestPoint_n>Options.X_range(:,2));
        if Adj_Delta_X
            [Delta_X,TestPoint]=PullStepInside3(TestPoint_n,J(TestPoint_n),J_Grad(TestPoint_n),...
                Delta_X,J,J_Grad,Y_Grad,Options2);
        else
            Particles_J_old=J(TestPoint); %NOT an vector
            [TestPoint,Particles_J,Delta_X]=ArmijoStep_1(...
                TestPoint,TestPoint+Delta_X,...
                Particles_J_old,J_Grad(TestPoint),Options2);
            % will not cross boundary, so no need to check
        end
% %         disp(Delta_X.')
%         TestPoint=TestPoint+Delta_X;
% %         disp(TestPoint.')
        if norm(Delta_X)<Options.MaxGradientSize
            Settled=true;
            break
        end
    end
    if Settled
        MiddleOutput=(Options.Y(TestPoint)-Y_Tips(1))<0;
        NearPoints=(norm(TestPoint-X_Tips(:,1))<Options.MaxClusterDist);
        TooNearPoints=(norm(TestPoint-X_Tips(:,1))<Options.MinClusterDist);
        OutOfRange=~(all(TestPoint>=Options.X_range(:,1)-...
            Options.BoundaryViolationAllowance)&&...
            all(TestPoint<=Options.X_range(:,2)+...
            Options.BoundaryViolationAllowance));
        if TooNearPoints||OutOfRange
            Finished=true;
            if PrintOut
            if TooNearPoints
            fprintf(['Point settled too near last point: Current',OutputStr,' Last',OutputStr,'\n'],...
                TestPoint, X_Tips(:,1));
            else
            fprintf(['Point settled out of range: Current',OutputStr,' Previous step',OutputStr,' Last Point',OutputStr,'\n'],...
                TestPoint, TestPoint-Delta_X, X_Tips(:,1));
            end
            end
        elseif MiddleOutput&&NearPoints
            Clusters.x=[...
                TestPoint,Clusters.x];
            Clusters.y=[...
                Options.Y(TestPoint),Clusters.y];
            Clusters.j=[...
                Options.J(TestPoint),Clusters.j];
            Y_Tips(1)=Clusters.y(1);
            X_Tips(:,1)=Clusters.x(:,1);
            J_Tips(1)=Clusters.j(1);
            if PrintOut
            fprintf(['New Cluster Point',OutputStr,'\n'],...
                Clusters.x(:,1));
            end
        else
            Finished=true;
            if PrintOut
            if ~NearPoints
            fprintf(['Point settled too far from last point: Current',OutputStr,' Last',OutputStr,'\n'],...
                TestPoint, X_Tips(:,1));
            else
            fprintf('Output no longer decreasing: Current %g; Previous %g\n',...
                Options.Y(TestPoint),Y_Tips(:,1));
            end
            end
        end
    else
        Finished=true;
        if PrintOut
        fprintf(['Point did not settle within max itterations for lower bound at',OutputStr,'\n'],TestPoint);
            fprintf(['Cost Gradient,',OutputStr,' Output Gradient',OutputStr,' Step',OutputStr,'\n'],...
            Options.Grad_J(TestPoint),Options.Grad_Y(TestPoint), ...
            Delta_X);
        end
    end
end
% Check for upper extensions
Finished=false;
indy=0;
TestPoint=X_Tips(:,2);
while ~Finished
    indy=indy+1;
    % Find step direction in output direction
    Grad_Out1=Options.Grad_Y(TestPoint);
    tempF2=max(sum(Grad_Out1.^2,1).^.5,Options.MinGrad_Out);
    Grad_Out2=Grad_Out1./repmat(tempF2,size(TestPoint,1),1);
    Delta_X=Options.Gamma_Out*Grad_Out2;
    % No need to perserve step direction exactly, so use simple method.
    % This does artifically reduce step length, corrected
    % This doesn't step to border and then along border, so isn't most efficient
    TestPoint_n=TestPoint+Delta_X;
    Adj_Delta_X=any((TestPoint_n<Options.X_range(:,1))|(TestPoint_n>Options.X_range(:,2)));
    if Adj_Delta_X
%         Delta_X=PullStepInside(TestPoint_n,Delta_X,Options.X_range);
        Delta_X((TestPoint_n<Options.X_range(:,1))|...
                (TestPoint_n>Options.X_range(:,2)))=0;
        Delta_X=Options.Gamma_Out*Delta_X/max(norm(Delta_X),Options.MinGrad_Out);
        % % % NOTE: this doesn't step right to edge % % % 
    end
    TestPoint=TestPoint+Delta_X;
    if PrintOut
        fprintf(['New Test Point:',OutputStr,'\n'],TestPoint);
    end
    TestPoint=max(min(TestPoint,Options.X_range(:,2)),Options.X_range(:,1));
    Settled=false;
    Options2=Options;
    for kim=1:Options.MaxTestSteps
        Delta_X=PSO_OptFun_CostDescent_Armijo_1(TestPoint,Options2.CostStepOpt);
        TestPoint_n=TestPoint+Delta_X;
%         if PrintOut
%             fprintf(['Test Point with step:',OutputStr,'\n'],TestPoint_n);
%         end
        Adj_Delta_X=any(TestPoint_n<Options.X_range(:,1))|...
            any(TestPoint_n>Options.X_range(:,2));
        if Adj_Delta_X
            temp_nan=Delta_X;
            [Delta_X,TestPoint]=PullStepInside3(TestPoint_n,J(TestPoint_n),J_Grad(TestPoint_n),...
                Delta_X,J,J_Grad,Y_Grad,Options2);
            if any(isnan(Delta_X))
                fprintf(['Faulty TestPoint:',OutputStr,'\n'],TestPoint_n);
                fprintf(['Faulty Delta_X:',OutputStr,'\n'],temp_nan);
                fprintf(['Resultant Delta_X:',OutputStr,'\n'],Delta_X);
                return
            end
        else
            Particles_J_old=J(TestPoint); %NOT an vector
            [TestPoint,Particles_J,Delta_X]=ArmijoStep_1(...
                TestPoint,TestPoint+Delta_X,...
                Particles_J_old,J_Grad(TestPoint),Options2);
            % will not cross boundary, so no need to check
        end
%         if PrintOut
%             fprintf(['Adj Test Point step :',OutputStr,'\n'],TestPoint);
%         end
        if norm(Delta_X)<Options.MaxGradientSize
            Settled=true;
            break
        end
    end
    if Settled
        MiddleOutput=...
            (Options.Y(TestPoint)-Y_Tips(:,2))>0;
        NearPoints=(norm(TestPoint-X_Tips(:,2))<Options.MaxClusterDist);
        TooNearPoints=(norm(TestPoint-X_Tips(:,2))<Options.MinClusterDist);
        OutOfRange=~(all(TestPoint>=Options.X_range(:,1)-...
            Options.BoundaryViolationAllowance)&&...
            all(TestPoint<=Options.X_range(:,2)+...
            Options.BoundaryViolationAllowance));
        if TooNearPoints||OutOfRange
            Finished=true;
            if PrintOut
            if TooNearPoints
            fprintf(['Point settled too near last point: Current',OutputStr,' Last',OutputStr,'\n'],...
                TestPoint, X_Tips(:,2));
            else
            fprintf(['Point settled out of range: Current',OutputStr,' Previous step',OutputStr,' Last Point',OutputStr,'\n'],...
                TestPoint, TestPoint-Delta_X, X_Tips(:,2));
            end
            end
        elseif MiddleOutput&&NearPoints
            Clusters.x=[...
                Clusters.x,TestPoint];
            Clusters.y=[...
                Clusters.y,Options.Y(TestPoint)];
            Clusters.j=[...
                Clusters.j,Options.J(TestPoint)];
            Y_Tips(2)=Clusters.y(end);
            X_Tips(:,2)=Clusters.x(:,end);
            J_Tips(2)=Clusters.j(end);
            if PrintOut
            fprintf(['New Cluster Point',OutputStr,'\n'],...
                Clusters.x(:,end));
            end
        else
            Finished=true;
            if PrintOut
            if ~NearPoints
            fprintf(['Point settled too far from last point: Current',OutputStr,' Last',OutputStr,'\n'],...
                TestPoint, X_Tips(:,2));
            else
            fprintf('Output no longer decreasing: Current %g; Previous %g\n',...
                Options.Y(TestPoint),Y_Tips(:,2));
            end
            end
        end
    else
        Finished=true;
        if PrintOut
        fprintf(['Point did not settle within max itterations for lower bound at',OutputStr,'\n'],TestPoint);
        fprintf(['Cost Gradient',OutputStr,' Output Gradient',OutputStr,' Step',OutputStr,'\n'],...
            Options.Grad_J(TestPoint),Options.Grad_Y(TestPoint), ...
            Delta_X);
        end
    end
end