#include <stdio.h>
#include <arpa/inet.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <math.h>
#include "projcommon.h"
#define HEADER_SIZE (9) 
#define METAP_HEADER_SIZE (17)
#define PACKET_TYPE_SIZE (1) 
#define PACKET_1_BYTE (1) 
#define PACKET_SEQUENCE_SIZE (4)
#define PACKET_LENGTH_SIZE (4)
#define PACKET_ADDR_SIZE (6) 
#define PACKET_INT_SIZE (4)
#define LENGTH_IPV4 (16)
#define LSB_MASK 0x0000FF
#define MAXLEN 15209
#define HOSTNAME_LENGTH (512)
#define NANO_TO_MILLI (1000000)

void error(char *message){
	fprintf(stderr, "A fatal error has occured: %s",  message);
	fflush(stderr);
	exit(1);
}

void packet_q__check(char *s, struct packet_q * packet_q, char on_iter){
	if(packet_q == NULL || 
			(on_iter && packet_q->w_iter_current==NULL)){
		fprintf(stderr, "Invalid packet_q for packet_q_%s call.", s);
		error("packet_q error");
	}
}
void packet_q__check_packet(char *s, struct packet* packet){
	if(packet->priority > 3){
		fprintf(stderr, "%s: INVALID packet. Has priority %02i.\n", s, packet->priority);
	}
}


void packet_q_new(struct packet_q* packet_q){
	/*packet_q->first = packet_q->last = */
	packet_q->w_last = packet_q->w_first = NULL;
	packet_q->length = 0;
	packet_q_reset_iter(packet_q);
}


void packet_q_enqueue(struct packet_q* packet_q, struct packet* packet){
	packet_q__check("enqueue", packet_q, 0);
	packet_q__check_packet("enqueue", packet);
	struct packet_q_wrapper *p_wrapper = (void *)
		malloc(sizeof(struct packet_q_wrapper));
	p_wrapper->packet = packet;
	/* if there is no first, update the first property */
	if(packet_q->w_first == NULL)
		packet_q->w_first = p_wrapper;
	p_wrapper->prev = packet_q->w_last;
	p_wrapper->next = NULL; /* its added to the end, so next is NULL */
	/* if there WAS a previous (is not first), then update its next */
	if(p_wrapper->prev)
		p_wrapper->prev->next = p_wrapper;
	packet_q->w_last = p_wrapper;
	packet_q->length++;
	/* Now, update the date on this wrappper */
	p_wrapper->time = get_time();
	p_wrapper->reset_count = 1;
}

void ff(char*c){ fprintf(stderr,">>>   %s\n",c);  fflush(stderr);}

void packet_q_add_by_sequence(struct packet_q* packet_q, struct packet* packet){
	packet_q__check("add_by_sequence", packet_q, 0);
	packet_q__check_packet("add_by_sequence", packet);
	struct packet_q_wrapper *p_wrapper = (void *)
		malloc(sizeof(struct packet_q_wrapper));
	struct packet *other_packet = NULL;
	p_wrapper->packet = packet;
	/* Now, update the date on this wrappper */
	p_wrapper->time = get_time();
	p_wrapper->reset_count = 1;
	packet_q_reset_iter(packet_q);
	do {
		if(other_packet && other_packet->sequence == packet->sequence) return;
		if(!packet_q->w_iter_next || packet_q->w_iter_next->packet->sequence
				> packet->sequence) break;
	} while((other_packet = packet_q_next(packet_q)));

	packet_q->length++;
	
	/* Should go between iter_current and iter_next */
	

	/* SHOULD GO AT END */
	//if(packet_q->w_iter_next == packet_q->w_iter_current == NULL)
	/* to be added before the first */
	p_wrapper->prev = packet_q->w_iter_current;
	p_wrapper->next = packet_q->w_iter_next;
	if (p_wrapper->prev) 
		p_wrapper->prev->next = p_wrapper; 
	if(p_wrapper->next)
		p_wrapper->next->prev = p_wrapper; 
	if(packet_q->w_first == p_wrapper->next) 
		packet_q->w_first = p_wrapper;
	/* is to be added in the end */
	if(packet_q->w_last == p_wrapper->prev) 
		packet_q->w_last = p_wrapper;
}



