/**
 * \file
 *
 * \brief ADP service implementation
 *
 * Copyright (c) 2015-2018 Microchip Technology Inc. and its subsidiaries.
 *
 * \asf_license_start
 *
 * \page License
 *
 * Subject to your compliance with these terms, you may use Microchip
 * software and any derivatives exclusively with Microchip products.
 * It is your responsibility to comply with third party license terms applicable
 * to your use of third party software (including open source software) that
 * may accompany Microchip software.
 *
 * THIS SOFTWARE IS SUPPLIED BY MICROCHIP "AS IS".  NO WARRANTIES,
 * WHETHER EXPRESS, IMPLIED OR STATUTORY, APPLY TO THIS SOFTWARE,
 * INCLUDING ANY IMPLIED WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY,
 * AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT WILL MICROCHIP BE
 * LIABLE FOR ANY INDIRECT, SPECIAL, PUNITIVE, INCIDENTAL OR CONSEQUENTIAL
 * LOSS, DAMAGE, COST OR EXPENSE OF ANY KIND WHATSOEVER RELATED TO THE
 * SOFTWARE, HOWEVER CAUSED, EVEN IF MICROCHIP HAS BEEN ADVISED OF THE
 * POSSIBILITY OR THE DAMAGES ARE FORESEEABLE.  TO THE FULLEST EXTENT
 * ALLOWED BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN ANY WAY
 * RELATED TO THIS SOFTWARE WILL NOT EXCEED THE AMOUNT OF FEES, IF ANY,
 * THAT YOU HAVE PAID DIRECTLY TO MICROCHIP FOR THIS SOFTWARE.
 *
 * \asf_license_stop
 *
 */
/*
 * Support and FAQ: visit <a href="https://www.microchip.com/support/">Microchip Support</a>
 */

#include <adp.h>
#include <adp_interface.h>
#include <err_codes.h>
#include <string.h>

/** ID of last received message */
static uint8_t last_received_message_id = 0xff;
/** true if valid message has been received */
static bool packet_received = false;
/** Number of bytes received */
static uint16_t bytes_received;
/** Judge previous received data */
static uint8_t prev_data = 0;
/** The length of received data */
static uint16_t length_received;
/** Current state */
static enum rx_state_e rx_state;

uint16_t adp_add_send_byte(uint8_t *buffer, uint8_t index, uint8_t *data, uint16_t length)
{
	for (uint16_t i = 0; i < length; i++) {
		if (*(data + i) == ADP_TOKEN) {
			*(buffer + index) = ADP_TOKEN;
			index++;
		}
		*(buffer + index) = *(data + i);
		index++;
	}

	return index;
}

/**
 * \internal Handle incoming data byte
 *
 * \param[in] data New data byte to handle
 *
 * \return
 * \retval true Given data byte is part of message data
 * \retval false Given data byte is not part of message data
 */
