#include <SPI.h>;
#include <mcp2515.h>;

const byte SoftwareVersionHigh = 3;
const byte SoftwareVersionLow = 0;
const byte HardwareVersionHigh = 6;
const byte HardwareVersionLow = 0;
const uint32_t HELP_PACKAGE_CAN_ID = 0xFFFF;
const uint32_t SWITCH_TRIGGERED_CAN_ID = 0x050;
const uint32_t TRIGGER_SWITCH_CAN_ID = 0x055;

struct can_frame _frame;

typedef struct
{
  uint32_t sourceDevId;
  byte sourceMeyPinId;
  byte targetMeyPinId;
  bool trigger;
  bool inverse;

  void InitSimple(uint32_t sourceDevId, byte sourceMeyPinId, byte targetMeyPinId)
  {
    this->sourceDevId = sourceDevId;
    this->sourceMeyPinId = sourceMeyPinId;
    this->targetMeyPinId = targetMeyPinId;
    trigger = false;
    inverse = false;
  }

  void InitTrigger(uint32_t sourceDevId, byte sourceMeyPinId, byte targetMeyPinId)
  {
    this->sourceDevId = sourceDevId;
    this->sourceMeyPinId = sourceMeyPinId;
    this->targetMeyPinId = targetMeyPinId;
    trigger = true;
    inverse = false;
  }

  void InitTriggerInverse(uint32_t sourceDevId, byte sourceMeyPinId, byte targetMeyPinId)
  {
    this->sourceDevId = sourceDevId;
    this->sourceMeyPinId = sourceMeyPinId;
    this->targetMeyPinId = targetMeyPinId;
    trigger = true;
    inverse = true;
  }
} Rule;

typedef struct
{
  int pin_id;
  bool pin_state;
  bool is_input;
  byte meyPinId;

  PinState() {}
  void Init(int pin_id, byte meyPinId)
  {
    this->pin_id = pin_id;
    this->pin_state = true;
    this->is_input = true;
    this->meyPinId = meyPinId;
  }
} PinState;


PinState MeyPins[8];

bool flag = false;
int32_t myDeviceId;

Rule Rules[10];

MCP2515 mcp2515_0(PIN_PA2);
MCP2515 mcp2515_1(PIN_PA3);
MCP2515 mcp2515_2(PIN_PB0);
MCP2515 mcp2515_3(PIN_PB1);

void setup() {
  SPI.begin();


  MeyPins[0] = PinState();
  MeyPins[0].Init(PIN_PC7, (byte) 1);

  MeyPins[1] = PinState();
  MeyPins[1].Init(PIN_PC6, (byte) 2);

  MeyPins[2] = PinState();
  MeyPins[2].Init(PIN_PC5, (byte) 3);

  MeyPins[3] = PinState();
  MeyPins[3].Init(PIN_PC4, (byte) 4);

  MeyPins[4] = PinState();
  MeyPins[4].Init(PIN_PC3, (byte) 5);

  MeyPins[5] = PinState();
  MeyPins[5].Init(PIN_PC2, (byte) 6);

  MeyPins[6] = PinState();
  MeyPins[6].Init(PIN_PC1, (byte) 7);

  MeyPins[7] = PinState();
  MeyPins[7].Init(PIN_PC0, (byte) 8);



  _PROTECTED_WRITE(CLKCTRL.MCLKCTRLA,  CLKCTRL.MCLKCTRLA | 1 << 7);


  mcp2515_0.reset();
  mcp2515_0.setBitrate(CAN_500KBPS, MCP_8MHZ); //Sets CAN at speed 500KBPS and Clock 8MHz
  mcp2515_0.setNormalMode();

  mcp2515_1.reset();
  mcp2515_1.setBitrate(CAN_500KBPS, MCP_8MHZ); //Sets CAN at speed 500KBPS and Clock 8MHz
  mcp2515_1.setNormalMode();

  mcp2515_2.reset();
  mcp2515_2.setBitrate(CAN_500KBPS, MCP_8MHZ); //Sets CAN at speed 500KBPS and Clock 8MHz
  mcp2515_2.setNormalMode();

  mcp2515_3.reset();
  mcp2515_3.setBitrate(CAN_500KBPS, MCP_8MHZ); //Sets CAN at speed 500KBPS and Clock 8MHz
  mcp2515_3.setNormalMode();

  //delay(20000);
  for (int i = 0; i < sizeof(MeyPins) / sizeof(PinState); i++)
  {
    pinMode(MeyPins[i].pin_id, OUTPUT);

    // digitalWrite(MeyPins[i].pin_id, HIGH);
    //delay(100);
    digitalWrite(MeyPins[i].pin_id, LOW);

  }
  CalculateMyDeviceId();

  SendSerialPackage(&mcp2515_0);
  SendSerialPackage(&mcp2515_1);
  SendSerialPackage(&mcp2515_2);
  SendSerialPackage(&mcp2515_3);

}

