#include <stddef.h>
#include <stdint.h>
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>
#include "log.h"
#include "gpio.h"

#define GLINE_BUTTON1  17
#define GLINE_BUTTON2  22
#define GLINE_BUTTON3  23
#define GLINE_BUTTON4  27

#define STATE_BUTTON1  (1 << 0)
#define STATE_BUTTON2  (1 << 1)
#define STATE_BUTTON3  (1 << 2)
#define STATE_BUTTON4  (1 << 3)

#define INPUT  1 /* is really 0 for control register! */
#define OUTPUT 0 /* is really 1 for control register! */
#define ALT0   4

#define PUD_OFF  0
#define PUD_DOWN 1
#define PUD_UP   2

#define FSEL_OFFSET                 0
#define SET_OFFSET                  7
#define CLR_OFFSET                  10
#define PINLEVEL_OFFSET             13
#define EVENT_DETECT_OFFSET         16
#define RISING_ED_OFFSET            19
#define FALLING_ED_OFFSET           22
#define HIGH_DETECT_OFFSET          25
#define LOW_DETECT_OFFSET           28
#define PULLUPDN_OFFSET             37
#define PULLUPDNCLK_OFFSET          38

#define PULLUPDN_OFFSET_2711_0      57
#define PULLUPDN_OFFSET_2711_1      58
#define PULLUPDN_OFFSET_2711_2      59
#define PULLUPDN_OFFSET_2711_3      60
#define PULLUPDN_2711_MAGIC         0x6770696F

#define BLOCK_SIZE (4*1024)

static int mem_fd = -1;
static volatile uint32_t *gpio_map = NULL;
static int is_2711 = 0;
static int last_state = 0;

static void short_wait(void)
{
    int i;

    for (i=0; i<150; i++)
    { /* wait 150 cycles */
        asm volatile("nop");
    }
}

static int setup_the_memmap(void)
{
  if ((mem_fd = open("/dev/gpiomem", O_RDWR|O_SYNC)) > 0)
  {
    if ((gpio_map = (uint32_t *)mmap(NULL, BLOCK_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, 0)) == MAP_FAILED)
    {
      gpio_map = NULL;
      close(mem_fd);
      Log(LFATAL, "Unable to map /dev/gpiomem (%d)", errno);
      return -1;
    }
  }

  /* ignore the rest of the possibilities for now */
  return 0;
}

static void set_pullupdn(int gpio, int pud)
{
  if (is_2711)
  { /* method for RPi 4 */
    int pullreg = PULLUPDN_OFFSET_2711_0 + (gpio >> 4);
    int pullshift = (gpio & 0xF) << 1;
    uint32_t pullbits;
    unsigned pull = 0;
    switch (pud)
    {
      case PUD_OFF:  pull = 0; break;
      case PUD_UP:   pull = 1; break;
      case PUD_DOWN: pull = 2; break;
      default:       pull = 0; /* keep as "off" */
    }
    pullbits = *(gpio_map + pullreg);
    pullbits &= ~(3 << pullshift);
    pullbits |= (pull << pullshift);
    *(gpio_map + pullreg) = pullbits;
  }
  else
  { /* method for older models */
    int clk_offset = PULLUPDNCLK_OFFSET + (gpio / 32);
    int shift = gpio % 32;

    if (pud == PUD_DOWN)
    {
      *(gpio_map + PULLUPDN_OFFSET) = (*(gpio_map + PULLUPDN_OFFSET) & ~3) | PUD_DOWN;
    }
    else if (pud == PUD_UP)
    {
      *(gpio_map + PULLUPDN_OFFSET) = (*(gpio_map + PULLUPDN_OFFSET) & ~3) | PUD_UP;
    }
    else
    { /* pud == PUD_OFF */
      *(gpio_map + PULLUPDN_OFFSET) &= ~3;
    }
    short_wait();
    *(gpio_map + clk_offset) = 1 << shift;
    short_wait();
    *(gpio_map + PULLUPDN_OFFSET) &= ~3;
    *(gpio_map + clk_offset) = 0;
  }
}

static void setup_gpio_line(int gpio, int direction, int pud)
{
  int offset = FSEL_OFFSET + (gpio / 10);
  int shift = (gpio % 10) * 3;

  set_pullupdn(gpio, pud);
  if (direction == OUTPUT)
    *(gpio_map + offset) = (*(gpio_map + offset) & ~(7 << shift)) | (1 << shift);
  else  /* direction == INPUT */
    *(gpio_map + offset) = (*(gpio_map + offset) & ~(7 << shift));
}

static int gpio_input(int gpio)
{
  int offset, value, mask;

  offset = PINLEVEL_OFFSET + (gpio / 32);
  mask = (1 << gpio % 32);
  value = *(gpio_map + offset) & mask;
  return value;
}

int Gpio_setup(void)
{
  if (setup_the_memmap() != 0)
    return -1;

  is_2711 = (*(gpio_map + PULLUPDN_OFFSET_2711_3) != PULLUPDN_2711_MAGIC);

  setup_gpio_line(GLINE_BUTTON1, INPUT, PUD_UP);
  setup_gpio_line(GLINE_BUTTON2, INPUT, PUD_UP);
  setup_gpio_line(GLINE_BUTTON3, INPUT, PUD_UP);
  setup_gpio_line(GLINE_BUTTON4, INPUT, PUD_UP);
  /* TODO: other setup */

  last_state = 0;
  return 0;
}

void Gpio_cleanup(void)
{
  /* TODO: additional cleanup may be required */
  
  setup_gpio_line(GLINE_BUTTON1, INPUT, PUD_OFF);
  setup_gpio_line(GLINE_BUTTON2, INPUT, PUD_OFF);
  setup_gpio_line(GLINE_BUTTON3, INPUT, PUD_OFF);
  setup_gpio_line(GLINE_BUTTON4, INPUT, PUD_OFF);
  
  if (gpio_map)
  {
    munmap((void *)gpio_map, BLOCK_SIZE);
    gpio_map = NULL;
  }
  if (mem_fd >= 0)
  {
    close(mem_fd);
    mem_fd = -1;
  }
}

int Gpio_poll_buttons(void)
{
  int new_state = 0;
  int tmp, ndx, mask;
  
  if (gpio_input(GLINE_BUTTON1) == 0)
    new_state |= STATE_BUTTON1;
  if (gpio_input(GLINE_BUTTON2) == 0)
    new_state |= STATE_BUTTON2;
  if (gpio_input(GLINE_BUTTON3) == 0)
    new_state |= STATE_BUTTON3;
  if (gpio_input(GLINE_BUTTON4) == 0)
    new_state |= STATE_BUTTON4;

  if (last_state != new_state)
  {
    tmp = last_state & ~new_state;
    for (ndx = 1, mask = 1; ndx <= 4; ndx++, mask <<= 1)
    {
      if (tmp & mask)
	Log(LDEBUG, "Button %d was released", ndx);
    }
    tmp = new_state & ~last_state;
    for (ndx = 1, mask = 1; ndx <= 4; ndx++, mask <<= 1)
    {
      if (tmp & mask)
	Log(LDEBUG, "Button %d was pressed", ndx);
    }
    last_state = new_state;
  }
  return new_state;
}