/*
   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/>.
 */

#include <AP_HAL/AP_HAL.h>
#include "AP_MotorsMatrix.h"
#include <AP_Vehicle/AP_Vehicle.h>

extern const AP_HAL::HAL& hal;

// init
void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
    // record requested frame class and type
    _active_frame_class = frame_class;
    _active_frame_type = frame_type;

    if (frame_class == MOTOR_FRAME_SCRIPTING_MATRIX) {
        // if Scripting frame class, do nothing scripting must call its own dedicated init function
        return;
    }

    // setup the motors
    setup_motors(frame_class, frame_type);

    // enable fast channels or instant pwm
    set_update_rate(_speed_hz);
}

#if AP_SCRIPTING_ENABLED
// dedicated init for lua scripting
bool AP_MotorsMatrix::init(uint8_t expected_num_motors)
{
    if (_active_frame_class != MOTOR_FRAME_SCRIPTING_MATRIX) {
        // not the correct class
        return false;
    }

    // Make sure the correct number of motors have been added
    uint8_t num_motors = 0;
    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i]) {
            num_motors++;
        }
    }

    set_initialised_ok(expected_num_motors == num_motors);

    if (!initialised_ok()) {
        _mav_type = MAV_TYPE_GENERIC;
        return false;
    }

    switch (num_motors) {
        case 3:
            _mav_type = MAV_TYPE_TRICOPTER;
            break;
        case 4:
            _mav_type = MAV_TYPE_QUADROTOR;
            break;
        case 6:
            _mav_type = MAV_TYPE_HEXAROTOR;
            break;
        case 8:
            _mav_type = MAV_TYPE_OCTOROTOR;
            break;
        case 10:
            _mav_type = MAV_TYPE_DECAROTOR;
            break;
        case 12:
            _mav_type = MAV_TYPE_DODECAROTOR;
            break;
        default:
            _mav_type = MAV_TYPE_GENERIC;
    }

    normalise_rpy_factors();

    set_update_rate(_speed_hz);

    return true;
}

// Set throttle factor from scripting
bool AP_MotorsMatrix::set_throttle_factor(int8_t motor_num, float throttle_factor)
{
    if ((_active_frame_class != MOTOR_FRAME_SCRIPTING_MATRIX) ) {
        // not the correct class
        return false;
    }

    if (initialised_ok() || !motor_enabled[motor_num]) {
        // Already setup or given motor is not enabled
        return false;
    }

    _throttle_factor[motor_num] = throttle_factor;
    return true;
}

#endif // AP_SCRIPTING_ENABLED

// set update rate to motors - a value in hertz
void AP_MotorsMatrix::set_update_rate(uint16_t speed_hz)
{
    // record requested speed
    _speed_hz = speed_hz;

    uint16_t mask = 0;
    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i]) {
            mask |= 1U << i;
        }
    }
    rc_set_freq(mask, _speed_hz);
}

// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
void AP_MotorsMatrix::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
{
    // exit immediately if armed or no change
    if (armed() || (frame_class == _active_frame_class && _active_frame_type == frame_type)) {
        return;
    }
    _active_frame_class = frame_class;
    _active_frame_type = frame_type;

    init(frame_class, frame_type);

}

void AP_MotorsMatrix::output_to_motors()
{
    int8_t i;

    switch (_spool_state) {
        case SpoolState::SHUT_DOWN: {
            // no output
            for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
                if (motor_enabled[i]) {
                    _actuator[i] = 0.0f;
                }
            }
            break;
        }
        case SpoolState::GROUND_IDLE:
            // sends output to motors when armed but not flying
            for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
                if (motor_enabled[i]) {
                    set_actuator_with_slew(_actuator[i], actuator_spin_up_to_ground_idle());
                }
            }
            break;
        case SpoolState::SPOOLING_UP:
        case SpoolState::THROTTLE_UNLIMITED:
        case SpoolState::SPOOLING_DOWN:
            // set motor output based on thrust requests
            for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
                if (motor_enabled[i]) {
                    set_actuator_with_slew(_actuator[i], thrust_to_actuator(_thrust_rpyt_out[i]));
                }
            }
            break;
    }

    // convert output to PWM and send to each motor
    for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i]) {
            rc_write(i, output_to_pwm(_actuator[i]));
        }
    }
}

// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
//  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsMatrix::get_motor_mask()
{
    uint16_t motor_mask = 0;
    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i]) {
            motor_mask |= 1U << i;
        }
    }
    uint16_t mask = motor_mask_to_srv_channel_mask(motor_mask);

    // add parent's mask
    mask |= AP_MotorsMulticopter::get_motor_mask();

    return mask;
}

// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrix::output_armed_stabilizing()
{
    uint8_t i;                          // general purpose counter
    float   roll_thrust;                // roll thrust input value, +/- 1.0
    float   pitch_thrust;               // pitch thrust input value, +/- 1.0
    float   yaw_thrust;                 // yaw thrust input value, +/- 1.0
    float   throttle_thrust;            // throttle thrust input value, 0.0 - 1.0
    float   throttle_avg_max;           // throttle thrust average maximum value, 0.0 - 1.0
    float   throttle_thrust_max;        // throttle thrust maximum value, 0.0 - 1.0
    float   throttle_thrust_best_rpy;   // throttle providing maximum roll, pitch and yaw range without climbing
    float   rpy_scale = 1.0f;           // this is used to scale the roll, pitch and yaw to fit within the motor limits
    float   yaw_allowed = 1.0f;         // amount of yaw we can fit in
    float   thr_adj;                    // the difference between the pilot's desired throttle and throttle_thrust_best_rpy

    // apply voltage and air pressure compensation
    const float compensation_gain = get_compensation_gain(); // compensation for battery voltage and altitude
    roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
    pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
    yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
    throttle_thrust = get_throttle() * compensation_gain;
    throttle_avg_max = _throttle_avg_max * compensation_gain;

    // If thrust boost is active then do not limit maximum thrust
    throttle_thrust_max = _thrust_boost_ratio + (1.0f - _thrust_boost_ratio) * _throttle_thrust_max * compensation_gain;

    // sanity check throttle is above zero and below current limited throttle
    if (throttle_thrust <= 0.0f) {
        throttle_thrust = 0.0f;
        limit.throttle_lower = true;
    }
    if (throttle_thrust >= throttle_thrust_max) {
        throttle_thrust = throttle_thrust_max;
        limit.throttle_upper = true;
    }

    // ensure that throttle_avg_max is between the input throttle and the maximum throttle
    throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, throttle_thrust_max);

    // calculate the highest allowed average thrust that will provide maximum control range
    throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);

    // calculate throttle that gives most possible room for yaw which is the lower of:
    //      1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
    //      2. the higher of:
    //            a) the pilot's throttle input
    //            b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
    //      Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
    //      Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
    //      We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
    //      We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low.  We favor reducing throttle instead of better yaw control because the pilot has commanded it

    // Under the motor lost condition we remove the highest motor output from our calculations and let that motor go greater than 1.0
    // To ensure control and maximum righting performance Hex and Octo have some optimal settings that should be used
    // Y6               : MOT_YAW_HEADROOM = 350, ATC_RAT_RLL_IMAX = 1.0,   ATC_RAT_PIT_IMAX = 1.0,   ATC_RAT_YAW_IMAX = 0.5
    // Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.375, ATC_RAT_PIT_IMAX = 0.375, ATC_RAT_YAW_IMAX = 0.375
    // Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.75,  ATC_RAT_PIT_IMAX = 0.75,  ATC_RAT_YAW_IMAX = 0.375
    // Usable minimums below may result in attitude offsets when motors are lost. Hex aircraft are only marginal and must be handles with care
    // Hex              : MOT_YAW_HEADROOM = 0,   ATC_RAT_RLL_IMAX = 1.0,   ATC_RAT_PIT_IMAX = 1.0,   ATC_RAT_YAW_IMAX = 0.5
    // Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.25,  ATC_RAT_PIT_IMAX = 0.25,  ATC_RAT_YAW_IMAX = 0.25
    // Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.5,   ATC_RAT_PIT_IMAX = 0.5,   ATC_RAT_YAW_IMAX = 0.25
    // Quads cannot make use of motor loss handling because it doesn't have enough degrees of freedom.

    // calculate amount of yaw we can fit into the throttle range
    // this is always equal to or less than the requested yaw from the pilot or rate controller
    float rp_low = 1.0f;    // lowest thrust value
    float rp_high = -1.0f;  // highest thrust value
    for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i]) {
            // calculate the thrust outputs for roll and pitch
            _thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
            // record lowest roll + pitch command
            if (_thrust_rpyt_out[i] < rp_low) {
                rp_low = _thrust_rpyt_out[i];
            }
            // record highest roll + pitch command
            if (_thrust_rpyt_out[i] > rp_high && (!_thrust_boost || i != _motor_lost_index)) {
                rp_high = _thrust_rpyt_out[i];
            }

            // Check the maximum yaw control that can be used on this channel
            // Exclude any lost motors if thrust boost is enabled
            if (!is_zero(_yaw_factor[i]) && (!_thrust_boost || i != _motor_lost_index)){
                if (is_positive(yaw_thrust * _yaw_factor[i])) {
                    yaw_allowed = MIN(yaw_allowed, fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]), 0.0f)/_yaw_factor[i]));
                } else {
                    yaw_allowed = MIN(yaw_allowed, fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[i], 0.0f)/_yaw_factor[i]));
                }
            }
        }
    }

    // calculate the maximum yaw control that can be used
    // todo: make _yaw_headroom 0 to 1
    float yaw_allowed_min = (float)_yaw_headroom * 0.001f;

    // increase yaw headroom to 50% if thrust boost enabled
    yaw_allowed_min = _thrust_boost_ratio * 0.5f + (1.0f - _thrust_boost_ratio) * yaw_allowed_min;

    // Let yaw access minimum amount of head room
    yaw_allowed = MAX(yaw_allowed, yaw_allowed_min);

    // Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
    if (_thrust_boost && motor_enabled[_motor_lost_index]) {
        // record highest roll + pitch command
        if (_thrust_rpyt_out[_motor_lost_index] > rp_high) {
            rp_high = _thrust_boost_ratio * rp_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index];
        }

        // Check the maximum yaw control that can be used on this channel
        // Exclude any lost motors if thrust boost is enabled
        if (!is_zero(_yaw_factor[_motor_lost_index])){
            if (is_positive(yaw_thrust * _yaw_factor[_motor_lost_index])) {
                yaw_allowed = _thrust_boost_ratio * yaw_allowed + (1.0f - _thrust_boost_ratio) * MIN(yaw_allowed, fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[_motor_lost_index]), 0.0f)/_yaw_factor[_motor_lost_index]));
            } else {
                yaw_allowed = _thrust_boost_ratio * yaw_allowed + (1.0f - _thrust_boost_ratio) * MIN(yaw_allowed, fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[_motor_lost_index], 0.0f)/_yaw_factor[_motor_lost_index]));
            }
        }
    }

    if (fabsf(yaw_thrust) > yaw_allowed) {
        // not all commanded yaw can be used
        yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed);
        limit.yaw = true;
    }

    // add yaw control to thrust outputs
    float rpy_low = 1.0f;   // lowest thrust value
    float rpy_high = -1.0f; // highest thrust value
    for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i]) {
            _thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i];

            // record lowest roll + pitch + yaw command
            if (_thrust_rpyt_out[i] < rpy_low) {
                rpy_low = _thrust_rpyt_out[i];
            }
            // record highest roll + pitch + yaw command
            // Exclude any lost motors if thrust boost is enabled
            if (_thrust_rpyt_out[i] > rpy_high && (!_thrust_boost || i != _motor_lost_index)) {
                rpy_high = _thrust_rpyt_out[i];
            }
        }
    }
    // Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
    if (_thrust_boost) {
        // record highest roll + pitch + yaw command
        if (_thrust_rpyt_out[_motor_lost_index] > rpy_high && motor_enabled[_motor_lost_index]) {
            rpy_high = _thrust_boost_ratio * rpy_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index];
        }
    }

    // calculate any scaling needed to make the combined thrust outputs fit within the output range
    if (rpy_high - rpy_low > 1.0f) {
        rpy_scale = 1.0f / (rpy_high - rpy_low);
    }
    if (throttle_avg_max + rpy_low < 0) {
        rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low);
    }

    // calculate how close the motors can come to the desired throttle
    rpy_high *= rpy_scale;
    rpy_low *= rpy_scale;
    throttle_thrust_best_rpy = -rpy_low;
    thr_adj = throttle_thrust - throttle_thrust_best_rpy;
    if (rpy_scale < 1.0f) {
        // Full range is being used by roll, pitch, and yaw.
        limit.roll = true;
        limit.pitch = true;
        limit.yaw = true;
        if (thr_adj > 0.0f) {
            limit.throttle_upper = true;
        }
        thr_adj = 0.0f;
    } else {
        if (thr_adj < 0.0f) {
            // Throttle can't be reduced to desired value
            // todo: add lower limit flag and ensure it is handled correctly in altitude controller
            thr_adj = 0.0f;
        } else if (thr_adj > 1.0f - (throttle_thrust_best_rpy + rpy_high)) {
            // Throttle can't be increased to desired value
            thr_adj = 1.0f - (throttle_thrust_best_rpy + rpy_high);
            limit.throttle_upper = true;
        }
    }

    // add scaled roll, pitch, constrained yaw and throttle for each motor
    const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj;
    for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i]) {
            _thrust_rpyt_out[i] = (throttle_thrust_best_plus_adj * _throttle_factor[i]) + (rpy_scale * _thrust_rpyt_out[i]);
        }
    }

    // determine throttle thrust for harmonic notch
    // compensation_gain can never be zero
    _throttle_out = throttle_thrust_best_plus_adj / compensation_gain;

    // check for failed motor
    check_for_failed_motor(throttle_thrust_best_plus_adj);
}

