AIRBAG = 0x050

onEcuTimer = Timer.new()
onEcuTimer : reset ()

TCU_1088_440 = 0x440
TCU_1344_540 = 0x540


BRAKE_1_416 = 0x1A0
BRAKE_8_428 = 0x1AC

Komf_1_912 = 0x390

BRAKE_3_1184 = 0x4a0
BRAKE_5_1192 = 0x4a8
BRAKE_2_1440 = 0x5A0


-- 640
MOTOR_1 = 0x280
-- 644
MOTOR_BRE = 0x284
-- 648
MOTOR_2 = 0x288
-- 896
MOTOR_3 = 0x380
-- 1152
MOTOR_5 = 0x480
-- 1160
MOTOR_6 = 0x488
-- 1386
ACC_GRA = 0x56A
-- 1408 the one with variable payload
MOTOR_INFO = 0x580
-- 1416
MOTOR_7 = 0x588

motor1Data = { 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
motorBreData = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
motor2Data = { 0x8A, 0x8D, 0x10, 0x04, 0x00, 0x4C, 0xDC, 0x87 }
motor2mux = { 0x8A, 0xE8, 0x2C, 0x64 }
canMotor3 = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
motor5Data = { 0x1C, 0x08, 0xF3, 0x55, 0x19, 0x00, 0x00, 0xAD }
motor6Data = { 0x00, 0x00, 0x00, 0x7E, 0xFE, 0xFF, 0xFF, 0x00 }
accGraData = { 0x00, 0x00, 0x08, 0x00, 0x1A, 0x00, 0x02, 0x01 }
canMotorInfo02 = { 0x00, 0x00, 0x00, 0x14, 0x1C, 0x93, 0x48, 0x14 }
canMotorInfo1 = { 0x99, 0x14, 0x00, 0x7F, 0x00, 0xF0, 0x47, 0x01 }
canMotorInfo3 = { 0x9B, 0x14, 0x00, 0x11, 0x1F, 0xE0, 0x0C, 0x46 }
motor7Data = { 0x1A, 0x66, 0x7E, 0x00, 0x00, 0x00, 0x00, 0x00 }

payload416 = {0x00, 0x43, 0x00, 0x00, 0x00, 0xfe, 0x00, 0x5d}
payload428 = {0xff, 0x0e, 0x00, 0x00, 0x69, 0x01, 0x0b, 0x92}
payload912 = {0x6a, 0x40, 0x50, 0x00, 0x00, 0x00, 0x20, 0x00}
payload1440 = {0x7e, 0x00, 0x00, 0x83, 0x33, 0x00, 0x10, 0xab}

canMotorInfoTotalCounter = 0

VWTP_OUT = 0x200
cuType = 0x02 -- TCU
VWTP_IN = 0x200 + cuType
VWTP_TESTER = 0x300

-- sometimes we want to cut a CAN bus and install rusEFI into that cut
-- https://en.wikipedia.org/wiki/Man-in-the-middle_attack

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

ECU_BUS = 1
TCU_BUS = 2

fakeTorque = 0
rpm = 0
tps = 0

function xorChecksum(data, targetIndex)
	local index = 1
	local result = 0
	while data[index] ~= nil do
		if index ~= targetIndex then
			result = result ~ data[index]
		end
		index = index + 1
	end
	data[targetIndex] = result
	return result
end

function setBitRange(data, totalBitIndex, bitWidth, value)
	local byteIndex = totalBitIndex >> 3
	local bitInByteIndex = totalBitIndex - byteIndex * 8
	if (bitInByteIndex + bitWidth > 8) then
		bitsToHandleNow = 8 - bitInByteIndex
		setBitRange(data, totalBitIndex + bitsToHandleNow, bitWidth - bitsToHandleNow, value >> bitsToHandleNow)
		bitWidth = bitsToHandleNow
	end
	mask = (1 << bitWidth) - 1
	data[1 + byteIndex] = data[1 + byteIndex] & (~(mask << bitInByteIndex))
	maskedValue = value & mask
	shiftedValue = maskedValue << bitInByteIndex
	data[1 + byteIndex] = data[1 + byteIndex] | shiftedValue
end

function relayFromECU(bus, id, dlc, data)
    onEcuTimer : reset ()
	totalEcuMessages = totalEcuMessages + 1
	-- 	print("Relaying to TCU " .. id)
	txCan(TCU_BUS, id, 0, data)
end

function setTwoBytes(data, offset, value)
	data[offset + 1] = value % 255
	data[offset + 2] = (value >> 8) % 255
end

function sendMotor1()
	engineTorque = fakeTorque * 0.9
	innerTorqWithoutExt = fakeTorque
	torqueLoss = 20
	requestedTorque = fakeTorque

	motor1Data[2] = engineTorque / 0.39
	setTwoBytes(motor1Data, 2, rpm / 0.25)
	motor1Data[5] = innerTorqWithoutExt / 0.4
	motor1Data[6] = tps / 0.4
	motor1Data[7] = torqueLoss / 0.39
	motor1Data[8] = requestedTorque / 0.39

	txCan(TCU_BUS, MOTOR_1, 0, motor1Data)
end

function getBitRange(data, bitIndex, bitWidth)
	byteIndex = bitIndex >> 3
	shift = bitIndex - byteIndex * 8
	value = data[1 + byteIndex]
	if (shift + bitWidth > 8) then
		value = value + data[2 + byteIndex] * 256
	end
	mask = (1 << bitWidth) - 1
	return (value >> shift) & mask
end

motor2counter = 0
function sendMotor2()
 motor2counter = (motor2counter + 1) % 16

    minTorque = fakeTorque / 2
    -- todo: add CLT
    motor2Data[7] = math.floor(minTorque / 0.39)

--print ( "brake " .. getBitRange(data, 16, 2) .. " " .. rpm)

    brakeBit = rpm < 2000 and 1 or 0
    setBitRange(motor2Data, 16, 1, brakeBit)

    index = math.floor(motor2counter / 4)
    motor2Data[1] = motor2mux[1 + index]

 txCan(TCU_BUS, MOTOR_2, 0, motor2Data)
end

function sendMotor3()
	desired_wheel_torque = fakeTorque
	iat = iat or 30
	canMotor3[2] = (iat + 48) / 0.75
	canMotor3[3] = tps / 0.4
	canMotor3[5] = 0x20
	setBitRange(canMotor3, 24, 12, math.floor(desired_wheel_torque / 0.39))
	canMotor3[8] = tps / 0.4
	txCan(TCU_BUS, MOTOR_3, 0, canMotor3)
end

motor3counter = 0
function onMotor3(bus, id, dlc, data)
 motor3counter = (motor3counter + 1) % 16
	totalEcuMessages = totalEcuMessages + 1
	iat = 30 -- getBitRange(data, 8, 8) * 0.75 - 48
	pps = 7 -- getBitRange(data, 16, 8) * 0.40
	tps = 7 -- getBitRange(data, 56, 8) * 0.40
    if motor3counter % 70 == 0 then
    	print ('MOTOR_3 pps ' ..pps ..' tps ' ..tps ..' iat ' ..iat)
    end
end

function onMotor1(bus, id, dlc, data)
	totalEcuMessages = totalEcuMessages + 1
	rpm = 0 -- getBitRange(data, 16, 16) * 0.25
	if rpm == 0 then
		canMotorInfoTotalCounter = 0
	end

	tps = 0 -- getBitRange(data, 40, 8) * 0.4

	fakeTorque = interpolate(0, 6, 100, 60, tps)

	sendMotor1()
end

function relayFromECUAndEcho(bus, id, dlc, data)
	totalEcuMessages = totalEcuMessages + 1
	print("Relaying to TCU " ..id)
	txCan(TCU_BUS, id, 0, data) -- relay non-TCU message to TCU
end

totalTcuMessages = 0
counter440 = 0
function onTcu440(bus, id, dlc, data)
	totalTcuMessages = totalTcuMessages + 1
	-- 	print("Relaying onTcu440 to ECU " .. id)

	    isShiftActive = getBitRange(data, 0, 1)
        tcuStatus = getBitRange(data, 1, 1)
        EGSRequirement = getBitRange(data, 7, 1)
    counter440 = counter440 + 1
    if counter440 % 70 == 0 then
        print("TCU " .. isShiftActive .. " " .. tcuStatus .. " " .. EGSRequirement)
    end
	txCan(ECU_BUS, id, 0, data)
end

function relayFromTCU(bus, id, dlc, data)
	totalTcuMessages = totalTcuMessages + 1
	-- 	print("Relaying to ECU " .. id)
	txCan(ECU_BUS, id, 0, data)
end

function relayTcuDiagHelloFromTCU(bus, id, dlc, data)
	totalTcuMessages = totalTcuMessages + 1
	print("Relaying TcuDiagHello to ECU " ..id ..arrayToString(data))
	txCan(ECU_BUS, id, 0, data)
end

local payLoadIndex = 0

function relayTpPayloadFromTCU(bus, id, dlc, data)
	totalTcuMessages = totalTcuMessages + 1
	-- print("Relaying TP ECU " ..id ..arrayToString(data))
	txCan(ECU_BUS, id, 0, data) -- relay non-ECU message to ECU


	if data[1] == 0xA3 then
		-- 		print ("Keep-alive")
		return
	end

	if data[1] == 0xA1 then
		print ("I sniff Happy 300 packet")
		return
	end

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

	if data[1] == 0x10 and dlc == 5 then
		return
	end

	top4 = math.floor(data[1] / 16)
	if top4 == 0xB then
		-- ACK
		return
	end

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

		if payLoadIndex == 1 then
			H4 = data[4]
			H7 = data[7] or -1

			-- 			print("h5/h7 " ..H4 .." " ..H7)

			if H7 == 0 then
--				print("I sniff diag: TCU is happy!")
			else
--				print("I sniff diag: TCU has ERROR ERROR ERROR")
			end

		end

		payLoadIndex = payLoadIndex + 1

		if top4 == 1 or top4 == 2 then
			payLoadIndex = 0
			len = data[3]

			if data[2] == 0 and data[4] == 0x58 then
				if len == 2 then
					print("I sniff TCU NO CODES")
				else
					print("****************                     I sniff ERROR ERROR                " .. data[5])
				end
			end

		end
		return
	end

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

function drop(bus, id, dlc, data)
end

motorBreCounter = 0
function sendMotorBre()
	motorBreCounter = (motorBreCounter + 1) % 16

	setBitRange(motorBreData, 8, 4, motorBreCounter)
	xorChecksum(motorBreData, 1)

	txCan(TCU_BUS, MOTOR_BRE, 0, motorBreData)
end

motor5counter = 0
motor5FuelCounter = 0
function sendMotor5()
--    motor5counter = (motor5counter + 1) % 16
-- todo	index = math.floor(motor5counter / 4)

	setBitRange(motor5Data, 5, 9, motor5FuelCounter)
	xorChecksum(motor5Data, 8)
	txCan(TCU_BUS, MOTOR_5, 0, motor5Data)
end

motor6Counter = 0
function sendMotor6()
	motor6Counter = (motor6Counter + 1) % 16

	engineTorque = fakeTorque * 0.9
	actualTorque = fakeTorque
	feedbackGearbox = 255

	motor6Data[2] = math.floor(engineTorque / 0.39)
	motor6Data[3] = math.floor(actualTorque / 0.39)
	motor6Data[6] = math.floor(feedbackGearbox / 0.39)
	setBitRange(motor6Data, 60, 4, motor6Counter)

	xorChecksum(motor6Data, 1)
	txCan(TCU_BUS, MOTOR_6, 0, motor6Data)
end

function sendMotor7()
	txCan(TCU_BUS, MOTOR_7, 0, motor7Data)
end

accGraCounter = 0
function sendAccGra()
 accGraCounter = (accGraCounter + 1) % 16
 setBitRange(accGraData, 60, 4, accGraCounter)
    xorChecksum(accGraData, 1)

 txCan(TCU_BUS, ACC_GRA, 0, accGraData)
end

canMotorInfoCounter = 0
function sendMotorInfo()
	canMotorInfoTotalCounter = canMotorInfoTotalCounter + 1
	canMotorInfoCounter = (canMotorInfoCounter + 1) % 16

	baseByte = canMotorInfoTotalCounter < 6 and 0x80 or 0x90
	canMotorInfo02[1] = baseByte + (canMotorInfoCounter)
	canMotorInfo1[1] = baseByte + (canMotorInfoCounter)
	canMotorInfo3[1] = baseByte + (canMotorInfoCounter)
	mod4 = canMotorInfoCounter % 4

	if (mod4 == 0 or mod4 == 2) then
		txCan(TCU_BUS, MOTOR_INFO, 0, canMotorInfo02)
	elseif (mod4 == 1) then
		txCan(TCU_BUS, MOTOR_INFO, 0, canMotorInfo1)
	else
		txCan(TCU_BUS, MOTOR_INFO, 0, canMotorInfo3)
	end
end

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

totalEcuMessages = 0

-- REQUIRED kombi 3
canRxAdd(ECU_BUS, 1312, relayFromECU)
-- REQUIRED Soll_Verbauliste_neu
canRxAdd(ECU_BUS, 1500, relayFromECU)
-- REQUIRED GRA_Neu
canRxAdd(ECU_BUS, 906, relayFromECU)

canRxAdd(ECU_BUS, BRAKE_1_416, relayFromECU)
--canRxAdd(ECU_BUS, BRAKE_2_1440, relayFromECU)
canRxAdd(ECU_BUS, BRAKE_8_428, relayFromECU)

canRxAdd(ECU_BUS, BRAKE_3_1184, relayFromECU)

canRxAdd(ECU_BUS, BRAKE_5_1192, relayFromECU)


--canRxAdd(ECU_BUS, Komf_1_912, relayFromECU)
-- Systeminfo_1
canRxAdd(ECU_BUS, 1488, relayFromECU)
-- REQUIRED? Diagnose_1
canRxAdd(ECU_BUS, 2000, relayFromECU)

canRxAdd(ECU_BUS, MOTOR_1, onMotor1)
-- canRxAdd(ECU_BUS, MOTOR_BRE, relayFromECU)
-- canRxAdd(ECU_BUS, MOTOR_2, relayFromECU)

canRxAdd(ECU_BUS, MOTOR_3, onMotor3)
-- canRxAdd(ECU_BUS, MOTOR_3, relayFromECU)

-- canRxAdd(ECU_BUS, MOTOR_5, onMotor5)
-- canRxAdd(ECU_BUS, MOTOR_5, relayFromECU)

--canRxAdd(ECU_BUS, MOTOR_6, sendMotor6)
-- canRxAdd(ECU_BUS, MOTOR_6, relayFromECU)

--canRxAdd(ECU_BUS, ACC_GRA, relayFromECU)
-- canRxAdd(ECU_BUS, MOTOR_INFO, relayFromECU)

canRxAdd(ECU_BUS, VWTP_OUT, relayFromECU)
canRxAdd(ECU_BUS, 0x760, relayFromECU)

canRxAddMask(ECU_BUS, 0, 0, drop)
-- canRxAddMask(ECU_BUS, 0, 0, relayFromECU)

canRxAdd(TCU_BUS, VWTP_IN, relayTcuDiagHelloFromTCU)
canRxAdd(TCU_BUS, VWTP_TESTER, relayTpPayloadFromTCU)
canRxAdd(TCU_1088_440, onTcu440)
canRxAddMask(TCU_BUS, 0, 0, relayFromTCU)

everySecondTimer = Timer.new()

function onTick()
    if onEcuTimer : getElapsedSeconds() > 1 then
        -- do not spam if ECU is silent
        return
    end

	sendMotor2()
	sendMotor3()
	sendMotor5()
	sendMotor6()
	sendMotor7()
	sendMotorBre()
	sendAccGra()

--    txCan(TCU_BUS, BRAKE_1_416, 0, payload416)
--    txCan(TCU_BUS, BRAKE_8_428, 0, payload428)
    txCan(TCU_BUS, BRAKE_2_1440, 0, payload1440)

    txCan(TCU_BUS, Komf_1_912, 0, payload912)

	if everySecondTimer : getElapsedSeconds() > 1 then
		everySecondTimer : reset()
		print("Total from ECU " ..totalEcuMessages)
		motor5FuelCounter = motor5FuelCounter + 20

		sendMotorInfo()
	end


end