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

#define DCSBIOS_DEFAULT_SERIAL
#include "DcsBios.h"

MCP_CAN CAN_1(10);

char Swithes_Id[][10] = {
  "CMSP_ARW1", "CMSP_ARW2", "CMSP_ARW3", "CMSP_ARW4",
  "CMSP_DISP", "CMSP_JMR", "CMSP_JTSN", "CMSP_MODE",
  "CMSP_MWS", "CMSP_RTN", "CMSP_RWR", "CMSP_UPDN"
};
char Potentiometer_Id[][10] = { "CMSP_BRT" };

byte can_buf[8];
byte rxBuf[8];
unsigned char len;
unsigned long int rxId;
byte Cauton_Led_LastState[48];
char DCS_buf[7];

char CMSP_STRING_2_1[] = "CHAF FLAR INTV CYCL";
char CMSP_STRING_2_2[] = "MWS  JMR  RWR  DISP";
char CMSP_STRING_2_3[] = "CHAF FLAR OTR1 PROG";

char CMSP_SEND_STRING_2_1[] = "CHF FLR INTV CYC";
char CMSP_SEND_STRING_2_2[] = "MWS JMR RWR DISP";
char CMSP_SEND_STRING_2_3[] = "CHF FLR OTR1 PRG";
char CMSP_SEND_STRING_2_4[] = "                ";

void onCmsp1Change(char* newValue) {
  can_buf[0] = newValue[0];
  can_buf[1] = newValue[1];
  can_buf[2] = newValue[2];
  can_buf[3] = newValue[3];
  can_buf[4] = newValue[5];
  can_buf[5] = newValue[6];
  can_buf[6] = newValue[7];
  can_buf[7] = newValue[8];
  CAN_TX(0x211, 8);
  can_buf[0] = newValue[10];
  can_buf[1] = newValue[11];
  can_buf[2] = newValue[12];
  can_buf[3] = newValue[13];
  can_buf[4] = newValue[14];
  can_buf[5] = newValue[15];
  can_buf[6] = newValue[16];
  can_buf[7] = newValue[17];
  CAN_TX(0x211, 8);
}
DcsBios::StringBuffer<19> cmsp1Buffer(0x1000, onCmsp1Change);

void onCmsp2Change(char* newValue) {
  if (memcmp(newValue, CMSP_STRING_2_1, 19) == 0 ) {
    CMSP_Send_String2(CMSP_SEND_STRING_2_1);
    return;
  };
  if (memcmp(newValue, CMSP_STRING_2_2, 19) == 0 ) {
    CMSP_Send_String2(CMSP_SEND_STRING_2_2);
    return;
  };
  if (memcmp(newValue, CMSP_STRING_2_3, 19) == 0 ) {
    CMSP_Send_String2(CMSP_SEND_STRING_2_3);
    return;
  };
  CMSP_Send_String2(CMSP_SEND_STRING_2_4);
}
DcsBios::StringBuffer<19> cmsp2Buffer(0x1014, onCmsp2Change);

void onClA1Change(unsigned int newValue) {
  Cauton_Led_State (0, newValue);
}
DcsBios::IntegerBuffer clA1Buffer(0x10d4, 0x0001, 0, onClA1Change);
void onClA2Change(unsigned int newValue) {
  Cauton_Led_State (1, newValue);
}
DcsBios::IntegerBuffer clA2Buffer(0x10d4, 0x0002, 1, onClA2Change);
void onClA3Change(unsigned int newValue) {
  Cauton_Led_State (2, newValue);
}
DcsBios::IntegerBuffer clA3Buffer(0x10d4, 0x0004, 2, onClA3Change);
void onClA4Change(unsigned int newValue) {
  Cauton_Led_State (3, newValue);
}
DcsBios::IntegerBuffer clA4Buffer(0x10d4, 0x0008, 3, onClA4Change);


