#include "kern/gpio/gpio_manager.h"

#include "arch/stm32l4xxx/peripherals/irq.h"
#include "arch/stm32l4xxx/peripherals/rcc.h"

/* A list of whether the pins are in use or not as a bitmask. */
uint32_t pins_inuse[N_GPIO_PINS / 32 + (N_GPIO_PINS % 32 != 0)];

struct gpio_afn_and_pin {
  int8_t afn_number;
  gpio_pin_t gpio_pin;
};

/*
 * Returns which (pin, afn) pairs provide the given alternate function.
 * The out array needs to have 5 positions.
 *
 * This function will use afn_number = -1 as the terminal.
 *
 * Note that EVENTOUT is a special case because all pins have an event out
 * at afn=15 and should be assumed by other logic and thus is not handled
 * by this function.
 */
static void get_ports_and_pins_for_alternate_function(
    gpio_alternate_function_t afn, struct gpio_afn_and_pin* out)
{
  switch (afn) {
#define AFN1(fn, ...) static_assert(false, "Unable to parse afn_table at " #fn);
#define AFN3(fn, ...) static_assert(false, "Unable to parse afn_table at " #fn);
#define AFN5(fn, ...) static_assert(false, "Unable to parse afn_table at " #fn);
#define AFN7(fn, ...) static_assert(false, "Unable to parse afn_table at " #fn);

#define AFN2(fn, afn, pin) \
  out[0].afn_number = afn; \
  out[0].gpio_pin = GPIO_PIN_##pin

#define AFN4(fn, afn0, pin0, afn1, pin1) \
  AFN2(fn, afn0, pin0);                  \
  out[1].afn_number = afn1;              \
  out[1].gpio_pin = GPIO_PIN_##pin1

#define AFN6(fn, afn0, pin0, afn1, pin1, afn2, pin2) \
  AFN4(fn, afn0, pin0, afn1, pin1);                  \
  out[2].afn_number = afn2;                          \
  out[2].gpio_pin = GPIO_PIN_##pin2

#define AFN8(fn, afn0, pin0, afn1, pin1, afn2, pin2, afn3, pin3) \
  AFN6(fn, afn0, pin0, afn1, pin1, afn2, pin2);                  \
  out[2].afn_number = afn3;                                      \
  out[2].gpio_pin = GPIO_PIN_##pin3

#define GET_MACRO(_1, _2, _3, _4, _5, _6, _7, _8, NAME, ...) NAME
#define GET_N(_1, _2, _3, _4, _5, _6, _7, _8, NAME, ...) NAME
#define AFN(fn, ...)                                                       \
  case GPIO_ALTERNATE_FUNCTION_##fn:                                       \
    GET_MACRO(__VA_ARGS__, AFN8, AFN7, AFN6, AFN5, AFN4, AFN3, AFN2, AFN1) \
    (fn, __VA_ARGS__);                                                     \
    out[GET_N(__VA_ARGS__, 4, 4, 3, 3, 2, 2, 1, 1)] =                      \
        (struct gpio_afn_and_pin){-1, -1};                                 \
    break;

#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/afn_table.inc"
    case GPIO_ALTERNATE_FUNCTION_EVENTOUT:
      return;
  }
}

static inline int offset_for_gpio_pin(gpio_pin_t pin)
{
  switch (pin) {
#define PORT(p, pn)       \
  case GPIO_PIN_P##p##pn: \
    return pn;
#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
#undef PORT
    case N_GPIO_PINS:
      return -1;
  }

  /* Should be unreachable. */
  return -1;
}

inline bool gpio_pin_in_use(gpio_pin_t pin)
{
  return !!(pins_inuse[pin / 32] & (1 << (pin % 32)));
}

#define A(...)
#define B(...)
#define C(...)
#define D(...)
#define E(...)
#define F(...)
#define G(...)
#define H(...)
#define I(...)
#define SELECT_MACRO(PORT) PORT
#define PORT(port, pin) SELECT_MACRO(port)(GPIO_PIN_P##port##pin, pin)
static int gc_port_a()
{
  return 0
#undef A
#define A(abspin, relpin) | (gpio_pin_in_use(abspin) << (relpin))
#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
      ;
#undef A
#define A(...)
}

