#pragma once

/*
   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/*
 *  AHRS View class - for creating a 2nd view of the vehicle attitude
 *
 */

#include "AP_AHRS.h"
#include <AP_Motors/AP_Motors.h>

// fwd declarations to avoid include errors
class AC_AttitudeControl;
class AC_PosControl;

class AP_AHRS_View
{
public:
    // Constructor
    AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation, float pitch_trim_deg=0);

    // update state
    void update();

    // empty virtual destructor
    virtual ~AP_AHRS_View() {}

    // return a smoothed and corrected gyro vector
    const Vector3f &get_gyro(void) const {
        return gyro;
    }

    // return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
    Vector3f get_gyro_latest(void) const;

    // return a DCM rotation matrix representing our current attitude in this view
    const Matrix3f &get_rotation_body_to_ned(void) const {
        return rot_body_to_ned;
    }

    // return a Quaternion representing our current attitude in this view
    void get_quat_body_to_ned(Quaternion &quat) const {
        quat.from_rotation_matrix(rot_body_to_ned);
    }

    // apply pitch trim
    void set_pitch_trim(float trim_deg);

    // helper trig value accessors
    float cos_roll() const {
        return trig.cos_roll;
    }
    float cos_pitch() const {
        return trig.cos_pitch;
    }
    float cos_yaw() const {
        return trig.cos_yaw;
    }
    float sin_roll() const {
        return trig.sin_roll;
    }
    float sin_pitch() const {
        return trig.sin_pitch;
    }
    float sin_yaw() const {
        return trig.sin_yaw;
    }


    /*
      wrappers around ahrs functions which pass-thru directly. See
      AP_AHRS.h for description of each function
     */
    bool get_location(struct Location &loc) const WARN_IF_UNUSED {
        return ahrs.get_location(loc);
    }

    Vector3f wind_estimate(void) {
        return ahrs.wind_estimate();
    }

    bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED {
        return ahrs.airspeed_estimate(airspeed_ret);
    }

    bool airspeed_estimate_true(float &airspeed_ret) const WARN_IF_UNUSED {
        return ahrs.airspeed_estimate_true(airspeed_ret);
    }

    float get_EAS2TAS(void) const {
        return ahrs.get_EAS2TAS();
    }

    Vector2f groundspeed_vector(void) {
        return ahrs.groundspeed_vector();
    }

    bool get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED {
        return ahrs.get_velocity_NED(vec);
    }

    bool get_relative_position_NED_home(Vector3f &vec) const WARN_IF_UNUSED {
        return ahrs.get_relative_position_NED_home(vec);
    }

    bool get_relative_position_NED_origin(Vector3f &vec) const WARN_IF_UNUSED {
        return ahrs.get_relative_position_NED_origin(vec);
    }

    bool get_relative_position_NE_home(Vector2f &vecNE) const WARN_IF_UNUSED {
        return ahrs.get_relative_position_NE_home(vecNE);
    }

    bool get_relative_position_NE_origin(Vector2f &vecNE) const WARN_IF_UNUSED {
        return ahrs.get_relative_position_NE_origin(vecNE);
    }

    void get_relative_position_D_home(float &posD) const {
        ahrs.get_relative_position_D_home(posD);
    }

    bool get_relative_position_D_origin(float &posD) const WARN_IF_UNUSED {
        return ahrs.get_relative_position_D_origin(posD);
    }

    float groundspeed(void) {
        return ahrs.groundspeed();
    }

    const Vector3f &get_accel_ef(void) const {
        return ahrs.get_accel_ef();
    }

    uint32_t getLastPosNorthEastReset(Vector2f &pos) WARN_IF_UNUSED {
        return ahrs.getLastPosNorthEastReset(pos);
    }

    uint32_t getLastPosDownReset(float &posDelta) WARN_IF_UNUSED {
        return ahrs.getLastPosDownReset(posDelta);
    }

    // rotate a 2D vector from earth frame to body frame
    // in result, x is forward, y is right
    Vector2f earth_to_body2D(const Vector2f &ef_vector) const;

    // rotate a 2D vector from earth frame to body frame
    // in input, x is forward, y is right
    Vector2f body_to_earth2D(const Vector2f &bf) const;

    // return the average size of the roll/pitch error estimate
    // since last call
    float get_error_rp(void) const {
        return ahrs.get_error_rp();
    }

    // return the average size of the yaw error estimate
    // since last call
    float get_error_yaw(void) const {
        return ahrs.get_error_yaw();
    }

    // Logging Functions
    void Write_AttitudeView(const Vector3f &targets) const;    
    void Write_Rate( const AP_Motors &motors, const AC_AttitudeControl &attitude_control,
                        const AC_PosControl &pos_control) const;

    float roll;
    float pitch;
    float yaw;
    int32_t roll_sensor;
    int32_t pitch_sensor;
    int32_t yaw_sensor;


    // get current rotation
    // note that this may not be the rotation were actually using, see _pitch_trim_deg
    enum Rotation get_rotation(void) const {
        return rotation;
    }

    // get pitch trim (deg)
    float get_pitch_trim() const { return _pitch_trim_deg; }

    // Rotate vector from AHRS reference frame to AHRS view refences frame
    void rotate(Vector3f &vec) const;

private:
    const enum Rotation rotation;
    AP_AHRS &ahrs;

    // body frame rotation for this View
    Matrix3f rot_view;
    // transpose of rot_view
    Matrix3f rot_view_T;
    Matrix3f rot_body_to_ned;
    Vector3f gyro;

    struct {
        float cos_roll;
        float cos_pitch;
        float cos_yaw;
        float sin_roll;
        float sin_pitch;
        float sin_yaw;
    } trig;

    float y_angle;
    float _pitch_trim_deg;
};