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


PinState *MeyPin = NULL;
CanInterface *CanBusses = NULL;
uint16_t myDeviceId;
byte _deviceTypeId = 0;
byte _majorHardwareVersion = 0;
byte _minorHardwareVersion = 0;
void (*LocalPinChangedHandler)(can_frame *frame);

void SetupMeyPin(PinState *state) {
  pinMode(state->pin_id, OUTPUT);
  digitalWrite(state->pin_id, LOW);
  state->pin_state = false;
}

void InitCanInterface(MCP2515 *interface, can_frame *frame)  // can_frame is NULL. Reuse ForEach Method for smaller footprint
{
  interface->reset();
  interface->setBitrate(CAN_1000KBPS, MCP_8MHZ);  //Sets CAN at speed 500KBPS and Clock 8MHz
  interface->setNormalMode();
  SendVersionPackage(interface);
}

void ForEachCanInterface(void (*handle)(MCP2515 *canInterace)) {
  CanBusses->ForEachInterface(handle);
}

void SetupMeyCan(byte majorHardwareVersion, byte minorHardwareVersion, byte deviceTypeId) {
  _majorHardwareVersion = majorHardwareVersion;
  _minorHardwareVersion = minorHardwareVersion;
  _deviceTypeId = deviceTypeId;

  if (MeyPin != NULL)
    ;
  MeyPin->ForEach(SetupMeyPin);

  if (CanBusses != NULL) {
    CanBusses->ForEach(InitCanInterface, NULL);
  }
}

void AddCanInterface(byte pinId) {
  MCP2515 *newCanInterface = new MCP2515(pinId);
  CanInterface *canInterface = new CanInterface();
  canInterface->interface = newCanInterface;

  if (CanBusses == NULL)
    CanBusses = canInterface;
  else
    CanBusses->AddCanInterface(canInterface);
}

void AddLocalPinChangedHandler(void (*localPinChangedHandler)(can_frame *frame)) {
  LocalPinChangedHandler = localPinChangedHandler;
}

void SetDevicedId(byte high, byte low) {
  myDeviceId = (high << 8) | low;
}

void SetMeyPin(byte meyPinId, byte pinId) {
  PinState *newState = new PinState();
  newState->Init(pinId, (byte)meyPinId);

  if (MeyPin == NULL)
    MeyPin = newState;
  else
    MeyPin->AddPinState(newState);
}


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;
      SendSwitchedTriggeredCanPackage(state->meyPinId, state->pin_state);
    }
  }
  return false;
}

void CheckMeyPinsTriggered() {
  MeyPin->ForEach(CheckPinStatus);
}

bool ReadPin(PinState *state) {
  return digitalRead(state->pin_id);
}


void HandleFrame(can_frame *frame, MCP2515 *source) {
  CanBusses->ForEach(DoSendCanPkg, frame, source);
  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;


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

    PinState *adressedPin = MeyPin->Find(meyPinId);

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


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 = 5;

  toSend.data[0] = SOFTWARE_VERSION_HIGH;
  toSend.data[1] = SOFTWARE_VERSION_LOW;
  toSend.data[2] = _majorHardwareVersion;
  toSend.data[3] = _minorHardwareVersion;
  toSend.data[4] = _deviceTypeId;

  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;

  HandleFrame(&toSend, NULL);
}

void BroadcastDebugPackage(uint8_t length, byte *data) {
  can_frame toSend;

  toSend.can_id = CreateCanId(DEBUG_PKG_CAN_ID);
  toSend.can_dlc = length;
  for (int i = 0; i < length; i++) {
    toSend.data[i] = data[i];
  }

  HandleFrame(&toSend, NULL);
}
void BroadcastDebugPackage(byte b0, byte b1) {
  byte data[2];
  data[0] = b0;
  data[1] = b1;
  BroadcastDebugPackage(2, data);
}
void BroadcastDebugPackage(byte b0, byte b1, byte b2) {
  byte data[3];
  data[0] = b0;
  data[1] = b1;
  data[2] = b2;
  BroadcastDebugPackage(3, data);
}

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);
  if (LocalPinChangedHandler) {
    LocalPinChangedHandler(&toSend);
  }
}

void DoSendCanPkg(can_frame *frame) {
  if (CanBusses != NULL)
    CanBusses->ForEach(DoSendCanPkg, frame);
}

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