#ifndef AP_DCM_h
#define AP_DCM_h

// teporarily include all other classes here
// since this naming is a bit off from the
// convention and the AP_DCM should be the top
// header file
#include "AP_DCM_HIL.h"

#include <FastSerial.h>
#include <AP_Math.h>
#include <inttypes.h>
#include "WProgram.h"
#include <AP_Compass.h>
#include <AP_ADC.h>
#include <AP_GPS.h>
#include <AP_IMU.h>


class AP_DCM
{
public:
	// Constructors
	AP_DCM(IMU *imu, GPS *gps) :
		_imu(imu),
		_gps(gps),
		_compass(0),
		_dcm_matrix(1, 0, 0,
					0, 1, 0,
					0, 0, 1),
		_course_over_ground_x(0),
		_course_over_ground_y(1)
	{}

	AP_DCM(IMU *imu, GPS *gps, Compass *withCompass) :
		_imu(imu),
		_gps(gps),
		_compass(withCompass),
		_dcm_matrix(1, 0, 0,
					0, 1, 0,
					0, 0, 1),
		_course_over_ground_x(0),
		_course_over_ground_y(1)
	{}

	// Accessors
	Vector3f	get_gyro(void) {return _omega_integ_corr; }		// We return the raw gyro vector corrected for bias
	Vector3f	get_accel(void) { return _accel_vector; }
	Matrix3f	get_dcm_matrix(void) {return _dcm_matrix; }
	Matrix3f	get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix;  return temp.transpose();}

	void		set_centripetal(bool b);
	void		set_compass(Compass *compass);

	// Methods
	void 		update_DCM(float _G_Dt);

	float		get_health(void);

	long		roll_sensor;					// Degrees * 100
	long		pitch_sensor;					// Degrees * 100
	long		yaw_sensor;						// Degrees * 100

	float		roll;							// Radians
	float		pitch;							// Radians
	float		yaw;							// Radians

	uint8_t 	gyro_sat_count;
	uint8_t 	renorm_sqrt_count;
	uint8_t 	renorm_blowup_count;

private:
	// Methods
	void 		read_adc_raw(void);
	void 		accel_adjust(void);
	float 		read_adc(int select);
	void 		matrix_update(float _G_Dt);
	void 		normalize(void);
	Vector3f 	renorm(Vector3f const &a, int &problem);
	void 		drift_correction(void);
	void 		euler_angles(void);

	// members
	Compass 	* _compass;
	GPS 				* _gps;
	IMU 				* _imu;

	Matrix3f	_dcm_matrix;

	Vector3f 	_accel_vector;				// Store the acceleration in a vector
	Vector3f 	_gyro_vector;				// Store the gyros turn rate in a vector
	Vector3f	_omega_P;					// Omega Proportional correction
	Vector3f 	_omega_I;					// Omega Integrator correction
	Vector3f 	_omega_integ_corr;			// Partially corrected Gyro_Vector data - used for centrepetal correction
	Vector3f 	_omega;						// Corrected Gyro_Vector data
	Vector3f 	_error_roll_pitch;
	Vector3f 	_error_yaw;
	float 		_errorCourse;
	float 		_course_over_ground_x; 		// Course overground X axis
	float 		_course_over_ground_y; 		// Course overground Y axis
	float		_health;
	bool		_centripetal;
};

#endif