#include <SPI.h>
#include <mcp2515.h>
#include <stdio.h>
#include <string.h>

// Uno R3 + MCP2515: CS=10, 8 MHz crystal, classic CAN at 125 kbit/s.
MCP2515 canController(10, 4000000);
const uint8_t DEVICE_ID = 49;
const unsigned long HEARTBEAT_MS = 2000;
const uint8_t LOG_CAPACITY = 16;
const uint8_t TXREQ0_STATUS = 0x04; // MCP2515 READ_STATUS: TXB0 pending.

struct PacketLog {
    struct can_frame packet;
    bool outgoing;
};
PacketLog logQueue[LOG_CAPACITY];
uint8_t logHead = 0, logTail = 0, logCount = 0;
uint16_t droppedLogs = 0;
bool hardwareOverflow = false, heartbeatProblem = false;
char logLine[224];
size_t logLength = 0, logPosition = 0;
uint8_t messageCounter = 0;
unsigned long lastHeartbeat = 0;
bool ready = false;

uint32_t encodeId(uint8_t priority, uint8_t protocol, uint8_t target,
                  uint8_t sender, uint8_t counter, uint8_t index,
                  bool first, bool last) {
    return ((uint32_t)priority << 27)   // 2 bits: 28-27
        | ((uint32_t)protocol << 24)   // 3 bits: 26-24
        | ((uint32_t)target << 17)     // 7 bits: 23-17
        | ((uint32_t)sender << 10)     // 7 bits: 16-10
        | ((uint32_t)counter << 7)     // 3 bits: 9-7
        | ((uint32_t)index << 2)       // 5 bits: 6-2
        | (first ? 2UL : 0UL)         // 1 bit: 1
        | (last ? 1UL : 0UL);          // 1 bit: 0
}

void queuePacketLog(const struct can_frame &packet, bool outgoing) {
    if (logCount == LOG_CAPACITY) {
        if (droppedLogs != 65535) ++droppedLogs;
        return;
    }
    logQueue[logTail].packet = packet;
    logQueue[logTail].outgoing = outgoing;
    logTail = (logTail + 1) % LOG_CAPACITY;
    ++logCount;
}

void queueReceivedPacket(const struct can_frame &packet) {
    if (!(packet.can_id & CAN_EFF_FLAG)
        || (packet.can_id & (CAN_RTR_FLAG | CAN_ERR_FLAG))
        || packet.can_dlc > 8) return;

    const uint8_t target = (packet.can_id >> 17) & 0x7F;
    if (target != DEVICE_ID && target != 0) return;

    const uint8_t sender = (packet.can_id >> 10) & 0x7F;
    if (sender == 0) return; // Reserved: only target ID 0 means broadcast.

    const uint8_t index = (packet.can_id >> 2) & 0x1F;
    if ((packet.can_id & 2) && index != 0) return;

    queuePacketLog(packet, false);
}

void pollReception() {
    if (canController.getErrorFlags()
        & (MCP2515::EFLG_RX0OVR | MCP2515::EFLG_RX1OVR)) {
        hardwareOverflow = true;
        canController.clearRXnOVRFlags();
    }

    // Read both buffers from one snapshot, rather than always preferring RXB0.
    const uint8_t pending = canController.getInterrupts();
    struct can_frame packet;
    if ((pending & MCP2515::CANINTF_RX0IF)
        && canController.readMessage(MCP2515::RXB0, &packet) == MCP2515::ERROR_OK) {
        queueReceivedPacket(packet);
    }
    if ((pending & MCP2515::CANINTF_RX1IF)
        && canController.readMessage(MCP2515::RXB1, &packet) == MCP2515::ERROR_OK) {
        queueReceivedPacket(packet);
    }
}

