Fixed small bug in FOC