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

#define CAN_CS_PIN 10
MCP_CAN CAN(CAN_CS_PIN);

unsigned long lastSend = 0;
uint8_t counter = 0;

void setup() {
  Serial.begin(115200);
  if (CAN.begin(MCP_ANY, CAN_100KBPS, MCP_8MHZ) == CAN_OK) Serial.println("CAN OK!");
  CAN.setMode(MCP_NORMAL);
}

// TWOJA SPRAWDZONA FUNKCJA - Nadpisuje ostatni bajt (7) sumą XOR
void sendWithCRC(unsigned long id, unsigned char* data) {
  byte crc = 0;
  for (int i = 0; i < 7; i++) crc ^= data[i];
  data[7] = crc; // To jest kluczowe miejsce!
  CAN.sendMsgBuf(id, 0, 8, data);
}

void loop() {
  if (millis() - lastSend > 40) { 

    // 0. ALTERNATYWNY ZAPŁON (ID 0x271) - Włączenie radia
    unsigned char data271[] = {0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    CAN.sendMsgBuf(0x271, 0, 8, data271);

    // 1. HANDSHAKE (ID 0x651) - Dodaj to, żeby połączenie było stabilne
    unsigned char hsk[] = {0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00};
    CAN.sendMsgBuf(0x651, 0, 8, hsk);

    // 2. ZAPŁON + WSTECZNY (ID 0x351) 
    // Bajt 0: 0x07 = Wsteczny ON, 0x01 = Wsteczny OFF
    unsigned char sys[] = {0x01, 0x07, 0x8E, 0x01, 0x00, 0x00, counter, 0x00};
    sendWithCRC(0x351, sys); 

    // 3. PRĘDKOŚĆ 
    unsigned char spd[] = {0x00, 0x00, 0x22, 0x00, 0x00, 0x00, 0x00, 0x00};
    CAN.sendMsgBuf(0x359, 0, 8, spd);

    // 4. DRZWI bajt 1, 0x01 LP, 0x02 PP, 0x03 LP PP, 0x04 LT, 0x05 LP LT, 0x06 PP LT, 0x07 PP LP LT, 0x08 PT, 0x09 LP PT
    // 0x0A PP PT, 0x0B LP PP PT, 0x0C LT PT, 0x0D LP LT PT, 0x0E PP LT PT, 0x0F LP PP LT PT, 0x20 B
    unsigned char d470[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, counter, 0x00};
    sendWithCRC(0x470, d470);

    // 5. ŚWIATŁA, Podświetlenie przycisków
    unsigned char d635[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    CAN.sendMsgBuf(0x635, 0, 8, d635);
    
    // 6. NAPIĘCIE 
    unsigned char d571[] = {0xB6, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    CAN.sendMsgBuf(0x571, 0, 8, d571);

    // 7. Temperatura zewnętrzna bajt 5
    byte msgOdo[] = {0x40, 0xE2, 0x01, 0x00, 0x00, 0x88, 0x00, 0x00};
    CAN.sendMsgBuf(0x527, 0, 8, msgOdo);

    // 8. Obroty bajt 2
    byte msg35B[] = {0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x00, 0x00};
    CAN.sendMsgBuf(0x35B, 0, 8, msg35B);

    // 9. Płyn spryskiwaczy i rezerwa bajt 0, 0x00 stan normalny, 0x02 rezerwa, 0x04 płyn, 0x06 rezerwa i płyn
    // Paliwo ilość bajt 3
    byte msgStatus621[] = {0x00, 0x00, 0x00, 0x45, 0x00, 0x00, 0x00, 0x00};
    CAN.sendMsgBuf(0x621, 0, 8, msgStatus621);

    // 10. Przebieg bajt 1, 2 i 3
    byte msgOdo65D[] = {0x00, 0xFF, 0xDD, 0x11, 0x00, 0x00, 0x00, 0x00}; 
    CAN.sendMsgBuf(0x65D, 0, 8, msgOdo65D);

    // 11. Ręczny bajt 0 (0x01 zaciągnięty, 0x64 OK)
    // Sterowanie z kierownicy bajt 1, 01 vol down, 02 vol up, 10 zatrzymanie vol, 
    // Czujniki bajt 1
    // byte msgStatus79D[] = {0x64, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    // CAN.sendMsgBuf(0x79D, 0, 8, msgStatus79D);

    // 12. Pasy bajt 1 0x00, 0xFF
    byte msgStatus951[] = {0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    CAN.sendMsgBuf(0x951, 0, 8, msgStatus951);

    // 13. Skręt kierownicy bajt 1
    byte turn[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    CAN.sendMsgBuf(0x3C3, 0, 8, turn); 

    // 14. ? GATEWAY (ID 0x000) -> Podtrzymanie życia magistrali
    unsigned char d000[] = {0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    CAN.sendMsgBuf(0x000, 0, 8, d000);

    counter++; if (counter > 15) counter = 0;
    lastSend = millis();
  }
}