"""Find CAN signals that follow a reference signal, such as OBD-II vehicle speed.

Python 3.12 or newer, standard library only. Run make_synthetic_log.py first to create synthetic.log.
"""


import bisect
import statistics
from collections import defaultdict

OBD_IDS = (0x7DF, 0x7E8)


def load_candump(path):
    """{can_id: [(timestamp, data), ...]} from a candump -l log."""
    frames = defaultdict(list)
    with open(path) as log:
        for line in log:
            stamp, _, frame = line.split()
            can_id, data = frame.split("#")
            frames[int(can_id, 16)].append((float(stamp.strip("()")), bytes.fromhex(data)))
    return frames


def obd_reference(frames, pid):
    """Reference series [(request time, value)] from OBD-II PID 0x0D (km/h) or 0x0C (rpm) replies.

    The reply arrives some milliseconds after the request, so it is timed at the request.
    """
    formulas = {0x0D: lambda d: d[3], 0x0C: lambda d: (256 * d[3] + d[4]) / 4}
    requests = [t for t, d in frames[0x7DF] if d[1:3] == bytes([0x01, pid])]
    series = []
    for t, d in frames[0x7E8]:
        index = bisect.bisect_right(requests, t) - 1
        if d[1:3] == bytes([0x41, pid]) and index >= 0:
            series.append((requests[index], formulas[pid](d)))
    return series


def survey(frames):
    """One line per CAN ID: frames, period and how often each byte changes."""
    for can_id, items in sorted(frames.items()):
        if can_id in OBD_IDS:
            continue
        datas = [d for _, d in items]
        period_ms = (items[-1][0] - items[0][0]) / (len(items) - 1) * 1000
        rates = [sum(a[i] != b[i] for a, b in zip(datas, datas[1:])) / (len(datas) - 1) for i in range(8)]
        print(f"{can_id:03X}  {len(items):6} frames  {period_ms:5.1f} ms  byte change rate: "
              + " ".join(f"{r:.3f}" for r in rates))


def frames_at(items, times):
    """The latest frame at or before each reference time."""
    stamps = [t for t, _ in items]
    return [items[max(0, bisect.bisect_right(stamps, t) - 1)][1] for t in times]


