Updated findClosestColor, findStorePos

findClosestColor deals with z-direction now