/*
  Please add MCP_CAN_LIB to your library first........
  MCP_CAN_LIB file in M5stack lib examples -> modules -> COMMU ->
  MCP_CAN_lib.rar
*/

#include <M5Core2.h>
#include <mcp_can.h>
#include "m5_logo.h"

/**
 * variable for loop
 */

byte data[8] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07};

/**
 * variable for CAN
 */
long unsigned int rxId;
unsigned char len = 0;
unsigned char rxBuf[8];
char msgString[128];  // Array to store serial string

#define CAN0_INT 2  // Set INT to pin 2
MCP_CAN CAN0(27);   // Set CS to pin 27

void init_can();
void test_can();

void setup() {
    M5.begin(true, true, false, true, kMBusModeOutput);
    //  kMBusModeOutput,powered by USB or Battery
    //  kMBusModeInput,powered by outside input

    Serial.begin(9600);
    Serial2.begin(9600, SERIAL_8N1, 13, 14);
    //  The 13(RX), 14(TX) pins of the CORE2 correspond to the 16(RX), 17(TX)
    //  pins of the COMX
    // Please make sure that the dialing switch of COMX is set to 16(RX),
    // 17(TX).

    M5.Lcd.pushImage(0, 0, 320, 240, (uint16_t *)gImage_logoM5);
    delay(500);
    M5.Lcd.setTextColor(BLACK);
    // M5.Lcd.setTextSize(1);

    init_can();
    Serial.println("Test CAN...");
}

void loop() {
    if (M5.BtnA.wasPressed()) {
        M5.Lcd.clear();
        M5.Lcd.printf("CAN Test A!\n");
        M5.Lcd.pushImage(0, 0, 320, 240, (uint16_t *)gImage_logoM5);
        init_can();
        Serial.println("Test CAN...");
    }
    test_can();
    M5.update();
}

void init_can() {
    M5.Lcd.setTextSize(1);
    M5.Lcd.setCursor(0, 10);
    M5.Lcd.pushImage(0, 0, 320, 240, (uint16_t *)gImage_logoM5);
    delay(500);

    M5.Lcd.printf("CAN Test A!\n");
    M5.Lcd.printf("Receive first, then testing for sending function!\n");

    // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the
    // masks and filters disabled.
    if (CAN0.begin(MCP_ANY, CAN_1000KBPS, MCP_8MHZ) == CAN_OK)
        Serial.println("MCP2515 Initialized Successfully!");
    else
        Serial.println("Error Initializing MCP2515...");

    CAN0.setMode(MCP_NORMAL);  // Set operation mode to normal so the MCP2515
                               // sends acks to received data.

    pinMode(CAN0_INT, INPUT);  // Configuring pin for /INT input

    Serial.println("MCP2515 Library Receive Example...");
}

void test_can() {
    if (!digitalRead(CAN0_INT))  // If CAN0_INT pin is low, read receive buffer
    {
        CAN0.readMsgBuf(
            &rxId, &len,
            rxBuf);  // Read data: len = data length, buf = data byte(s)

        if ((rxId & 0x80000000) ==
            0x80000000)  // Determine if ID is standard (11 bits) or extended
                         // (29 bits)
            sprintf(msgString, "Extended ID: 0x%.8lX  DLC: %1d  Data:",
                    (rxId & 0x1FFFFFFF), len);
        else
            sprintf(msgString,
                    "Standard ID: 0x%.3lX       DLC: %1d  Data:", rxId, len);

        Serial.print(msgString);
        M5.Lcd.printf(msgString);
        if ((rxId & 0x40000000) ==
            0x40000000) {  // Determine if message is a remote request frame.
            sprintf(msgString, " REMOTE REQUEST FRAME");
            Serial.print(msgString);
        } else {
            for (byte i = 0; i < len; i++) {
                sprintf(msgString, " 0x%.2X", rxBuf[i]);
                Serial.print(msgString);
                M5.Lcd.printf(msgString);
            }
        }
        M5.Lcd.printf("\n");
        Serial.println();
    }
}
