#ifndef AP_NAVIGATION_h
#define AP_NAVIGATION_h

#define XTRACK_GAIN 10                                  // Amount to compensate for crosstrack (degrees/100 per meter)
#define XTRACK_ENTRY_ANGLE 3000                 // Max angle used to correct for track following	degrees*100
#include <GPS.h>                                                // ArduPilot GPS Library
#include "Waypoints.h"                                  // ArduPilot Waypoints Library
#if defined(ARDUINO) && ARDUINO >= 100
 #include "Arduino.h"
#else
 #include "WProgram.h"
#endif

#define T7 10000000

class Navigation {
public:
    Navigation(GPS *withGPS, Waypoints *withWP);

    void                    update_gps(void);                                   // called 50 Hz
    void                    set_home(Waypoints::WP loc);
    void                    set_next_wp(Waypoints::WP loc);
    void                    load_first_wp(void);
    void                    load_wp_with_index(uint8_t i);
    void                    load_home(void);
    void                    return_to_home_with_alt(uint32_t alt);

    void                    reload_wp(void);
    void                    load_wp_index(uint8_t i);
    void                    hold_location();
    void                    set_wp(Waypoints::WP loc);

    void                    set_hold_course(int16_t b);                         // -1 deisables, 0-36000 enables
    int16_t                 get_hold_course(void);

    int32_t                 get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2);
    int32_t                 get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2);
    void                    set_bearing_error(int32_t error);

    void                    set_loiter_vector(int16_t v);
    void                    update_crosstrack(void);

    int32_t                 wrap_180(int32_t error);                    // utility
    int32_t                 wrap_360(int32_t angle);                    // utility

    int32_t                 bearing;                                                    // deg * 100 : 0 to 360 current desired bearing to navigate
    int32_t                 distance;                                                   // meters : distance plane to next waypoint
    int32_t                 altitude_above_home;                        // meters * 100 relative to home position
    int32_t                 total_distance;                                     // meters : distance between waypoints
    int32_t                 bearing_error;                                      // deg * 100 : 18000 to -18000
    int32_t                 altitude_error;                                     // deg * 100 : 18000 to -18000

    int16_t                 loiter_sum;
    Waypoints::WP           home, location, prev_wp, next_wp;

private:
    void                    calc_int32_t_scaling(int32_t lat);
    void                    calc_bearing_error(void);
    void                    calc_altitude_error(void);
    void                    calc_distance_error(void);
    void                    calc_long_scaling(int32_t lat);
    void                    reset_crosstrack(void);

    int16_t                 _old_bearing;                                       // used to track delta on the bearing
    GPS *                   _gps;
    Waypoints *             _wp;
    int32_t                 _crosstrack_bearing;                                // deg * 100 : 0 to 360 desired angle of plane to target
    float                   _crosstrack_error;                                  // deg * 100 : 18000 to -18000  meters we are off trackline
    int16_t                 _hold_course;                                       // deg * 100 dir of plane
    int32_t                 _target_altitude;                   // used for
    int32_t                 _offset_altitude;                   // used for
    float                   _altitude_estimate;
    float                   _scaleLongUp;                       // used to reverse int32_ttitude scaling
    float                   _scaleLongDown;                     // used to reverse int32_ttitude scaling
    int16_t                 _loiter_delta;
};


#endif // AP_NAVIGATION_h