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


struct can_frame _frame;
MCP2515 mcp2515(PIN_PD4);
const byte SoftwareVersionHigh = 4;
const byte SoftwareVersionLow = 0;
const byte HardwareVersionHigh = 2;   
const byte HardwareVersionLow = 0;

typedef struct PinState
{
  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;
  }
} ;

int PinCount = 8;
PinState MeyPins[8];
int16_t myDeviceId;

void setup() {
  SPI.begin();

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

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

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

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

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

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

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

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



  _PROTECTED_WRITE(CLKCTRL.MCLKCTRLA,  CLKCTRL.MCLKCTRLA | 1 << 7);
  delay(20);
  mcp2515.reset();
  mcp2515.setBitrate(CAN_500KBPS, MCP_8MHZ); //Sets CAN at speed 500KBPS and Clock 8MHz
  mcp2515.setNormalMode();

  for (int i = 0; i <= PinCount - 1; i++)
  {
    pinMode(MeyPins[i].pin_id, INPUT_PULLUP);
    MeyPins[i].pin_state = ReadPin(&MeyPins[i]);
  }


  CalculateMyDeviceId();
  SendSerialPackage();
}


void loop()
{
  delay(1000);
  SendSerialPackage();
  
  for (int i = 0; i <= PinCount - 1; i++)
  {
    if (CheckPinStatus(&MeyPins[i]))
      SendSwitchedTriggeredCanPackage(MeyPins[i].meyPinId, MeyPins[i].pin_state);
  }

  if (mcp2515.readMessage(&_frame) == MCP2515::ERROR_OK)
  {
    if (GetDeviceId(_frame.can_id) == GetMyDeviceId())
    {
      int meyPinId = _frame.data[0];
      bool state = _frame.data[1] > 0;

      PinState *adressedPin;
      for (int i = 0; i <= PinCount - 1; 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 SendSerialPackage()
{
  _frame.can_id = CreateCanId(0xFFFF);
  _frame.can_dlc = 4;

  _frame.data[0] = SoftwareVersionHigh;
  _frame.data[1] = SoftwareVersionLow;
  _frame.data[2] = HardwareVersionHigh;
  _frame.data[3] = HardwareVersionLow;
  DoSendCanPkg(&mcp2515, &_frame);
}


void SendSwitchedTriggeredCanPackage(byte pinId, int state)
{
  _frame.can_id = CreateCanId(0x050);
  _frame.can_dlc = 2;
  _frame.data[0] = pinId;
  _frame.data[1] = state;
  DoSendCanPkg(&mcp2515, &_frame);
}

void DoSendCanPkg(MCP2515 *interface, can_frame *frame)
{
  // ToggleDebug();
  byte cnt = 0;
  while (interface->sendMessage(&_frame)) {
    cnt++;
    if (cnt > 10) return;
  }

}

void SendDoTriggerSwitchCanPackage(uint32_t targetCanId, byte pinId, byte state)
{
  _frame.can_id = CreateCanId(0x050);
  _frame.can_dlc = 4;
  _frame.data[0] = targetCanId & 0xFF;
  _frame.data[1] = (targetCanId & 0xFF00) >> 8;
  _frame.data[2] = pinId;
  _frame.data[3] = state;
  DoSendCanPkg(&mcp2515, &_frame);
}

bool ReadPin(PinState * state)
{
  if (state->pin_id == PIN_PD2)
    return digitalReadFast(PIN_PD2);
  else if (state->pin_id == PIN_PC7)
    return digitalReadFast(PIN_PC7);
  else if (state->pin_id == PIN_PD1)
    return digitalReadFast(PIN_PD1);
  else if (state->pin_id == PIN_PD0)
    return digitalReadFast(PIN_PD0);
  else if (state->pin_id == PIN_PD6)
    return digitalReadFast(PIN_PD6);
  else if (state->pin_id == PIN_PD5)
    return digitalReadFast(PIN_PD5);
  else if (state->pin_id == PIN_PD4)
    return digitalReadFast(PIN_PD4);
  else if (state->pin_id == PIN_PD3)
    return digitalReadFast(PIN_PD3);
  else
    return digitalRead(state->pin_id);
}


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

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

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

bool CheckPinStatus(PinState * state)
{
  if (!state->is_input)
    return false;

  bool newValue = ReadPin(state);


  if (newValue != state->pin_state)
  {
    delay(10);
    newValue = ReadPin(state);
    if (newValue != state->pin_state)
    {
      state->pin_state = newValue;
      return true;
    }
  }
  return false;

}