// CAN Send Example
//

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

#define CAN0_INT 2
MCP_CAN CAN0(10);     // Set CS to pin 10
long unsigned int rxId;
unsigned char len = 0;
unsigned char rxBuf[8];
char msgString[128];                        // Array to store serial string



/*Dodatkowe*/
byte Kod[8];

int a = 0;
int b = 0;
int ID = 0;
int Length = 0;
int temp;

char Input[29];
char BufferID[5];
char BufferLoop[4];
char BufferLength[2];

String IDString = "";
String commandString = "";
String LengthString = "";


/*Dodatkowe*/

void setup()
{
  pinMode(LED_BUILTIN, OUTPUT);
  Serial.begin(115200);
  pinMode(CAN0_INT, INPUT);
  // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the masks and filters disabled.
  if(CAN0.begin(MCP_ANY, CAN_100KBPS, MCP_8MHZ) == CAN_OK) Serial.println("MCP2515 Initialized Successfully!");
  else Serial.println("Error Initializing MCP2515...");

  CAN0.setMode(MCP_NORMAL);   // Change to normal mode to allow messages to be transmitted
      /*
Input[0] = '0';
Input[1] = '1';
Input[2] = '7';
Input[3] = '0';

Input[4] = '8';

Input[5] = '0';
Input[6] = '9';
Input[7] = '5';

Input[8] = '0';
Input[9] = '8';
Input[10] = '9';

Input[11] = '0';
Input[12] = '4';
Input[13] = '0';

Input[14] = '2';
Input[15] = '5';
Input[16] = '4';
//
Input[17] = '0';
Input[18] = '5';
Input[19] = '2';

Input[20] = '2';
Input[21] = '5';
Input[22] = '5';

Input[23] = '1';
Input[24] = '2';
Input[25] = '8';

Input[26] = '1';
Input[27] = '5';
Input[28] = '3';
*/
}







byte data[8] = {0x45, 0x40, 0x21, 0x8F, 0xFE};
byte data2[8] = {0xFF, 0x12,0xF7};
byte data3[8] = {0xFD,0xFF,0xE0,0xF3,0x00,0x30,0xFC,0xBA};
byte data5[8] = {0xA2,0x71,0x1E,0xFF,0xff,0xff,0xEA};
byte data6[8] = {0x5F,0x59,0x28,0xFE,0x34,0xFF,0x80,0x99};
byte data69[8] = {0x6C,0x06,0x28,0xFE,0x00,0x00,0x94,0x00};
int counter=0x0;
void loop()
{

  if (Serial.available() > 0) {
  digitalWrite(LED_BUILTIN, HIGH);   // turn the LED on (HIGH is the voltage level)
  delay(1500);                       // wait for a second
  digitalWrite(LED_BUILTIN, LOW);    // turn the LED off by making the voltage LOW
    for(int xdi =0; xdi < 29; xdi++) {
      Input[xdi] = Serial.read();
      delay(30);
    }
  }
  if(sizeof(Input)>27){
  BufferID[0] = Input[0];
  BufferID[1] = Input[1];
  BufferID[2] = Input[2];
  BufferID[3] = Input[3];
  BufferID[4] = 0;

  IDString = BufferID;
  ID = IDString.toInt();


  BufferLength[0] = Input[4];
  BufferLength[1] = 0;
  LengthString = BufferLength;
  Length = LengthString.toInt();

  for(int i = 5 ;i<29;i+=3){

    BufferLoop[0] = Input[i];
    BufferLoop[1] = Input[i+1];
    BufferLoop[2] = Input[i+2];
    BufferLoop[3] = 0;

    commandString = BufferLoop; //Byte[] to String
    temp = commandString.toInt(); //String to Int
    //Serial.print("temp (Int) : "); Serial.println(temp);
    Kod[b] = (byte)temp; //Int to Byte
    //Serial.print("temp (Byte) : "); Serial.println(Kod[b]);
    b++;
    }
    if (counter>0x0f) counter=0x0;
    //byte sndStat2 = CAN0.sendMsgBuf(0xAA, 8, Kod);
  }


  if (counter>0x0f) counter=0x0;
  byte data4[8] = {0xF0+counter,0xFF,0xE0,0xF3,0x00,0x30,0xFC,0xBA};
  // send data:  ID = 0x100, Standard CAN Frame, Data length = 8 bytes, 'data' = array of data bytes to send
  /*
  Serial.print("ID: ");
  Serial.println(ID);
  //Serial.print(BufferID[0]); Serial.print(BufferID[1]);Serial.print(BufferID[2]);Serial.print(BufferID[10]);

  Serial.print("Length: ");
  Serial.println(Length);

  for(int i = 0; i < 8; i++)
  {
  Serial.print("Kod: ");
  Serial.println(Kod[i]);
  }


  //Serial.println("Dlugosc: " + Length);
  //Serial.println("Koniec");
*/

  byte sndStat = CAN0.sendMsgBuf(0x130, 5, data);
  //byte sndStat2 = CAN0.sendMsgBuf(ID, Length, Kod);

  //byte sndStat2 = CAN0.sendMsgBuf(0x21A, 0, 3, data2);
  //byte sndStat3 = CAN0.sendMsgBuf(0x34F, 0, 2, data3);
  //byte sndStat4 = CAN0.sendMsgBuf(0x0C0, 0, 8, data4);
  //byte sndStat5 = CAN0.sendMsgBuf(0x362, 0, 7, data5);
  //byte sndStat6 = CAN0.sendMsgBuf(0xAA, 0, 8, data6);
  counter++;
  //Serial.println((String)data4[0]);


  if(sndStat == CAN_OK){
    Serial.println("Message Sent Successfully!");
  } else {
    Serial.println("Error Sending Message...");
  }

 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);

    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);
      }
    }

    //Serial.println();
  }

  delay(100);   // send data per 100ms
}

/*********************************************************************************************************
  END FILE
*********************************************************************************************************/