#include <SPI.h>
#include <mcp2515.h>
#include "nRF24L01.h"
#include "RF24.h"

struct can_frame canMsg;
MCP2515 mcp2515(10);
byte rmp1 = 0;
byte rmp2 = 0;
byte actualSpeed = 0;
int msg[1];
RF24 radio(7,8);
const uint64_t pipe = 0xE8E8F0F0E1LL;
byte diodeIntensity = 0;

void setup() {

  Serial.begin(115200);
  SPI.begin();
  radio.begin();
  radio.openWritingPipe(pipe);

  mcp2515.reset();
  mcp2515.setBitrate(CAN_500KBPS);
  mcp2515.setNormalMode();

  Serial.println("------- CAN Read ----------");
  Serial.println("ID  DLC   DATA");

  pinMode(3, OUTPUT);
}

void loop() {

  if (mcp2515.readMessage(&canMsg) == MCP2515::ERROR_OK) {

    Serial.print(canMsg.can_id, HEX); // print ID
    Serial.print(" ");
    Serial.print(canMsg.can_dlc, HEX); // print DLC
    Serial.print(" ");

    rmp1 = canMsg.data[5];
    rmp2 = canMsg.data[6];
    actualSpeed = canMsg.data[7];

    for (int i = 0; i<canMsg.can_dlc; i++)  {

      Serial.print(canMsg.data[i],HEX);
      Serial.print(" ");
    }

    // Wypisanie odebrancyh wartosci
    Serial.println("");
    Serial.print("Rmp: ");
    Serial.println(rmp1);
    Serial.print("Aktualna predkosc: ");
    Serial.println(actualSpeed);
    Serial.print("RMP NRF: ");

    // Przesłanie przez NRF odebranych wartosci
    msg[0] = rmp1;
    msg[1] = rmp2;
    radio.write(msg, 1);
    Serial.print(msg[0]);
    Serial.println("00");

    diodeIntensity = map(rmp1, 0, 31, 0, 255);

    analogWrite(3, diodeIntensity);
  }
}