static bool adp_add_receive_byte(uint8_t data)
{
	static uint8_t message_id;

	if ((rx_state == RX_STATE_GOT_SYMBOL) && (data != ADP_TOKEN)) {
		/* Abort packet reception, new packet incoming */
		rx_state = RX_STATE_WAIT_LENGTH_LSB;
	}

	switch (rx_state) {
	case RX_STATE_IDLE:
		packet_received          = false;
		last_received_message_id = 0xFF;
		/* We are waiting for a new packet. */
		if (data != ADP_TOKEN) {
			return false;
		}
		/* Got start symbol, wait for message ID */
		rx_state = RX_STATE_WAIT_MSG_ID;
		return false;

	case RX_STATE_WAIT_MSG_ID:
		if (data == ADP_TOKEN) {
			/* Restart. Don't change state. Wait for new message ID */
			return false;
		}
		message_id = data;
		rx_state   = RX_STATE_WAIT_LENGTH_LSB;
		return false;

	case RX_STATE_WAIT_LENGTH_LSB:
		if (data == ADP_TOKEN) {
			if (prev_data != ADP_TOKEN) {
				prev_data = ADP_TOKEN;
				return false;
			}
		}
		length_received = data;
		rx_state        = RX_STATE_WAIT_LENGTH_MSB;
		prev_data       = 0;
		return false;

	case RX_STATE_WAIT_LENGTH_MSB:
		if (data == ADP_TOKEN) {
			if (prev_data != ADP_TOKEN) {
				prev_data = ADP_TOKEN;
				return false;
			}
		}
		length_received += (uint16_t)data << 8;
		prev_data = 0;
		/* Got valid length, do we expect data? */
		if (length_received == 0) {
			/* No data here, wait for next packet */
			rx_state                 = RX_STATE_IDLE;
			packet_received          = true;
			last_received_message_id = message_id;
			return false;
		}

		/* Wait for packet data */
		bytes_received = 0;
		rx_state       = RX_STATE_GET_DATA;
		return false;

	case RX_STATE_GET_DATA:
	case RX_STATE_GOT_SYMBOL:
		if ((data == ADP_TOKEN) && (rx_state == RX_STATE_GET_DATA)) {
			rx_state = RX_STATE_GOT_SYMBOL;
			return false;
		}
		/* Add new data to rx buffer */
		bytes_received++;
		/* Are we done yet? */
		if (length_received == bytes_received) {
			/* Yes we are! */
			packet_received          = true;
			rx_state                 = RX_STATE_IDLE;
			last_received_message_id = message_id;
			return true;
		}
		/* Not done yet.. keep on receiving */
		rx_state = RX_STATE_GET_DATA;
		return true;
	}
	return false;
}

static bool adp_is_received(void)
{
	if (bytes_received == 0) {
		return false;
	}
	return packet_received;
}

static uint8_t adp_packet_received_get_id(void)
{
	return last_received_message_id;
}

static bool adp_protocol_add_byte(uint8_t rx_id, uint8_t *rx_buf, uint8_t length, uint8_t *protocol_buf)
{
	uint8_t i;

	for (i = 0; i < length; i++) {
		if (adp_add_receive_byte(*(rx_buf + i)) == true) {
			/* This is a data byte */
			protocol_buf[bytes_received - 1] = *(rx_buf + i);
		}
		if (adp_is_received() & (adp_packet_received_get_id() == rx_id)) {
			return true;
		}
	}
	return false;
}

static bool adp_check_for_response(uint8_t rx_id, uint8_t *protocol_buf, uint8_t length)
{
	uint8_t retry;
	bool    status                           = false;
	uint8_t rx_buf[ADP_MAX_PACKET_DATA_SIZE] = {
	    0,
	};

	retry           = 50;
	length          = length + ADP_LENGTH_PACKET_HEADER;
	packet_received = false;
	while ((adp_is_received() == false) & (retry-- > 0)) {
		if (adp_interface_read_response(rx_buf, length) == ERR_NONE) {
			status = adp_protocol_add_byte(rx_id, rx_buf, length, protocol_buf);
			if (status == true) {
				break;
			}
		}
	}

	return status;
}

static void adp_wait_for_response(uint8_t rx_id, uint8_t *protocol_buf, uint8_t length)
{
	uint8_t rx_buf[ADP_MAX_PACKET_DATA_SIZE] = {
	    0,
	};

	packet_received = false;
	length          = length + ADP_LENGTH_PACKET_HEADER;
	while ((adp_is_received() == false)) {
		if (adp_interface_read_response(rx_buf, length) == ERR_NONE) {
			adp_protocol_add_byte(rx_id, rx_buf, length, protocol_buf);
		}
	}
}

/**
 * \brief Initialization of the ADP service
 *
 * \return None
 */
void adp_init(void)
{
	// adp_interface_init();
}

/**
 * \brief Send handshake to PC and check for response
 *
 * \param[in]  protocol_version  ADP version used in this implementation
 * \param[out] status            Status returned by PC
 *
 * \return PC returned valid response to out handshake
 * \retval true  If we got valid response
 * \retval false If we didn't receive a valid handshake response
 */