void onClB1Change(unsigned int newValue) {
  Cauton_Led_State (4, newValue);
}
DcsBios::IntegerBuffer clB1Buffer(0x10d4, 0x0010, 4, onClB1Change);
void onClB2Change(unsigned int newValue) {
  Cauton_Led_State (5, newValue);
}
DcsBios::IntegerBuffer clB2Buffer(0x10d4, 0x0020, 5, onClB2Change);
void onClB3Change(unsigned int newValue) {
  Cauton_Led_State (6, newValue);
}
DcsBios::IntegerBuffer clB3Buffer(0x10d4, 0x0040, 6, onClB3Change);
void onClB4Change(unsigned int newValue) {
  Cauton_Led_State (7, newValue);
}
DcsBios::IntegerBuffer clB4Buffer(0x10d4, 0x0080, 7, onClB4Change);

void onClC1Change(unsigned int newValue) {
  Cauton_Led_State (8, newValue);
}
DcsBios::IntegerBuffer clC1Buffer(0x10d4, 0x0100, 8, onClC1Change);
void onClC2Change(unsigned int newValue) {
  Cauton_Led_State (9, newValue);
}
DcsBios::IntegerBuffer clC2Buffer(0x10d4, 0x0200, 9, onClC2Change);
void onClC3Change(unsigned int newValue) {
  Cauton_Led_State (10, newValue);
}
DcsBios::IntegerBuffer clC3Buffer(0x10d4, 0x0400, 10, onClC3Change);
void onClC4Change(unsigned int newValue) {
  Cauton_Led_State (11, newValue);
}
DcsBios::IntegerBuffer clC4Buffer(0x10d4, 0x0800, 11, onClC4Change);

void onClD1Change(unsigned int newValue) {
  Cauton_Led_State (12, newValue);
}
DcsBios::IntegerBuffer clD1Buffer(0x10d4, 0x1000, 12, onClD1Change);
void onClD2Change(unsigned int newValue) {
  Cauton_Led_State (13, newValue);
}
DcsBios::IntegerBuffer clD2Buffer(0x10d4, 0x2000, 13, onClD2Change);
void onClD3Change(unsigned int newValue) {
  Cauton_Led_State (14, newValue);
}
DcsBios::IntegerBuffer clD3Buffer(0x10d4, 0x4000, 14, onClD3Change);
void onClD4Change(unsigned int newValue) {
  Cauton_Led_State (15, newValue);
}
DcsBios::IntegerBuffer clD4Buffer(0x10d4, 0x8000, 15, onClD4Change);

void onClE1Change(unsigned int newValue) {
  Cauton_Led_State (16, newValue);
}
DcsBios::IntegerBuffer clE1Buffer(0x10d6, 0x0001, 0, onClE1Change);
void onClE2Change(unsigned int newValue) {
  Cauton_Led_State (17, newValue);
}
DcsBios::IntegerBuffer clE2Buffer(0x10d6, 0x0002, 1, onClE2Change);
void onClE3Change(unsigned int newValue) {
  Cauton_Led_State (18, newValue);
}
DcsBios::IntegerBuffer clE3Buffer(0x10d6, 0x0004, 2, onClE3Change);
void onClE4Change(unsigned int newValue) {
  Cauton_Led_State (19, newValue);
}
DcsBios::IntegerBuffer clE4Buffer(0x10d6, 0x0008, 3, onClE4Change);

void onClF1Change(unsigned int newValue) {
  Cauton_Led_State (20, newValue);
}
DcsBios::IntegerBuffer clF1Buffer(0x10d6, 0x0010, 4, onClF1Change);
void onClF2Change(unsigned int newValue) {
  Cauton_Led_State (21, newValue);
}
DcsBios::IntegerBuffer clF2Buffer(0x10d6, 0x0020, 5, onClF2Change);
void onClF3Change(unsigned int newValue) {
  Cauton_Led_State (22, newValue);
}
DcsBios::IntegerBuffer clF3Buffer(0x10d6, 0x0040, 6, onClF3Change);
void onClF4Change(unsigned int newValue) {
  Cauton_Led_State (23, newValue);
}
DcsBios::IntegerBuffer clF4Buffer(0x10d6, 0x0080, 7, onClF4Change);

