// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

/// @file	AP_MotorsHeli.h
/// @brief	Motor control class for Traditional Heli

#ifndef AP_MOTORSHELI
#define AP_MOTORSHELI

#include <inttypes.h>
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel.h>     // RC Channel Library
#include <APM_RC.h>         // ArduPilot Mega RC Library
#include <AP_Motors.h>

// below is required to make "map" function available to this library
#if defined(ARDUINO) && ARDUINO >= 100
 #include "Arduino.h"
#else
 #include "WProgram.h"
#endif

#define AP_MOTORS_HELI_SPEED_DEFAULT 125        // default servo update rate for helicopters
#define AP_MOTORS_HELI_SPEED_DIGITAL_SERVOS 125 // update rate for digital servos
#define AP_MOTORS_HELI_SPEED_ANALOG_SERVOS 125  // update rate for analog servos

#define AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS 3

// tail servo uses channel 7
#define AP_MOTORS_HELI_EXT_GYRO CH_7

// frame definitions
#define AP_MOTORS_HELI_SWASH_CCPM       0
#define AP_MOTORS_HELI_SWASH_H1         1

// ext RSC definitions
#define AP_MOTORS_HELI_EXT_RSC CH_8
#define AP_MOTORSHELI_RSC_MODE_CH8_PASSTHROUGH  1
#define AP_MOTORSHELI_RSC_MODE_EXT_GOV                  2

// head definitions
#define FLYBARLESS_HEAD 0
#define FLYBAR_HEAD 1


class AP_HeliControls;

/// @class      AP_MotorsHeli
class AP_MotorsHeli : public AP_Motors {
public:

    /// Constructor
    AP_MotorsHeli( uint8_t          APM_version,
                   APM_RC_Class*    rc_out,
                   RC_Channel*      rc_roll,
                   RC_Channel*      rc_pitch,
                   RC_Channel*      rc_throttle,
                   RC_Channel*      rc_yaw,
                   RC_Channel*      rc_8,
                   RC_Channel*      swash_servo_1,
                   RC_Channel*      swash_servo_2,
                   RC_Channel*      swash_servo_3,
                   RC_Channel*      yaw_servo,
                   uint16_t         speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
        AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz),
        _servo_1(swash_servo_1),
        _servo_2(swash_servo_2),
        _servo_3(swash_servo_3),
        _servo_4(yaw_servo),
        _rc_8(rc_8)
    {
        throttle_mid = 0;
        _roll_scaler = 1;
        _pitch_scaler = 1;
        _collective_scalar = 1;
		_stab_throttle_scalar = 1;
        _swash_initialised = false;
		stab_throttle = false;
    };

    RC_Channel      *_servo_1;
    RC_Channel      *_servo_2;
    RC_Channel      *_servo_3;
    RC_Channel  *_servo_4;
    RC_Channel  *_rc_8;
    AP_Int8 swash_type;
    AP_Int16 servo1_pos;
    AP_Int16 servo2_pos;
    AP_Int16 servo3_pos;
    AP_Int16 collective_min;
    AP_Int16 collective_max;
    AP_Int16 collective_mid;
    AP_Int16 ext_gyro_enabled;
    AP_Int16 ext_gyro_gain;
    AP_Int16 roll_max;
    AP_Int16 pitch_max;
    AP_Int16 phase_angle;
    AP_Int16 collective_yaw_effect;
    AP_Int8 servo_manual;                       // used to trigger swash reset from mission planner
    AP_Int16 ext_gov_setpoint;                  // maximum output to the motor governor
    AP_Int8 rsc_mode;                           // sets the mode for rotor speed controller
    AP_Int16 rsc_ramp_up_rate;                  // sets the time in 100th seconds the RSC takes to ramp up to speed
    AP_Int8 flybar_mode;                        // selects FBL Acro Mode, or Flybarred Acro Mode
    int16_t throttle_mid;                       // throttle mid point in pwm form (i.e. 0 ~ 1000)
	AP_Int8 stab_col_min;						// collective pitch minimum in Stabilize Mode
	AP_Int8 stab_col_max;						// collective pitch maximum in Stabilize Mode
	bool stab_throttle;							// true if we are in Stabilize Mode for reduced Swash Range


    // init
    void Init();

    // set update rate to motors - a value in hertz
    // you must have setup_motors before calling this
    void set_update_rate( uint16_t speed_hz );

    // enable - starts allowing signals to be sent to motors
    void enable();

    // get basic information about the platform
    uint8_t get_num_motors() {
        return 5;
    };

    // motor test
    void output_test();

    // output_min - sends minimum values out to the motors
    void output_min();

    // init_swash - initialise the swash plate
    void init_swash();

    // output - sends commands to the motors
    void output_armed();

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

protected:

    // heli_move_swash - moves swash plate to attitude of parameters passed in
    void move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_out, int16_t yaw_out);

    // reset_swash - free up swash for maximum movements. Used for set-up
    void reset_swash();

    void output_disarmed();

    void rsc_control();

    float _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
    float _pitchFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
    float _collectiveFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];



    // internally used variables

    float _roll_scaler;                         // scaler to convert roll input from radio (i.e. -4500 ~ 4500) to max roll range
    float _pitch_scaler;                        // scaler to convert pitch input from radio (i.e. -4500 ~ 4500) to max pitch range
    float _collective_scalar;                   // throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500)
	float _stab_throttle_scalar;				// throttle scalar to reduce the range of the collective movement in stabilize mode
    bool _swash_initialised;                    // true if swash has been initialised
    int16_t rsc_output;                         // final output to the external motor governor 1000-2000
    int16_t rsc_ramp;                           // current state of ramping


};

#endif  // AP_MOTORSHELI