#include "Arduino.h"

#include "driver/gpio.h"
#include "esp_err.h"
#include "esp_rom_sys.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include <array>
#include <cstdio>

namespace {

std::array<voidFuncPtr, GPIO_NUM_MAX> g_gpio_callbacks{};
bool g_isr_service_installed = false;

void IRAM_ATTR gpioIsrThunk(void* arg) {
  const auto pin = static_cast<uint32_t>(reinterpret_cast<uintptr_t>(arg));
  if (pin < g_gpio_callbacks.size() && g_gpio_callbacks[pin] != nullptr) {
    g_gpio_callbacks[pin]();
  }
}

gpio_int_type_t toGpioInterrupt(uint32_t mode) {
  switch (mode) {
    case RISING:
      return GPIO_INTR_POSEDGE;
    case FALLING:
      return GPIO_INTR_NEGEDGE;
    case CHANGE:
      return GPIO_INTR_ANYEDGE;
    default:
      return GPIO_INTR_DISABLE;
  }
}

void printUnsigned(unsigned long long value, int base) {
  if (base == HEX) {
    std::printf("%llX", value);
  } else {
    std::printf("%llu", value);
  }
}

void printSigned(long long value, int base) {
  if (base == HEX) {
    std::printf("%llX", static_cast<unsigned long long>(value));
  } else {
    std::printf("%lld", value);
  }
}

}  // namespace

uint32_t millis() { return static_cast<uint32_t>(esp_timer_get_time() / 1000ULL); }

uint32_t micros() { return static_cast<uint32_t>(esp_timer_get_time()); }

void delay(uint32_t millis) { vTaskDelay(pdMS_TO_TICKS(millis)); }

void delayMicroseconds(unsigned int howLong) { esp_rom_delay_us(howLong); }

void pinMode(uint32_t pin, uint32_t mode) {
  if (pin >= GPIO_NUM_MAX) {
    return;
  }
  gpio_config_t config{};
  config.pin_bit_mask = 1ULL << pin;
  config.mode = mode == OUTPUT ? GPIO_MODE_OUTPUT : GPIO_MODE_INPUT;
  config.pull_up_en = mode == INPUT_PULLUP ? GPIO_PULLUP_ENABLE : GPIO_PULLUP_DISABLE;
  config.pull_down_en = mode == INPUT_PULLDOWN ? GPIO_PULLDOWN_ENABLE : GPIO_PULLDOWN_DISABLE;
  config.intr_type = GPIO_INTR_DISABLE;
  gpio_config(&config);
}

void digitalWrite(uint32_t pin, uint32_t value) {
  if (pin < GPIO_NUM_MAX) {
    gpio_set_level(static_cast<gpio_num_t>(pin), value == LOW ? 0 : 1);
  }
}

uint32_t digitalRead(uint32_t pin) {
  if (pin >= GPIO_NUM_MAX) {
    return LOW;
  }
  return gpio_get_level(static_cast<gpio_num_t>(pin)) == 0 ? LOW : HIGH;
}

void attachInterrupt(uint32_t pin, voidFuncPtr callback, uint32_t mode) {
  if (pin >= GPIO_NUM_MAX) {
    return;
  }
  if (!g_isr_service_installed) {
    const esp_err_t err = gpio_install_isr_service(ESP_INTR_FLAG_IRAM);
    g_isr_service_installed = err == ESP_OK || err == ESP_ERR_INVALID_STATE;
  }
  if (!g_isr_service_installed) {
    return;
  }
  gpio_set_intr_type(static_cast<gpio_num_t>(pin), toGpioInterrupt(mode));
  gpio_isr_handler_remove(static_cast<gpio_num_t>(pin));
  g_gpio_callbacks[pin] = callback;
  if (callback != nullptr) {
    gpio_isr_handler_add(static_cast<gpio_num_t>(pin), gpioIsrThunk,
                         reinterpret_cast<void*>(static_cast<uintptr_t>(pin)));
  }
}

void print(const char value[]) { std::printf("%s", value == nullptr ? "" : value); }

void print(char value) { std::printf("%c", value); }

void print(unsigned char value, int base) { printUnsigned(value, base); }

void print(int value, int base) { printSigned(value, base); }

void print(unsigned int value, int base) { printUnsigned(value, base); }

void print(long value, int base) { printSigned(value, base); }

void print(unsigned long value, int base) { printUnsigned(value, base); }

void print(long long value, int base) { printSigned(value, base); }

void print(unsigned long long value, int base) { printUnsigned(value, base); }

void print(double value) { std::printf("%f", value); }

void println(const char value[]) {
  print(value);
  std::printf("\n");
}

void println(char value) {
  print(value);
  std::printf("\n");
}

void println(unsigned char value, int base) {
  print(value, base);
  std::printf("\n");
}

void println(int value, int base) {
  print(value, base);
  std::printf("\n");
}

void println(unsigned int value, int base) {
  print(value, base);
  std::printf("\n");
}

void println(long value, int base) {
  print(value, base);
  std::printf("\n");
}

void println(unsigned long value, int base) {
  print(value, base);
  std::printf("\n");
}

void println(long long value, int base) {
  print(value, base);
  std::printf("\n");
}

void println(unsigned long long value, int base) {
  print(value, base);
  std::printf("\n");
}

void println(double value) {
  print(value);
  std::printf("\n");
}

void println(void) { std::printf("\n"); }