FindBoundedMinChangePath.m

% Derived from https://raw.githubusercontent.com/Coloquinte/stackexchange-fun/master/solver.py
% See also https://or.stackexchange.com/questions/5183/pava-like-solution-to-simple-qp

% Find X with minimal change such that L <= X <= U.
function X = FindBoundedMinChangePath( L, U )

    n = numel( L );
    assert( numel( U ) == n );
    assert( all( L <= U ) );

    X = NaN( n, 1 );
    
    [ FirstBPIndex, FirstBPValue ] = FindFirstBreakPoint( L, U );
    X( 1 : FirstBPIndex ) = FirstBPValue;
    
    if FirstBPIndex == n
        return
    end
    
    [ LastBPIndex, LastBPValue ] = FindLastBreakPoint( L, U );
    X( LastBPIndex : end ) = LastBPValue;
    
    BPIndex = FirstBPIndex;
    BPValue = FirstBPValue;
    
    while BPIndex < LastBPIndex
        
        [ NextBPIndex, NextBPValue ] = FindNextBreakPoint( L, U, BPIndex, BPValue, LastBPIndex, LastBPValue );
        
        i = ( BPIndex + 1 ) : NextBPIndex;
        X( i ) = ( ( NextBPIndex - i ) * BPValue + ( i - BPIndex ) * NextBPValue ) / ( NextBPIndex - BPIndex );
        
        BPIndex = NextBPIndex;
        BPValue = NextBPValue;
        
    end
    
    X = max( L(:), min( U(:), X ) );
    
end

% Special case for the first breakpoint
function [ BPIndex, BPValue ] = FindFirstBreakPoint( L, U )

    n = numel( L );

    % Find the first point where the solution departs from a constant line
    
    LBIndex = 0;
    UBIndex = 0;
    LBValue = -Inf;
    UBValue =  Inf;
    
    for i = 1 : n
        
        NewLB = L( i );
        NewUB = U( i );
        
        if NewLB > UBValue
            BPIndex = UBIndex;
            BPValue = UBValue;
            return
        end
        if NewUB < LBValue
            BPIndex = LBIndex;
            BPValue = LBValue;
            return
        end
        if NewUB <= UBValue
            UBIndex = i;
            UBValue = NewUB;
        end
        if NewLB >= LBValue
            LBIndex = i;
            LBValue = NewLB;
        end
        
    end
    
    % No breakpoint found the optimal solution is a straight line
    BPIndex = n;
    BPValue = 0.5 * ( LBValue + UBValue );
    
end

% Special case for the last breakpoint
function [ BPIndex, BPValue ] = FindLastBreakPoint( L, U )

    n = numel( L );

    % Find the point where the solution returns to a constant line

    [ BPIndex, BPValue ] = FindFirstBreakPoint( flip( L ), flip( U ) );
    
    BPIndex = n + 1 - BPIndex;
    
end

% General case to find a breakpoint
function [ BPIndex, BPValue ] = FindNextBreakPoint( L, U, BPIndex, BPValue, LastBPIndex, LastBPValue )

    % Find the next breakpoint in the solution

    LBPIndex = 0;
    UBPIndex = 0;
    LBSlope = -Inf;
    UBSlope =  Inf;
    
    for i = ( BPIndex + 1 ) : LastBPIndex
        
        if i ~= LastBPIndex
            LBValue = L( i );
            UBValue = U( i );
        else
            % Special case for the last breakpoint, as there is a straight line afterwards
            LBValue = LastBPValue;
            UBValue = LastBPValue;
        end
        
        NewLBSlope = ( LBValue - BPValue ) / ( i - BPIndex );
        NewUBSlope = ( UBValue - BPValue ) / ( i - BPIndex );
        
        if NewLBSlope > UBSlope
            BPIndex = UBPIndex;
            BPValue = U( UBPIndex );
            return
        end
        if NewUBSlope < LBSlope
            BPIndex = LBPIndex;
            BPValue = L( LBPIndex );
            return
        end
        if NewUBSlope <= UBSlope
            UBPIndex = i;
            UBSlope  = NewUBSlope;
        end
        if NewLBSlope >= LBSlope
            LBPIndex = i;
            LBSlope  = NewLBSlope;
        end
        
    end
    
    % No breakpoint found the solution will go back to a straight line at the last breakpoint
    BPIndex = LastBPIndex;
    BPValue = LastBPValue;
    
end