#include "bexus_udp_client.h"


extern struct netif gnetif;
extern ip4_addr_t ipaddr;  //own
struct udp_pcb *udp;
ip_addr_t ip; //remote
//struct pbuf  *p;
uint32_t structLen;

u16_t groundPort = 1234;
u16_t rawPort = 1111;
u16_t debugPort = 1337;

char * reset_key = "RESET!!";

#define UDP_RAW_LEN (11 * 4)

uint8_t udpMsg[IN_LEN * 3];// max size 1472
uint8_t udpMsgRaw[UDP_RAW_LEN * 2];

void udp_echo_recv(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) {
  if(strcmp(p->payload, reset_key) == 0) {
		
		NVIC_SystemReset();
	}
	pbuf_free(p);
}
void Udp_Init() {
__HAL_RCC_ETHMAC_CLK_ENABLE(); 
__HAL_RCC_ETHMACTX_CLK_ENABLE(); 
__HAL_RCC_ETHMACRX_CLK_ENABLE();
	
	
	IP_ADDR4(&ip, 172, 16, 18, 255); 
	
	
	udp = udp_new();
	structLen = sizeof(Dataset_Master[0]);

	memset(udpMsg, 'E', sizeof(udpMsg));
	
	//p = pbuf_alloc(PBUF_TRANSPORT, sizeof(udpMsg), PBUF_REF);
	//p->payload = udpMsg; //pointer
	
	udp_recv(udp, udp_echo_recv, NULL);
	udp_bind(udp, &ipaddr, 65100);
}
err_t errorstate = HAL_OK;
err_t UDP_send(uint8_t * udp_buff, uint16_t size, uint16_t port) {
	
	struct pbuf *pb;
	pb = pbuf_alloc(PBUF_TRANSPORT, size, PBUF_POOL);
	pbuf_take(pb, udp_buff, size);
	errorstate = udp_sendto(udp, pb, &ip, port); //dest port
	pbuf_free(pb);

	return errorstate;
}
err_t er;

void Udp_Send_Ground() {

	memcpy(&udpMsg[0], &Dataset_Master[buff_sensor_data_index], structLen);
	memcpy(&udpMsg[structLen], &Dataset_Slave[buff_sensor_data_index], structLen);
	memcpy(&udpMsg[structLen*2], &Dataset_Fusion[buff_sensor_data_index], structLen);
	//udp_sendto(udp, p, &ip, groundPort);
	er = UDP_send(udpMsg, sizeof(udpMsg), groundPort);
	
}
void Udp_Send_Raw() {
	
	memcpy(&udpMsgRaw[0], &Dataset_Master[buff_sensor_data_index].accel_raw, 9*4);
	memcpy(&udpMsgRaw[9*4], &Dataset_Master[buff_sensor_data_index].time, 4);
	memcpy(&udpMsgRaw[10*4], &Dataset_Master[buff_sensor_data_index].count, 4);
	
	memcpy(&udpMsgRaw[UDP_RAW_LEN], &Dataset_Slave[buff_sensor_data_index].accel_raw, 9*4);
	memcpy(&udpMsgRaw[UDP_RAW_LEN + 9*4], &Dataset_Slave[buff_sensor_data_index].time, 4);
	memcpy(&udpMsgRaw[UDP_RAW_LEN + 10*4], &Dataset_Slave[buff_sensor_data_index].count, 4);
	//udp_sendto(udp, p, &ip, rawPort);
	UDP_send(udpMsgRaw, sizeof(udpMsgRaw), rawPort);
}
void Udp_Send_Debug(uint8_t *msg) {
	
	UDP_send(msg, sizeof(msg), debugPort);
	
}

u8_t isNetworkUp() {
	return netif_is_link_up(&gnetif);
}