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

/*
  SBUS output support
 */

#include "ch.h"
#include "hal.h"
#include "iofirmware.h"
#include "rc.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_SBusOut/AP_SBusOut.h>

extern const AP_HAL::HAL& hal;

// usart3 is for SBUS input and output
static const SerialConfig sbus_cfg = {
    100000,   // speed
    USART_CR1_PCE | USART_CR1_M, // cr1, enable even parity
    USART_CR2_STOP_1,            // cr2, two stop bits
    0,        // cr3
    nullptr,  // irq_cb
    nullptr,  // ctx
};

// listen for parity errors on sd3 input
static event_listener_t sd3_listener;

static uint8_t sd3_config;

void sbus_out_write(uint16_t *channels, uint8_t nchannels)
{
    if (sd3_config == 0) {
        uint8_t buffer[25];
        AP_SBusOut::sbus_format_frame(channels, nchannels, buffer);
        chnWrite(&SD3, buffer, sizeof(buffer));
    }
}

// usart1 is for DSM input and (optionally) debug to FMU
static const SerialConfig dsm_cfg = {
    115200,   // speed
    0,        // cr1
    0,        // cr2
    0,        // cr3
    nullptr,  // irq_cb
    nullptr,  // ctx
};

static enum {
    RC_SEARCHING,
    RC_DSM_PORT,
    RC_SBUS_PORT
} rc_state;

/*
  init rcin on DSM USART1
 */
void AP_IOMCU_FW::rcin_serial_init(void)
{
    sdStart(&SD1, &dsm_cfg);
    sdStart(&SD3, &sbus_cfg);
    chEvtRegisterMaskWithFlags(chnGetEventSource(&SD3),
                               &sd3_listener,
                               EVENT_MASK(1),
                               SD_PARITY_ERROR);
    // disable input for SBUS with pulses, we will use the UART for
    // SBUS and FPORT
#if AP_RCPROTOCOL_SBUS_ENABLED
    AP::RC().disable_for_pulses(AP_RCProtocol::SBUS);
#endif
#if AP_RCPROTOCOL_SBUS_NI_ENABLED
    AP::RC().disable_for_pulses(AP_RCProtocol::SBUS_NI);
#endif
#if AP_RCPROTOCOL_FPORT_ENABLED
    AP::RC().disable_for_pulses(AP_RCProtocol::FPORT);
#endif
}

static struct {
    uint32_t num_dsm_bytes;
    uint32_t num_sbus_bytes;
    uint32_t num_sbus_errors;
    eventflags_t sbus_error;
    uint32_t last_good_ms;
} rc_stats;

/*
  check for data on DSM RX uart
 */
void AP_IOMCU_FW::rcin_serial_update(void)
{
    uint8_t b[16];
    uint32_t n;
    uint32_t now = AP_HAL::millis();
    auto &rc = AP::RC();

    if (rc.should_search(now)) {
        rc_state = RC_SEARCHING;
    }

    // read from DSM port
    if ((rc_state == RC_SEARCHING || rc_state == RC_DSM_PORT) &&
        (n = chnReadTimeout(&SD1, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
        n = MIN(n, sizeof(b));
        // don't mix two 115200 uarts
        if (sd3_config == 0) {
            rc_stats.num_dsm_bytes += n;
            for (uint8_t i=0; i<n; i++) {
                if (rc.process_byte(b[i], 115200)) {
                    rc_stats.last_good_ms = now;
                    if (!rc.should_search(now)) {
                        rc_state = RC_DSM_PORT;
                    }
                }
            }
        }
        //BLUE_TOGGLE();
    }

    // read from SBUS port
    if ((rc_state == RC_SEARCHING || rc_state == RC_SBUS_PORT) &&
        (n = chnReadTimeout(&SD3, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
        eventflags_t flags;
        if (sd3_config == 0 && ((flags = chEvtGetAndClearFlags(&sd3_listener)) & SD_PARITY_ERROR)) {
            rc_stats.sbus_error = flags;
            rc_stats.num_sbus_errors++;
        } else {
            n = MIN(n, sizeof(b));
            rc_stats.num_sbus_bytes += n;
            for (uint8_t i=0; i<n; i++) {
                if (rc.process_byte(b[i], sd3_config==0?100000:115200)) {
                    rc_stats.last_good_ms = now;
                    if (!rc.should_search(now)) {
                        rc_state = RC_SBUS_PORT;
                    }
                }
            }
        }
    }
    /*
      when not using SBUS output we switch UART3 between 100000 baud
      and 115200 baud in order to support RC input protocols that are
      115200 inverted (such as FPort). If SBUS output is enabled then
      we need to disable this as the uart is shared between input and
      output
     */
    const bool sbus_out_enabled = (reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) != 0;
    if (rc_state == RC_SEARCHING &&
        now - rc_stats.last_good_ms > 2000 && (sd3_config==1 || !sbus_out_enabled)) {
        rc_stats.last_good_ms = now;
        sd3_config ^= 1;
        sdStop(&SD3);
        sdStart(&SD3, sd3_config==0?&sbus_cfg:&dsm_cfg);
    }
}

/*
  sleep for 1ms using a busy loop
 */
static void delay_one_ms(uint32_t &now)
{
    while (now == AP_HAL::millis()) ;
    now = AP_HAL::millis();
}

/*
  perform a DSM bind operation
 */
void AP_IOMCU_FW::dsm_bind_step(void)
{
    uint32_t now = last_ms;
    switch (dsm_bind_state) {
    case 1:
        palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, PAL_MODE_OUTPUT_PUSHPULL);
        SPEKTRUM_POWER(0);
        palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_OUTPUT_PUSHPULL);
        SPEKTRUM_SET(1);
        dsm_bind_state = 2;
        last_dsm_bind_ms = now;
        break;

    case 2:
        if (now - last_dsm_bind_ms >= 500) {
            SPEKTRUM_POWER(1);
            dsm_bind_state = 3;
            last_dsm_bind_ms = now;
        }
        break;

    case 3: {
        if (now - last_dsm_bind_ms >= 72) {
            // 9 pulses works with all satellite receivers, and supports the highest
            // available protocol
            delay_one_ms(now);
            const uint8_t num_pulses = 9;
            for (uint8_t i=0; i<num_pulses; i++) {
                // the delay should be 120us, but we are running our
                // clock at 1kHz, and 1ms works fine
                delay_one_ms(now);
                SPEKTRUM_SET(0);
                delay_one_ms(now);
                SPEKTRUM_SET(1);
            }
            last_dsm_bind_ms = now;
            dsm_bind_state = 4;
        }
        break;
    }

    case 4:
        if (now - last_dsm_bind_ms >= 50) {
            // set back as input pin
            palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_OUT, PAL_MODE_INPUT);
            dsm_bind_state = 0;
        }
        break;

    default:
        dsm_bind_state = 0;
        break;
    }
}