static bool adp_request_handshake(uint8_t protocol_version, uint8_t options, uint8_t *protocol_buf)
{
	uint16_t data_length                   = MSQ_REQ_HANDSHAKE_LEN + ADP_LENGTH_PACKET_HEADER;
	uint8_t  key[8]                        = ADP_HANDSHAKE_KEY;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format            msg_format;
	struct adp_msg_request_handshake msg_request_handshake;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_REQ_HANDSHAKE;
	msg_format.data_length     = MSQ_REQ_HANDSHAKE_LEN;

	msg_request_handshake.protocol_version = ADP_VERSION;
	msg_request_handshake.options          = ADP_HANDSHAKE_OPTIONS_GPIO;
	memcpy(&msg_request_handshake.key, key, 8);
	memcpy((uint8_t *)&msg_format.data, &msg_request_handshake, sizeof(msg_request_handshake));
	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	return adp_check_for_response(MSG_RES_HANDSHAKE, protocol_buf, 1);
}

/**
 * \brief Send handshake to PC until we get a valid response
 *
 * \warning Blocking function
 *
 * \return Handshake status. See /ref something
 */
enum adp_handshake_status adp_wait_for_handshake(void)
{
	uint8_t handshake_status;

	/* Keep sending handshake until we get something back */
	while (adp_request_handshake(ADP_VERSION, 0, &handshake_status) == false) {
	}

	/* Return status */
	return ((enum adp_handshake_status)handshake_status);
}

/**
 * \brief Request status from PC
 *
 * Sends a MSG_REQ_STATUS and waits for response
 *
 * \return PC Status. See /ref something
 */
enum adp_status_code adp_request_status(void)
{
	uint16_t data_length = MSG_REQ_STATUS_LEN + ADP_LENGTH_PACKET_HEADER;
	uint16_t status;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_REQ_STATUS;
	msg_format.data_length     = MSG_REQ_STATUS_LEN;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);
	/* Wait for response from PC */
	adp_wait_for_response(MSG_RES_STATUS, (uint8_t *)&status, 2);
	return ((enum adp_status_code)status);
}

/**
 * \brief Send MSG_CONF_INFO message and wait for response
 *
 * \param[in] title       Application title
 * \param[in] description Application description
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_configure_info(const char *title, const char *description)
{
	uint16_t title_len;
	uint16_t description_len;
	uint8_t  ack;
	uint16_t data_length;
	uint16_t index = 0;

	struct adp_msg_format msg_format;

	/* Add null-termination to length */
	title_len       = strlen(title) + 1;
	description_len = strlen(description) + 1;
	/* Make sure the strings are not too long */
	ASSERT(title_len + description_len <= ADP_MAX_PACKET_DATA_SIZE);

	uint8_t rx_buf[ADP_MAX_PACKET_LENGTH] = {
	    0,
	};

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_INFO;
	msg_format.data_length     = title_len + description_len;
	index                      = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)title, title_len);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)description, description_len);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);

	return (ack == ADP_ACK_OK);
}