int i = 0;
bool debugState = false;


void loop() {

  
  if (mcp2515_0.readMessage(&_frame) == MCP2515::ERROR_OK)
  {
    mcp2515_1.sendMessage(MCP2515::TXB1, &_frame);
    mcp2515_2.sendMessage(MCP2515::TXB1, &_frame);
    mcp2515_3.sendMessage(MCP2515::TXB1, &_frame);
    HandleFrame(&_frame);
  }
  if (mcp2515_1.readMessage(&_frame) == MCP2515::ERROR_OK)
  {
    mcp2515_0.sendMessage(MCP2515::TXB1, &_frame);
    mcp2515_2.sendMessage(MCP2515::TXB1, &_frame);
    mcp2515_3.sendMessage(MCP2515::TXB1, &_frame);
    HandleFrame(&_frame);
  }
  if (mcp2515_2.readMessage(&_frame) == MCP2515::ERROR_OK)
  {
    mcp2515_0.sendMessage(MCP2515::TXB1, &_frame);
    mcp2515_1.sendMessage(MCP2515::TXB1, &_frame);
    mcp2515_3.sendMessage(MCP2515::TXB1, &_frame);
    HandleFrame(&_frame);
  }
  if (mcp2515_3.readMessage(&_frame) == MCP2515::ERROR_OK)
  {
    mcp2515_0.sendMessage(MCP2515::TXB1, &_frame);
    mcp2515_1.sendMessage(MCP2515::TXB1, &_frame);
    mcp2515_2.sendMessage(MCP2515::TXB1, &_frame);
    HandleFrame(&_frame);
  }
  delay(20);
}

void HandleFrame(can_frame *frame)
{
  HandleSwitchTriggeredCanPackage(frame);
  HandleTriggerMeypinCanPackage(frame);

}

void HandleTriggerMeypinCanPackage(can_frame *frame)
{
  if (GetDeviceId(frame->can_id) == GetMyDeviceId())
  {

    int meyPinId = frame->data[0];
    bool state = frame->data[1] > 0;

    PinState *adressedPin;
    for (int i = 0; i < sizeof(MeyPins) / sizeof(PinState); i++)
      if (MeyPins[i].meyPinId == meyPinId)
      {
        adressedPin = &MeyPins[i];
        break;
      }

    if (adressedPin != NULL)
    {
      if (adressedPin->is_input == true)
      {
        pinMode(adressedPin->pin_id, OUTPUT);
        adressedPin->is_input = false;
      }
      adressedPin->pin_state = state;
      digitalWrite(adressedPin->pin_id, state);
    }

  }
}

void HandleSwitchTriggeredCanPackage(can_frame *frame)
{
  if (GetPackageType(frame->can_id) == SWITCH_TRIGGERED_CAN_ID)
  {
    for (int i = 0; i < sizeof(Rules) / sizeof(Rule); i++)
    {
      if (Rules[i].sourceDevId == 0x0000 || ( Rules[i].sourceDevId == GetDeviceId(frame->can_id) && Rules[i].sourceMeyPinId == frame->data[0] ))
      {
        ProcessTriggerSwitchPackage( Rules[i].targetMeyPinId, _frame.data[1], Rules[i].trigger, Rules[i].inverse );
      }
    }
  }
}

