First implementation, math