#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;
const uint32_t DEBUG_CAN_ID = 0x000;

struct can_frame _frame;

typedef struct
{
  uint32_t sourceDevId;
  byte sourceMeyPinId;
  uint32_t targetDevId;
  byte targetMeyPinId;
  bool toggle;
  bool inverse;

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

  void InitToggle(uint32_t sourceDevId, byte sourceMeyPinId, uint32_t targetDevId, byte targetMeyPinId)
  {
    this->sourceDevId = sourceDevId;
    this->sourceMeyPinId = sourceMeyPinId;
    this->targetDevId = targetDevId;
    this->targetMeyPinId = targetMeyPinId;
    toggle = true;
    inverse = false;
  }

  void InitToggleInverse(uint32_t sourceDevId, byte sourceMeyPinId, uint32_t targetDevId, byte targetMeyPinId)
  {
    this->sourceDevId = sourceDevId;
    this->sourceMeyPinId = sourceMeyPinId;
    this->targetDevId = targetDevId;
    this->targetMeyPinId = targetMeyPinId;
    toggle = 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;

struct RemotePinInfo
{
  const byte MAX_REMOTE_PIN_COUNT = 50;
  uint16_t DeviceId = 0; //  the id of the device
  uint16_t pinState = 0; // bitmap of 8 MeyPin states of the device. 0000 0100, MeyPin #3 is HIGH in this example
  RemotePinInfo *next = NULL;

  bool getPinState(byte meyPin)
  {
    return (this->pinState >> (meyPin - 1)) & 1;
  }

  void setPinState(byte meyPin, bool state)
  {
    if (state)
      this->pinState = this->pinState | (1 << (meyPin - 1)); // 0001 0000
    else
      this->pinState = this->pinState & (~(1 << (meyPin - 1))); // 1110 1111 -> not
  }


  int16_t Count()
  {
    if (this->next == NULL) return 1;
    return this->next->Count() + 1;  
  }

  RemotePinInfo* FindOrAdd(uint16_t deviceId, byte count = 0)
  {
    if (count > MAX_REMOTE_PIN_COUNT)
      return NULL;


    if (this->DeviceId == 0 && this->pinState == 0)
    {
      this->DeviceId = deviceId;
      this->pinState = 0;
    }

    if (this->DeviceId == deviceId)
    {
      //ToggleDebug();
      return this;
    }

    if (next != NULL)
    {
      return next->FindOrAdd(deviceId, count + 1);
    }

    RemotePinInfo *theNext = new RemotePinInfo;
    theNext->DeviceId = deviceId;
    theNext->pinState = 0;
    theNext->next = NULL;

    this->next = theNext;
    return this->next;
  }

};

struct RemotePinInfo remotePinInfos = RemotePinInfo();
PinState MeyPins[8];

bool flag = false;
int32_t myDeviceId;

Rule Rules[8];

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

void setup() {
  SPI.begin();

  Rules[0] = Rule();
  Rules[0].InitToggle(0x1EC3, 1, 0x3D2D, 2); // should switch MeyPin #2 of Device 3D2D to the settet state of MeyPin #1 of 0x055F is changed
  Rules[1] = Rule();
  Rules[1].InitToggle(0xD8C0, 2, 0x3D2D, 2); // should switch MeyPin #2 of Device 3D2D to the settet state of MeyPin #1 of 0x055F is changed
  Rules[2] = Rule();
  Rules[2].InitToggle(0x1EC2, 3, 0x3D2D, 3); // should switch MeyPin #2 of Device 3D2D to the settet state of MeyPin #1 of 0x055F is changed
  Rules[3] = Rule();
  Rules[3].InitToggle(0x1EC2, 4, 0x3D2D, 4); // should switch MeyPin #2 of Device 3D2D to the settet state of MeyPin #1 of 0x055F is changed
  Rules[4] = Rule();
  Rules[4].InitToggle(0x1EC2, 5, 0x3D2D, 5); // should switch MeyPin #2 of Device 3D2D to the settet state of MeyPin #1 of 0x055F is changed
  Rules[5] = Rule();
  Rules[5].InitToggle(0x1EC2, 6, 0x3D2D, 6); // should switch MeyPin #2 of Device 3D2D to the settet state of MeyPin #1 of 0x055F is changed
  Rules[6] = Rule();
  Rules[6].InitToggle(0x1EC2, 7, 0x3D2D, 7); // should switch MeyPin #2 of Device 3D2D to the settet state of MeyPin #1 of 0x055F is changed
  Rules[7] = Rule();
  Rules[7].InitToggle(0x1EC2, 8, 0x3D2D, 8); // should switch MeyPin #2 of Device 3D2D to the settet state of MeyPin #1 of 0x055F is changed


  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);

}

  void ToggleDebug()
  {
    delay(500);
    byte debugState = digitalRead(PIN_PC7)^true;
    if (debugState)
      digitalWrite(PIN_PC7, HIGH);
    else
      digitalWrite(PIN_PC7, LOW);
  }


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(10);
}

