Algorithm working, but too complex. Finding way to reduce cyclomatic complexity.