/*
   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/>.
 */
#pragma once

#include "AP_Proximity.h"

#if HAL_PROXIMITY_ENABLED
#include <AP_Common/AP_Common.h>
#include <AP_HAL/Semaphores.h>

class AP_Proximity_Backend
{
public:
    // constructor. This incorporates initialisation as well.
	AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_Proximity_Params &_params);

    // we declare a virtual destructor so that Proximity drivers can
    // override with a custom destructor if need be
    virtual ~AP_Proximity_Backend(void) {}

    // update the state structure
    virtual void update() = 0;

    // get maximum and minimum distances (in meters) of sensor
    virtual float distance_max() const = 0;
    virtual float distance_min() const = 0;

    // get distance upwards in meters. returns true on success
    virtual bool get_upward_distance(float &distance) const { return false; }

    // handle mavlink messages
    virtual void handle_msg(const mavlink_message_t &msg) {}

#if AP_SCRIPTING_ENABLED
    // handle scripting obstacle messages
    virtual bool set_distance_min_max(float min, float max) { return false; }
    // this is in body frame
    virtual bool handle_script_distance_msg(float dist_m, float yaw_deg, float pitch_deg, bool push_to_boundary) { return false; }
    virtual bool handle_script_3d_msg(const Vector3f &vec_to_obstacle, bool push_to_boundary) { return false; }
    virtual bool update_virtual_boundary() { return false; }
#endif

    // return the type of sensor
    AP_Proximity::Type type() const { return (AP_Proximity::Type)params.type.get(); }

protected:

    // set status and update valid_count
    void set_status(AP_Proximity::Status status);

    // correct an angle (in degrees) based on the orientation and yaw correction parameters
    float correct_angle_for_orientation(float angle_degrees) const;

    // check if a reading should be ignored because it falls into an ignore area (check_for_ign_area should be sent as false if this check is not needed)
    // pitch is the vertical body-frame angle (in degrees) to the obstacle (0=directly ahead, 90 is above the vehicle)
    // yaw is the horizontal body-frame angle (in degrees) to the obstacle (0=directly ahead of the vehicle, 90 is to the right of the vehicle)
    // Also checks if obstacle is near land or out of range
    // angles should be in degrees and in the range of 0 to 360, distance should be in meteres
    bool ignore_reading(float pitch, float yaw, float distance_m, bool check_for_ign_area = true) const;
    bool ignore_reading(float yaw, float distance_m, bool check_for_ign_area = true) const { return ignore_reading(0.0f, yaw, distance_m, check_for_ign_area); }

    // database helpers. All angles are in degrees
    static bool database_prepare_for_push(Vector3f &current_pos, Matrix3f &body_to_ned);
    // Note: "angle" refers to yaw (in body frame) towards the obstacle
    static void database_push(float angle, float pitch, float distance);
    static void database_push(float angle, float distance) {
        database_push(angle, 0.0f, distance);
    }

    static void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned) {
        database_push(angle, 0.0f, distance, timestamp_ms, current_pos, body_to_ned);
    };
    static void database_push(float angle, float pitch, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned);

    // semaphore for access to shared frontend data
    HAL_Semaphore _sem;

    AP_Proximity::Type _backend_type;

    AP_Proximity &frontend;
    AP_Proximity::Proximity_State &state;   // reference to this instances state
    AP_Proximity_Params &params;            // parameters for this backend
};

#endif // HAL_PROXIMITY_ENABLED