#!/usr/bin/env python

from __future__ import print_function

import can
from can.interfaces.bmcan import BmCanBus
from can.interfaces.bmcan import bmapi
import ctypes
import threading

UDS_REQUEST_ID = 0x791
UDS_RESPONSE_ID = 0x799

def hex2str(data):
    show_as_dot = False
    showed_data = data
    if len(data) > 8:
        show_as_dot = True
        showed_data = data[:8]
    hex_string = ' '.join(f'{b:02x}' for b in showed_data)
    if show_as_dot:
        hex_string += ' ... (%d bytes)' % len(data)
    return hex_string

def run_simulator(bus):
    while True:
        request = bus.receive_isotp()
        print(str(bus) + " UDS Request:", hex2str(request))
        response = None
        if request[0] == 0x10:
            p2_server_max = 1000 #ms
            p2_star_server_max = 100 #10ms
            response = bytes([0x50, request[1], p2_server_max // 256, p2_server_max % 256, p2_star_server_max // 256, p2_star_server_max % 256])
        elif request[0] == 0x27:
            if request[1] & 0x1: # Request seed
                response = bytes([0x67, request[1], 0x11, 0x22, 0x33, 0x44])
            else: # Send key
                response = bytes([0x67, request[1]])
        elif request[0] == 0x22:
            response = bytes([0x62, request[1], request[2], 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF])
        elif request[0] == 0x2E:
            response = bytes([0x6E, request[1], request[2]])
        elif request[0] == 0x34:
            response = bytes([0x74, 0x40, 0x00, 0x00, 0x08, 0x00])
        else:
            l = len(request)
            if l != 1:
                response = bytes([request[0] + 0x40, request[1]])
            else:
                response = bytes([request[0] + 0x40])
        print(str(bus) + " UDS Response:", hex2str(response))
        if response:
            #print('Write %s to channel %s' % (str(response), str(bus)))
            bus.send_isotp(response, timeout=5.0)


if __name__ == "__main__":
    buses = []
    workers = []

    for i in range(4):
        #bus = can.interface.Bus(bustype='bmcan', channel=0, bitrate=500000, data_bitrate=2000000, tres=True)
        # Custruct a BmCanBus object directly using its constructor, please add the following line before calling the constructor:
        # from can.interfaces.bmcan import BmCanBus
        bus = can.interface.Bus(bustype='bmcan', channel=i, bitrate=500000, data_bitrate=2000000, tres=True, fd=False)
        bus.config_isotp(tester_msg_id=UDS_REQUEST_ID, ecu_msg_id=UDS_RESPONSE_ID, mode=bmapi.BM_ISOTP_NORMAL_ECU, fd=False, dlc=8, functional=True)
        buses.append(bus)

    for bus in buses:
        worker = threading.Thread(target=run_simulator, args=[bus], name=str(bus))
        worker.start()
        workers.append(worker)
        print("ECU UDS simulator started on %s, listening on 0x7DF and 0x%x ..." % (str(bus), UDS_REQUEST_ID))
        
    for worker in workers:
        worker.join()

