#include <stdint.h>
#include "palmipede.h"
#include "main.h"

#define FIND_AND_NUL(s, p, c) ( \
   (p) = strchr(s, c), \
   *(p) = '\0', \
   ++(p), \
   (p))

// uart rx
char uart1_rx_buf[2], uart2_rx_buf[2], uart3_rx_buf[2], rx_cmd_rb[RX_CMD_RB_SIZE];
uint8_t rx_cmd_rb_in_idx, rx_cmd_rb_out_idx;

void enqueue_cmd(char in){
        rx_cmd_rb[rx_cmd_rb_in_idx] = in;
        ringbuf_increment(&rx_cmd_rb_in_idx, RX_CMD_RB_SIZE_MASK);
}

void enqueue_nmea(char in){
	static uint8_t start, sentence, checksum_index, valuebuf_index, commas_index;
	static char valuebuf[128];
	if(in == '$'){
//		display_write(valuebuf,24);
		print_nmea(valuebuf);
		valuebuf_index = 0;
	}
	valuebuf[valuebuf_index] = in;
	valuebuf_index++;
}

void print_nmea(char *buf){
	char outbuf[128];
	char origbuf[128];
	strcpy(origbuf, buf);
	char* message_id = buf;
	char* time = FIND_AND_NUL(message_id, time, ',');
	char* latitude = FIND_AND_NUL(time, latitude, ',');
	char* longitude = FIND_AND_NUL(latitude, longitude, ',');
	char* fix = FIND_AND_NUL(longitude, fix, ',');
	char* sat = FIND_AND_NUL(fix, sat, ',');
	char* hdop = FIND_AND_NUL(sat, hdop, ',');
	char* alt = FIND_AND_NUL(hdop, alt, ',');
	if(strcmp(message_id,"$GPGGA") == 0){
		strcpy(outbuf, "\f");
		strcat(outbuf, message_id);
		strcat(outbuf, " time: ");
		strcat(outbuf, time);
		strcat(outbuf, " lat: ");
		strcat(outbuf, latitude);
		strcat(outbuf, " lon: ");
		strcat(outbuf, longitude);
		strcat(outbuf, " fix: ");
		strcat(outbuf, fix);
		strcat(outbuf, " sat: ");
		strcat(outbuf, sat);
		display_write2(origbuf);
	}
}

void dequeue_cmd(void){
//        decode_cmd(rx_cmd_rb[rx_cmd_rb_out_idx]);
        display_write(&rx_cmd_rb[rx_cmd_rb_out_idx], 1);
        ringbuf_increment(&rx_cmd_rb_out_idx, RX_CMD_RB_SIZE_MASK);
}


inline uint16_t ringbuf_increment(uint8_t *index, uint8_t buff_size_mask) {
    (*index)++;
    *index &= buff_size_mask;
    return *index;
}

uint8_t create_groupset(char *buf, uint8_t gbw, float tfv, float rfv, uint8_t Tx_CTCSS, uint8_t SQ, uint8_t Rx_CTCSS){
	return sprintf(buf, "AT+DMOSETGROUP=%01d,%.4f,%.4f,%04d,%01d,%04d\r\n", gbw, tfv, rfv, Tx_CTCSS, SQ, Rx_CTCSS);
}
