Cleaned up code, attempt a new solver