#ifndef _COMMANDS_H
#define _COMMANDS_H

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "type_def.h"


// ----------------------
// Helper stuff

#define MAX_TYPE 6
#define MAX_SUBTYPE 100

enum Message{
	BEGIN_CHAR = 0xBE,
	END_CHAR   = 0xED
};

// This should also have double to avoid confusion with float values.
enum DataType
{
	floatType,
	intType,
	stringType
};

// MESSAGE SUBTYPES
struct MessageSubtype{
	char ID;
	char cmdText[100];
	char cmdDataType;
	int (*functionPtr)(unsigned char *command, int dataLen, modular_structs_t *structs);
};

// MESSAGE TYPES
struct MessageType{
	char ID;
	struct MessageSubtype subtypes[MAX_SUBTYPE];
};

int debug(unsigned char *c, int dataLen, modular_structs_t *structs);
int update(unsigned char *c, int dataLen, modular_structs_t *structs);
int logdata(unsigned char *c, int dataLen, modular_structs_t *structs);
int response(unsigned char *packet, int dataLen, modular_structs_t *structs);
int setyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
int setyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
int setyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
int setroll(unsigned char *c, int dataLen, modular_structs_t *structs);
int setrollp(unsigned char *c, int dataLen, modular_structs_t *structs);
int setrolld(unsigned char *c, int dataLen, modular_structs_t *structs);
int setpitch(unsigned char *c, int dataLen, modular_structs_t *structs);
int setpitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
int setpitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
int setthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
int setthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
int setthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
int setthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
int getyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
int getyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
int getyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
int getroll(unsigned char *c, int dataLen, modular_structs_t *structs);
int getrollp(unsigned char *c, int dataLen, modular_structs_t *structs);
int getrolld(unsigned char *c, int dataLen, modular_structs_t *structs);
int getpitch(unsigned char *c, int dataLen, modular_structs_t *structs);
int getpitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
int getpitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
int getthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
int getthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
int getthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
int getthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
int getaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
int getgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
int getpitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
int getrollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
int respaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
int respgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
int resppitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
int resprollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
int respyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
int respyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
int respyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
int resproll(unsigned char *c, int dataLen, modular_structs_t *structs);
int resprollp(unsigned char *c, int dataLen, modular_structs_t *structs);
int resprolld(unsigned char *c, int dataLen, modular_structs_t *structs);
int resppitch(unsigned char *c, int dataLen, modular_structs_t *structs);
int resppitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
int resppitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
int respthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
int respthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
int respthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
int respthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
int respaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
int respgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
int resppitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
int resprollangle(unsigned char *c, int dataLen, modular_structs_t *structs);

float getFloat(unsigned char* str, int pos);
int getInt(unsigned char* str, int pos);

// TODO add in string to be read from the command line when sending a subtype of message
extern struct MessageType MessageTypes[MAX_TYPE];

#endif