/*
You should type your messages as IDH,IDH,LEN,D0,D1,D2,D3,D4,D5,D6,D7\n, where:
IDH,IDL are the high and low parts of the CAN ID, in hexadecimal.
LEN is the data length (0 to 8).
D0 to D7 are the data bytes, in hexadecimal.
If you type 123,456,3,12,34,56, this will send a CAN message with ID 0x123456, length 3, and data bytes 0x12, 0x34, 0x56.

This code listens for user input. Once a newline is received, it parses the input and sends it as a CAN message. Note that this code does not handle errors in the input format, you should ensure you enter the data correctly.
*/

#include <mcp_can.h>
#include <SPI.h>

MCP_CAN CAN(10); // Set CS to pin 10
String inputString = "";

void setup() {
    Serial.begin(115200);

    while (CAN_OK != CAN.begin(CAN_500KBPS)) {            // init can bus : baudrate = 500k
        Serial.println("CAN BUS Shield init fail");
        Serial.println(" Init CAN BUS Shield again");
        delay(100);
    }
    Serial.println("CAN BUS Shield init ok!");
}

void loop() {
    unsigned char len = 0;
    unsigned char buf[8];

    if (CAN_MSGAVAIL == CAN.checkReceive()) {            // check if data coming
        CAN.readMsgBuf(&len, buf);    // read data,  len: data length, buf: data buf

        unsigned long canId = CAN.getCanId();

        Serial.println("-----------------------------");
        Serial.println("Get data from CAN BUS");
        Serial.print("CAN ID: ");
        Serial.println(canId, HEX); // Prints ID in HEX format

        Serial.print("CAN Data: ");
        for(int i = 0; i<len; i++) {    // print the data
            Serial.print(buf[i], HEX); // Prints data bytes in HEX format
            Serial.print("\t");
        }
        Serial.println();
    }

    // check if there's any serial available
    while (Serial.available()) {
        char inChar = (char)Serial.read();
        inputString += inChar;

        // if received character is a newline, parse the command
        if (inChar == '\n') {
            parseCommand(inputString);
            inputString = "";
        }
    }
}

void parseCommand(String command) {
    // parse the command in the format: IDH,IDH,LEN,D0,D1,D2,D3,D4,D5,D6,D7
    int parts[11];
    int i = 0;

    int idx1 = 0;
    int idx2 = command.indexOf(',');
    while (idx2 != -1 && i < 11) {
        parts[i++] = strtol(command.substring(idx1, idx2).c_str(), NULL, 16);
        idx1 = idx2 + 1;
        idx2 = command.indexOf(',', idx1);
    }
    if (i < 11) {
        parts[i++] = strtol(command.substring(idx1).c_str(), NULL, 16);
    }

    // sanity check
    if (parts[2] > 8 || parts[2] < 0) {
        Serial.println("Invalid length");
        return;
    }

    // prepare id and data
    unsigned long canId = (parts[0] << 8) + parts[1];
    unsigned char len = parts[2];
    unsigned char data[8];
    for (int i = 0; i < len; i++) {
        data[i] = parts[3 + i];
    }

    // send the message
    CAN.sendMsgBuf(canId, 0, len, data);
    Serial.println("Sent CAN message");
}