/*
   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_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include <AP_Param/AP_Param.h>
#include <AC_PID/AC_PID.h>
#include <AP_WheelEncoder/AP_WheelEncoder.h>

// winch rate control default gains
#define AP_WINCH_POS_P      1.00f
#define AP_WINCH_RATE_P     1.00f
#define AP_WINCH_RATE_I     0.50f
#define AP_WINCH_RATE_IMAX  1.00f
#define AP_WINCH_RATE_D     0.00f
#define AP_WINCH_RATE_FILT  5.00f
#define AP_WINCH_RATE_DT    0.10f

class AP_Winch_Backend;

class AP_Winch {
    friend class AP_Winch_Backend;
    friend class AP_Winch_Servo;

public:
    AP_Winch();

    // indicate whether this module is enabled
    bool enabled() const;

    // initialise the winch
    void init(const AP_WheelEncoder *wheel_encoder = nullptr);

    // update the winch
    void update();

    // relax the winch so it does not attempt to maintain length or rate
    void relax() { config.state = STATE_RELAXED; }

    // get current line length
    float get_line_length() const { return config.length_curr; }

    // release specified length of cable (in meters) at the specified rate
    // if rate is zero, the RATE_MAX parameter value will be used
    void release_length(float length, float rate = 0.0f);

    // deploy line at specified speed in m/s (+ve deploys line, -ve retracts line, 0 stops)
    void set_desired_rate(float rate);

    // get rate maximum in m/s
    float get_rate_max() const { return MAX(config.rate_max, 0.0f); }

    static const struct AP_Param::GroupInfo        var_info[];

private:

    // parameters
    AP_Int8     _enabled;               // grabber enable/disable

    // winch states
    typedef enum {
        STATE_RELAXED = 0,              // winch is not operating
        STATE_POSITION,                 // moving or maintaining a target length
        STATE_RATE,                     // deploying or retracting at a target rate
    }  winch_state;

    struct Backend_Config {
        AP_Int8     type;               // winch type
        AP_Float    rate_max;           // deploy or retract rate maximum (in m/s).
        AP_Float    pos_p;              // position error P gain
        AC_PID      rate_pid = AC_PID(AP_WINCH_RATE_P, AP_WINCH_RATE_I, AP_WINCH_RATE_D, 0.0f, AP_WINCH_RATE_IMAX, 0.0f, AP_WINCH_RATE_FILT, 0.0f, AP_WINCH_RATE_DT);           // rate control PID
        winch_state state;              // state of winch control (using target position or target rate)
        float       length_curr;        // current length of the line (in meters) that has been deployed
        float       length_desired;     // target desired length (in meters)
        float       rate_desired;       // target deploy rate (in m/s, +ve = deploying, -ve = retracting)
    } config;

    AP_Winch_Backend *backend;
};