// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
   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/>.
 */
/*
  SRV_Channel.cpp - object to separate input and output channel
  ranges, trim and reversal
 */

#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include "SRV_Channel.h"
#include "RC_Channel.h"

extern const AP_HAL::HAL& hal;

const AP_Param::GroupInfo SRV_Channels::var_info[] = {
    // @Param: RNG_ENABLE
    // @DisplayName: Enable servo output ranges
    // @Description: This enables the use of separate servo output ranges from input ranges. 
    // @Values: 0:Disable,1:Enable
    // @User: Advanced
    AP_GROUPINFO_FLAGS("_RNG_ENABLE",  1, SRV_Channels, enable, 0, AP_PARAM_FLAG_ENABLE),

    // @Param: 1_MIN
    // @DisplayName: Servo1 min PWM
    // @Description: servo1 minimum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: pwm
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("1_MIN",  2, SRV_Channels, servo_min[0], 1100),

    // @Param: 1_MAX
    // @DisplayName: Servo1 max PWM
    // @Description: servo1 maximum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: pwm
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("1_MAX",  3, SRV_Channels, servo_max[0], 1900),

    // @Param: 1_TRIM
    // @DisplayName: Servo1 trim PWM
    // @Description: servo1 trim PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: pwm
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("1_TRIM",  4, SRV_Channels, servo_trim[0], 1500),
    
    // @Param: 1_REV
    // @DisplayName: Servo1 reverse
    // @Description: Reverse servo operation. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel.
    // @Values: -1:Reversed,1:Normal
    // @User: Advanced
    AP_GROUPINFO("1_REV",  5, SRV_Channels, reverse[0], 1),

    // @Param: 2_MIN
    // @DisplayName: Servo1 min PWM
    // @Description: servo1 minimum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: pwm
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("2_MIN",  6, SRV_Channels, servo_min[1], 1100),

    // @Param: 2_MAX
    // @DisplayName: Servo1 max PWM
    // @Description: servo1 maximum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: pwm
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("2_MAX",  7, SRV_Channels, servo_max[1], 1900),

    // @Param: 2_TRIM
    // @DisplayName: Servo1 trim PWM
    // @Description: servo1 trim PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: pwm
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("2_TRIM",  8, SRV_Channels, servo_trim[1], 1500),
    
    // @Param: 2_REV
    // @DisplayName: Servo1 reverse
    // @Description: Reverse servo operation. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel.
    // @Values: -1:Reversed,1:Normal
    // @User: Advanced
    AP_GROUPINFO("2_REV",  9, SRV_Channels, reverse[1], 1),

    // @Param: 3_MIN
    // @DisplayName: Servo1 min PWM
    // @Description: servo1 minimum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: pwm
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("3_MIN",  10, SRV_Channels, servo_min[2], 1100),

    // @Param: 3_MAX
    // @DisplayName: Servo1 max PWM
    // @Description: servo1 maximum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: pwm
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("3_MAX",  11, SRV_Channels, servo_max[2], 1900),

    // @Param: 3_TRIM
    // @DisplayName: Servo1 trim PWM
    // @Description: servo1 trim PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: pwm
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("3_TRIM",  12, SRV_Channels, servo_trim[2], 1500),
    
    // @Param: 3_REV
    // @DisplayName: Servo1 reverse
    // @Description: Reverse servo operation. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel.
    // @Values: -1:Reversed,1:Normal
    // @User: Advanced
    AP_GROUPINFO("3_REV",  13, SRV_Channels, reverse[2], 1),

    // @Param: 4_MIN
    // @DisplayName: Servo1 min PWM
    // @Description: servo1 minimum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: pwm
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("4_MIN",  14, SRV_Channels, servo_min[3], 1100),

    // @Param: 4_MAX
    // @DisplayName: Servo1 max PWM
    // @Description: servo1 maximum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: pwm
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("4_MAX",  15, SRV_Channels, servo_max[3], 1900),

    // @Param: 4_TRIM
    // @DisplayName: Servo1 trim PWM
    // @Description: servo1 trim PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: pwm
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("4_TRIM",  16, SRV_Channels, servo_trim[3], 1500),
    
    // @Param: 4_REV
    // @DisplayName: Servo1 reverse
    // @Description: Reverse servo operation. Set to 1 for normal (forward) operation. Set to -1 to reverse this channel.
    // @Values: -1:Reversed,1:Normal
    // @User: Advanced
    AP_GROUPINFO("4_REV",  17, SRV_Channels, reverse[3], 1),
    
    AP_GROUPEND
};


