#include <stdio.h>          //printf
#include <sys/types.h>      //open
#include <sys/stat.h>       //open
#include <fcntl.h>          //open
#include <unistd.h>         //close
#include <errno.h>          //perror
#include <stdlib.h>         //exit
#include <sys/mman.h>       //mmap
#include <signal.h>         //signal
#include <assert.h>         //assert
#include <sys/socket.h>     //socket, inet_aton
#include <netinet/in.h>     //socket, inet_aton
#include <netinet/udp.h>    //socket
#include <arpa/inet.h>      //htons, ntohs, htonl, ntohl, inet_aton

#include "hps_0.h"

#define LW_FPGA_SLAVES_BASE 0xFF200000
#define LW_FPGA_SVAVES_END  0xFF3FFFFF
#define LW_FPGA_SLAVES_SPAN 0x00200000

void bail();
void sig_handler(int signum);

//NOTE: use void* Pointer to avoid pointer arithmetic during offset addition
void* lw_bridge = NULL;
int mem_fd = -1;

int main(int argc, char** argv){

    //Register Signal Handling
    signal(SIGHUP, sig_handler);
    signal(SIGINT, sig_handler);
    signal(SIGQUIT, sig_handler);
    signal(SIGABRT, sig_handler);

    mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
    if(mem_fd < 0) {
        perror("Could not open /dev/mem");
        bail(EXIT_FAILURE);
    }

    lw_bridge = (uint32_t*)mmap(NULL, LW_FPGA_SLAVES_SPAN, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, LW_FPGA_SLAVES_BASE);
    if(lw_bridge == MAP_FAILED) {
        perror("Could not mmap /dev/mem");
        bail(EXIT_FAILURE);
    }

    // Polling Address
    uint32_t* ADDR_0 = (uint32_t*)(lw_bridge + TEST_FPGA_0_BASE);
    // Read Address
    uint32_t* ADDR_1 = ADDR_0 + 1; //Pointer Arithmetic
    // Write Address
    uint32_t* ADDR_2 = ADDR_0 + 2; //Pointer Arithmetic
    
    struct sockaddr_in src, dest;

    src.sin_family  = AF_INET;
    src.sin_port    = htons(5000);
    inet_aton("192.168.0.10", &(src.sin_addr));

    dest.sin_family = AF_INET;
    dest.sin_port   = htons(4000);
    inet_aton("192.168.0.15", &(dest.sin_addr));

    uint32_t tmp;
    struct in_addr tmp_addr;
    
    assert(*(ADDR_0) == 0);
    // Write SRC Address
    *ADDR_2 = src.sin_addr.s_addr;
    // Write DEST Address
    *ADDR_2 = dest.sin_addr.s_addr;
    // Write UDP Ports
    *ADDR_2 = ((src.sin_port << 16) & 0xFFFF0000) | (dest.sin_port & 0xFFFF);
    // Write Packet Length
    *ADDR_2 = htonl(2);
    // Write BYTE 1
    *ADDR_2 = 0xAAAABBBB;
    // Write BYTE 2
    *ADDR_2 = 0xCCCCDDDD;

    // Read SRC Address
    tmp_addr.s_addr = *ADDR_1;
    printf("SRC Address: %s\n", inet_ntoa(tmp_addr));
    assert(*ADDR_0 == 1);
    // Read DEST Address
    tmp_addr.s_addr = *ADDR_1;
    printf("DEST Address: %s\n", inet_ntoa(tmp_addr));
    // Read UDP Ports
    tmp = *ADDR_1;
    printf("UDP SRC Port: %d, UDP DEST Port: %d\n", ntohs((tmp >> 16) & 0xFFFF), ntohs(tmp & 0xFFFF));
    // Read Packet Length
    printf("Packet Length: %d\n", ntohl(*ADDR_1));
    // Read BYTE1
    printf("Byte 1: %X\n", *ADDR_1);
    // Read BYTE2
    printf("Byte 2: %X\n", *ADDR_1);
    // Read BYTE3
    printf("Byte 3: %X\n", *ADDR_1);

    assert(*(ADDR_0) == 0);
    printf("Done.\n");
    exit(EXIT_SUCCESS);
}

void bail(int code){
    // Close File Descriptor
    if (mem_fd != -1){
        close(mem_fd);
    }

    // Unmap /dev/mem
    if (lw_bridge != NULL){
        munmap(lw_bridge, LW_FPGA_SLAVES_SPAN);
    }
    exit(code);
}

void sig_handler(int signum){
    printf("Received Signal %d.\n Exiting...\n", signum);
    bail(EXIT_SUCCESS);
}