#ifndef AP_Quaternion_h
#define AP_Quaternion_h

#include <AP_Math.h>
#include <inttypes.h>
#include <AP_Compass.h>
#include <AP_GPS.h>
#include <AP_IMU.h>

#if defined(ARDUINO) && ARDUINO >= 100
	#include "Arduino.h"
#else
	#include "WProgram.h"
#endif

class AP_Quaternion
{
public:
	// Constructor
	AP_Quaternion(IMU *imu, GPS *&gps, Compass *compass = NULL) :
		_imu(imu),
		_gps(gps),
		_compass(compass)
	{
		// initial quaternion
		SEq_1 = 1;
		SEq_2 = 0;
		SEq_3 = 0;
		SEq_4 = 0;

		// reference direction of flux in earth frame
		b_x = 0;
		b_z = -1;

		// scaled gyro drift limits
		beta = sqrt(3.0f / 4.0f) * gyroMeasError;
		zeta = sqrt(3.0f / 4.0f) * gyroMeasDrift;
	}

	// Accessors
	void		set_centripetal(bool b) {_centripetal = b;}
	bool		get_centripetal(void) {return _centripetal;}
	void		set_compass(Compass *compass);

	// Methods
	void 		update(void);

	// Euler angles (radians)
	float		roll;
	float		pitch;
	float		yaw;

	// integer Euler angles (Degrees * 100)
	int32_t		roll_sensor;
	int32_t		pitch_sensor;
	int32_t		yaw_sensor;


	// compatibility methods with DCM
	void update_DCM(void) { update(); }
	void update_DCM_fast(void) { update(); }
	Vector3f get_gyro(void) {
		// notice the sign reversals here
		return Vector3f(-_gyro_corrected.x, -_gyro_corrected.y, _gyro_corrected.z);
	}
	Vector3f get_integrator(void) {
		// notice the sign reversals here
		return Vector3f(-gyro_bias.x, -gyro_bias.y, gyro_bias.z);
        }
	float get_accel_weight(void) { return 0; }
	float get_renorm_val(void) { return 0; }
	float get_health(void) { return 0; }
	void matrix_reset(void) { }
	uint8_t gyro_sat_count;
	uint8_t renorm_range_count;
	uint8_t renorm_blowup_count;
	float get_error_rp(void);
	float get_error_yaw(void);

private:
	void 		update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
	void 		update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);

	bool		_have_initial_yaw;

	// Methods
	void 		accel_adjust(void);

	// members
	Compass 	* _compass;

	// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
	//       IMU under us without our noticing.
	GPS 		*&_gps;
	IMU 		*_imu;

	// true if we are doing centripetal acceleration correction
	bool		_centripetal;

	// maximum gyroscope measurement error in rad/s (set to 10 degrees/second)
	static const float gyroMeasError = 10.0 * (M_PI/180.0);

	// maximum gyroscope drift rate in radians/s/s (set to 0.02
	// degrees/s/s, which is 1.2 degrees/s/minute)
	static const float gyroMeasDrift = 0.02 * (PI/180.0);

	float beta;
	float zeta;

	// quaternion elements
	float SEq_1, SEq_2, SEq_3, SEq_4;

	float b_x;
	float b_z;

	// estimate gyroscope biases error
	Vector3f gyro_bias;

	// the current corrected gyro vector
	Vector3f _gyro_corrected;

	// estimate of error
	float		_error_rp_sum;
	uint16_t	_error_rp_count;
	float		_error_yaw_sum;
	uint16_t	_error_yaw_count;
};

#endif