-- DRIVE LOOP
----------------------------------------

while true do

	-- Input values taken from the VehicleSeat
	local steerFloat = vehicleSeat.SteerFloat  -- Forward and backward direction, between -1 and 1
	local throttle = vehicleSeat.ThrottleFloat  -- Left and right direction, between -1 and 1

	-- Convert "steerFloat" to an angle for the HingeConstraint servos
	local turnAngle = steerFloat * MAX_TURN_ANGLE
	wheelHingeR.TargetAngle = turnAngle
	wheelHingeL.TargetAngle = turnAngle

	-- Apply torque to the CylindricalConstraint motors depending on our throttle input and the current speed of the car
	local currentVel = getAverageVelocity()
	local targetVel = 0
	local motorTorque = 0

	-- Idling
	if math.abs(throttle) < 0.1 then
		motorTorque = 100

	-- Accelerating
	elseif math.abs(throttle * currentVel) > 0 then
		-- Reduce torque with speed (if torque was constant, there would be a jerk reaching the target velocity)
		-- This also produces a reduction in speed when turning
		local r = math.abs(currentVel) / MAX_SPEED
		-- Torque should be more sensitive to input at low throttle than high, so square the "throttle" value
		motorTorque = math.exp( - 3 * r * r ) * TORQUE * throttle * throttle
		targetVel = math.sign(throttle) * 10000  -- Arbitrary large number

	-- Braking
	else
		motorTorque = BRAKING_TORQUE * throttle * throttle
	end

	-- Use helper functions to apply torque and target velocity to all motors
	setMotorTorque(motorTorque)
	setMotorVelocity(targetVel)
	wait()
end