// check for failed motor
//   should be run immediately after output_armed_stabilizing
//   first argument is the sum of:
//      a) throttle_thrust_best_rpy : throttle level (from 0 to 1) providing maximum roll, pitch and yaw range without climbing
//      b) thr_adj: the difference between the pilot's desired throttle and throttle_thrust_best_rpy
//   records filtered motor output values in _thrust_rpyt_out_filt array
//   sets thrust_balanced to true if motors are balanced, false if a motor failure is detected
//   sets _motor_lost_index to index of failed motor
void AP_MotorsMatrix::check_for_failed_motor(float throttle_thrust_best_plus_adj)
{
    // record filtered and scaled thrust output for motor loss monitoring purposes
    float alpha = 1.0f / (1.0f + _loop_rate * 0.5f);
    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i]) {
            _thrust_rpyt_out_filt[i] += alpha * (_thrust_rpyt_out[i] - _thrust_rpyt_out_filt[i]);
        }
    }

    float rpyt_high = 0.0f;
    float rpyt_sum = 0.0f;
    uint8_t number_motors = 0.0f;
    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i]) {
            number_motors += 1;
            rpyt_sum += _thrust_rpyt_out_filt[i];
            // record highest filtered thrust command
            if (_thrust_rpyt_out_filt[i] > rpyt_high) {
                rpyt_high = _thrust_rpyt_out_filt[i];
                // hold motor lost index constant while thrust boost is active
                if (!_thrust_boost) {
                    _motor_lost_index = i;
                }
            }
        }
    }

    float thrust_balance = 1.0f;
    if (rpyt_sum > 0.1f) {
        thrust_balance = rpyt_high * number_motors / rpyt_sum;
    }
    // ensure thrust balance does not activate for multirotors with less than 6 motors
    if (number_motors >= 6 && thrust_balance >= 1.5f && _thrust_balanced) {
        _thrust_balanced = false;
    }
    if (thrust_balance <= 1.25f && !_thrust_balanced) {
        _thrust_balanced = true;
    }

    // check to see if thrust boost is using more throttle than _throttle_thrust_max
    if ((_throttle_thrust_max * get_compensation_gain() > throttle_thrust_best_plus_adj) && (rpyt_high < 0.9f) && _thrust_balanced) {
        _thrust_boost = false;
    }
}