void pollHeartbeat() {
    const unsigned long now = millis();
    if (now - lastHeartbeat < HEARTBEAT_MS) return;
    lastHeartbeat = now;

    // Do not overwrite a frame still awaiting arbitration/ACK. Never wait here.
    if ((canController.getStatus() & TXREQ0_STATUS)
        || (canController.getErrorFlags() & MCP2515::EFLG_TXBO)) {
        heartbeatProblem = true;
        return;
    }

    struct can_frame heartbeat = {}; // Zero-initialize all payload bytes.
    heartbeat.can_id = CAN_EFF_FLAG | encodeId(
        3, 0, 0, DEVICE_ID, messageCounter, 0, true, true);
    heartbeat.can_dlc = 8; // Transmit the full eight-byte zero payload.
    messageCounter = (messageCounter + 1) % 8;
    canController.clearTXInterrupts();
    if (canController.sendMessage(MCP2515::TXB0, &heartbeat) != MCP2515::ERROR_OK) {
        heartbeatProblem = true;
    }
    else {
        // The controller accepted a transmit request; completion is not confirmed.
        queuePacketLog(heartbeat, true);
    }
}

void prepareLogLine() {
    if (logCount) {
        const PacketLog entry = logQueue[logHead];
        const struct can_frame &packet = entry.packet;
        logHead = (logHead + 1) % LOG_CAPACITY;
        --logCount;
        const uint32_t id = packet.can_id & CAN_EFF_MASK;
        // Explicit unsigned casts match printf's %u on AVR.
        logLength = snprintf(logLine, sizeof(logLine),
            "%s: priority=%u protocol=%u target_id=%u sender_id=%u "
            "message_counter=%u packet_index=%u is_first_packet=%u is_last_packet=%u payload=[",
            entry.outgoing ? "QUEUED" : "RECV",
            (unsigned)((id >> 27) & 3), (unsigned)((id >> 24) & 7),
            (unsigned)((id >> 17) & 127), (unsigned)((id >> 10) & 127),
            (unsigned)((id >> 7) & 7), (unsigned)((id >> 2) & 31),
            (unsigned)((id >> 1) & 1), (unsigned)(id & 1));
        const char hex[] = "0123456789ABCDEF";
        for (uint8_t i = 0; i < packet.can_dlc; ++i) {
            if (i) logLine[logLength++] = ' ';
            logLine[logLength++] = hex[packet.data[i] >> 4];
            logLine[logLength++] = hex[packet.data[i] & 15];
        }
        logLine[logLength++] = ']';
        logLine[logLength++] = '\r';
        logLine[logLength++] = '\n';
    } else if (droppedLogs) {
        logLength = snprintf(logLine, sizeof(logLine),
            "Serial log queue full: %u packet logs dropped\r\n", (unsigned)droppedLogs);
        droppedLogs = 0;
    } else if (hardwareOverflow) {
        strcpy(logLine, "MCP2515 receive overflow: packets lost\r\n");
        logLength = strlen(logLine);
        hardwareOverflow = false;
    } else if (heartbeatProblem) {
        strcpy(logLine, "Heartbeat pending/failed; check CAN peer and bus\r\n");
        logLength = strlen(logLine);
        heartbeatProblem = false;
    } else {
        logLength = 0;
    }
    logPosition = 0;
}

void pollLogging() {
    if (logPosition == logLength) prepareLogLine();
    const int available = Serial.availableForWrite();
    if (available <= 0) return;
    size_t count = logLength - logPosition;
    if (count > (size_t)available) count = available;
    if (count) {
        // Only enqueue what fits: never wait for serial transmission.
        logPosition += Serial.write(
            (const uint8_t *)logLine + logPosition, count);
    }
}

void setup() {
    Serial.begin(115200);
    SPI.begin();
    ready = canController.reset() == MCP2515::ERROR_OK
        && canController.setBitrate(CAN_125KBPS, MCP_8MHZ) == MCP2515::ERROR_OK
        && canController.setNormalMode() == MCP2515::ERROR_OK;
    if (ready) Serial.println(F("Device 49: classic CAN logger + 2s heartbeat"));
    else Serial.println(F("CAN initialization failed; reset Arduino"));
    lastHeartbeat = millis();
}

void loop() {
    if (ready) {
        pollReception();
        pollHeartbeat();
    }
    pollLogging();
}