/// @file	AP_Motors6DOF.h
/// @brief	Motor control class for ROVs with direct control over 6DOF (or fewer) in movement

#pragma once

#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel/RC_Channel.h>     // RC Channel Library
#include "AP_MotorsMatrix.h"
#define NETRULPWM 1490

/// @class      AP_MotorsMatrix
class AP_Motors6DOF : public AP_MotorsMatrix {
public:

    AP_Motors6DOF(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
        AP_MotorsMatrix(loop_rate, speed_hz) {
        AP_Param::setup_object_defaults(this, var_info);
		_singleton = this;
		for (uint8_t i= 0; i < 6; i++)
		{
			motor_to_can[i]=0;
			motor_to_detect[i] = NETRULPWM;
		}
		pwm_track[0] = 1500;
		pwm_track[1] = 1500;
    };

    // Supported frame types
    typedef enum {
        SUB_FRAME_BLUEROV1,
        SUB_FRAME_VECTORED,
        SUB_FRAME_VECTORED_6DOF,
        SUB_FRAME_VECTORED_6DOF_90DEG,
        SUB_FRAME_SIMPLEROV_3,
        SUB_FRAME_SIMPLEROV_4,
        SUB_FRAME_SIMPLEROV_5,
        SUB_FRAME_CUSTOM
    } sub_frame_t;
	//----------selfdefine start------------------------
		//---------------外部可以调用句柄--------------
	static AP_Motors6DOF *_singleton;
	static AP_Motors6DOF *get_singleton() {
				return _singleton;
			}
	int32_t motor_to_can[6];//发送给CAN的数据
	int32_t motor_to_detect[6];//发送给CAN的数据
	int16_t pwm_track[2];
	float last_thrust_in[AP_MOTORS_MAX_NUM_MOTORS];
	int32_t last_thrust_Dhot[AP_MOTORS_MAX_NUM_MOTORS];
	//----------selfdefine end------------------------

    // Override parent
    void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override;

    // Override parent
    void output_min() override;

    // Map thrust input -1~1 to pwm output 1100~1900
    int16_t calc_thrust_to_pwm(float thrust_in) const;
	
	int32_t calc_thrust_to_motor(float thrust_in,int8_t i);
	 void output_motor8_and_motor9(void);

    // output_to_motors - sends minimum values out to the motors
    void output_to_motors() override;
	void output_to_PWM() override;//dshot赋值

    // This allows us to read back the output of the altidude controllers
    // The controllers are in charge of the throttle input, so this gives vehicle access/visibility to the output of those controllers
    float get_throttle_in_bidirectional() const { return constrain_float(2*(_throttle_in - 0.5f), -1.0f, 1.0f); }
    
    // returns a vector with roll, pitch, and yaw contributions
    Vector3f get_motor_angular_factors(int motor_number);

    // returns true if motor is enabled
    bool motor_is_enabled(int motor_number);

    bool set_reversed(int motor_number, bool reversed);

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

protected:
    // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
    float               get_current_limit_max_throttle() override;

    //Override MotorsMatrix method
    void add_motor_raw_6dof(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, float climb_fac, float forward_fac, float lat_fac, uint8_t testing_order);

    void output_armed_stabilizing() override;
    void output_armed_stabilizing_vectored();
    void output_armed_stabilizing_vectored_6dof();

    // Parameters
    AP_Int8             _motor_reverse[AP_MOTORS_MAX_NUM_MOTORS];
    AP_Float            _forwardVerticalCouplingFactor;

    float               _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to throttle (climb/descent)
    float               _forward_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to forward/backward
    float               _lateral_factor[AP_MOTORS_MAX_NUM_MOTORS];  // each motors contribution to lateral (left/right)

    // current limiting
    float _output_limited = 1.0f;
    float _batt_current_last = 0.0f;
};
namespace AP {
    AP_Motors6DOF &motors6dof();
};