#include <SPI.h>
#include <TimerOne.h>
#include "Arduino.h"
#include "mcp_can.h"

//#define __DEBUG

#define BODY_HLFAKE_PIN 3 // тут у нас висит бодик, пин 21, RLEW
#define LED_PIN 4
#define DEBUG_PIN 9
#define CANBUS_CS_PIN 10

MCP_CAN CAN0(CANBUS_CS_PIN);
char msgString[128];
unsigned char len = 0;
unsigned char buf[8];
unsigned long int rxId = 0;

void blink(int delay_ms = 200) {
digitalWrite(LED_PIN, HIGH);
delay(delay_ms);
digitalWrite(LED_PIN, LOW);
delay(delay_ms);
}

// таймер для эмуляции штатных фар, пин 21 и 24 на бодике.
// тут нужно просто шпарить импульсы +12 вольт 5гц пока
// включен свет фар

void Timer1Handler()
{
digitalWrite(BODY_HLFAKE_PIN,
digitalRead(BODY_HLFAKE_PIN) ^ 1);
}

void setTimer5HzForStockHeadlightFeedbackEmulation() {
Timer1.initialize(50000);
Timer1.attachInterrupt(Timer1Handler);
}

void initializeCanModule() {
if (CAN0.begin(MCP_ANY, CAN_500KBPS, MCP_8MHZ) == CAN_OK) {
CAN0.setMode(MCP_NORMAL);
}
}

void setup() {
#ifdef __DEBUG
Serial.begin(115200);
#endif
pinMode(LED_PIN, OUTPUT);
pinMode(BODY_HLFAKE_PIN, OUTPUT);
setTimer5HzForStockHeadlightFeedbackEmulation();
initializeCanModule();
#ifdef __DEBUG
Serial.println("CAN Driver : Initialized");
#endif
}


/*
*
--------------------------------
-- пакеты штатного приборного щита. Щиток шлет их
-- когда переключается состояние света
--------------------------------

08.826 622 8 12 00 00 00 00 00 00 00 - свет выключен
18.594 622 8 12 00 E8 30 00 00 00 00 - свет в авто, ближний включился
46.506 622 8 12 00 48 10 00 00 00 00 - габариты включены принудительно, свет выключен
18.988 622 8 12 00 88 30 00 00 00 00 - ближний включен принудительно
50.934 622 8 12 00 88 70 00 00 00 00 - дальний включен принудительно

25.601 622 8 12 80 88 70 00 00 00 00 - мограю дальним, первый пакет сразу
25.771 622 8 12 80 88 70 00 00 20 00 - второй пакет
26.261 622 8 12 80 88 30 00 00 00 00 - отпустил флеш дальнего при включенном ближнем

10.863 622 8 12 80 00 70 00 00 20 00 - дальний вкл и быстро сска!
*/

void EnableHighBeam() {
byte data[8] = { 0x70, 0x05, 0x30, 0x15, 0x00, 0x20, 0x00, 0x00 };
CAN0.sendMsgBuf(0x750, 8, data);
}

void DisableHighBeam() {
byte data[8] = { 0x70, 0x05, 0x30, 0x15, 0x00, 0x10, 0x00, 0x00 };
CAN0.sendMsgBuf(0x750, 8, data);
}

void selectRightModeForHeadlights(unsigned char * dataBytes) {
if (dataBytes[3] == 0x70) {
EnableHighBeam();
}
else if (dataBytes[3] == 0x30) {
DisableHighBeam();
}
}


void loop() {

if (CAN_MSGAVAIL == CAN0.checkReceive()){

CAN0.readMsgBuf(&rxId, &len, buf);

if (rxId == 0x622) {

if (len == 8 && buf[0] == 0x12) {

#ifdef __DEBUG
if ((rxId & 0x80000000) == 0x80000000) {
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) {
sprintf(msgString, " REMOTE REQUEST FRAME");
Serial.print(msgString);
} else {
for (byte i = 0; i<len; i++) {
sprintf(msgString, " 0x%.2X", buf);
Serial.print(msgString);
}
}

Serial.println();
#endif

selectRightModeForHeadlights(buf);

}
}
}
}