-- this controls onCanRx rate as well!
setTickRate(300)

timeout = 3000

rpmSensor = Sensor.new("rpm")
rpmSensor : setTimeout(timeout)

--ppsSensor = Sensor.new("AcceleratorPedal")
--ppsSensor : setTimeout(timeout)

speedSensor = Sensor.new("VehicleSpeed")
speedSensor : setTimeout(timeout)

hexstr = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, "A", "B", "C", "D", "E", "F" }

function toHexString(num)
	if num == 0 then
		return '0'
	end

	local result = ""
	while num > 0 do
		local n = num % 16
		result = hexstr[n + 1] ..result
		num = math.floor(num / 16)
	end
	return result
end

function arrayToString(arr)
	local str = ""
	local index = 1
	while arr[index] ~= nil do
		str = str.." "..toHexString(arr[index])
		index = index + 1
	end
	return str
end

-- pretty LOL if you ask me: RPM and vehicle speed uses different byte order
function getTwoBytesMSB(data, offset, factor)
	return (data[offset + 1] * 256 + data[offset + 2]) * factor
end

function getTwoBytesLSB(data, offset, factor)
	return (data[offset + 2] * 256 + data[offset + 1]) * factor
end

function onRpmData(bus, id, dlc, data)
--    print('onRpmData ' ..arrayToString(data))
    rpm = getTwoBytesLSB(data, 3, 3.15)
    print ('RPM ' .. rpm)
    rpmSensor : set(rpm)
end

function onSpeedData(bus, id, dlc, data)
--    print('onSpeedData ' ..arrayToString(data))
    speed = getTwoBytesMSB(data, 4, 0.01)
    print ('speed ' .. speed)
    speedSensor : set(speed)
end

canRxAdd(1, 0x23D, onRpmData)
canRxAdd(1, 0x280, onSpeedData)