#include "GPIO_Direct_Access.h"

#include <Arduino.h>

#if defined(ARDUINO_ARCH_ESP8266)
  # ifndef CORE_POST_3_0_0
    #  define IRAM_ATTR ICACHE_RAM_ATTR
  # endif // ifndef CORE_POST_3_0_0
#endif    // if defined(ARDUINO_ARCH_ESP8266)


#if defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32)

IO_REG_TYPE DIRECT_pinRead(IO_REG_TYPE pin)
{
  # ifdef ARDUINO_ARCH_ESP8266

  // ESP8266 GPIO 16 doesn't have direct access
  if (pin == 16) { return digitalRead(pin); }
  # endif // ifdef ARDUINO_ARCH_ESP8266

  return DIRECT_READ(reg, PIN_TO_BITMASK(pin));
}

void DIRECT_pinWrite(IO_REG_TYPE pin, bool pinstate)
{
  # ifdef ARDUINO_ARCH_ESP8266

  // ESP8266 GPIO 16 doesn't have direct access
  if (pin == 16) {
    digitalWrite(pin, pinstate);
    return;
  }
  # endif // ifdef ARDUINO_ARCH_ESP8266

  if (pinstate) { DIRECT_WRITE_HIGH(reg, PIN_TO_BITMASK(pin)); }
  else { DIRECT_WRITE_LOW(reg, PIN_TO_BITMASK(pin)); }
}

void DIRECT_PINMODE_OUTPUT(IO_REG_TYPE pin)
{
  # ifdef ARDUINO_ARCH_ESP8266

  // ESP8266 GPIO 16 doesn't have direct access
  if (pin == 16) {
    pinMode(pin, OUTPUT);
    return;
  }
  GPF(pin) = GPFFS(GPFFS_GPIO(pin));//Set mode to GPIO
  GPC(pin) = (GPC(pin) & (0xF << GPCI)); //SOURCE(GPIO) | DRIVER(NORMAL) | INT_TYPE(UNCHANGED) | WAKEUP_ENABLE(DISABLED)
  # endif // ifdef ARDUINO_ARCH_ESP8266

  DIRECT_MODE_OUTPUT(reg, PIN_TO_BITMASK(pin));
}

void DIRECT_PINMODE_INPUT(IO_REG_TYPE pin)
{
  # ifdef ARDUINO_ARCH_ESP8266

  // ESP8266 GPIO 16 doesn't have direct access
  if (pin == 16) {
    pinMode(pin, INPUT);
    return;
  }
  # endif // ifdef ARDUINO_ARCH_ESP8266

  DIRECT_MODE_INPUT(reg, PIN_TO_BITMASK(pin));
}

IO_REG_TYPE IRAM_ATTR DIRECT_pinRead_ISR(IO_REG_TYPE pin)
{
  # ifdef ARDUINO_ARCH_ESP8266

  // ESP8266 GPIO 16 doesn't have direct access
  if (pin == 16) { return digitalRead(pin); }
  # endif // ifdef ARDUINO_ARCH_ESP8266

  return DIRECT_READ(reg, PIN_TO_BITMASK(pin));
}

void IRAM_ATTR DIRECT_pinWrite_ISR(IO_REG_TYPE pin, bool pinstate)
{
  # ifdef ARDUINO_ARCH_ESP8266

  // ESP8266 GPIO 16 doesn't have direct access
  if (pin == 16) {
    digitalWrite(pin, pinstate);
    return;
  }
  # endif // ifdef ARDUINO_ARCH_ESP8266

  if (pinstate) { DIRECT_WRITE_HIGH(reg, PIN_TO_BITMASK(pin)); }
  else { DIRECT_WRITE_LOW(reg, PIN_TO_BITMASK(pin)); }
}

void  IRAM_ATTR DIRECT_PINMODE_OUTPUT_ISR(IO_REG_TYPE pin)
{
  # ifdef ARDUINO_ARCH_ESP8266

  // ESP8266 GPIO 16 doesn't have direct access
  if (pin == 16) {
    pinMode(pin, OUTPUT);
    return;
  }
  GPF(pin) = GPFFS(GPFFS_GPIO(pin));//Set mode to GPIO
  GPC(pin) = (GPC(pin) & (0xF << GPCI)); //SOURCE(GPIO) | DRIVER(NORMAL) | INT_TYPE(UNCHANGED) | WAKEUP_ENABLE(DISABLED)
  
  # endif // ifdef ARDUINO_ARCH_ESP8266

  DIRECT_MODE_OUTPUT(reg, PIN_TO_BITMASK(pin));
}

void  IRAM_ATTR DIRECT_PINMODE_INPUT_ISR(IO_REG_TYPE pin)
{
  # ifdef ARDUINO_ARCH_ESP8266

  // ESP8266 GPIO 16 doesn't have direct access
  if (pin == 16) {
    pinMode(pin, INPUT);
    return;
  }
  # endif // ifdef ARDUINO_ARCH_ESP8266

  DIRECT_MODE_INPUT(reg, PIN_TO_BITMASK(pin));
}

int32_t IRAM_ATTR DIRECT_measureWaitForPinState_ISR(
  IO_REG_TYPE gpio_pin_rx, 
  uint32_t start_usec, 
  int32_t timeout_usec, 
  bool newState)
{
  int32_t passed{};

  do {
    passed = (int32_t) ((uint32_t)micros() - start_usec);
  } while (passed < timeout_usec &&
           (!DIRECT_pinRead_ISR(gpio_pin_rx) == newState)); // Using '!' to do a quick cast to bool

  if ((passed > timeout_usec) ||
      (!DIRECT_pinRead_ISR(gpio_pin_rx) == newState)) {
    return -1;
  }

  // N.B. we allow the situation where passed == timeout_usec
  // and pin might have reached the state we're waiting for.
  // This way we don't need to average for time before reading pin state and after.
  return passed;

}

#endif // if defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32)