// CAN Receive and send Example
//

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

long unsigned int rxId;
unsigned char len = 0;
unsigned char rxBuf[8];
int EngineTemp;
int RoadSpeed;
int val;
char msgString[128];                        // Array to store serial string
char input[3];

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


void setup()
{
  Serial.begin(115200);
  
  // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the masks and filters disabled.
  if(CAN0.begin(MCP_ANY, CAN_500KBPS, MCP_16MHZ) == 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 loop()
{
  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
      if(rxId==0x288){
        byte data320[8] = {0x00, 0x00, 0x00, 0x36, 0x00, 0x00, 0x00, 0x00}; // Kombi1
        byte data420[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; // Kombi2
        byte data520[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; // kombi 3
        // 050H - Airbag 1, 
        // 2C0 - Rad 1, 8mms raster, 5 bytes
        // 590 - Level 1, 48ms raster 6 byte
        // 5E0 - Climate, 20ms rast 8 byte
        // 573 - ZAS 1, 2 byte ?
        byte data1A0[8] = {0x00, 0x00, 0x00, 0x36, 0x00, 0x00, 0x00, 0x00}; //ABS1
        byte data4A0[8] = {0x00, 0x36, 0x00, 0x36, 0x00, 0x36, 0x00, 0x36}; //ABS3
        
        byte sndStat = CAN0.sendMsgBuf(0x320, 0, 8, data320);
        if(sndStat == CAN_OK){
          Serial.print("320 okay");
        } else {
          Serial.print("320 error");
        }
        sndStat = CAN0.sendMsgBuf(0x420, 0, 8, data420);
        if(sndStat == CAN_OK){
          Serial.print("\t420 okay");
        } else {
          Serial.print("\t420 error");
        }
        sndStat = CAN0.sendMsgBuf(0x520, 0, 8, data520);
        if(sndStat == CAN_OK){
          Serial.print("\t520 okay");
        } else {
          Serial.print("\t520 error");
        }

        sndStat = CAN0.sendMsgBuf(0x1A0, 0, 8, data1A0);
        if(sndStat == CAN_OK){
          Serial.print("\t1A0 okay");
        } else {
          Serial.print("\t1A0 error");
        }

        sndStat = CAN0.sendMsgBuf(0x4A0, 0, 8, data4A0);
        if(sndStat == CAN_OK){
          Serial.print("\t4A0 okay");
        } else {
          Serial.print("\t4A0 error");
        }
        Serial.println();
      }  
  }
}