#include "update.h"
#include "../../../common/commands.h"
#include "bitwise.h"

#include <sys/types.h>

enum UpdateData {
	ID_1,
	ID_2,
	ID_3,
	ID_4,
	Y_1,
	Y_2,
	Y_3,
	Y_4,
	X_1,
	X_2,
	X_3,
	X_4,
	Z_1,
	Z_2,
	Z_3,
	Z_4,
	ROLL_1,
	ROLL_2,
	ROLL_3,
	ROLL_4,
	PITCH_1,
	PITCH_2,
	PITCH_3,
	PITCH_4,
	YAW_1,
	YAW_2,
	YAW_3,
	YAW_4,
	UPDATE_SIZE
};

/* Creates data and metadata for an update packet
 * Returns data size.
 */
ssize_t EncodeUpdate(
        struct metadata * m,        /* data_len and msg_type will be populated*/
        uint8_t * data,             /* Output buffer */
        size_t data_size,           /* Max buffer size */
        const struct position_update * u)       /* Message to encode */
{
	m->msg_type = UPDATE_ID;
	m->data_len = UPDATE_SIZE;

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

	data[ID_1] = IntByte1(u->id);
	data[ID_2] = IntByte2(u->id);
	data[ID_3] = IntByte3(u->id);
	data[ID_4] = IntByte4(u->id);

	data[Y_1] = FloatByte1(u->y);
	data[Y_2] = FloatByte2(u->y);
	data[Y_3] = FloatByte3(u->y);
	data[Y_4] = FloatByte4(u->y);

	data[X_1] = FloatByte1(u->x);
	data[X_2] = FloatByte2(u->x);
	data[X_3] = FloatByte3(u->x);
	data[X_4] = FloatByte4(u->x);

	data[Z_1] = FloatByte1(u->z);
	data[Z_2] = FloatByte2(u->z);
	data[Z_3] = FloatByte3(u->z);
	data[Z_4] = FloatByte4(u->z);

	data[ROLL_1] = FloatByte1(u->roll);
	data[ROLL_2] = FloatByte2(u->roll);
	data[ROLL_3] = FloatByte3(u->roll);
	data[ROLL_4] = FloatByte4(u->roll);

	data[PITCH_1] = FloatByte1(u->pitch);
	data[PITCH_2] = FloatByte2(u->pitch);
	data[PITCH_3] = FloatByte3(u->pitch);
	data[PITCH_4] = FloatByte4(u->pitch);

	data[YAW_1] = FloatByte1(u->yaw);
	data[YAW_2] = FloatByte2(u->yaw);
	data[YAW_3] = FloatByte3(u->yaw);
	data[YAW_4] = FloatByte4(u->yaw);

	return UPDATE_SIZE;
}

/* Decode a metadata and data to populate an update.
 * Returns 0 on success, -1 on failure.
 */
int DecodeUpdate(
        struct position_update * u,     /* Decoded controller message */
        const struct metadata * m,          /* Metadata to aid in decoding */
        const uint8_t * data)               /* Data to decode */
{
	if (m->data_len < UPDATE_SIZE) {
		return -1;
	}
	if (m->msg_type != UPDATE_ID) {
		return -1;
	}

	u->id = BytesToInt(data[ID_1], data[ID_2], data[ID_3], data[ID_4]);
	u->x = BytesToFloat(data[X_1], data[X_2], data[X_3], data[X_4]);
	u->y = BytesToFloat(data[Y_1], data[Y_2], data[Y_3], data[Y_4]);
	u->z = BytesToFloat(data[Z_1], data[Z_2], data[Z_3], data[Z_4]);

	u->pitch = BytesToFloat(data[PITCH_1], data[PITCH_2], 
			data[PITCH_3], data[PITCH_4]);
	u->roll = BytesToFloat(data[ROLL_1], data[ROLL_2], 
			data[ROLL_3], data[ROLL_4]);
	u->yaw = BytesToFloat(data[YAW_1], data[YAW_2], 
			data[YAW_3], data[YAW_4]);

	return 0;
}