#include <sys/types.h>
#include <inttypes.h>
#include <string.h>

#include "output.h"
#include "commands.h"
#include "bitwise.h"

#define NUM_OUTPUTS 4
#define DATA_SIZE (1+NUM_OUTPUTS*4)

/* Creates data and metadata for a overrideoutput packet
 * Returns data size.
 */
ssize_t EncodeSetOutputOverride(
        struct metadata * m,        /* data_len and msg_type will be populated*/
        uint8_t * data,             /* Output buffer */
        size_t data_size,           /* Max buffer size */
        const char * msg)          /* Message to encode */
{
	m->msg_type = OUTPUT_OVERRIDE_ID;
	m->data_len = DATA_SIZE;

	if (data_size < DATA_SIZE) {
		return -1;
	}

	float values[NUM_OUTPUTS];
	values[0] = -1;
	char* tok = strtok(msg, " ");
	char* ena = strtok(NULL, " ");
	data[0] = atoi(ena);
	int i;
	for (i = 0; i < NUM_OUTPUTS; i++) {
		char* value = strtok(NULL, " ");
		if (value == NULL) {
			return -1;
		}
		float a;
		sscanf(value, "%f", &a);
		values[i] = a;
	}
	
	for (i = 0; i < NUM_OUTPUTS; i++) {
		data[1+i*4+0] = FloatByte1(values[i]);
		data[1+i*4+1] = FloatByte2(values[i]);
		data[1+i*4+2] = FloatByte3(values[i]);
		data[1+i*4+3] = FloatByte4(values[i]);
	}
	return DATA_SIZE;
}