void onClG1Change(unsigned int newValue) {
  Cauton_Led_State (24, newValue);
}
DcsBios::IntegerBuffer clG1Buffer(0x10d6, 0x0100, 8, onClG1Change);
void onClG2Change(unsigned int newValue) {
  Cauton_Led_State (25, newValue);
}
DcsBios::IntegerBuffer clG2Buffer(0x10d6, 0x0200, 9, onClG2Change);
void onClG3Change(unsigned int newValue) {
  Cauton_Led_State (26, newValue);
}
DcsBios::IntegerBuffer clG3Buffer(0x10d6, 0x0400, 10, onClG3Change);
void onClG4Change(unsigned int newValue) {
  Cauton_Led_State (27, newValue);
}
DcsBios::IntegerBuffer clG4Buffer(0x10d6, 0x0800, 11, onClG4Change);

void onClH1Change(unsigned int newValue) {
  Cauton_Led_State (28, newValue);
}
DcsBios::IntegerBuffer clH1Buffer(0x10d6, 0x1000, 12, onClH1Change);
void onClH2Change(unsigned int newValue) {
  Cauton_Led_State (29, newValue);
}
DcsBios::IntegerBuffer clH2Buffer(0x10d6, 0x2000, 13, onClH2Change);
void onClH3Change(unsigned int newValue) {
  Cauton_Led_State (30, newValue);
}
DcsBios::IntegerBuffer clH3Buffer(0x10d6, 0x4000, 14, onClH3Change);
void onClH4Change(unsigned int newValue) {
  Cauton_Led_State (31, newValue);
}
DcsBios::IntegerBuffer clH4Buffer(0x10d6, 0x8000, 15, onClH4Change);

void onClI1Change(unsigned int newValue) {
  Cauton_Led_State (32, newValue);
}
DcsBios::IntegerBuffer clI1Buffer(0x10d8, 0x0001, 0, onClI1Change);
void onClI2Change(unsigned int newValue) {
  Cauton_Led_State (33, newValue);
}
DcsBios::IntegerBuffer clI2Buffer(0x10d8, 0x0002, 1, onClI2Change);
void onClI3Change(unsigned int newValue) {
  Cauton_Led_State (34, newValue);
}
DcsBios::IntegerBuffer clI3Buffer(0x10d8, 0x0004, 2, onClI3Change);
void onClI4Change(unsigned int newValue) {
  Cauton_Led_State (35, newValue);
}
DcsBios::IntegerBuffer clI4Buffer(0x10d8, 0x0008, 3, onClI4Change);

void onClJ1Change(unsigned int newValue) {
  Cauton_Led_State (36, newValue);
}
DcsBios::IntegerBuffer clJ1Buffer(0x10d8, 0x0010, 4, onClJ1Change);
void onClJ2Change(unsigned int newValue) {
  Cauton_Led_State (37, newValue);
}
DcsBios::IntegerBuffer clJ2Buffer(0x10d8, 0x0020, 5, onClJ2Change);
void onClJ3Change(unsigned int newValue) {
  Cauton_Led_State (38, newValue);
}
DcsBios::IntegerBuffer clJ3Buffer(0x10d8, 0x0040, 6, onClJ3Change);
void onClJ4Change(unsigned int newValue) {
  Cauton_Led_State (39, newValue);
}
DcsBios::IntegerBuffer clJ4Buffer(0x10d8, 0x0080, 7, onClJ4Change);

void onClK1Change(unsigned int newValue) {
  Cauton_Led_State (40, newValue);
}
DcsBios::IntegerBuffer clK1Buffer(0x10d8, 0x0100, 8, onClK1Change);
void onClK2Change(unsigned int newValue) {
  Cauton_Led_State (41, newValue);
}
DcsBios::IntegerBuffer clK2Buffer(0x10d8, 0x0200, 9, onClK2Change);
void onClK3Change(unsigned int newValue) {
  Cauton_Led_State (42, newValue);
}
DcsBios::IntegerBuffer clK3Buffer(0x10d8, 0x0400, 10, onClK3Change);
void onClK4Change(unsigned int newValue) {
  Cauton_Led_State (43, newValue);
}
DcsBios::IntegerBuffer clK4Buffer(0x10d8, 0x0800, 11, onClK4Change);

