#include <SPI.h>;
#include <mcp2515.h>;
#include "MeyCan.cpp";

const byte SoftwareVersionHigh = 1;
const byte SoftwareVersionLow = 0;
const byte HardwareVersionHigh = 2;
const byte HardwareVersionLow = 0;

struct can_frame _frame;
MCP2515 mcp2515_0(PIN_PB1);
MCP2515 mcp2515_1(PIN_PB0);
MCP2515 mcp2515_2(PIN_PA3);

int PinCount = 8;
PinState MeyPins[8];

bool flag = false;

Rule Rules[6];
int RulesCount = 6;

void setup() {
  SPI.begin();

  Rules[0] = Rule();
  Rules[0].InitSimple(0x051F, 1, 1);
  Rules[1] = Rule();
  Rules[1].InitSimple(0x051F, 2, 2);
  Rules[2] = Rule();
  Rules[2].InitSimple(0x051F, 3, 3);
  Rules[3] = Rule();
  Rules[3].InitSimple(0x051F, 4, 4);
  Rules[4] = Rule();
  Rules[4].InitSimple(0x051F, 5, 5);
  Rules[5] = Rule();
  Rules[5].InitSimple(0x051F, 6, 6);
  Rules[6] = Rule();
  Rules[6].InitSimple(0x051F, 7, 7);

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

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

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

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

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

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

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

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

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


}

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);
    HandleFrame();
  }
  if (mcp2515_1.readMessage(&_frame) == MCP2515::ERROR_OK)
  {
    mcp2515_0.sendMessage(MCP2515::TXB1, &_frame);
    mcp2515_2.sendMessage(MCP2515::TXB1, &_frame);
    HandleFrame();
  }
  if (mcp2515_2.readMessage(&_frame) == MCP2515::ERROR_OK)
  {
    mcp2515_0.sendMessage(MCP2515::TXB1, &_frame);
    mcp2515_1.sendMessage(MCP2515::TXB1, &_frame);
    HandleFrame();
  }
  delay(20);
}

void HandleFrame()
{
  if (GetPackageType(_frame.can_id) == SWITCH_TRIGGERED_CAN_ID)
  {
    for (int i = 0; i <= RulesCount - 1; i++)
    {
      if (Rules[i].sourceDevId == GetDeviceId(_frame.can_id) && Rules[i].sourceMeyPinId == _frame.data[0] )
      {
        ProcessTriggerSwitchPackage(Rules[i].targetMeyPinId, _frame.data[1], Rules[i].trigger );
        return;
      }
    }
  }
}

void ProcessTriggerSwitchPackage(byte meyPinId, byte state, bool asTrigger)
{
  int pinState = state > 0;

  PinState *foundPinId = 0;
  for (int i = 0; i < PinCount; 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;
}

void SendSerialPackage()
{
  _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;
  mcp2515_1.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);
}