/*
  constructor
 */
SRV_Channels::SRV_Channels(void)
{
    AP_Param::setup_object_defaults(this, var_info);
}


// remap a PWM value from a channel in value
uint16_t SRV_Channels::remap_pwm(uint8_t i, uint16_t pwm) const
{
    const RC_Channel *ch = RC_Channel::rc_channel(i);
    if (ch == nullptr) {
        return 0;
    }
    float v = ch->get_radio_out_normalised(pwm);
    uint16_t radio_out;
    if (ch->get_type_out() == RC_CHANNEL_TYPE_RANGE) {
        if (reverse[i] == -1) {
            v = 1 - v;
        }
        radio_out = servo_min[i] + v * (servo_max[i] - servo_min[i]);
    } else {
        if (reverse[i] == -1) {
            v = -v;
        }
        if (v > 0) {
            radio_out = servo_trim[i] + v * (servo_max[i] - servo_trim[i]);
        } else {
            radio_out = servo_trim[i] + v * (servo_trim[i] - servo_min[i]);
        }
    }
    radio_out = constrain_int16(radio_out, servo_min[i], servo_max[i]);
    return radio_out;
}

/*
  remap radio_out values for first 4 servos using SERVO* parameters, if enabled
  This should be called with cork enabled in hal.rcout
 */
void SRV_Channels::remap_servo_output(void)
{
    // cope with SERVO_RNG_ENABLE being changed at runtime. If we
    // don't change the ESC scaling immediately then some ESCs will
    // fire up for one second
    if (last_enable != enable && esc_cal_chan != -1) {
        last_enable = enable;
        set_esc_scaling(esc_cal_chan);
    }
    if (!enable) {
        return;
    }
    for (uint8_t i=0; i<NUM_SERVO_RANGE_CHANNELS; i++) {
        RC_Channel *ch = RC_Channel::rc_channel(i);
        if (ch == nullptr) {
            continue;
        }
        uint16_t radio_out = remap_pwm(i, ch->get_radio_out());
        ch->set_radio_out(radio_out);
    }
}


/*
  set trim values for output channels
 */
void SRV_Channels::set_trim(void)
{
    if (!enable) {
        return;
    }
    for (uint8_t i=0; i<NUM_SERVO_RANGE_CHANNELS; i++) {
        const RC_Channel *ch = RC_Channel::rc_channel(i);
        if (ch == nullptr) {
            continue;
        }
        if (ch->get_type_out() == RC_CHANNEL_TYPE_RANGE) {
            // we don't trim range channels (like throttle)
            continue;
        }
        uint16_t new_trim = remap_pwm(i, ch->get_radio_trim());
        servo_trim[i].set_and_save(new_trim);
    }
}

void SRV_Channels::set_esc_scaling(uint8_t chnum)
{
    esc_cal_chan = chnum;
    if (!enable || chnum >= NUM_SERVO_RANGE_CHANNELS) {
        const RC_Channel *ch = RC_Channel::rc_channel(chnum);
        hal.rcout->set_esc_scaling(ch->get_radio_min(), ch->get_radio_max());
    } else {
        hal.rcout->set_esc_scaling(servo_min[chnum], servo_max[chnum]);
    }
}