/*

int packet_q_remove(struct packet_q* packet_q, struct packet* packet){
	struct packet_q_wrapper *p_wrapper = p_wrapper->w_first;
	if(p_wrapper == NULL) return 0;
	do {
	} while(p_wrapper = p_wrapper->next);
	malloc(sizeof(struct packet_q_wrapper));
	packet_q->length--;
}*/
/*
**
** example usage:
** struct packet_q q;
** packet_q_new(&q);
** [...]
** packet_q_enqueue(&q, &some_packet);
** [...]
** To loop through:
** struct packet *packet;
** packet_q_reset_iter(&q);
** while(packet = packet_q_next(&q)){
**    //do stuff with packet..
**    // To remove from the packet list:
**    if(packet->sequence == 0xBAD){
**        packet_q_remove_current(&q);
**        free(packet);
**    }
**    if(q.current_time == some value){
**        do_something();
**    }
** }
**
**
** */


void packet_q_reset_iter(struct packet_q* packet_q) {
	packet_q__check("reset_iter", packet_q, 0);
	packet_q->w_iter_next = packet_q->w_first;
	packet_q->w_iter_current = NULL;
	packet_q->current_time = -1.0;
}

void packet_q_remove_current(struct packet_q* packet_q){
	packet_q__check("remove_current", packet_q, 1);
	
	if(packet_q->w_first == packet_q->w_iter_current)
		packet_q->w_first = packet_q->w_iter_current->next;
	else packet_q->w_iter_current->prev->next = 
			packet_q->w_iter_current->next;

	if(packet_q->w_last == packet_q->w_iter_current)
		packet_q->w_last  = packet_q->w_iter_current->prev;
	else packet_q->w_iter_current->next->prev = 
		packet_q->w_iter_current->prev;
	/* patch it around iter_current */
	free(packet_q->w_iter_current);
	packet_q->w_iter_current = NULL;
	packet_q->length--;
}

void packet_q_reset_current_time(struct packet_q* packet_q){
	packet_q->w_iter_current->time = get_time();
	packet_q->w_iter_current->reset_count++;
}

struct packet* packet_q_next(struct packet_q* packet_q){
	packet_q__check("next", packet_q, 0);
	packet_q->w_iter_current = packet_q->w_iter_next;
	if(packet_q->w_iter_current==NULL) return NULL;
	packet_q->w_iter_next = packet_q->w_iter_current->next;
	/* set current_time */ 
	packet_q->current_time = packet_q->w_iter_current->time;
	packet_q->current_resets = packet_q->w_iter_current->reset_count;
	return packet_q->w_iter_current->packet;
}


void print_packet_q(struct packet_q* q){
	struct packet *packet;
	packet_q_reset_iter(q);
	fprintf(stderr, "Packet q (len=%02i): ", q->length);
	while((packet = packet_q_next(q))){
		fprintf(stderr, "(s:%02i, p:%02i), ", packet->sequence, packet->priority);
	}
	fprintf(stderr, "    ----\n ");
}
int Socket(int domain, int type, int protocol){
	int sockfd;
	if((sockfd = socket(domain, type, protocol)) == -1)
		error("socket didn't set up correctly\n");
	return sockfd;
}

int Bind(int sockfd, const struct sockaddr *my_addr, socklen_t addrlen){
	int rc;
	if((rc = bind(sockfd, my_addr, addrlen)) == -1){
		perror("Bind:");
		error("bind  didn't work \n");
	}
	return rc; 
}


int Sendto(int s, void *buf, size_t len, int flags, 
		struct sockaddr *to, socklen_t tolen){
	int rc;
	if((rc = sendto(s, buf, len, flags, to, tolen))== -1){
		perror("Error in sendto");
		error("sendto didn't work \n");
	}
	return rc; 
}

ssize_t Write(int fd, const void *buf, size_t count){
	int ret;
	if((ret = write(fd, buf, count)) == -1 || ret != count){
		error("Write failed!\n");
	}
	return ret;
}


ssize_t Recvfrom(int s, void *buf, size_t len, int flags,
		struct sockaddr *from, socklen_t *fromlen){
	int rc;
	if((rc = recvfrom(s,buf,len,flags,from,fromlen)) == -1)
		error("recvfrom is broken.");
	return rc;
}

FILE *Fopen(char *path, char *mode){
	FILE *f;
	if((f=fopen(path, mode))==NULL)
		error("fopen failed, file not found\n");
	return f;
}

