#define _INCP_INTERNAL

#include <string.h>

#include "incp2.h"
#include "service.h"
#include "messaging.h"
#include "asn1/asn1_wrapper.h"



static err_t handle_incoming(endpoint_t source, BitStream *in_bs);



void incp_receiver_task(void *args)
{
	(void)args; //Stfu

	while (1)
	{
		//LATER: Blocking wait for RTOS queue to fetch bus / radio message (go wild on that routing)
		BitStream in_bs = { 0 };						// <-- Populate from transport
		endpoint_t source = INCP_ENDPOINT_NVAL;			// <-- Populate from transport

		handle_incoming(source, &in_bs);
	}
}



err_t incp_async_datareq(endpoint_t subsystem, U32 id)
{
	err_t err; //Don't init on purpose
	BitStream out_bs;
	uint8_t buf[DEFAULT_MSGBUF_SIZE];
	BitStream_Init(&out_bs, buf, sizeof(buf));

	IncpMsg msg;
	msg.kind = dataReq_PRESENT;
	msg.u.dataReq.id = id;

	err = incp_encode_header(&out_bs, &msg);
	if (err != ERR_SUCCESS) goto _exit;

	err = impl_transmit(subsystem, buf, BitStream_GetLength(&out_bs));

_exit:
	return err;
}

err_t incp_async_ping(endpoint_t subsystem, uint8_t *bytes)
{
	err_t err; //Don't init on purpose
	BitStream out_bs;
	uint8_t buf[DEFAULT_MSGBUF_SIZE];
	BitStream_Init(&out_bs, buf, sizeof(buf));

	IncpMsg msg;
	msg.kind = ping_PRESENT;
	memcpy(&msg.u.ping.payload, bytes != NULL ? bytes : "I am a very fancy ping payload!", 32);
	
	err = incp_encode_header(&out_bs, &msg);
	if (err != ERR_SUCCESS) goto _exit;

	err = impl_transmit(subsystem, buf, BitStream_GetLength(&out_bs));

_exit:
	return err;
}



static inline err_t handle_incoming_ping(endpoint_t source, IncpMsg *in_msg, BitStream *in_bs)
{
	(void)in_bs;
	err_t err; //Don't init on purpose
	BitStream out_bs;
	uint8_t buf[DEFAULT_MSGBUF_SIZE];
	BitStream_Init(&out_bs, buf, sizeof(buf));

	IncpMsg msg = *in_msg;
	msg.kind = pong_PRESENT;

	err = incp_encode_header(&out_bs, &msg);
	if (err != ERR_SUCCESS) goto _exit;

	err = impl_transmit(source, buf, BitStream_GetLength(&out_bs)); //"Destroy a Ghast with a fireball"

_exit:
	return err;
}

static inline err_t handle_incoming_pong(endpoint_t source, IncpMsg *in_msg, BitStream *in_bs)
{
	(void)in_bs;
	err_t err; //Don't init on purpose

	impl_callback_pong(source, in_msg);
	err = ERR_SUCCESS;

_exit:
	return err;
}

static inline err_t handle_incoming_datareq(endpoint_t source, IncpMsg *in_msg, BitStream *in_bs)
{
	err_t err; //Don't init on purpose
	BitStream out_bs;
	uint8_t buf[DEFAULT_MSGBUF_SIZE];
	BitStream_Init(&out_bs, buf, sizeof(buf));

	AnyData data = { 0 };
	data.kind = in_msg->u.dataReq.id; //REVIEW: Dubious types

	err = impl_populate_dataret(&data);
	if (err != ERR_SUCCESS) goto _exit;

	err = incp_encode_dataret(&out_bs, &data);
	if (err != ERR_SUCCESS) goto _exit;

	err = impl_transmit(source, buf, BitStream_GetLength(&out_bs));

_exit:
	return err;
}

static inline err_t handle_incoming_dataret(endpoint_t source, IncpMsg *in_msg, BitStream *in_bs)
{
	err_t err; //Don't init on purpose
	AnyData data; //Careful with lifetime of this! Rust my beloved (never tried it)

	err = incp_decode_dataret(in_bs, &data);
	if (err != ERR_SUCCESS) goto _exit;

	impl_callback_dataret(source, &data);
	err = ERR_SUCCESS;

_exit:
	return err;
}

static err_t handle_incoming(endpoint_t source, BitStream *in_bs)
{
	err_t err; //Don't init on purpose
	IncpMsg msg;


	err = incp_decode_header(in_bs, &msg);
	if (err != ERR_SUCCESS) goto _exit;
	
	switch (msg.kind)
	{
	case ping_PRESENT:
		err = handle_incoming_ping(source, &msg, in_bs);
		break;

	case pong_PRESENT:
		err = handle_incoming_pong(source, &msg, in_bs);
		break;

	case dataReq_PRESENT:
		err = handle_incoming_datareq(source, &msg, in_bs);
		break;

	case dataRet_PRESENT:
		err = handle_incoming_dataret(source, &msg, in_bs);
		break;
	
	default:
		err = ERR_NOT_IMPL;
		break;
	}

_exit:
	return err;
}