void ProcessTriggerSwitchPackage( byte meyPinId, byte state, bool asTrigger, bool inverse)
{

  int pinState = state > 0;
  if (inverse)
    pinState = !pinState;

  PinState *foundPinId = 0;
  for (int i = 0; i <  sizeof(MeyPins) / sizeof(PinState); i++)
  {
    if (MeyPins[i].meyPinId == meyPinId)
    {
      foundPinId = &MeyPins[i];
      break;
    }
  }
  if (foundPinId == 0)return;

  if (asTrigger)
  {
    pinState = (foundPinId->pin_state ^ true) > 0;
  }

  digitalWrite(foundPinId->pin_id, pinState);
  foundPinId->pin_state = pinState;
}

byte CircularShift(byte b)
{
  return (b << 1) | (b >> 7 & 1);
}

uint32_t GetDeviceId(uint32_t canFrameId)
{
  return canFrameId & 0xFFFF;
}

uint32_t GetPackageType(uint32_t canFrameId)
{
  return (canFrameId / 0x10000) & 0xFFF;
}

uint32_t CreateCanId(uint32_t commandId)
{
  return ((commandId & 0xFFF) * 0x10000) | myDeviceId | CAN_EFF_FLAG;
}

void CalculateMyDeviceId()
{
  myDeviceId = (GetDeviceIdHigh() << 8) | GetDeviceIdLow();
}

uint32_t GetMyDeviceId()
{
  return myDeviceId;
}

byte GetDeviceIdLow() {
  return (SIGROW.SERNUM0 ^
          CircularShift(SIGROW.SERNUM2) << 1 ^
          CircularShift( CircularShift(SIGROW.SERNUM4)) ^
          CircularShift( CircularShift( CircularShift(SIGROW.SERNUM6))) ^
          CircularShift( CircularShift( CircularShift( CircularShift(SIGROW.SERNUM8)))));
}

byte GetDeviceIdHigh() {
  return (SIGROW.SERNUM1 ^
          CircularShift(SIGROW.SERNUM3) << 1 ^
          CircularShift( CircularShift(SIGROW.SERNUM5)) ^
          CircularShift( CircularShift( CircularShift(SIGROW.SERNUM7))) ^
          CircularShift( CircularShift( CircularShift( CircularShift(SIGROW.SERNUM9)))));
}

void SendSerialPackage(MCP2515 *interface)
{
  _frame.can_id = CreateCanId(HELP_PACKAGE_CAN_ID);
  _frame.can_dlc = 4;

  _frame.data[0] = SoftwareVersionHigh;
  _frame.data[1] = SoftwareVersionLow;
  _frame.data[2] = HardwareVersionHigh;
  _frame.data[3] = HardwareVersionLow;
  interface->sendMessage(MCP2515::TXB1, &_frame);
}

void SendSwitchedTriggeredCanPackage(byte pinId, int state)
{
  _frame.can_id = CreateCanId(SWITCH_TRIGGERED_CAN_ID);
  _frame.can_dlc = 2;
  _frame.data[0] = pinId;
  _frame.data[1] = state;
  mcp2515_1.sendMessage(MCP2515::TXB1, &_frame);
}

void SendDoTriggerSwitchCanPackage(uint32_t targetCanId, byte pinId, byte state)
{
  _frame.can_id = CreateCanId(TRIGGER_SWITCH_CAN_ID);
  _frame.can_dlc = 4;
  _frame.data[0] = targetCanId & 0xFF;
  _frame.data[1] = (targetCanId & 0xFF00) >> 8;
  _frame.data[2] = pinId;
  _frame.data[3] = state;
  mcp2515_1.sendMessage(MCP2515::TXB1, &_frame);
}