-- J2819 TP2.0 vehicle diagnostics protocol
-- very limited implementation
-- this controls onCanRx rate as well!
tickRate = 100

setTickRate(tickRate)

timeout = 3000

withErrorsCounter = 0

cuType = 0x02 -- TCU

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


VWTP_OUT = 0x200

VWTP_IN = 0x200 + cuType
VWTP_TESTER = 0x300

local cuId = 0

function onCanHello(bus, id, dlc, data)
	-- here we handle 201 packets
	code = data[2]
	print('Got 0x201 packet code ' ..toHexString(code) ..': TP Response ' ..arrayToString(data))

	if code ~= 0xD0 then
		print('BAD RESPONSE, trying longer sleep')
		byeByeStateCounter = 13 * tickRate
		return
	end

	cuId = data[6] * 256 + data[5]

	out = { 0xA0, 0x0F, 0x8A, 0xFF, 0x32, 0xFF }
	print("Responding to 0x201 packet from " ..cuId ..": Saying hello " ..arrayToString(out))
	txCan(1, cuId, 0, out)
end

local sendCounter = 0
local packetCounter = 1
local payLoadIndex = 0

local groupIndex = 1

withErrorCodes = 0

byeByeStateCounter = 2

function nextReq()
	local result = 0x10 + sendCounter
	sendCounter = sendCounter + 1
	if sendCounter == 16 then
		sendCounter = 0
	end
	return result
end

function startTp()
	sendCounter = 0
	packetCounter = 1
	payLoadIndex = 0
	print("Sending 0x200: Starting TP with TCU")

	txCan(1, VWTP_OUT, 0, { cuType, 0xC0, 0x00, 0x10, 0x00, 0x03, 0x01 })
end

function requestErrorCodes()
	out = { nextReq(), 0x00, 0x04, 0x18, 0x02, 0xFF, 0x00 }
	txCan(1, cuId, 0, out)
	print("Requesting error codes " ..arrayToString(out))
end

function requestEraseCodes()
	out = { nextReq(), 00, 0x03, 0x14, 0xFF, 0x00 }
	txCan(1, cuId, 0, out)
	print("******************* Requesting to Erase Code(s) *****************************")
end

function scheduleTpRestart()
	byeByeStateCounter = 3 * tickRate
end

keepAliveCounter = 0

function onCanTester(bus, id, dlc, data)
	-- here we handle 300 packets
	scheduleTpRestart()

	if data[1] == 0xA3 then
		print ("0x300 A3 Keep-alive withErrorCodes=" ..withErrorCodes)
		txCan(1, cuId, 0, { 0xA1, 0x0F, 0x8A, 0xFF, 0x4A, 0xFF })

		if withErrorCodes > 0 then
		  withErrorsCounter = withErrorsCounter + 1
			requestEraseCodes()
		else
			keepAliveCounter = keepAliveCounter + 1
			if keepAliveCounter > 4 then
				requestErrorCodes()

				keepAliveCounter = 0
			end
		end
		return
	end

	if data[1] == 0xA1 then
		print ("Happy 300 startup packet " ..arrayToString(data))
		txCan(1, cuId, 0, { nextReq(), 0x00, 0x02, 0x10, 0x89 })
		return
	end

	if data[1] == 0xA8 then
		print ("0x300 They said Bye-Bye")
		return
	end

	print('0x300 Got from tester ' ..arrayToString(data))

	if data[1] == 0x10 and dlc == 5 then
		ackPacket = 0xB0 + packetCounter
		print ("Sending ACK Bx " ..ackPacket)
		txCan(1, cuId, 0, { ackPacket })

		if data[3] == 2 and data[4] == 0x50 then
			print('Got Hello2 Response ' ..arrayToString(data))

			requestErrorCodes()
		end
	end

	top4 = math.floor(data[1] / 16)

	if top4 == 0xB then
		print("Got 0xBx ACK")
		return
	end

	if top4 == 2 or top4 == 1 then
		print ("Looks like payload index " ..payLoadIndex ..": " ..arrayToString(data))

		if payLoadIndex == 0 and data[4] == 0x58 then
			len = data[3]
			print("Looks like CODES_REQ response of length " ..len)

			if len ~= 2 then
				print("HAVE CODES " ..len)
				withErrorCodes = 1
			else
				withErrorCodes = 0
			end

      -- warning: random custom packet ID!
			txCan(1, 0x699, 0, { withErrorCodes })
		end

		payLoadIndex = payLoadIndex + 1

		packetCounter = packetCounter + 1
		if packetCounter > 15 then
			packetCounter = 0
		end

		ackPacket = 0xB0 + packetCounter
		print ("Sending payload ACK " ..ackPacket)
		txCan(1, cuId, 0, { ackPacket })

		if top4 == 1 then
			payLoadIndex = 0

			if data[2] == 0 and data[3] == 2 and data[4] == 0x58 then
				print("****************** NO CODES had " .. withErrorsCounter)
			end
		end

		return
	end

	print('Got unexpected ' ..arrayToString(data))
end

canRxAdd(VWTP_IN, onCanHello)
canRxAdd(VWTP_TESTER, onCanTester)

startTp()

function onTick()

	if byeByeStateCounter > 0 then
		byeByeStateCounter = byeByeStateCounter - 1
	end

	if byeByeStateCounter == 1 then
		scheduleTpRestart()
		startTp()
	end

end