#ifndef QUADCOPTERDATA_H_
#define QUADCOPTERDATA_H_

#include "vrpn_Connection.h"
#include "vrpn_Tracker.h"
#include "structures.h"
#include "CCrazyflie.h"
#include "computations/NetworkForwarding.h"

#include <iostream>
#include <fstream>

// The number of radios to use
#define NUM_RADIOS 1

// The number of quadcopters to initialize
#define NUM_QUADS 1

#define BASE_THRUST 42000
#define Kp	5000
//#define Kp	50
//#define Kp	0
//#define Ki	10*3/7
#define Ki	0
#define Kd	750

extern CONTROLLER_SGSF_GAINS ssGains[STATE_END+1];

typedef struct CONTROLLER_DATA {
	double loopTimeStartPrevious;	// The start time of the last loop to run
	double loopTime;				// The time taken by running the last loop
	double timeBetweenLoop;			// The time between loops
	double vrpnTimePrevious;		// The time of the last VRPN packet
	int vrpnPacketQueueLen;			// The number of VRPN packets in the queue to be processed
	float yawOffset;				// Offset of the yaw in degrees
	float xSetpoint;				// Setpoint for the x axis
	float ySetpoint;				// Setpoint for the y axis
	float zSetpoint;				// Setpoint for the z axis
	float yawSetpoint;				// Setpoint for the yaw angle
	float xPosition;				// X position from last VRPN packet
	float yPosition;				// Y position from last VRPN packet
	float zPosition;				// Z position from last VRPN packet
	float pitchPosition;			// Pitch angle from last VRPN packet
	float rollPosition;				// Roll angle from last VRPN packet
	float yawPosition;				// Yaw angle from last VRPN packet
	uint16_t baseThrust;			// The base thrust to use for the quadcopter
	int frameCount;					// The number of frames received
	int resetController;			// Flag to reset the controllers
	float bridgePos;				// Ratio for the distance to land between the bridge markers
	bool resetSent;
} CONTROLLER_DATA_t;

// This structure contains the distinctive information about each crazyflie
typedef struct QUADCOPTERS {
	// VRPN variables
	const char* vrpn_trackableName;											// The VPRN trackable name
	vrpn_Tracker_Remote *vrpn_tracker;										// The VRPN tracker object

	CCrazyflie *cflieCopter;		// The crazyflie copter object
	uint8_t channelNumber;			// The channel number of the CrazyFlie
	uint8_t radioNumber;			// The radio to use for this CrazyFlie
	XferRate dataRate;				// The data rate of the radio used with this CrazyFlie
	double initTime;				// Time the crazyflie was initialized

	CONTROLLER_DATA_t controllerData;	// Data associated with the controller for the quadcopter

	const char* baselogfileName;	// The base name for the log file
	struct PID_ControllerParams pidParams[9];	// The PID parameters for all 9 internal controllers
} QUADCOPTERS_t;

// This structure contains the information about each radio in use
typedef struct RADIOS {
	CCrazyRadio* radio;			// The radio
	int ARDtime;				// Wait Time between Packet Retries (**MUST BE IN INCREMENTS OF 250**)
	int ARC;					// Number of times Retries Packet Send
	enum Power powerLevel;		// The power level
	enum XferRate dataRate;	// The data rate
} RADIOS_t;

// This structure contains the information for the hand object
typedef struct HAND {
	double loopTimeStartPrevious;	// The start time of the last loop to run
	double loopTime;				// The time taken by running the last loop
	double vrpnTimePrevious;		// The time of the last VRPN packet
	int vrpnPacketQueueLen;			// The number of VRPN packets in the queue to be processed
	float xPosition;				// The X position of the hand
	float yPosition;				// The Y position of the hand
	float zPosition;				// The Z position of the hand
	float yawAngle;					// The yaw angle of the hand
	float pitchAngle;				// The pitch angle of the hand
	float rollAngle;				// The roll angle of the hand
	const char* logfileName;		// The name for the log file
	FILE *logfile;					// The logfile object for the hand
} HAND_t;

// Array to hold the quadcopter data
extern QUADCOPTERS_t crazyflie_info[];

// Array to hold the radio data
extern RADIOS_t radios[];

// Array to hold the hand data
extern HAND_t hand;

extern int networkNumNodes;
extern int networkNumEdges;
extern edge_t networkEdges[];

extern int quad1_numTakeoffSteps;
extern takeoffProfileStep quad1_takeoffSteps[];

// Array to hold the offsets for mirror flight
extern float mirrorOffsets[][2];

// Array to hold the offsets for hand flight
extern float handOffsets[][2];

// Array to hold the default setpoints for each quadcopter
extern float defaultSetpoints[][3];

#endif