/*************************************************************************************************
    OBD-II_PIDs TEST CODE
    LOOVEE @ JUN24, 2017

    Query
    send id: 0x7df
      dta: 0x02, 0x01, PID_CODE, 0, 0, 0, 0, 0

    Response
    From id: 0x7E9 or 0x7EA or 0x7EB
      dta: len, 0x41, PID_CODE, byte0, byte1(option), byte2(option), byte3(option), byte4(option)

    https://en.wikipedia.org/wiki/OBD-II_PIDs

    Input a PID, then you will get reponse from vehicle, the input should be end with '\n'
***************************************************************************************************/
#include <SPI.h>

#define CAN_2515
// #define CAN_2518FD

// Set SPI CS Pin according to your hardware

#if defined(SEEED_WIO_TERMINAL) && defined(CAN_2518FD)
// For Wio Terminal w/ MCP2518FD RPi Hat：
// Channel 0 SPI_CS Pin: BCM 8
// Channel 1 SPI_CS Pin: BCM 7
// Interupt Pin: BCM25
const int SPI_CS_PIN  = BCM8;
const int CAN_INT_PIN = BCM25;
#else

// For Arduino MCP2515 Hat:
// the cs pin of the version after v1.1 is default to D9
// v0.9b and v1.0 is default D10
const int SPI_CS_PIN = 10;
const int CAN_INT_PIN = 2;
#endif


#ifdef CAN_2518FD
#include "mcp2518fd_can.h"
mcp2518fd CAN(SPI_CS_PIN); // Set CS pin
#endif

#ifdef CAN_2515
#include "mcp2515_can.h"
mcp2515_can CAN(SPI_CS_PIN); // Set CS pin
#endif

#define PID_ENGIN_PRM       0x0C
#define PID_VEHICLE_SPEED   0x0D
#define PID_COOLANT_TEMP    0x05

#define CAN_ID_PID          0x7DF

// CAN TX Variables
unsigned long prevTx = 0;
unsigned int invlTx = 100;
unsigned char PID_INPUT;
unsigned char getPid    = 0;
byte txVINData[] = {0x02,0x09,0x02,0x55,0x55,0x55,0x55,0x55};
unsigned char tmp[8] =  {0x02,0x09,0x02,0x00,0x00,0x00,0x00,0x00};

// send MSG for VIN Request
unsigned char len = 0;
unsigned char buf1[8];
unsigned char buf2[8];
unsigned char buf3[8];
unsigned char buf4[8];

void set_mask_filt() {
    /*
        set mask, set both the mask to 0x3ff
    */
    CAN.init_Mask(0, 0, 0x7FC);
    CAN.init_Mask(1, 0, 0x7FC); 

    /*
        set filter, we can receive id from 0x04 ~ 0x09
    */
    CAN.init_Filt(0, 0, 0x7E8);
    CAN.init_Filt(1, 0, 0x7E8);

    CAN.init_Filt(2, 0, 0x7E8);
    CAN.init_Filt(3, 0, 0x7E8);
    CAN.init_Filt(4, 0, 0x7E8);
    CAN.init_Filt(5, 0, 0x7E8); 
}

void sendPid(unsigned char __pid) {
    //unsigned char tmp[8] = {0x02, 0x01, __pid, 0, 0, 0, 0, 0};
    unsigned char tmp[8] =  {0x02,0x09,__pid,0,0,0,0,0};
    Serial.print("SEND PID: 0x");
    Serial.println(__pid, HEX);
    CAN.sendMsgBuf(CAN_ID_PID, 1, 8, tmp);
}

void setup() {
    Serial.begin(115200);
    while(!Serial){};

    while (CAN_OK != CAN.begin(CAN_1000KBPS)) {             // init can bus : baudrate = 500k
        Serial.println("CAN init fail, retry...");
        delay(100);
    }
    Serial.println("CAN init ok!");
    set_mask_filt();
}


void loop() {
    //Serial.println("loop");
    CAN.sendMsgBuf(CAN_ID_PID, 0, 8, txVINData);

    delay(10);
    taskCanRecv();   
    
   


  
}

void taskCanRecv() {
    unsigned char len = 0;
    //Serial.println(len, HEX);
    //unsigned char buf[8];

    while (CAN_MSGAVAIL == CAN.checkReceive()) {                // check if get data
        CAN.readMsgBuf(&len, buf1);    // read data,  len: data length, buf: data buf
        
        Serial.println("\r\n------------------------------------------------------------------");
        Serial.print("Get Data From id: 0x");
        Serial.println(CAN.getCanId(), HEX);
        //Serial.println(len, HEX);
        for (int i = 0; i < len; i++) { // print the data
            Serial.print("0x");
            if(buf1[i] < 0x10)                // If data byte is less than 0x10, add a leading zero
            {
              Serial.print("0");
            }
            Serial.print(buf1[i], HEX);
            Serial.print("\t");
        }
        Serial.println();
       
    }
    
delay(3000);
}


// END FILE