/**
 * \brief Send MSG_CONF_STREAM and wait for response
 *
 * \param[in] config Configuration struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_configure_stream(struct adp_msg_configure_stream *const config, const char *label)
{
	uint8_t  ack;
	uint16_t data_length;
	uint16_t label_len;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	/* Add null-termination to length */
	label_len = strlen(label) + 1;
	/* Make sure the strings are not too long */
	ASSERT(label_len <= ADP_MAX_PACKET_DATA_SIZE);

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_STREAM;
	msg_format.data_length     = MSG_CONF_STREAM_LEN + label_len;
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->stream_id, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->type, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->mode, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->state, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)label, label_len);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Send MSG_CONF_TOGGLE_STREAM and wait for response
 *
 * \param[in] stream_id ID of stream to toggle
 * \param[in] state     New state for given stream
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_toggle_stream(struct adp_msg_toggle_stream *const config)
{
	uint8_t  ack;
	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_TOGGLE_STREAM;
	msg_format.data_length     = MSG_CONF_TOGGLE_STREAM_LEN;
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->stream_id, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->state, 1);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Send MSG_CONF_GRAPH and wait for response
 *
 * \param[in] config Graph configuration struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_configure_graph(struct adp_msg_configure_graph *const config, const char *graph_label, const char *x_label)
{
	uint8_t  ack;
	uint16_t graph_label_len, x_label_len;

	/* Add 0-termination to label string length */
	graph_label_len = strlen(graph_label) + 1;
	x_label_len     = strlen(x_label) + 1;

	/* Make sure label isn't too big */
	ASSERT(MSG_CONF_GRAPH_LEN + graph_label_len + x_label_len <= ADP_MAX_PACKET_DATA_SIZE);

	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_GRAPH;
	msg_format.data_length     = MSG_CONF_GRAPH_LEN + graph_label_len + x_label_len;
	index                      = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->graph_id, 2);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)graph_label, graph_label_len);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->x_min, 4);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->x_max, 4);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)x_label, x_label_len);
	index = adp_add_send_byte(
	    (uint8_t *)&msg_format.data, index, (uint8_t *)&config->x_scale_numerator, MSG_CONF_GRAPH_LEN - 10);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Send MSG_CONF_TERMINAL and wait for response
 *
 * \param[in] config Terminal configuration struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_configure_terminal(struct adp_msg_conf_terminal *const config, const char *label)
{
	uint8_t  ack;
	uint16_t label_len;

	/* Add 0-termination to label string length */
	label_len = strlen(label) + 1;

	/* Make sure label isn't too big */
	ASSERT(MSG_CONF_TERMINAL_LEN + label_len <= ADP_MAX_PACKET_DATA_SIZE);

	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_TERMINAL;
	msg_format.data_length     = MSG_CONF_TERMINAL_LEN + label_len;
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->terminal_id, 2);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)label, label_len);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->width, MSG_CONF_TERMINAL_LEN - 1);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Send MSG_CONF_ADD_TO_TERMINAL and wait for response
 *
 * \param[in] config Stream-to-terminal configuration struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_stream_to_terminal(struct adp_msg_add_stream_to_terminal *const config, const char *tag_text)
{
	uint8_t  ack;
	uint16_t data_length;
	uint16_t tag_text_len;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	/* Add 0-termination to label string length */
	tag_text_len = strlen(tag_text) + 1;

	/* Make sure label isn't too big */
	ASSERT(MSG_CONF_ADD_TO_TERMINAL_LEN + tag_text_len <= ADP_MAX_PACKET_DATA_SIZE);

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_ADD_TO_TERMINAL;
	msg_format.data_length     = MSG_CONF_ADD_TO_TERMINAL_LEN + tag_text_len;
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->terminal_id, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->stream_id, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->mode, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->text_color, 3);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)tag_text, tag_text_len);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->tag_text_color, 3);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Send MSG_CONF_AXIS and wait for response
 *
 * \param[in] config Axis configuration struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_axis_to_graph(struct adp_msg_conf_axis *const config, const char *label)
{
	uint8_t  ack;
	uint16_t data_length;
	uint16_t index = 0;
	uint16_t label_len;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	/* Add 0-termination to label string length */
	label_len = strlen(label) + 1;

	/* Make sure label isn't too big */
	ASSERT(MSG_CONF_AXIS_LEN + label_len <= ADP_MAX_PACKET_DATA_SIZE);

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_AXIS;
	msg_format.data_length     = MSG_CONF_AXIS_LEN + label_len;
	index                      = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->axis_id, 2);
	index                      = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->graph_id, 2);
	index                      = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)label, label_len);
	index                      = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->y_min, 4);
	index                      = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->y_max, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->x_scale_numerator, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->x_scale_denominator, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->mode, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->color, 3);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Send MSG_CONF_ADD_STREAM_TO_AXIS and wait for response
 *
 * \param[in] config Axis configuration struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_stream_to_axis(struct adp_msg_add_stream_to_axis *const config)
{
	uint8_t  ack;
	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_ADD_STREAM_TO_AXIS;
	msg_format.data_length     = MSG_CONF_ADD_STREAM_TO_AXIS_LEN;
	index                      = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->graph_id, 2);
	index                      = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->axis_id, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->stream_id, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->sample_rate_numerator, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->sample_rate_denominator, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->y_scale_numerator, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->y_scale_denominator, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->y_offset, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->transparency, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->mode, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->line_thickness, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->line_color, 3);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Send MSG_CONF_CURSOR_TO_GRAPH and wait for response
 *
 * \param[in] config Cursor configuration struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_cursor_to_graph(struct adp_msg_add_cursor_to_graph *const config, const char *label)
{
	uint8_t  ack;
	uint16_t data_length;
	uint16_t index = 0;
	uint16_t label_len;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	/* Add 0-termination to label string length */
	label_len = strlen(label) + 1;

	/* Make sure label isn't too big */
	ASSERT(MSG_CONF_CURSOR_TO_GRAPH_LEN + label_len <= ADP_MAX_PACKET_DATA_SIZE);

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_CURSOR_TO_GRAPH;
	msg_format.data_length     = MSG_CONF_CURSOR_TO_GRAPH_LEN + label_len;
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->stream_id, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->graph_id, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->axis_id, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)label, label_len);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->thickness, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->color, 3);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->initial_value, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->minimum_value, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->maximum_value, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->scale_numerator, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->scale_denominator, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->scale_offset, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->line_style, 1);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Send MSG_CONF_CURSOR_TO_GRAPH and wait for response
 *
 * \param[in] config         Cursor configuration struct
 * \param[in] tag_high_state Tag text to display when GPIO pin is high.
 * \param[in] tag_low_state  Tag text to display when GPIO pin is low.
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_gpio_to_graph(struct adp_msg_conf_gpio_to_graph *const config, const char *tag_high_state,
                           const char *tag_low_state)
{
	uint8_t  ack;
	uint16_t data_length;
	uint16_t index = 0;
	uint16_t label_len, tag_high_state_len, tag_low_state_len;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	/* Add 0-termination to label string length */
	tag_high_state_len = strlen(tag_high_state) + 1;
	tag_low_state_len  = strlen(tag_low_state) + 1;
	label_len          = tag_high_state_len + tag_low_state_len;

	/* Make sure label isn't too big */
	ASSERT(MSG_CONF_GPIO_TO_GRAPH_LEN + label_len <= ADP_MAX_PACKET_DATA_SIZE);

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_GPIO_TO_GRAPH;
	msg_format.data_length     = MSG_CONF_GPIO_TO_GRAPH_LEN + label_len;
	index                      = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->graph_id, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->gpio_number, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->graph_id, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)tag_high_state, tag_high_state_len);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)tag_low_state, tag_low_state_len);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->transparency, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->mode, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->line_thickness, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->line_color_high_state, 3);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->line_color_low_state, 3);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->line_style, 1);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Add a dashboard MSG_CONF_DASHBOARD
 *
 * \param[in] config    Pointer to dashboard configuration data struct
 * \param[in] label     Dashboard label (0-terminated string)
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_dashboard(struct adp_msg_conf_dashboard *const config, const char *label)
{
	uint8_t  ack;
	uint16_t label_len;

	/* Add 0-termination to label string length */
	label_len = strlen(label) + 1;

	/* Make sure label isn't too big */
	ASSERT(MSG_CONF_DASHBOARD_LEN + label_len <= ADP_MAX_PACKET_DATA_SIZE);

	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_DASHBOARD;
	msg_format.data_length     = MSG_CONF_DASHBOARD_LEN + label_len;
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->dashboard_id, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)label, label_len);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->color, 3);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->height, 2);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