static int gc_port_b()
{
  return 0
#undef B
#define B(abspin, relpin) | (gpio_pin_in_use(abspin) << (relpin))
#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
      ;
#undef B
#define B(...)
}

static int gc_port_c()
{
  return 0
#undef C
#define C(abspin, relpin) | (gpio_pin_in_use(abspin) << (relpin))
#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
      ;
#undef C
#define C(...)
}

static int gc_port_d()
{
  return 0
#undef D
#define D(abspin, relpin) | (gpio_pin_in_use(abspin) << (relpin))
#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
      ;
#undef D
#define D(...)
}

static int gc_port_e()
{
  return 0
#undef E
#define E(abspin, relpin) | (gpio_pin_in_use(abspin) << (relpin))
#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
      ;
#undef E
#define E(...)
}

static int gc_port_f()
{
  return 0
#undef F
#define F(abspin, relpin) | (gpio_pin_in_use(abspin) << (relpin))
#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
      ;
#undef F
#define F(...)
}

static int gc_port_g()
{
  return 0
#undef G
#define G(abspin, relpin) | (gpio_pin_in_use(abspin) << (relpin))
#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
      ;
#undef G
#define G(...)
}

static int gc_port_h()
{
  return 0
#undef H
#define H(abspin, relpin) | (gpio_pin_in_use(abspin) << (relpin))
#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
      ;
#undef H
#define H(...)
}

static int gc_port_i()
{
  return 0
#undef I
#define I(abspin, relpin) | (gpio_pin_in_use(abspin) << (relpin))
#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
      ;
#undef I
#define I(...)
}

static inline bool gpio_pin_try_reserve(gpio_pin_t pin)
{
  int in_use = __sync_fetch_and_or(&pins_inuse[pin / 32], 1 << (pin % 32));
  return !(in_use & (1 << (pin % 32)));
}

inline static gpio_port_config_t* get_gpio_port_config(gpio_port_t port)
{
  switch (port) {
    case GPIO_PORT_A:
      return (gpio_port_config_t*)GPIOA_BASE;
    case GPIO_PORT_B:
      return (gpio_port_config_t*)GPIOB_BASE;
    case GPIO_PORT_C:
      return (gpio_port_config_t*)GPIOC_BASE;
    case GPIO_PORT_H:
      return (gpio_port_config_t*)GPIOH_BASE;
    default:
      return NULL;
  }
}

inline static gpio_port_config_t* get_gpio_port_config_for_pin(gpio_pin_t pin)
{
  gpio_port_t port = get_port_for_pin(pin);
  return get_gpio_port_config(port);
}

gpio_reserved_pin_t reserve_gpio_pin(
    gpio_pin_t pin, gpio_pin_opts_t* opts, int* error_out)
{
  *error_out = 0;
  if (!gpio_pin_try_reserve(pin)) {
    *error_out = GPIO_ERROR_IN_USE;
    return (gpio_reserved_pin_t){.v_ = -1};
  }

  gpio_port_t port = get_port_for_pin(pin);
  regset(RCC.ahb2en_r, rcc_gpioen(port), 1);

  gpio_port_config_t* port_config = get_gpio_port_config(port);

  int off = offset_for_gpio_pin(pin);

  regset(port_config->mode_r, gpio_mode_n(off), opts->mode);
  regset(port_config->pupd_r, gpio_pupd_n(off), opts->pull_dir);

  switch (opts->mode) {
    case GPIO_MODE_INPUT:
      break;

    case GPIO_MODE_OUTPUT:
      regset(
          port_config->ospeed_r, gpio_ospeed_n(off), opts->output_opts.speed);
      regset(port_config->otype_r, gpio_otype_n(off), opts->output_opts.type);
      break;

    case GPIO_MODE_ALTERNATE:
      if (off < 8) {
        regset(
            port_config->af_rl,
            gpio_afsel_n(off),
            opts->alternate_opts.function);
      } else {
        regset(
            port_config->af_rh,
            gpio_afsel_n(off - 8),
            opts->alternate_opts.function);
      }
      break;

    case GPIO_MODE_ANALOG:
      regset(port_config->asc_r, gpio_asc_n(off), 1);
      break;
  }

  return (gpio_reserved_pin_t){.v_ = pin};
}

