#include "AP_RCProtocol_config.h"

#if AP_RCPROTOCOL_FDM_ENABLED

#include "AP_RCProtocol_FDM.h"

#include <AP_HAL/AP_HAL.h>
#include <SITL/SITL.h>

extern const AP_HAL::HAL& hal;

void AP_RCProtocol_FDM::update()
{
    const auto sitl = AP::sitl();
    if (sitl == nullptr) {
        return;
    }

    const auto &fdm = sitl->state;

    // see if there's fresh input.  Until timestamps are worked out,
    // just check for non-zero values:
    if (fdm.rcin_chan_count == 0) {
        return;
    }

    // simulate RC input at 50Hz
    if (AP_HAL::millis() - last_input_ms < 20) {
        return;
    }

    last_input_ms = AP_HAL::millis();

    // scale from FDM 0-1 floats to PWM values
    // these are the values that will be fed into the autopilot.
    uint16_t pwm_input[16];
    const uint8_t count = MIN(ARRAY_SIZE(pwm_input), fdm.rcin_chan_count);
    for (uint8_t i=0; i<count; i++) {
        pwm_input[i] = 1000 + fdm.rcin[i] * 1000;
    }
    add_input(
        count,
        pwm_input,
        false,  // failsafe
        0, // check me
        0  // link quality
        );
}

#endif // AP_RCPROTOCOL_FDM_ENABLED