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


PinState MeyPins[8];
MCP2515* canInterfaces[4];
uint16_t myDeviceId;


void SetupMeyCan()
{
  for (int i = 0; i < sizeof(MeyPins) / sizeof(PinState); i++)
  {
    pinMode(MeyPins[i].pin_id, OUTPUT);
    digitalWrite(MeyPins[i].pin_id, LOW);
    MeyPins[i].pin_state = false;
  }
  CalculateMyDeviceId();

  for (int i = 0; i < sizeof(canInterfaces) / sizeof(MCP2515*); i++) {

    canInterfaces[i]->reset();
    canInterfaces[i]->setBitrate(CAN_500KBPS, MCP_8MHZ); //Sets CAN at speed 500KBPS and Clock 8MHz
    canInterfaces[i]->setNormalMode();


    SendVersionPackage(canInterfaces[i]);
  }
}

void SetCanInterface(byte index, byte pinId)
{
  canInterfaces[index] =  new  MCP2515(pinId);
}

void SetMeyPin(byte index, byte meyPinId, byte pinId)
{
  MeyPins[index] = PinState();
  MeyPins[index].Init(pinId, (byte) meyPinId);

  canInterfaces[index] =  new  MCP2515(pinId);
}

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

void HandleTriggerMeypinCanPackage(can_frame *frame)
{
  if (GetPackageType(frame->can_id) == TRIGGER_SWITCH_CAN_ID)
  {
    uint16_t adressedDeviceId = ((uint16_t )frame->data[0] << 8) | frame->data[1] ;
    if (adressedDeviceId != myDeviceId) return;


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

    PinState *adressedPin = NULL;
    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;
      }

      bool pinChanged = adressedPin->pin_state != state;
      adressedPin->pin_state = state;

      if (pinChanged) {
        digitalWrite(adressedPin->pin_id, state);
        SendSwitchedTriggeredCanPackage(adressedPin->meyPinId, state);
      }

    }
  }
}

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

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

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

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

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

void SendVersionPackage(MCP2515 *interface)
{
  can_frame toSend;
  toSend.can_id = CreateCanId(HELP_PACKAGE_CAN_ID);
  toSend.can_dlc = 6;

  toSend.data[0] = SOFTWARE_VERSION_HIGH;
  toSend.data[1] = SOFTWARE_VERSION_LOW;
  toSend.data[2] = HARDWARE_VERSION_HIGH;
  toSend.data[3] = HARDWARE_VERSION_LOW;
  toSend.data[4] = (myDeviceId >> 8) & 0xFF;
  toSend.data[5] = myDeviceId & 0xFF;

  DoSendCanPkg(interface,  &toSend);
}

void BroadcastTriggerMeyPinCanPackage(uint16_t targetCanId, byte pinId, byte state)
{
  can_frame toSend;

  toSend.can_id = CreateCanId(TRIGGER_SWITCH_CAN_ID);
  toSend.can_dlc = 4;
  toSend.data[0] = (targetCanId & 0xFF00) >> 8;
  toSend.data[1] = targetCanId & 0xFF;
  toSend.data[2] = pinId;
  toSend.data[3] = state;

  DoSendCanPkg(&toSend);
  HandleFrame(&toSend);
}

void SendSwitchedTriggeredCanPackage(byte pinId, int state)
{
  can_frame toSend;
  toSend.can_id = CreateCanId(SWITCH_TRIGGERED_CAN_ID);
  toSend.can_dlc = 2;
  toSend.data[0] = pinId;
  toSend.data[1] = state;

  DoSendCanPkg(&toSend);
}

void DoSendCanPkg(can_frame *frame)
{
  for (int i = 0; i < sizeof(canInterfaces) / sizeof(MCP2515*); i++)
    DoSendCanPkg(canInterfaces[i], frame);
}

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

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