int Fclose(FILE *fp){
	int ret_val;
	if((ret_val = fclose(fp)) != 0){
		error("fclose was not successful\n");
	}
	return ret_val;
}

size_t Fwrite(void *ptr, size_t size, size_t nmemb, FILE* stream){
	int ret_val;
	if((ret_val = fwrite(ptr, size, nmemb, stream)) == 0){
		error("fwrite was not successful\n");
	}
	return ret_val;
}
/* parses optarg assumed to be a port and then returns, error checking */
unsigned int options_parse_port(){
	unsigned int port = atoi(optarg);
	if(port < 1024 || port > 65536)
		error("Invalid entry for port. Must be in range 1024 - 65536\n");
	return port;
}

void str_to_sockaddr_in(struct sockaddr_in *addr, const char *s){
	/* NOTE: redo this function probably, making it do fewer steps */
	/* MEMORY LEAK: does host need to be freed? */ 
	char dot_dec[32];
	struct hostent *host;
	if((host = gethostbyname(s)) == NULL){
		fprintf(stderr, "Invalid address: %s\n", s);
		error("Could not find the address specifed.\n");
	}
	addr->sin_family = host->h_addrtype;
	if(inet_ntop(host->h_addrtype, host->h_addr, dot_dec, HOSTNAME_LENGTH) == 0)
		error("inet_ntop() failed!\n");
	if(inet_pton(host->h_addrtype, dot_dec, &addr->sin_addr) == 0)
		error("inet_pton() failed...\n");
}


/* simple gettimeofday() wrapper,  returns time as a double */
double get_time(){
	struct timeval tv;
	if(gettimeofday(&tv, NULL)!=0) error("gettimeofday failed.");
	return (double) ((double)tv.tv_sec + ((double)tv.tv_usec / 1e6));
}

void get_current_time(char* time_str){
	char tstr[20], nano_str[10];
	time_t t = time(NULL);
	(void) strftime(tstr, 20, "%H:%M:%S", localtime(&t));
	double nano = get_time();
	nano = nano - floor(nano);
	sprintf(nano_str, "%0.3f", nano);
	sprintf(time_str, "%s%s", tstr, (nano_str + 1));
}

struct packet* nonblocking_receive_packet(int sockfd){
	socklen_t len;
	uint8_t buffer[MAXLEN];
	struct sockaddr_in addr;
	struct packet *packet;
	int ret_recvfrom;
	if((ret_recvfrom = recvfrom(sockfd, buffer, MAXLEN, MSG_DONTWAIT, (struct sockaddr*) &(addr), &len)) == -1/* && errno == EAGAIN*/){
		return NULL;
	}
	else if(ret_recvfrom == -1){
		error("Problem trying to do the non-blocking recvfrom function\n");
	}
//	fprintf(stderr, "nonblocking: real length: %02i            | ", ret_recvfrom);
	/* Note: some error checking could feasibly be introduced here, by way 
	** of checking if len and the packet->length match up */
	packet = buffer_to_packet(buffer);
//	fprintf(stderr, "nonblocking: packet length: %02i\n", packet->length);
//	packet->source = addr; // TODO this probably breaks everything
	return packet;
}



struct packet* packet_receive(int sockfd){
	socklen_t len;
	uint8_t buffer[MAXLEN];
	struct sockaddr_in addr;
	struct packet *packet;
	Recvfrom(sockfd, buffer, MAXLEN, 0, (struct sockaddr*) &(addr), &len);
	/* Note: some error checking could feasibly be introduced here, by way 
	** of checking if len and the packet->length match up */
	packet = buffer_to_packet(buffer);
	//packet->source = addr; // TODO this probably breaks everything
	return packet;
}
/* identifal to packet_receive, except it has a timeout in milliseconds */
struct packet* packet_receive_timeout(int sockfd, int timeout){
	struct packet* packet = NULL; 
	struct timespec sleep_amount;
	sleep_amount.tv_sec = 0;
	sleep_amount.tv_nsec = NANO_TO_MILLI;
	while(timeout--){
		if((packet = nonblocking_receive_packet(sockfd)))
			break;
		nanosleep(&sleep_amount, NULL);
	}
	return packet;
}

void packet_send_to_addr(int sockfd, struct packet* packet,
		struct sockaddr_in* addr){
	unsigned int buffer_size;
	uint8_t *buffer;
	buffer = packet_to_buffer(packet, &buffer_size);
	if(sendto(sockfd, buffer, buffer_size, 0, (struct sockaddr*) addr, 
				sizeof(struct sockaddr_in)) == -1){
		error("Error in the sendto function!\n");
	}
}