// output_test_seq - spin a motor at the pwm value specified
//  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsMatrix::_output_test_seq(uint8_t motor_seq, int16_t pwm)
{
    // loop through all the possible orders spinning any motors that match that description
    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i] && _test_order[i] == motor_seq) {
            // turn on this motor
            rc_write(i, pwm);
        }
    }
}

// output_test_num - spin a motor connected to the specified output channel
//  (should only be performed during testing)
//  If a motor output channel is remapped, the mapped channel is used.
//  Returns true if motor output is set, false otherwise
//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
bool AP_MotorsMatrix::output_test_num(uint8_t output_channel, int16_t pwm)
{
    if (!armed()) {
        return false;
    }

    // Is channel in supported range?
    if (output_channel > AP_MOTORS_MAX_NUM_MOTORS - 1) {
        return false;
    }

    // Is motor enabled?
    if (!motor_enabled[output_channel]) {
        return false;
    }

    rc_write(output_channel, pwm); // output
    return true;
}

// add_motor
void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order, float throttle_factor)
{
    if (initialised_ok()) {
        // do not allow motors to be set if the current frame type has init correctly
        return;
    }

    // ensure valid motor number is provided
    if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {

        // enable motor
        motor_enabled[motor_num] = true;

        // set roll, pitch, yaw and throttle factors
        _roll_factor[motor_num] = roll_fac;
        _pitch_factor[motor_num] = pitch_fac;
        _yaw_factor[motor_num] = yaw_fac;
        _throttle_factor[motor_num] = throttle_factor;

        // set order that motor appears in test
        _test_order[motor_num] = testing_order;

        // call parent class method
        add_motor_num(motor_num);
    }
}

// add_motor using just position and prop direction - assumes that for each motor, roll and pitch factors are equal
void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order)
{
    add_motor(motor_num, angle_degrees, angle_degrees, yaw_factor, testing_order);
}

// add_motor using position and prop direction. Roll and Pitch factors can differ (for asymmetrical frames)
void AP_MotorsMatrix::add_motor(int8_t motor_num, float roll_factor_in_degrees, float pitch_factor_in_degrees, float yaw_factor, uint8_t testing_order)
{
    add_motor_raw(
        motor_num,
        cosf(radians(roll_factor_in_degrees + 90)),
        cosf(radians(pitch_factor_in_degrees)),
        yaw_factor,
        testing_order);
}

// remove_motor - disabled motor and clears all roll, pitch, throttle factors for this motor
void AP_MotorsMatrix::remove_motor(int8_t motor_num)
{
    // ensure valid motor number is provided
    if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
        // disable the motor, set all factors to zero
        motor_enabled[motor_num] = false;
        _roll_factor[motor_num] = 0.0f;
        _pitch_factor[motor_num] = 0.0f;
        _yaw_factor[motor_num] = 0.0f;
        _throttle_factor[motor_num] = 0.0f;
    }
}