void HandleFrame(can_frame *frame)
{
  HandleMeyPinTriggeredCanPackage(frame);
  HandleTriggerMeypinCanPackage(frame);
  // Handle rules needs to be the last call
  HandleRules(frame);
}

// this method will save the state of the triggered pin to be present for the rules
void HandleMeyPinTriggeredCanPackage(can_frame *frame)
{
  if (GetPackageType(frame->can_id) == SWITCH_TRIGGERED_CAN_ID)
  {

    RemotePinInfo *currentPinState = remotePinInfos.FindOrAdd(GetDeviceId(frame->can_id) );


    if (currentPinState == NULL)
    { 
    int16_t test2[3];
    test2[0] = 'S';
    test2[1] = 'O';
    test2[2] = 'S';
   // SendDebugCanPackage(&mcp2515_3, test2, sizeof(test2) / sizeof(int16_t));
      return;
    }

    int16_t test[1];
    test[0] = remotePinInfos.Count();
   // SendDebugCanPackage(&mcp2515_3, test, sizeof(test) / sizeof(int16_t));

    currentPinState->setPinState(frame->data[0], frame->data[1]);


  }
}

void HandleTriggerMeypinCanPackage(can_frame *frame)
{
  if (GetPackageType(frame->can_id) == TRIGGER_SWITCH_CAN_ID && (GetDeviceId(frame->can_id) == GetMyDeviceId()))
  {
    int meyPinId = frame->data[2];
    bool state = frame->data[3] > 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 HandleRules(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 == GetDeviceId(frame->can_id))
        if (Rules[i].sourceMeyPinId == frame->data[0])
          HandleRule(&Rules[i], _frame.data[1] );
    }
  }
}

void HandleRule(Rule *rule, byte state)
{
  struct RemotePinInfo *currentPinState = remotePinInfos.FindOrAdd(rule->targetDevId);

  if (currentPinState == NULL)
  {
    return;
  }

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


  if (rule->toggle)
  {
    pinState = (currentPinState->getPinState(rule->targetMeyPinId) ^ true) > 0;
  }

  SendDoTriggerSwitchCanPackage(&mcp2515_0, rule->targetDevId, rule->targetMeyPinId, pinState);
  SendDoTriggerSwitchCanPackage(&mcp2515_1, rule->targetDevId, rule->targetMeyPinId, pinState);
  SendDoTriggerSwitchCanPackage(&mcp2515_2, rule->targetDevId, rule->targetMeyPinId, pinState);
  SendDoTriggerSwitchCanPackage(&mcp2515_3, rule->targetDevId, rule->targetMeyPinId, pinState);
  //digitalWrite(foundPinId->pin_id, pinState);
  //foundPinId->pin_state = pinState;

  HandleFrame(&_frame);
  currentPinState->setPinState(rule->targetMeyPinId, 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(MCP2515 *interface, byte pinId, int state)
{
  _frame.can_id = CreateCanId(SWITCH_TRIGGERED_CAN_ID);
  _frame.can_dlc = 2;
  _frame.data[0] = pinId;
  _frame.data[1] = state;
  interface->sendMessage(MCP2515::TXB1, &_frame);
}


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

void SendDebugCanPackage(MCP2515 *interface, int16_t *data, int len)
{
  can_frame frame = can_frame();
  frame.can_id = 0;
  frame.can_id = CreateCanId(DEBUG_CAN_ID);


  frame.can_dlc = len * 2;

  for (int i = 0; i < frame.can_dlc / 2; i++)
  {
    frame.data[2 * i] = ((byte[])data)[2 * i + 1];
    frame.data[2 * i + 1] = ((byte[])data)[2 * i];
  }

  interface->sendMessage(MCP2515::TXB1, &frame);
}