void get_my_addr(struct sockaddr_in *addr){
	char dot_dec[HOSTNAME_LENGTH];
	gethostname(dot_dec, HOSTNAME_LENGTH);
	str_to_sockaddr_in(addr, dot_dec);
	/*
	// getifaddrs returns a linked list of interface entries;
	// find the first active non-loopback interface with IPv4:
	UInt32 address = 0;
	struct ifaddrs *interfaces;
	if( getifaddrs(&interfaces) == 0 ) {
		struct ifaddrs *interface;
		for( interface=interfaces; interface; interface=interface->ifa_next ) {
			if( (interface->ifa_flags & IFF_UP) && ! (interface->ifa_flags & IFF_LOOPBACK) ) {
				addr = (const struct sockaddr_in*) interface->ifa_addr;
			}
		}
		freeifaddrs(interfaces);
	}
	*/
}


void packet_send(int sockfd, struct packet* packet){
	packet_send_to_addr(sockfd, packet, &packet->destination);
}

void print_packet(struct packet* packet){
	fprintf(stderr, "\n-----Packet: %016lx -----\n", 
			(unsigned long) packet);
	fprintf(stderr, "priority: %i\n", packet->priority);
	fprintf(stderr, "type: %c\n", packet->type);
	fprintf(stderr, "Sequence: %08i\n", packet->sequence);
	fprintf(stderr, "Length: %08i\n", packet->length);
	fprintf(stderr, "Payload location: %016lx\n",
			(unsigned long) packet->payload);
	fprintf(stderr, "-----------DESTINATION SOCKADDR_IN");
	print_sockaddr(&packet->destination);
	fprintf(stderr, "-----------SOURCE      SOCKADDR_IN");
	print_sockaddr(&packet->source);
	fprintf(stderr, "-----------END PACKET------------\n\n");
	fflush(stderr);
}

void print_sockaddr(struct sockaddr_in* addr){
	fprintf(stderr, "\n-----sockaddr_in: %016lx -----\n",
			(unsigned long) addr);
	fprintf(stderr, "sin_family: %04i\n", addr->sin_family);
	fprintf(stderr, "sin_port: %04u\n", addr->sin_port);
	fprintf(stderr, "in_addr.s_addr: %016lu\n", 
			(unsigned long) addr->sin_addr.s_addr);
	fflush(stderr);
}
void alog_data(char *d, char* s){
	uint8_t i = 0;
	do {
		if(*(d+i)=='\n') { *(s++)='\\'; *(s++)='n'; }
		else *(s++)=*(d+i);
	} while(i++<3);
	*s='\0';
}
void alog_print_packet(struct packet* packet, char is_send){
	if(packet->type ==  'E') return;
	char data[9], tstr[20], nano_str[10];
	time_t t = time(NULL);
	(void) strftime(tstr, 20, "%H:%M:%S", localtime(&t));
	double nano = get_time();
	nano = nano - floor(nano);
	sprintf(nano_str, "%0.3f", nano);
	alog_data((char*)packet->payload, data);
	printf("Seq#: %i  ", packet->sequence);
	if(is_send) printf("To: ");
	else printf("Size: %i  From: ", packet->length);
	printf("%s  ", packet->dot_addr);
	printf("Time: %s%s  Data: %s\n", tstr, (nano_str+1), data);
}
void num_to_buffer32(uint8_t* b, uint32_t num){
	*(b+3) = num>>24; *(b+2) = num>>16; *(b+1) = num>>8; *b = num;
}
void num_to_buffer16(uint8_t* b, uint16_t num){
	*(b+1) = num>>8; *b = num;
}
uint32_t buffer_to_num32(uint8_t* b){
	return  ((uint32_t) b[3]<<24) |
		((uint32_t) b[2]<<16) |
		((uint32_t) b[1]<<8)  |
		((uint32_t) b[0]);
}
uint16_t buffer_to_num16(uint8_t* b){
	return  ((uint16_t) b[1]<<8) |
		((uint16_t) b[0]);
}

/* Converts a sockaddr_in to a buffer */
void sockaddr_in_to_buffer(uint8_t *buffer, struct sockaddr_in* addr){
	num_to_buffer32(buffer, htonl((uint32_t) addr->sin_addr.s_addr));
	buffer += PACKET_INT_SIZE;
	num_to_buffer16(buffer, htons((uint16_t) addr->sin_port));
}