void onClL1Change(unsigned int newValue) {
  Cauton_Led_State (44, newValue);
}
DcsBios::IntegerBuffer clL1Buffer(0x10d8, 0x1000, 12, onClL1Change);
void onClL2Change(unsigned int newValue) {
  Cauton_Led_State (45, newValue);
}
DcsBios::IntegerBuffer clL2Buffer(0x10d8, 0x2000, 13, onClL2Change);
void onClL3Change(unsigned int newValue) {
  Cauton_Led_State (46, newValue);
}
DcsBios::IntegerBuffer clL3Buffer(0x10d8, 0x4000, 14, onClL3Change);
void onClL4Change(unsigned int newValue) {
  Cauton_Led_State (47, newValue);
}
DcsBios::IntegerBuffer clL4Buffer(0x10d8, 0x8000, 15, onClL4Change);



void setup() {
  Serial.begin(250000);
  pinMode(2, INPUT_PULLUP);
  CAN_1.begin(MCP_STDEXT, CAN_500KBPS, MCP_8MHZ);
  CAN_1.init_Mask(0, 0, 0x0FFF0000);
  CAN_1.init_Filt(0, 0, 0x02010000);
  CAN_1.init_Mask(1, 0, 0x0FFF0000);
  CAN_1.init_Filt(1, 0, 0x02020000);
  CAN_1.setMode(MCP_NORMAL);
  DcsBios::setup();
  //attachInterrupt(digitalPinToInterrupt(2), CAN_RX, LOW);
  for (int i = 0; i <= 47; i++) { Cauton_Led_LastState[i] = 0; }
}

void loop() {
  DcsBios::loop();
  if (digitalRead(2) == LOW) CAN_RX();
}

void CAN_RX() {
  CAN_1.readMsgBuf(&rxId, &len, rxBuf);
  switch (rxId) {
    case 0x201 : {
         utoa(rxBuf[1], DCS_buf, 10);
         sendDcsBiosMessage(Swithes_Id[rxBuf[0]], DCS_buf);} 
         break; 
    case 0x202 : {
         unsigned int state = rxBuf[1] << 8;
         state += rxBuf[2];
         state = map(state, 4, 1020, 0, 65535);
         utoa(state, DCS_buf, 10);
         sendDcsBiosMessage(Potentiometer_Id[rxBuf[0]], DCS_buf);}
         break; 
      } 
}

void CAN_TX(int packet_id, byte lenght_packet) {
  CAN_1.sendMsgBuf(packet_id, 0, lenght_packet, can_buf); }

void CMSP_Send_String2( char* newValue) {
  can_buf[0] = newValue[0];
  can_buf[1] = newValue[1];
  can_buf[2] = newValue[2];
  can_buf[3] = newValue[3];
  can_buf[4] = newValue[4];
  can_buf[5] = newValue[5];
  can_buf[6] = newValue[6];
  can_buf[7] = newValue[7];
  CAN_TX(0x212, 8);
  can_buf[0] = newValue[8];
  can_buf[1] = newValue[9];
  can_buf[2] = newValue[10];
  can_buf[3] = newValue[11];
  can_buf[4] = newValue[12];
  can_buf[5] = newValue[13];
  can_buf[6] = newValue[14];
  can_buf[7] = newValue[15];
  CAN_TX(0x212, 8);
}

void Cauton_Led_State (byte pin, unsigned int State) {
  if (Cauton_Led_LastState[pin] != State) {
    Cauton_Led_LastState[pin] = State;
    can_buf[0] = pin;
    can_buf[1] = State;
    CAN_TX(0x230, 2);
  }
}