static uint16_t adp_add_dashboard_element_common_send_byte(uint8_t *add_buf, uint16_t index,
                                                           struct adp_msg_conf_dashboard_element_common *const config)
{
	index = adp_add_send_byte(add_buf, index, (uint8_t *)&config->dashboard_id, 2);
	index = adp_add_send_byte(add_buf, index, (uint8_t *)&config->element_id, 2);
	index = adp_add_send_byte(add_buf, index, (uint8_t *)&config->z_index, 1);
	index = adp_add_send_byte(add_buf, index, (uint8_t *)&config->x, 2);
	index = adp_add_send_byte(add_buf, index, (uint8_t *)&config->y, 2);
	index = adp_add_send_byte(add_buf, index, (uint8_t *)&config->width, 2);
	index = adp_add_send_byte(add_buf, index, (uint8_t *)&config->height, 2);
	index = adp_add_send_byte(add_buf, index, (uint8_t *)&config->element_type, 1);

	return index;
}

/**
 * \brief Add a label to dashboard
 *
 * \param[in] config    Pointer to dashboard label struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_label_to_dashboard(struct adp_msg_conf_dashboard_element_label *const config, const char *label)
{
	uint8_t  ack;
	uint16_t label_len;

	/* Add 0-termination to label string length */
	label_len = strlen(label) + 1;

	/* Make sure label isn't too big */
	ASSERT(ADP_ELEMENT_TYPE_LABEL_LEN + label_len <= ADP_MAX_PACKET_DATA_SIZE);

	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_DASHBOARD_ELEMENT;
	msg_format.data_length     = ADP_ELEMENT_TYPE_LABEL_LEN + label_len;
	index                      = adp_add_dashboard_element_common_send_byte(
        (uint8_t *)&msg_format.data, index, (struct adp_msg_conf_dashboard_element_common *)config);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->font_size, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->attribute, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->horisontal_alignment, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->vertical_alignment, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->background_transparency, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->background_color, 3);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->foreground_transparency, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->foreground_color, 3);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)label, label_len);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Add a button to dashboard
 *
 * \param[in] config    Pointer to dashboard button struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_button_to_dashboard(struct adp_msg_conf_dashboard_element_button *const config, const char *label)
{
	uint8_t  ack;
	uint16_t label_len;

	/* Add 0-termination to label string length */
	label_len = strlen(label) + 1;

	/* Make sure label isn't too big */
	ASSERT(ADP_ELEMENT_TYPE_BUTTON_LEN + label_len <= ADP_MAX_PACKET_DATA_SIZE);

	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_DASHBOARD_ELEMENT;
	msg_format.data_length     = ADP_ELEMENT_TYPE_BUTTON_LEN + label_len;
	index                      = adp_add_dashboard_element_common_send_byte(
        (uint8_t *)&msg_format.data, index, (struct adp_msg_conf_dashboard_element_common *)config);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->font_size, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)label, label_len);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Add a slider to dashboard
 *
 * \param[in] config    Pointer to dashboard slider struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_slider_to_dashboard(struct adp_msg_conf_dashboard_element_slider *const config)
{
	uint8_t  ack;
	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_DASHBOARD_ELEMENT;
	msg_format.data_length     = ADP_ELEMENT_TYPE_SLIDER_LEN;
	index                      = adp_add_dashboard_element_common_send_byte(
        (uint8_t *)&msg_format.data, index, (struct adp_msg_conf_dashboard_element_common *)config);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->minimum_value, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->maximum_value, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->initial_value, 4);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Add a progress bar to dashboard
 *
 * \param[in] config    Pointer to dashboard progress bar struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_progress_to_dashboard(struct adp_msg_conf_dashboard_element_progress *const config)
{
	uint8_t  ack;
	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_DASHBOARD_ELEMENT;
	msg_format.data_length     = ADP_ELEMENT_TYPE_PROGRESS_LEN;
	index                      = adp_add_dashboard_element_common_send_byte(
        (uint8_t *)&msg_format.data, index, (struct adp_msg_conf_dashboard_element_common *)config);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->minimum_value, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->maximum_value, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->initial_value, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->color, 3);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Add a signal to dashboard
 *
 * \param[in] config    Pointer to dashboard signal struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_signal_to_dashboard(struct adp_msg_conf_dashboard_element_signal *const config)
{
	uint8_t  ack;
	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_DASHBOARD_ELEMENT;
	msg_format.data_length     = ADP_ELEMENT_TYPE_SIGNAL_LEN;
	index                      = adp_add_dashboard_element_common_send_byte(
        (uint8_t *)&msg_format.data, index, (struct adp_msg_conf_dashboard_element_common *)config);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->on_transparency, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->on_color, 3);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->off_transparency, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->off_color, 3);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Add a segment display to dashboard
 *
 * \param[in] config    Pointer to dashboard segment struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_segment_to_dashboard(struct adp_msg_conf_dashboard_element_segment *const config)
{
	uint8_t  ack;
	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_DASHBOARD_ELEMENT;
	msg_format.data_length     = ADP_ELEMENT_TYPE_SEGMENT_LEN;
	index                      = adp_add_dashboard_element_common_send_byte(
        (uint8_t *)&msg_format.data, index, (struct adp_msg_conf_dashboard_element_common *)config);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->segment_count, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->base, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->transparency, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->color, 3);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Add a graph to dashboard
 *
 * \param[in] config    Pointer to dashboard graph struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_graph_to_dashboard(struct adp_msg_conf_dashboard_element_graph *const config, const char *title)
{
	uint8_t  ack;
	uint16_t title_len;

	/* Add 0-termination to label string length */
	title_len = strlen(title) + 1;

	/* Make sure label isn't too big */
	ASSERT(ADP_ELEMENT_TYPE_GRAPH_LEN + title_len <= ADP_MAX_PACKET_DATA_SIZE);

	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_DASHBOARD_ELEMENT;
	msg_format.data_length     = ADP_ELEMENT_TYPE_GRAPH_LEN + title_len;
	index                      = adp_add_dashboard_element_common_send_byte(
        (uint8_t *)&msg_format.data, index, (struct adp_msg_conf_dashboard_element_common *)config);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->title_color, 3);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->background_color, 3);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->graph_background_color, 3);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)title, title_len);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->plot_count, 1);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->x_min, 4);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->x_max, 4);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->y_min, 4);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->y_max, 4);
	index = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->mode, 1);

	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Add data fields to dashboard
 *
 * \param[in] config    Pointer to dashboard text struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_text_to_dashboard(struct adp_msg_conf_dashboard_element_text *const config)
{
	uint8_t  ack;
	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_DASHBOARD_ELEMENT;
	msg_format.data_length     = ADP_ELEMENT_TYPE_TEXT_LEN;
	index                      = adp_add_dashboard_element_common_send_byte(
        (uint8_t *)&msg_format.data, index, (struct adp_msg_conf_dashboard_element_common *)config);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->minimum, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->maximum, 4);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->value, 4);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Add a radio to dashboard
 *
 * \param[in] config    Pointer to dashboard radio struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_radio_to_dashboard(struct adp_msg_conf_dashboard_element_radio *const config, const char *text)
{
	uint8_t  ack;
	uint16_t text_len;

	/* Add 0-termination to label string length */
	text_len = strlen(text) + 1;

	/* Make sure label isn't too big */
	ASSERT(ADP_ELEMENT_TYPE_RADIO_LEN + text_len <= ADP_MAX_PACKET_DATA_SIZE);

	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_DASHBOARD_ELEMENT;
	msg_format.data_length     = ADP_ELEMENT_TYPE_RADIO_LEN + text_len;
	index                      = adp_add_dashboard_element_common_send_byte(
        (uint8_t *)&msg_format.data, index, (struct adp_msg_conf_dashboard_element_common *)config);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->font_size, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->number_items, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->orientation, 1);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)text, text_len);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Add a pie to dashboard
 *
 * \param[in] config    Pointer to dashboard pie struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_pie_to_dashboard(struct adp_msg_conf_dashboard_element_pie *const config, const char *title)
{
	uint8_t  ack;
	uint16_t title_len;

	/* Add 0-termination to label string length */
	title_len = strlen(title) + 1;

	/* Make sure label isn't too big */
	ASSERT(ADP_ELEMENT_TYPE_PIE_LEN + title_len <= ADP_MAX_PACKET_DATA_SIZE);

	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_DASHBOARD_ELEMENT;
	msg_format.data_length     = ADP_ELEMENT_TYPE_PIE_LEN + title_len;
	index                      = adp_add_dashboard_element_common_send_byte(
        (uint8_t *)&msg_format.data, index, (struct adp_msg_conf_dashboard_element_common *)config);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->background_color, 3);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->title_color, 3);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)title, title_len);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->number_slices, 1);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

