#include "rsp.h"

/* recv() */
#include <sys/types.h>
#include <sys/socket.h>

#include <string.h> /* memset(), strncmp() */
#include <stdio.h> /* perror(), fprintf(), snprintf(), sscanf() */
#include <stdlib.h> /* malloc(), free() */
#include <unistd.h> /* close() */
#include <errno.h> /* EAGAIN... */

void _rsp_sniff_garbage(struct rsp_state *rsp) {
	int recvbytes, rv;

	// Sniff garbages before sending command
	recvbytes = recv(rsp->sockfd, rsp->response, rsp->data_maxlen, MSG_DONTWAIT);
	if ( recvbytes < 0 ) {
		rv = errno;
		if ( rv == EAGAIN || rv == EWOULDBLOCK ) {
			/* No garbage found, that's a good news */
		} else {
			LOG_DEBUG("! <-(error %i)\n", rv);
		}
		recvbytes=0;
	}
	rsp->response[recvbytes] = '\0';
	if ( recvbytes > 0 ) {
		LOG_DEBUG("! <-'%s'\n",rsp->response);
	}
}

int rsp_recv_full(struct rsp_state *rsp) {

	int rv, error=0, complete=0, full=0, sentbytes;
	ssize_t bufoldpos=0, bufpos=0;
	char *bom=NULL, *eom=NULL, *ptr;
	unsigned char c, cack='+';
	unsigned int computed_checksum, read_checksum;

	do {
		rv = recv(rsp->sockfd, rsp->response + bufpos, rsp->data_maxlen - bufpos, 0);
		if ( rv < 0 ) {
			switch (errno) {
				case EAGAIN:
					continue;
				default:
					error=1;
					continue;
			}
		}
		if ( rv == 0 ) {
			error=1;
			continue;
		}

		bufoldpos = bufpos;
		bufpos += rv;

		if ( bom == NULL ) {
			bom = memchr(rsp->response + bufoldpos, '$', bufpos - bufoldpos);
		}

		if ( bom != NULL && eom == NULL ) {
			eom = memchr(bom, '#', bufpos - (bom - rsp->response) );
		}

		complete = bom && eom && (eom+2) <= (rsp->response + bufpos); // 2 char checksum after EOM
		full = (bufpos >= rsp->data_maxlen);

	} while (!error && !complete && !full);

	if (error) return 1;
	if (full) return 2;

	rsp->response[bufpos]='\0';

	c=eom[3];
	eom[3]='\0';
	LOG_DEBUG("  <- '%s'\n", rsp->response);

	if ( (eom+3) < (rsp->response + bufpos - 1)) {
		eom[3]=c;
		LOG_DEBUG("? <- '%s'\n", eom+3);
	}


	rsp->response_len = eom - bom - 1;
	rsp->response_bom = bom;
	rsp->response_eom = eom;

	computed_checksum=0;
	for (ptr = bom+1; ptr < eom; ptr++) {
		computed_checksum = (computed_checksum + ((unsigned char)*ptr) ) % 256;
	}

	rv = sscanf(eom + 1, "%02x", &read_checksum);
	if ( rv != 1 || computed_checksum != read_checksum) return 3;

	rsp->replied = 1;

	if (rsp->noackmode !=1) {
		sentbytes = send(rsp->sockfd, &cack, 1, 0);
		if ( sentbytes < 1 ) return 4;
		LOG_DEBUG("-> '+'\n");
	}

	return 0;
}

int rsp_init(int sockfd, int data_maxlen, struct rsp_state *rsp) {
	memset(rsp,0,sizeof(struct rsp_state));
	rsp->sockfd = sockfd;
	rsp->data_maxlen = data_maxlen;

	rsp->command = malloc(data_maxlen+1);
	if ( rsp->command == NULL ) return 1;

	rsp->response = malloc(data_maxlen+1);
	if ( rsp->response == NULL ) return 1;
	
	rsp->decoded = malloc(data_maxlen+1);
	if ( rsp->decoded == NULL ) return 1;
	
/*
	rsp_query(rsp, "QStartNoAckMode");
	if ( rsp_check_and_clear(rsp, "OK") == 0 ) rsp->noackmode=1;
*/
	return 0;
}

