Another crash during decomposing in debug mode about coacd HOT 1 OPEN

if (p1 < costSize)
{
    rowIdx = (addrI * p1) >> 1;
    size_t top_row = erase_idx;
    for (size_t i = 0; i < p1; ++i)
    {
        //I ADDED THIS CHECK BECAUSE OUT OF BOUNDS STUFF WAS HAPPENING UNTIL I CAN FIGURE OUT WHAT IS GOING ON HERE
        if (rowIdx < costMatrix.size() && rowIdx < precostMatrix.size() &&
            top_row < costMatrix.size() && top_row < precostMatrix.size())
        {
            if (i != p2)
            {
                costMatrix[rowIdx] = costMatrix[top_row];
                precostMatrix[rowIdx] = precostMatrix[top_row];
            }
        }
        ++rowIdx;
        ++top_row;
    }

    ++top_row;
    rowIdx += p1;
    for (size_t i = p1 + 1; i < (costSize + 1); ++i)
    {
        //I ADDED THIS CHECK BECAUSE OUT OF BOUNDS STUFF WAS HAPPENING UNTIL I CAN FIGURE OUT WHAT IS GOING ON HERE
        if (rowIdx < costMatrix.size() && rowIdx < precostMatrix.size() &&
            top_row < costMatrix.size() && top_row < precostMatrix.size())
        {
            costMatrix[rowIdx] = costMatrix[top_row];
            precostMatrix[rowIdx] = precostMatrix[top_row];
        }
        top_row++;
        rowIdx += i;
        assert(rowIdx >= 0);
    }
}