#define PIO_CONFIG_BASE (0x01c20800)                    // Peripheral Base

struct portconfig_t {
    volatile unsigned int CFG0;
    volatile unsigned int CFG1;
    volatile unsigned int CFG2;
    volatile unsigned int CFG3;
    volatile unsigned int DAT;
    volatile unsigned int DRV0;
    volatile unsigned int DRV1;
    volatile unsigned int PUL0;
    volatile unsigned int PUL1;
};

struct gpio_peripheral {
    unsigned long addr_p;
    int mem_fd;
    void *map;
    volatile unsigned int *addr;

    // Register
//  volatile unsigned int hwreg[];
    struct portconfig_t *PORTA;             // Port A config structure
    struct portconfig_t *PORTB;             // Port B config structure
    struct portconfig_t *PORTC;             // Port C config structure
    struct portconfig_t *PORTD;             // Port D config structure
    struct portconfig_t *PORTE;             // Port E config structure
    struct portconfig_t *PORTF;             // Port F config structure
    struct portconfig_t *PORTG;             // Port G config structure
    struct portconfig_t *PORTH;             // Port H config structure
    struct portconfig_t *PORTI;             // Port I config structure

    volatile unsigned int *PIO_INT_CFG0;        // Interrupt Config 0
    volatile unsigned int *PIO_INT_CFG1;        // Interrupt Config 1
    volatile unsigned int *PIO_INT_CFG2;        // Interrupt Config 2
    volatile unsigned int *PIO_INT_CFG3;        // Interrupt Config 3
    volatile unsigned int *PIO_INT_CTL;     // Interrupt Control
    volatile unsigned int *PIO_INT_STA;     // Interrupt Status
    volatile unsigned int *PIO_INT_DEB;     // Interrupt Debounce
};

int map_peripheral(unsigned long *addr_p, int *mem_fd, void **map, volatile unsigned int **addr)
{
    if (!(*addr_p)) {
        printf("Called map_peripheral with uninitilized struct.\n");
        return -1;
    }

    // Open /dev/mem
    if ((*mem_fd = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
        printf("Failed to open /dev/mem, try checking permissions.\n");
        return -1;
    }

    *map = mmap(
        NULL,
        BLOCK_SIZE,
        PROT_READ | PROT_WRITE,
        MAP_SHARED,
        *mem_fd,      // file descriptor to physical memory virtual file '/dev/mem'
        *addr_p       // address in physical map to be exposed
    );

    if (*map == MAP_FAILED) {
        perror("mmap error");
        return -1;
    }

    *addr = (volatile unsigned int *)*map;

    return 0;
}

/// <summary>
/// gpio_init
///
///     Initializes the GPIO peripheral
/// </summary>
/// <param name="p">Pointer to peripheral struct.<see cref = "struct gpio_peripheral" / ></param>
/// <returns>0 on success, nonzero on failure</returns>
int gpio_init(struct gpio_peripheral *p)
{
    p->addr_p = PIO_CONFIG_BASE;

    if (map_peripheral(&(p->addr_p), &(p->mem_fd), &(p->map), &(p->addr)) < 0) {
        printf("Unable to map peripheral %d\n", errno);
        return -1;
    }


    p->PORTA = (p->addr + PORT_OFFSET(A));
    p->PORTB = (p->addr + PORT_OFFSET(B));
    p->PORTC = (p->addr + PORT_OFFSET(C));
    p->PORTD = (p->addr + PORT_OFFSET(D));
    p->PORTE = (p->addr + PORT_OFFSET(E));
    p->PORTF = (p->addr + PORT_OFFSET(F));
    p->PORTG = (p->addr + PORT_OFFSET(G));
    p->PORTH = (p->addr + PORT_OFFSET(H));
    p->PORTI = (p->addr + PORT_OFFSET(I));

    p->PIO_INT_CFG0 = (p->addr + PIO_INT_CFG_BASE_OFFSET(0));
    p->PIO_INT_CFG1 = (p->addr + PIO_INT_CFG_BASE_OFFSET(1));
    p->PIO_INT_CFG2 = (p->addr + PIO_INT_CFG_BASE_OFFSET(2));
    p->PIO_INT_CFG3 = (p->addr + PIO_INT_CFG_BASE_OFFSET(3));
    p->PIO_INT_CTL = (p->addr + PIO_INT_CTL_BASE_OFFSET);
    p->PIO_INT_STA = (p->addr + PIO_INT_STA_BASE_OFFSET);
    p->PIO_INT_DEB = (p->addr + PIO_INT_DEB_BASE_OFFSET);

    return 0;
}