def extract(data_int, start, length, big_endian, signed, width=64):
    """Raw value of a DBC-style field. data_int is the payload as one integer (little or big endian)."""
    if big_endian:  # Motorola: start is the most significant bit, numbered like Intel bits
        msb = (start // 8) * 8 + (7 - start % 8)
        raw = (data_int >> (width - msb - length)) & ((1 << length) - 1)
    else:
        raw = (data_int >> start) & ((1 << length) - 1)
    if signed and raw >> (length - 1):
        raw -= 1 << length
    return raw


def rank_messages(frames, reference):
    """Stage 1: which CAN ID and byte pair follows the reference? Spearman rank correlation."""
    times = [t for t, _ in reference]
    values = [v for _, v in reference]
    results = []
    for can_id, items in frames.items():
        if can_id in OBD_IDS:
            continue
        datas = frames_at(items, times)
        little = [int.from_bytes(d, "little") for d in datas]
        big = [int.from_bytes(d, "big") for d in datas]
        for byte in range(8):
            for size, big_endian in ((8, False), (16, False), (16, True)):
                if byte + size // 8 > 8:
                    continue
                start = byte * 8 + 7 if big_endian else byte * 8
                raw = [extract(x, start, size, big_endian, False) for x in (big if big_endian else little)]
                if len(set(raw)) > 2:
                    rho = statistics.correlation(raw, values, method="ranked")
                    results.append((round(abs(rho), 4), can_id, byte, size, "big" if big_endian else "little"))
    return sorted(results, reverse=True)


def flip_rates(items):
    """How often each Intel bit (0-63) changes between consecutive frames."""
    ints = [int.from_bytes(d, "little") for _, d in items]
    counts = [0] * 64
    for a, b in zip(ints, ints[1:]):
        changed = a ^ b
        for bit in range(64):
            counts[bit] += changed >> bit & 1
    return [c / (len(ints) - 1) for c in counts]


def field_bits(start, length, big_endian):
    """Intel bit numbers covered by a field, from LSB to MSB."""
    if not big_endian:
        return list(range(start, start + length))
    bits, bit = [], start
    for _ in range(length):  # Motorola: walk down from the MSB, jumping to the next byte at bit 0
        bits.append(bit)
        bit = bit - 1 if bit % 8 else bit + 15
    return bits[::-1]


def search_bits(items, reference, max_length=16):
    """Stage 2: every start bit, length, byte order and sign on one CAN ID, scored by R²."""
    times = [t for t, _ in reference]
    values = [v for _, v in reference]
    datas = frames_at(items, times)
    rates = flip_rates(items)
    little = [int.from_bytes(d, "little") for d in datas]
    big = [int.from_bytes(d, "big") for d in datas]
    results = []
    for big_endian in (False, True):
        for start in range(64):
            for length in range(2, max_length + 1):
                bits = field_bits(start, length, big_endian)
                if max(bits) > 63 or not rates[bits[0]] or not rates[bits[-1]]:
                    continue  # both ends must change at least once
                if rates[bits[0]] < 0.5 * max(rates[b] for b in bits):
                    continue  # in a number, the lowest bit flips most often
                for signed in (False, True):
                    raw = [extract(x, start, length, big_endian, signed) for x in (big if big_endian else little)]
                    if len(set(raw)) > 2:
                        r2 = statistics.correlation(raw, values) ** 2
                        results.append((round(r2, 4), length, start, big_endian, signed))
    results.sort(key=lambda r: (r[0], r[1]), reverse=True)  # best fit, then the longest field
    return results


def pad_to_byte(items, start, length, big_endian):
    """Grow the field over unused top bits up to a byte boundary. A guess: the data cannot prove them."""
    rates = flip_rates(items)
    if big_endian:
        while start % 8 != 7 and not rates[start + 1]:
            start, length = start + 1, length + 1
    else:
        while (start + length) % 8 and not rates[start + length]:
            length += 1
    return start, length


NICE_FACTORS = sorted({m * 10.0 ** e for m in (1, 2, 2.5, 4, 5) for e in range(-4, 3)} | {2.0 ** -n for n in range(1, 9)})


def fit_scale(items, reference, start, length, big_endian, signed):
    """Factor and offset by least squares, factor rounded to a value OEMs commonly use."""
    datas = frames_at(items, [t for t, _ in reference])
    raw = [extract(int.from_bytes(d, "big" if big_endian else "little"), start, length, big_endian, signed)
           for d in datas]
    values = [v for _, v in reference]
    slope, intercept = statistics.linear_regression(raw, values)
    nice = min(NICE_FACTORS, key=lambda f: abs(f / abs(slope) - 1))
    factor = nice if abs(nice / abs(slope) - 1) < 0.03 else slope
    factor = factor if slope > 0 else -factor
    offset = round(statistics.fmean(v - factor * r for r, v in zip(raw, values)), 1) + 0.0
    return factor, offset, slope, intercept


def score(items, reference, start, length, big_endian, signed, factor, offset):
    """R² and RMS error of the decoded signal against the reference."""
    datas = frames_at(items, [t for t, _ in reference])
    decoded = [factor * extract(int.from_bytes(d, "big" if big_endian else "little"), start, length,
                                big_endian, signed) + offset for d in datas]
    values = [v for _, v in reference]
    rms = statistics.fmean((a - b) ** 2 for a, b in zip(decoded, values)) ** 0.5
    return statistics.correlation(decoded, values) ** 2, rms


def dbc_line(name, start, length, big_endian, signed, factor, offset, unit):
    order, sign = ("0" if big_endian else "1"), ("-" if signed else "+")
    return f' SG_ {name} : {start}|{length}@{order}{sign} ({factor:g},{offset:g}) [0|0] "{unit}" Vector__XXX'


frames = load_candump("synthetic.log")
survey(frames)

for pid, name, unit in ((0x0D, "VehicleSpeed", "km/h"), (0x0C, "EngineSpeed", "rpm")):
    reference = obd_reference(frames, pid)
    split = int(len(reference) * 0.6)
    train, test = reference[:split], reference[split:]  # fit on the first 60 %, check on the rest

    print(f"\n{name}: fields that follow the reference")
    ranked = rank_messages(frames, train)
    for rho, can_id, byte, size, order in ranked[:5]:
        print(f"  {can_id:03X}  byte {byte}  {size:2}-bit {order:<6}  rho {rho}")

    can_id = ranked[0][1]
    r2, length, start, big_endian, signed = search_bits(frames[can_id], train)[0]
    start, length = pad_to_byte(frames[can_id], start, length, big_endian)
    factor, offset, *_ = fit_scale(frames[can_id], train, start, length, big_endian, signed)
    r2, rms = score(frames[can_id], test, start, length, big_endian, signed, factor, offset)
    print(f"  0x{can_id:03X}: held-out R² {r2:.4f}, RMS error {rms:.2f} {unit}")
    print(dbc_line(name, start, length, big_endian, signed, factor, offset, unit))
