#include <iterator>
#include "esp32-hal-gpio.h"
#include "MeyCan.h";
#include "driver/twai.h"

PinState *MeyPin = NULL;
uint16_t myDeviceId = 0;
byte _deviceTypeId = 0;
byte _majorHardwareVersion = 0;
byte _minorHardwareVersion = 0;


void SetupMeyPin(PinState *state) {
  if (state->is_input) {
    pinMode(state->pin_id, INPUT_PULLUP);
    state->pin_state = digitalRead(state->pin_id);
  } else {
    pinMode(state->pin_id, OUTPUT);
    digitalWrite(state->pin_id, LOW);
    state->pin_state = true;
  }
}



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

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


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 ReadPin(PinState *state) {
  return digitalRead(state->pin_id);
}


void SendSwitchedTriggeredCanPackage(byte pinId, int state) {
  twai_message_t message;
  message.extd = 1;
  message.rtr = 0;
  message.ss = 0;
  message.self = 0;
  message.dlc_non_comp = 0;

  message.identifier = CreateCanId(SWITCH_TRIGGERED_CAN_ID);
  message.data_length_code = 2;
  message.data[0] = pinId;
  message.data[1] = state;

  DoSendCanPkg(&message);
}

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

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

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


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

void  HandleTriggerMeypinCanPackage(twai_message_t *frame) {


  if (GetPackageType(frame->identifier) == 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);
      }
    }
  }
}


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



void DoSendCanPkg(twai_message_t *frame) {
  esp_err_t ret = twai_transmit(frame, pdMS_TO_TICKS(1000));
}