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

#define CAN_CS_PIN 5
#define CAN_INT_PIN 4

MCP_CAN CAN(CAN_CS_PIN);
bool canReady = false;
// void init_can() {
//     Serial.begin(115200);

//     while (CAN_OK != CAN.begin(MCP_ANY, CAN_500KBPS, MCP_8MHZ) == CAN_OK) {
//         Serial.println("MCP2515 initialization failed!");
//         delay(100);
//     }
    
//     Serial.println("MCP2515 initialization successful!");
//     CAN.setMode(MCP_NORMAL);
    
// }

void init_can() {

    Serial.begin(115200);
    SPI.begin(18, 19, 23, CAN_CS_PIN);
    pinMode(CAN_INT_PIN, INPUT);
    delay(100);

    byte result = CAN.begin(MCP_ANY, CAN_250KBPS, MCP_16MHZ);

    Serial.print("CAN init result: ");
    Serial.println(result);

    if (result == CAN_OK) {
        Serial.println("MCP2515 initialization successful!");
        byte modeResult = CAN.setMode(MCP_NORMAL);
        Serial.print("CAN error flags: ");
        Serial.println(CAN.checkError(), HEX);
        Serial.print("Set normal mode result: ");
        Serial.println(modeResult);
        canReady = true;
    } else {
        Serial.println("MCP2515 initialization failed!");
        canReady = false;
    }
}

void receive_can() {

    unsigned long rxId;
    unsigned char len = 0;
    unsigned char rxBuf[8];

    if (CAN_MSGAVAIL == CAN.checkReceive()) {
        CAN.readMsgBuf(&rxId, &len, rxBuf);

        Serial.print("ID: ");
        Serial.print(rxId, HEX);
        Serial.print(" Data: ");

        for(int i = 0; i < len; i++) {
            Serial.print(rxBuf[i], HEX);
            Serial.print(" ");
        }
        Serial.println();
    }
}

void send_can() {
    if (!canReady) {
    Serial.println("CAN not ready, not sending");
    delay(1000);
    return;
}
    byte data[8] = {0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88};
    Serial.println("Sending ID 0x100...");

byte status = CAN.sendMsgBuf(0x100, 0, 8, data);

Serial.print("CAN send status = ");
Serial.println(status);

if (status == CAN_OK) {
    Serial.println("Message sent successfully");
} else {
    Serial.println("Sending failed");
}
    // Serial.println("Sending ID 0x100...");
    // if (CAN.sendMsgBuf(0x100, 0, 8, data) == CAN_OK) {
    //     Serial.println("Message sent successfully!");
    // } else {
    //     Serial.println("sending failed.");
    // }
    delay(1000);
}