void rsp_quit(struct rsp_state *rsp) {
	//rsp_query(rsp, "k");
	if ( rsp->command != NULL ) free(rsp->command);
	if ( rsp->response != NULL ) free(rsp->response);
	if ( rsp->decoded != NULL ) free(rsp->decoded);
	close(rsp->sockfd);
	memset(rsp,0,sizeof(struct rsp_state));
}

void rsp_send_break(struct rsp_state *rsp) {
	int sentbytes;
	char cbreak=3;

	rsp->replied = 0;
	rsp->response_len = 0;

	_rsp_sniff_garbage(rsp);

	sentbytes = send(rsp->sockfd, &cbreak, 1, 0);
	if ( sentbytes < 1 ) return;
	LOG_DEBUG("-> '^C'\n");

	rsp_recv_full(rsp);
}

void rsp_query(struct rsp_state *rsp, char command[]) {
	int sentbytes;
	unsigned int checksum, i;
	unsigned char c;


	rsp->replied = 0;
	rsp->response_len = 0;

	i=0; checksum=0;
	while ( (c=command[i++]) != '\0' ) {
		checksum = (checksum + c ) % 256;
	}

	rsp->command_len = snprintf(rsp->command, rsp->data_maxlen, "$%s#%02x", command, checksum);
	if (rsp->command_len < 5) return;

	_rsp_sniff_garbage(rsp);

	sentbytes = send(rsp->sockfd, rsp->command, rsp->command_len, 0);
	if ( sentbytes < rsp->command_len ) return;
	LOG_DEBUG("-> '%s'\n", rsp->command);


	rsp_recv_full(rsp);
}

int rsp_check_and_clear(struct rsp_state *rsp, char expected_response[]) {

	int expected_responselen = strlen(expected_response);

	if ( rsp->replied != 1 ) return 1;
	rsp->replied = 0;

	if ( rsp->response_len < expected_responselen ) return 2;
	if ( strncmp(rsp->response_bom + 1, expected_response, expected_responselen) != 0 ) return 3;

	return 0;
}

int rsp_decode(struct rsp_state *rsp) {
/*
Response data can be run-length encoded to save space. Run-length encoding replaces runs of identical characters with one instance of the repeated character, followed by a ‘*’ and a repeat count. The repeat count is itself sent encoded, to avoid binary characters in data: a value of n is sent as n+29. For a repeat count greater or equal to 3, this produces a printable ascii character, e.g. a space (ascii code 32) for a repeat count of 3. (This is because run-length encoding starts to win for counts 3 or more.) Thus, for example, ‘0* ’ is a run-length encoding of “0000”: the space character after ‘*’ means repeat the leading 0 32 - 29 = 3 more times.

The printable characters ‘#’ and ‘$’ or with a numeric value greater than 126 must not be used. Runs of six repeats (‘#’) or seven repeats (‘$’) can be expanded using a repeat count of only five (‘"’). For example, ‘00000000’ can be encoded as ‘0*"00’. 
*/
	char src, *srcptr, *dstptr;
	int repe, max;
	
	if ( rsp->replied != 1 ) return -1;

	dstptr = rsp->decoded;
	for ( srcptr = rsp->response_bom+1; srcptr < rsp->response_eom; srcptr++ ) {
		// FIXME : implemnt RLE decoding
		src = *srcptr;
		switch (src) {
			case '*':
				srcptr++;
				repe=(*srcptr)-29;
				if (repe < 2 ) return -1;
				max = rsp->decoded + rsp->data_maxlen - 1 - dstptr;
				if ( repe > max ) { repe = max; }
				memset(dstptr, *(srcptr-2), repe);
				dstptr += repe;
				break;
			default:
				*(dstptr++) = *srcptr;
				break;
		}
	}
	*dstptr='\0';

	return dstptr - rsp->decoded;
}

int rsp_poke(struct rsp_state *rsp, unsigned int poke_addr, unsigned int poke_value) {
	char command[16];
	printf("DEBUG : rsp_poke()\n");
	if ( snprintf(command, 15, "M %4x,2:%2x", poke_addr & 0xffff, poke_value & 0xff) < 11 ) return 1;
	printf("DEBUG : command == '%s'\n", command);
	rsp_query(rsp, command); // Send the poke command to GDB stub 
	printf("DEBUG : response == '%s'\n", rsp->response);
	if ( rsp_check_and_clear(rsp, "OK") != 0 ) return 2;
	
	return 0;
}