#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>

#include "libbcm2835.h"

static volatile u32_t *reg = MAP_FAILED;

u32_t __bcm_bank(u32_t pin) {
  return (pin >> 5);
}

int bcm_init(void) {
  int fd = open("/dev/gpiomem", O_RDWR | O_SYNC);
  if (fd < 0) {
    fprintf(stderr, "Failed to open() /dev/gpiomem!\n");
    return -1;
  }
  reg = (u32_t *) mmap(NULL, 0xB4, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
  close(fd);
  if (reg == MAP_FAILED) {
    fprintf(stderr, "Bad, mmap() failed!\n");
    return -1;
  }
  return 0;
}

void bcm_fsel(u32_t pin, bcm_fsel_e fsel) {
  int idx = BCM_FSEL0 + (pin / 10);
  int shift = 3 * (pin % 10);
  reg[idx] = (reg[idx] & ~(7 << shift)) | (fsel << shift);
}

void bcm_set(u32_t pin) {
  int idx = BCM_SET0 + __bcm_bank(pin);
  reg[idx] = (1 << (pin & 0x1F));
}

void bcm_clr(u32_t pin) {
  int idx = BCM_CLR0 + __bcm_bank(pin);
  reg[idx] = (1 << (pin & 0x1F));
}

int bcm_lev(u32_t pin) {
  int idx = BCM_LEV0 + __bcm_bank(pin);
  return (reg[idx] & (1 << (pin & 0x1F))) != 0;
}

void bcm_pud(u32_t pin, bcm_pud_e pud) {
  reg[BCM_PUD] = pud;
  usleep(20);
  reg[BCM_PUDCLK0 + __bcm_bank(pin)] = (1 << (pin & 0x1F));
  usleep(20);
  reg[BCM_PUD] = PUD_OFF;
  reg[BCM_PUDCLK0 + __bcm_bank(pin)] = 0x00;
}

void bcm_write(u32_t pin, int level) {
  level == HIGH
    ? bcm_set(pin)
    : bcm_clr(pin);
}

int bcm_read(u32_t pin) {
  return bcm_lev(pin);
}

void bcm_pulse(u32_t pin, int level, u32_t sleep) {
  level == HIGH
    ? bcm_set(pin)
    : bcm_clr(pin);
  usleep(sleep);
  level != HIGH
    ? bcm_set(pin)
    : bcm_clr(pin);
}