#ifndef AP_AHRS_H
#define AP_AHRS_H
/*
 *  AHRS (Attitude Heading Reference System) interface for ArduPilot
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 */

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

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

class AP_AHRS
{
public:
    // Constructor
    AP_AHRS(IMU *imu, GPS *&gps) :
        _imu(imu),
        _gps(gps),
        _barometer(NULL)
    {
        // base the ki values by the sensors maximum drift
        // rate. The APM2 has gyros which are much less drift
        // prone than the APM1, so we should have a lower ki,
        // which will make us less prone to increasing omegaI
        // incorrectly due to sensor noise
        _gyro_drift_limit = imu->get_gyro_drift_rate();
    }

    // empty init
    virtual void init() {
    };

    // Accessors
    void            set_fly_forward(bool b) {
        _fly_forward = b;
    }
    void            set_compass(Compass *compass) {
        _compass = compass;
    }
    void            set_barometer(AP_Baro *barometer) {
        _barometer = barometer;
    }
    void            set_airspeed(AP_Airspeed *airspeed) {
        _airspeed = airspeed;
    }

    IMU*            get_imu() { 
	    return _imu; 
    }

    // Methods
    virtual void update(void) = 0;

    // 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;

    float get_pitch_rate_earth(void) {
	    Vector3f omega = get_gyro();
	    return cos(roll) * omega.y - sin(roll) * omega.z;
    }
    float get_roll_rate_earth(void)  {
	    Vector3f omega = get_gyro();
	    return omega.x + tan(pitch)*(omega.y*sin(roll) + omega.z*cos(roll));
    }

    // return a smoothed and corrected gyro vector
    virtual Vector3f get_gyro(void) = 0;

    // return the current estimate of the gyro drift
    virtual Vector3f get_gyro_drift(void) = 0;

    // reset the current attitude, used on new IMU calibration
    virtual void reset(bool recover_eulers=false) = 0;

    // how often our attitude representation has gone out of range
    uint8_t renorm_range_count;

    // how often our attitude representation has blown up completely
    uint8_t renorm_blowup_count;

    // return the average size of the roll/pitch error estimate
    // since last call
    virtual float get_error_rp(void) = 0;

    // return the average size of the yaw error estimate
    // since last call
    virtual float get_error_yaw(void) = 0;

    // return a DCM rotation matrix representing our current
    // attitude
    virtual Matrix3f get_dcm_matrix(void) = 0;

    // get our current position, either from GPS or via
    // dead-reckoning. Return true if a position is available,
    // otherwise false. This only updates the lat and lng fields
    // of the Location
    bool get_position(struct Location *loc) {
        if (!_gps || _gps->status() != GPS::GPS_OK) {
            return false;
        }
        loc->lat = _gps->latitude;
        loc->lng = _gps->longitude;
        return true;
    }

    // return a wind estimation vector, in m/s
    Vector3f wind_estimate(void) {
        return Vector3f(0,0,0);
    }

    // return true if yaw has been initialised
    bool yaw_initialised(void) {
        return _have_initial_yaw;
    }

    // set the fast gains flag
    void set_fast_gains(bool setting) {
        _fast_ground_gains = setting;
    }

    // settable parameters
    AP_Float _kp_yaw;
    AP_Float _kp;
    AP_Float gps_gain;
    AP_Int8 _gps_use;

    // for holding parameters
    static const struct AP_Param::GroupInfo var_info[];

protected:
    // whether the yaw value has been intialised with a reference
    bool _have_initial_yaw;

    // pointer to compass object, if available
    Compass         * _compass;

    // pointer to airspeed object, if available
    AP_Airspeed     * _airspeed;

    // time in microseconds of last compass update
    uint32_t _compass_last_update;

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

    // should we raise the gain on the accelerometers for faster
    // convergence, used when disarmed for ArduCopter
    bool _fast_ground_gains;

    // true if we can assume the aircraft will be flying forward
    // on its X axis
    bool _fly_forward;

    // the limit of the gyro drift claimed by the sensors, in
    // radians/s/s
    float _gyro_drift_limit;

    // acceleration due to gravity in m/s/s
    static const float _gravity = 9.80665;
};

#include <AP_AHRS_DCM.h>
#include <AP_AHRS_MPU6000.h>
#include <AP_AHRS_HIL.h>

#endif // AP_AHRS_H