gpio_reserved_pin_t gpio_enable_alternate_function(
    gpio_alternate_function_t fn, gpio_pin_t hint, int* error_out)
{
  int i = 0;
  gpio_pin_opts_t opts;
  struct gpio_afn_and_pin afn_and_pin[5];

  if (gpio_pin_out_of_range(hint) && hint != -1) {
    *error_out = GPIO_ERROR_INVALID_PIN;
    return (gpio_reserved_pin_t){.v_ = -1};
  }

  opts.mode = GPIO_MODE_ALTERNATE;

  if (fn == GPIO_ALTERNATE_FUNCTION_EVENTOUT) {
    afn_and_pin[i].afn_number = GPIO_ALTERNATE_FUNCTION_EVENTOUT;
    if (hint == -1) {
      hint = GPIO_PIN_PA0;
    }
    afn_and_pin[i].gpio_pin = hint;
  } else {
    get_ports_and_pins_for_alternate_function(fn, afn_and_pin);

    if (hint == -1) {
      hint = afn_and_pin[0].gpio_pin;
    }

    for (i = 0; i < 5 && afn_and_pin[i].gpio_pin != hint &&
                afn_and_pin[i].gpio_pin != -1;
         ++i)
      ;

    if (afn_and_pin[i].gpio_pin == -1 || i == 5) {
      *error_out = GPIO_ERROR_INVALID_PIN_FOR_ALTERNATE_FUNCTION;
      return (gpio_reserved_pin_t){.v_ = -1};
    }
  }

  opts.alternate_opts.function = afn_and_pin[i].afn_number;
  return reserve_gpio_pin(afn_and_pin[i].gpio_pin, &opts, error_out);
}

void release_gpio_pin(gpio_reserved_pin_t rpin)
{
  gpio_pin_t pin = rpin.v_;
  // TODO this should be a critical section.
  gpio_port_t port = get_port_for_pin(pin);
  pins_inuse[pin / 32] &= ~(1 << (pin % 32));

  int used;
  switch (port) {
    case GPIO_PORT_A:
      used = gc_port_a();
      break;
    case GPIO_PORT_B:
      used = gc_port_b();
      break;
    case GPIO_PORT_C:
      used = gc_port_c();
      break;
    case GPIO_PORT_D:
      used = gc_port_d();
      break;
    case GPIO_PORT_E:
      used = gc_port_e();
      break;
    case GPIO_PORT_F:
      used = gc_port_f();
      break;
    case GPIO_PORT_G:
      used = gc_port_g();
      break;
    case GPIO_PORT_H:
      used = gc_port_h();
      break;
    case GPIO_PORT_I:
      used = gc_port_i();
      break;

    case N_GPIO_PORTS:
      used = 1;
      break;
  }

  if (!used) {
    regset(RCC.ahb2en_r, rcc_gpioen(port), 0);
  }
}

inline void get_gpio_pin_port_off(
    gpio_pin_t pin, gpio_port_config_t** out_cfg, int* out_off)
{
  *out_cfg = get_gpio_port_config_for_pin(pin);
  *out_off = offset_for_gpio_pin(pin);
}

void set_gpio_pin_high(gpio_reserved_pin_t pin)
{
  int off;
  gpio_port_config_t* portcfg;
  get_gpio_pin_port_off(pin.v_, &portcfg, &off);

  regset(portcfg->od_r, (1 << off), 1);
}

void set_gpio_pin_low(gpio_reserved_pin_t pin)
{
  int off;
  gpio_port_config_t* portcfg;
  get_gpio_pin_port_off(pin.v_, &portcfg, &off);

  regset(portcfg->od_r, (1 << off), 0);
}

bool get_gpio_pin_input_state(gpio_reserved_pin_t pin)
{
  int off;
  gpio_port_config_t* portcfg;
  get_gpio_pin_port_off(pin.v_, &portcfg, &off);

  return !!regget(portcfg->id_r, (1 << off));
}