I recommend that you define a generic clamp(x, min, max) function for enforcing the boundary conditions. It would dramatically simplify the code below.