#include <inttypes.h>
#include <stdbool.h>

#include "gpio.h"

void gpio_set_mode(uint16_t pin, GPIO_MODE mode) {
  struct gpio *gpio = GPIO(PINPORT(pin));     // GPIO port address
  int pn = PINNUM(pin);                       // Pin number
  gpio->MODER &= ~(0b11 << (pn * 2));         // Clear existing setting. Each pin uses 2 bits
  gpio->MODER |= (mode & 0b11) << (pn * 2);   // Set new mode. Each pin uses 2 bits
}

void gpio_set_af(uint16_t pin, uint8_t af) {
  struct gpio *gpio = GPIO(PINPORT(pin));
  int pn = PINNUM(pin);
  if (pn < 8) {
	gpio->AFRL &= ~(0b1111 << (pn * 4)); // Each pin uses 4 bits
	gpio->AFRL |= (af & 0b1111) << (pn * 4);
  } else {
	gpio->AFRH &= ~(0b1111 << (pn * 4)); // Each pin uses 4 bits
	gpio->AFRH |= (af & 0b1111) << (pn * 4);
  }
}

void gpio_write(uint16_t pin, bool val) {
  struct gpio *gpio = GPIO(PINPORT(pin));
  gpio->BSRR = (0b0011 << PINNUM(pin)) << (val ? 0 : 16);
}