/* Converts a buffer to a sockaddr_in */
void buffer_to_sockaddr_in(struct sockaddr_in* addr, uint8_t *buffer){
	addr->sin_addr.s_addr = ntohl(buffer_to_num32(buffer));
	buffer += PACKET_INT_SIZE;
	addr->sin_port = ntohs(buffer_to_num16(buffer));
	addr->sin_family = AF_INET; /* set sin family to the default */
}

/* 
** FORMAT:
**   ---------------------------------------------------------------------------
** | 8 bit   | 32 bit src | 16 bit src | 32 bit dest | 16 bit dest | 32 bit |pay
** |priority | IP address |  port      | IP address  |    port     | length |loa
**  ----------------------------------------------------------------------------
**    --------------------------------------------------------------
**   |    8 bit     |   32 bit    |    32 bit   |   variable length |
**   | packet type  |   sequence  |    length   |      payload      |
**    --------------------------------------------------------------
*
*/

uint8_t* packet_to_buffer(struct packet *packet, unsigned int *packet_size){
	uint8_t  *buffer;
	uint8_t *buffer_start;
	uint8_t subpacket_size;
	*packet_size = METAP_HEADER_SIZE + HEADER_SIZE + packet->length;
	subpacket_size = HEADER_SIZE + packet->length;
	buffer_start = buffer = malloc(*packet_size);
	
	/**********************************
	** META PACKET ********************
	***********************************/
	/* 8 bit priority     */
	*buffer = packet->priority;
	buffer += PACKET_1_BYTE;
	/* 32 bit src IP addr, and 16 bit src port */
	sockaddr_in_to_buffer(buffer, &packet->source);
	buffer += PACKET_ADDR_SIZE;
	/* 32 bit dest IP addr, and 16 bit dest port */
	sockaddr_in_to_buffer(buffer, &packet->destination);
	buffer += PACKET_ADDR_SIZE;
	/* Insert 32 bit meta-length   */
	num_to_buffer32(buffer, htonl(packet->length));
	buffer += PACKET_INT_SIZE;
	
	/**********************************
	** INNER PACKET (old style) *******
	***********************************/
	/* Insert packet type */
	*buffer = packet->type;
	buffer += PACKET_TYPE_SIZE;
	/* Insert 32 bit sequence */
	num_to_buffer32(buffer, htonl(packet->sequence));
	buffer += PACKET_SEQUENCE_SIZE;
	/* Insert 32 bit length   */
	num_to_buffer32(buffer, htonl(packet->length));
	buffer += PACKET_LENGTH_SIZE;
	/* Insert variable length payload */
	(void) memcpy(buffer, packet->payload, packet->length);
	return buffer_start;
}

struct packet* buffer_to_packet(uint8_t *buffer){
	struct packet *packet = malloc(sizeof(struct packet));
	
	/**********************************
	** META PACKET ********************
	***********************************/
	/* extract 8 bit priority     */
	packet->priority = (char) *buffer;
	buffer += PACKET_1_BYTE;
	/* extract 32 bit src IP addr & 16 bit port */
	buffer_to_sockaddr_in(&packet->source, buffer);
	buffer += PACKET_ADDR_SIZE;
	/* extract 32 bit destination IP addr  & 16 bit port*/
	buffer_to_sockaddr_in(&packet->destination, buffer);
	buffer += PACKET_ADDR_SIZE;

	/* we don't even need to use the "meta length" property here, 
	** so we just skip over it */
	buffer += PACKET_INT_SIZE;
	
	/**********************************
	** INNER PACKET (old style) *******
	***********************************/
	/* Get packet type */
	packet->type = (char) *buffer;
	buffer += PACKET_TYPE_SIZE;
	/* Extract 32 bit sequence */
	packet->sequence = ntohl(buffer_to_num32(buffer));
	buffer += PACKET_SEQUENCE_SIZE;
	/* Extract 32 bit length   */
	packet->length   = ntohl(buffer_to_num32(buffer));
	buffer += PACKET_LENGTH_SIZE;
	/* Extract variable length payload */
	packet->payload = malloc(packet->length * sizeof(uint8_t));
	(void) memcpy(packet->payload, buffer, packet->length);
	return packet;
}