void AP_MotorsMatrix::add_motors(const struct MotorDef *motors, uint8_t num_motors)
{
    for (uint8_t i=0; i<num_motors; i++) {
        const auto &motor = motors[i];
        add_motor(i, motor.angle_degrees, motor.yaw_factor, motor.testing_order);
    }
}
void AP_MotorsMatrix::add_motors_raw(const struct MotorDefRaw *motors, uint8_t num_motors)
{
    for (uint8_t i=0; i<num_motors; i++) {
        const auto &m = motors[i];
        add_motor_raw(i, m.roll_fac, m.pitch_fac, m.yaw_fac, m.testing_order);
    }
}

void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type)
{
    // remove existing motors
    for (int8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        remove_motor(i);
    }
    set_initialised_ok(false);
    bool success = true;

    switch (frame_class) {
#if AP_MOTORS_FRAME_QUAD_ENABLED
        case MOTOR_FRAME_QUAD:
            _frame_class_string = "QUAD";
            _mav_type = MAV_TYPE_QUADROTOR;
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_PLUS: {
                    _frame_type_string = "PLUS";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {  90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
                        { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
                        {   0, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
                        { 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_X: {
                    _frame_type_string = "X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
                        { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
                        {  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
                        {  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
                case MOTOR_FRAME_TYPE_NYT_PLUS: {
                    _frame_type_string = "NYT_PLUS";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {  90, 0,  2 },
                        { -90, 0,  4 },
                        {   0, 0,  1 },
                        { 180, 0,  3 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_NYT_X: {
                    _frame_type_string = "NYT_X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   45, 0,  1 },
                        { -135, 0,  3 },
                        {  -45, 0,  4 },
                        {  135, 0,  2 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
#endif //APM_BUILD_TYPE(APM_BUILD_ArduPlane)
                case MOTOR_FRAME_TYPE_BF_X: {
                    // betaflight quad X order
                    // see: https://fpvfrenzy.com/betaflight-motor-order/
                    _frame_type_string = "BF_X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2 },
                        {   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
                        { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
                        {  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_BF_X_REV: {
                    // betaflight quad X order, reversed motors
                    _frame_type_string = "X_REV";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {  135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
                        {   45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
                        { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
                        {  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_DJI_X: {
                    // DJI quad X order
                    // see https://forum44.djicdn.com/data/attachment/forum/201711/26/172348bppvtt1ot1nrtp5j.jpg
                    _frame_type_string = "DJI_X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
                        {  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
                        { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
                        {  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_CW_X: {
                    // "clockwise X" motor order. Motors are ordered clockwise from front right
                    // matching test order
                    _frame_type_string = "CW_X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
                        {  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
                        { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
                        {  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_V: {
                    _frame_type_string = "V";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   45,   0.7981f,   1 },
                        { -135,   1.0000f,   3 },
                        {  -45,  -0.7981f,   4 },
                        {  135,  -1.0000f,   2 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_H: {
                    // H frame set-up - same as X but motors spin in opposite directiSons
                    _frame_type_string = "H";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
                        { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
                        {  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
                        {  135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_VTAIL: {
                    /*
                        Tested with: Lynxmotion Hunter Vtail 400
                        - inverted rear outward blowing motors (at a 40 degree angle)
                        - should also work with non-inverted rear outward blowing motors
                        - no roll in rear motors
                        - no yaw in front motors
                        - should fly like some mix between a tricopter and X Quadcopter

                        Roll control comes only from the front motors, Yaw control only from the rear motors.
                        Roll & Pitch factor is measured by the angle away from the top of the forward axis to each arm.

                        Note: if we want the front motors to help with yaw,
                            motors 1's yaw factor should be changed to sin(radians(40)).  Where "40" is the vtail angle
                            motors 3's yaw factor should be changed to -sin(radians(40))
                    */
                    _frame_type_string = "VTAIL";
                    add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1);
                    add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
                    add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
                    add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    break;
                }
                case MOTOR_FRAME_TYPE_ATAIL:
                    /*
                        The A-Shaped VTail is the exact same as a V-Shaped VTail, with one difference:
                        - The Yaw factors are reversed, because the rear motors are facing different directions

                        With V-Shaped VTails, the props make a V-Shape when spinning, but with
                        A-Shaped VTails, the props make an A-Shape when spinning.
                        - Rear thrust on a V-Shaped V-Tail Quad is outward
                        - Rear thrust on an A-Shaped V-Tail Quad is inward

                        Still functions the same as the V-Shaped VTail mixing below:
                        - Yaw control is entirely in the rear motors
                        - Roll is is entirely in the front motors
                    */
                    _frame_type_string = "ATAIL";
                    add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1);
                    add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
                    add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
                    break;
                case MOTOR_FRAME_TYPE_PLUSREV:{
                    // plus with reversed motor directions
                    _frame_type_string = "PLUSREV";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        { 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2 },
                        { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4 },
                        { 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
                        { 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_Y4:
                    _frame_type_string = "Y4";
                    // Y4 motor definition with right front CCW, left front CW
                    static const AP_MotorsMatrix::MotorDefRaw motors[] {
                        { -1.0f,  1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
                        {  0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2 },
                        {  0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
                        {  1.0f,  1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4 },
                    };
                    add_motors_raw(motors, ARRAY_SIZE(motors));
                    break;
                default:
                    // quad frame class does not support this frame type
                    _frame_type_string = "UNSUPPORTED";
                    success = false;
                    break;
            }
            break;  // quad
#endif //AP_MOTORS_FRAME_QUAD_ENABLED
#if AP_MOTORS_FRAME_HEXA_ENABLED
        case MOTOR_FRAME_HEXA:
            _frame_class_string = "HEXA";
            _mav_type = MAV_TYPE_HEXAROTOR;
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_PLUS: {
                    _frame_type_string = "PLUS";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {    0, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
                        {  180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
                        { -120, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   5 },
                        {   60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
                        {  -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  6 },
                        {  120, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_X: {
                    _frame_type_string = "X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
                        {  -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
                        {  -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
                        {  150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
                        {   30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
                        { -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_H: {
                    // H is same as X except middle motors are closer to center
                    _frame_type_string = "H";
                    static const AP_MotorsMatrix::MotorDefRaw motors[] {
                        { -1.0f, 0.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
                        { 1.0f, 0.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
                        { 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
                        { -1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
                        { -1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
                        { 1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
                    };
                    add_motors_raw(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_DJI_X: {
                    _frame_type_string = "DJI_X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
                        {  -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
                        {  -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
                        { -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
                        {  150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
                        {   90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_CW_X: {
                    _frame_type_string = "CW_X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
                        {   90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
                        {  150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
                        { -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
                        {  -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
                        {  -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                default:
                    // hexa frame class does not support this frame type
                    _frame_type_string = "UNSUPPORTED";
                    success = false;
                    break;
            }
            break;
#endif //AP_MOTORS_FRAME_HEXA_ENABLED
#if AP_MOTORS_FRAME_OCTA_ENABLED
        case MOTOR_FRAME_OCTA:
            _frame_class_string = "OCTA";
            _mav_type = MAV_TYPE_OCTOROTOR;
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_PLUS: {
                    _frame_type_string = "PLUS";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {    0, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
                        {  180, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   5 },
                        {   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
                        {  135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
                        {  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  8 },
                        { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  6 },
                        {  -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   7 },
                        {   90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
                    };

                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_X: {
                    _frame_type_string = "X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   22.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
                        { -157.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,   5 },
                        {   67.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
                        {  157.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
                        {  -22.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  8 },
                        { -112.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  6 },
                        {  -67.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,   7 },
                        {  112.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_V: {
                    _frame_type_string = "V";
                    static const AP_MotorsMatrix::MotorDefRaw motors[] {
                        {  0.83f,  0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  7 },
                        { -0.67f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3 },
                        {  0.67f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
                        { -0.50f, -1.00f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
                        {  1.00f,  1.00f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
                        { -0.83f,  0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
                        { -1.00f,  1.00f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1 },
                        {  0.50f, -1.00f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  5 },
                    };
                    add_motors_raw(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_H: {
                    _frame_type_string = "H";
                    static const AP_MotorsMatrix::MotorDefRaw motors[] {
                        { -1.0f,    1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1 },
                        {  1.0f,   -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  5 },
                        { -1.0f,  0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
                        { -1.0f,   -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
                        {  1.0f,    1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
                        {  1.0f, -0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
                        {  1.0f,  0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  7 },
                        { -1.0f, -0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3 },
                    };
                    add_motors_raw(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_I: {
                    _frame_type_string = "I";
                    static const AP_MotorsMatrix::MotorDefRaw motors[] {
                        { 0.333f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   5 },
                        { -0.333f,  1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1 },
                        {    1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
                        {  0.333f,  1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
                        { -0.333f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
                        {   -1.0f,  1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
                        {   -1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3 },
                        {    1.0f,  1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  7 },
                    };
                    add_motors_raw(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_DJI_X: {
                    _frame_type_string = "DJI_X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   22.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
                        {  -22.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,   8 },
                        {  -67.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  7 },
                        { -112.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
                        { -157.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
                        {  157.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
                        {  112.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
                        {   67.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_CW_X: {
                    _frame_type_string = "CW_X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   22.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
                        {   67.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
                        {  112.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
                        {  157.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
                        { -157.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
                        { -112.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
                        {  -67.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  7 },
                        {  -22.5f,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,   8 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                default:
                    // octa frame class does not support this frame type
                    _frame_type_string = "UNSUPPORTED";
                    success = false;
                    break;
            } // octa frame type
            break;
#endif //AP_MOTORS_FRAME_OCTA_ENABLED
#if AP_MOTORS_FRAME_OCTAQUAD_ENABLED
        case MOTOR_FRAME_OCTAQUAD:
            _mav_type = MAV_TYPE_OCTOROTOR;
            _frame_class_string = "OCTAQUAD";
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_PLUS: {
                    _frame_type_string = "PLUS";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
                        { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   7 },
                        { 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
                        {  90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
                        { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  8 },
                        {   0, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
                        {  90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
                        { 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_X: {
                    _frame_type_string = "X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
                        {  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   7 },
                        { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
                        {  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
                        {  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  8 },
                        {   45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
                        {  135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
                        { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_V: {
                    _frame_type_string = "V";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   45,   0.7981f,  1 },
                        {  -45,  -0.7981f,  7 },
                        { -135,   1.0000f,  5 },
                        {  135,  -1.0000f,  3 },
                        {  -45,   0.7981f,  8 },
                        {   45,  -0.7981f,  2 },
                        {  135,   1.0000f,  4 },
                        { -135,  -1.0000f,  6 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_H: {
                    // H frame set-up - same as X but motors spin in opposite directions
                    _frame_type_string = "H";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
                        {  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  7 },
                        { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   5 },
                        {  135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
                        {  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   8 },
                        {   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
                        {  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
                        { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  6 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_CW_X:
                    _frame_type_string = "CW_X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
                        {   45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
                        {  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
                        {  135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
                        { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
                        { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
                        {  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   7 },
                        {  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  8 },
                            };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                default:
                    // octaquad frame class does not support this frame type
                    _frame_type_string = "UNSUPPORTED";
                    success = false;
                    break;
            }
            break;
#endif //AP_MOTORS_FRAME_OCTAQUAD_ENABLED
#if AP_MOTORS_FRAME_DODECAHEXA_ENABLED
        case MOTOR_FRAME_DODECAHEXA: {
            _mav_type = MAV_TYPE_DODECAROTOR;
            _frame_class_string = "DODECAHEXA";
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_PLUS: {
                    _frame_type_string = "PLUS";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {    0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 }, // forward-top
                        {    0, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   2 }, // forward-bottom
                        {   60, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   3 }, // forward-right-top
                        {   60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 }, // forward-right-bottom
                        {  120, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 }, // back-right-top
                        {  120, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   6 }, // back-right-bottom
                        {  180, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   7 }, // back-top
                        {  180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  8 }, // back-bottom
                        { -120, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  9 }, // back-left-top
                        { -120, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   10 }, // back-left-bottom
                        {  -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   11 }, // forward-left-top
                        {  -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  12 }, // forward-left-bottom
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_X: {
                    _frame_type_string = "X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   1 }, // forward-right-top
                        {   30, AP_MOTORS_MATRIX_YAW_FACTOR_CW,    2 }, // forward-right-bottom
                        {   90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,    3 }, // right-top
                        {   90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   4 }, // right-bottom
                        {  150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   5 }, // back-right-top
                        {  150, AP_MOTORS_MATRIX_YAW_FACTOR_CW,    6 }, // back-right-bottom
                        { -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW,    7 }, // back-left-top
                        { -150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   8 }, // back-left-bottom
                        {  -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   9 }, // left-top
                        {  -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   10 }, // left-bottom
                        {  -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   11 }, // forward-left-top
                        {  -30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,  12 }, // forward-left-bottom
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                default:
                    // dodeca-hexa frame class does not support this frame type
                    _frame_type_string = "UNSUPPORTED";
                    success = false;
                    break;
            }}
            break;
#endif //AP_MOTORS_FRAME_DODECAHEXA_ENABLED
#if AP_MOTORS_FRAME_Y6_ENABLED
        case MOTOR_FRAME_Y6:
            _mav_type = MAV_TYPE_HEXAROTOR;
            _frame_class_string = "Y6";
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_Y6B: {
                    // Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise
                    _frame_type_string = "Y6B";
                    static const AP_MotorsMatrix::MotorDefRaw motors[] {
                        { -1.0f,  0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1 },
                        { -1.0f,  0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
                        {  0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3 },
                        {  0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
                        {  1.0f,  0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  5 },
                        {  1.0f,  0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
                    };
                    add_motors_raw(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_Y6F: {
                    // Y6 motor layout for FireFlyY6
                    _frame_type_string = "Y6F";
                    static const AP_MotorsMatrix::MotorDefRaw motors[] {
                        {  0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
                        { -1.0f,  0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
                        {  1.0f,  0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
                        {  0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4 },
                        { -1.0f,  0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2 },
                        {  1.0f,  0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  6 },
                    };
                    add_motors_raw(motors, ARRAY_SIZE(motors));
                    break;
                }
                default: {
                    _frame_type_string = "default";
                    static const AP_MotorsMatrix::MotorDefRaw motors[] {
                        { -1.0f,  0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
                        {  1.0f,  0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  5 },
                        {  1.0f,  0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
                        {  0.0f, -1.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4 },
                        { -1.0f,  0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1 },
                        {  0.0f, -1.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
                    };
                    add_motors_raw(motors, ARRAY_SIZE(motors));
                    break;
                }
            }
            break;
#endif //AP_MOTORS_FRAME_Y6_ENABLED
#if AP_MOTORS_FRAME_DECA_ENABLED
        case MOTOR_FRAME_DECA:
            _mav_type = MAV_TYPE_DECAROTOR;
            _frame_class_string = "DECA";
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_PLUS: {
                    _frame_type_string = "PLUS";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {    0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   1 },
                        {   36, AP_MOTORS_MATRIX_YAW_FACTOR_CW,    2 },
                        {   72, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   3 },
                        {  108, AP_MOTORS_MATRIX_YAW_FACTOR_CW,    4 },
                        {  144, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   5 },
                        {  180, AP_MOTORS_MATRIX_YAW_FACTOR_CW,    6 },
                        { -144, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   7 },
                        { -108, AP_MOTORS_MATRIX_YAW_FACTOR_CW,    8 },
                        {  -72, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   9 },
                        {  -36, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   10 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                case MOTOR_FRAME_TYPE_X:
                case MOTOR_FRAME_TYPE_CW_X: {
                    _frame_type_string = "X/CW_X";
                    static const AP_MotorsMatrix::MotorDef motors[] {
                        {   18, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   1 },
                        {   54, AP_MOTORS_MATRIX_YAW_FACTOR_CW,    2 },
                        {   90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   3 },
                        {  126, AP_MOTORS_MATRIX_YAW_FACTOR_CW,    4 },
                        {  162, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   5 },
                        { -162, AP_MOTORS_MATRIX_YAW_FACTOR_CW,    6 },
                        { -126, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   7 },
                        {  -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,    8 },
                        {  -54, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,   9 },
                        {  -18, AP_MOTORS_MATRIX_YAW_FACTOR_CW,   10 },
                    };
                    add_motors(motors, ARRAY_SIZE(motors));
                    break;
                }
                default:
                    // deca frame class does not support this frame type
                    success = false;
                    break;
            }
            break;
#endif //AP_MOTORS_FRAME_DECA_ENABLED
        default:
            // matrix doesn't support the configured class
            _frame_class_string = "UNSUPPORTED";
            success = false;
            _mav_type = MAV_TYPE_GENERIC;
            break;


    } // switch frame_class

    // normalise factors to magnitude 0.5
    normalise_rpy_factors();

    set_initialised_ok(success);
}

// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
// normalizes throttle factors so max value is 1 and no value is less than 0
void AP_MotorsMatrix::normalise_rpy_factors()
{
    float roll_fac = 0.0f;
    float pitch_fac = 0.0f;
    float yaw_fac = 0.0f;
    float throttle_fac = 0.0f;

    // find maximum roll, pitch and yaw factors
    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i]) {
            roll_fac = MAX(roll_fac,fabsf(_roll_factor[i]));
            pitch_fac = MAX(pitch_fac,fabsf(_pitch_factor[i]));
            yaw_fac = MAX(yaw_fac,fabsf(_yaw_factor[i]));
            throttle_fac = MAX(throttle_fac,MAX(0.0f,_throttle_factor[i]));
        }
    }

    // scale factors back to -0.5 to +0.5 for each axis
    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i]) {
            if (!is_zero(roll_fac)) {
                _roll_factor[i] = 0.5f * _roll_factor[i] / roll_fac;
            }
            if (!is_zero(pitch_fac)) {
                _pitch_factor[i] = 0.5f * _pitch_factor[i] / pitch_fac;
            }
            if (!is_zero(yaw_fac)) {
                _yaw_factor[i] = 0.5f * _yaw_factor[i] / yaw_fac;
            }
            if (!is_zero(throttle_fac)) {
                _throttle_factor[i] = MAX(0.0f,_throttle_factor[i] / throttle_fac);
            }
        }
    }
}


/*
  call vehicle supplied thrust compensation if set. This allows
  vehicle code to compensate for vehicle specific motor arrangements
  such as tiltrotors or tiltwings
*/
void AP_MotorsMatrix::thrust_compensation(void)
{
    if (_thrust_compensation_callback) {
        _thrust_compensation_callback(_thrust_rpyt_out, AP_MOTORS_MAX_NUM_MOTORS);
    }
}

/*
  disable the use of motor torque to control yaw. Used when an
  external mechanism such as vectoring is used for yaw control
*/
void AP_MotorsMatrix::disable_yaw_torque(void)
{
    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
        _yaw_factor[i] = 0;
    }
}

// singleton instance
AP_MotorsMatrix *AP_MotorsMatrix::_singleton;