/**
 * \brief Connect a stream to dashboard element
 *
 * \param[in] config    Pointer to stream element configuration struct
 *
 * \return Status from PC
 * \retval true   Message received and accepted
 * \retval false  Message received but not accepted
 */
bool adp_add_stream_to_element(struct adp_conf_add_stream_to_element *const config)
{
	uint8_t  ack;
	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };

	struct adp_msg_format msg_format;

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_CONF_ADD_STREAM_TO_ELEMENT;
	msg_format.data_length     = MSG_CONF_ADD_STREAM_TO_ELEMENT_LEN;
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->dashboard_id, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->element_id, 2);
	index       = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&config->stream_id, 2);
	data_length = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	/* Wait for response and return status */
	adp_wait_for_response(MSG_CONF_ACK, &ack, 1);
	return (ack == ADP_ACK_OK);
}

bool adp_transceive_stream(struct adp_msg_data_stream *const stream_data, uint8_t *protocol_buf)
{
	uint8_t  stream_num;
	uint16_t data_length;
	uint16_t index                         = 0;
	uint8_t  rx_buf[ADP_MAX_PACKET_LENGTH] = {
        0,
    };
	uint16_t actual_bytes = 0;

	struct adp_msg_format msg_format;

	index        = adp_add_send_byte((uint8_t *)&msg_format.data, index, (uint8_t *)&stream_data->number_of_streams, 1);
	actual_bytes = 1; /* number of stream */

	/* find packet size */
	for (stream_num = 0; stream_num < stream_data->number_of_streams; stream_num++) {
		index = adp_add_send_byte(
		    (uint8_t *)&msg_format.data, index, (uint8_t *)&stream_data->stream[stream_num].stream_id, 2);
		index = adp_add_send_byte(
		    (uint8_t *)&msg_format.data, index, (uint8_t *)&stream_data->stream[stream_num].data_size, 1);
		index = adp_add_send_byte((uint8_t *)&msg_format.data,
		                          index,
		                          stream_data->stream[stream_num].data,
		                          stream_data->stream[stream_num].data_size);
		/* 2 bytes stream.stream_id and 1 byte stream.data_size */
		actual_bytes += 2 + 1 + stream_data->stream[stream_num].data_size;
	}

	msg_format.protocol_token  = ADP_TOKEN;
	msg_format.protocol_msg_id = MSG_DATA_STREAM;
	msg_format.data_length     = actual_bytes;
	data_length                = ADP_LENGTH_PACKET_HEADER + index;

	/* Send the protocol packet data */
	adp_interface_transceive_procotol((uint8_t *)&msg_format, data_length, rx_buf);

	return adp_protocol_add_byte(MSG_RES_DATA, rx_buf, data_length, protocol_buf);
}

/**
 * \brief Send and read data on one single stream
 *
 * \param[in] stream_id ID of stream
 * \param[in] data      Pointer to data to send
 * \param[in] data_size Size of data to send
 *
 * \return None
 */
bool adp_transceive_single_stream(uint16_t stream_id, uint8_t *data, uint8_t data_size, uint8_t *protocol_buf)
{
	struct adp_msg_data_stream data_stream;
	volatile uint8_t           status;

	data_stream.number_of_streams   = 1;
	data_stream.stream[0].stream_id = stream_id;
	data_stream.stream[0].data_size = data_size;
	data_stream.stream[0].data      = data;
	status                          = adp_transceive_stream(&data_stream, protocol_buf);

	return status;
}
