/*
(c) 2017 night_ghost@ykoctpa.ru
 
*/


#pragma GCC optimize ("O2")

#include "AP_HAL_F4Light.h"
#include "RCOutput.h"
#include <AP_Param_Helper/AP_Param_Helper.h>
#include <4way/serial_4way.h>

#include "GPIO.h"

using namespace F4Light;


// only one!
//#define  DEBUG_PWM 5 // motor 6
//#define DEBUG_INT 5


#define F4Light_OUT_CHANNELS 6 // motor's channels enabled by default

// #if FRAME_CONFIG == QUAD_FRAME // this is only QUAD layouts

// ArduCopter
static const uint8_t output_channels_arducopter[]= {  // pin assignment
    SERVO_PIN_1, //Timer3/3  - 1
    SERVO_PIN_2, //Timer3/4  - 2
    SERVO_PIN_3, //Timer2/3  - 3
    SERVO_PIN_4, //Timer2/2  - 4
#ifdef SERVO_PIN_5
    SERVO_PIN_5, //Timer2/1
#endif
#ifdef SERVO_PIN_6
    SERVO_PIN_6, //Timer2/0
#endif

    // servo channels on input port
    4, // PB14  CH1_IN - PPM1 Use this channels asd servo only if you use DSM via UART as RC
    5, // PB15  CH2_IN - PPM2 
    12, // PC6  CH3_IN UART6
    13, // PC7  CH4_IN UART6
    14, // PC8  CH5_IN i2c
    15, // PC9  CH6_IN i2c
};


// mimics foreign layouts

// OpenPilot
static const uint8_t output_channels_openpilot[]= {  // pin assignment
    SERVO_PIN_2, //Timer3/4  - 2
    SERVO_PIN_4, //Timer2/2  - 4
    SERVO_PIN_1, //Timer3/3  - 1
    SERVO_PIN_3, //Timer2/3  - 3
#ifdef SERVO_PIN_5
    SERVO_PIN_5, //Timer2/1
#endif
#ifdef SERVO_PIN_6
    SERVO_PIN_6, //Timer2/0
#endif

    // servo channels on input port
//    4, // PB14  CH1_IN - PPM1 Use this channels asd servo only if you use DSM via UART as RC
    5, // PB15  CH2_IN - PPM2 
    12, // PC6  CH3_IN UART6
    13, // PC7  CH4_IN UART6
    14, // PC8  CH5_IN i2c
    15, // PC9  CH6_IN i2c
};


// Cleanflight
static const uint8_t output_channels_cleanflight[]= {  // pin assignment
    SERVO_PIN_2, //Timer3/4  - 2
    SERVO_PIN_3, //Timer2/3  - 3
    SERVO_PIN_4, //Timer2/2  - 4
    SERVO_PIN_1, //Timer3/3  - 1
#ifdef SERVO_PIN_5
    SERVO_PIN_5, //Timer2/1
#endif
#ifdef SERVO_PIN_6
    SERVO_PIN_6, //Timer2/0
#endif

    // servo channels on input port
//    4, // PB14  CH1_IN - PPM1 Use this channels asd servo only if you use DSM via UART as RC
    5, // PB15  CH2_IN - PPM2 
    12, // PC6  CH3_IN UART6
    13, // PC7  CH4_IN UART6
    14, // PC8  CH5_IN i2c
    15, // PC9  CH6_IN i2c

};

// Arducopter,shifted 2 pins right to use up to 2 servos
static const uint8_t output_channels_servo[]= {  // pin assignment
    SERVO_PIN_3, //Timer2/3  - 1
    SERVO_PIN_4, //Timer2/2  - 2
#ifdef SERVO_PIN_5
    SERVO_PIN_5, //Timer2/1  - 3
#endif
#ifdef SERVO_PIN_6
    SERVO_PIN_6, //Timer2/0  - 4
#endif
    SERVO_PIN_1, //Timer3/3    servo1
    SERVO_PIN_2, //Timer3/4    servo2

    // servo channels on input port
//    4, // PB14  CH1_IN - PPM1 Use this channels as servo only if you use DSM via UART as RC
    5, // PB15  CH2_IN - PPM2 
    12, // PC6  CH3_IN UART6
    13, // PC7  CH4_IN UART6
    14, // PC8  CH5_IN i2c
    15, // PC9  CH6_IN i2c
};



static const uint8_t * const revo_motor_map[]={
    output_channels_arducopter,
    output_channels_servo,
    output_channels_openpilot,
    output_channels_cleanflight,
};


// #endif

static const uint8_t *output_channels = output_channels_openpilot;  // current pin assignment

enum     BOARD_PWM_MODES RCOutput::_mode = BOARD_PWM_NORMAL;
bool     RCOutput::_once_mode = false;

uint16_t RCOutput::_period[F4Light_MAX_OUTPUT_CHANNELS] IN_CCM;
uint16_t RCOutput::_freq[F4Light_MAX_OUTPUT_CHANNELS] IN_CCM;
uint8_t  RCOutput::_initialized[F4Light_MAX_OUTPUT_CHANNELS] IN_CCM;
uint16_t RCOutput::_enabled_channels=0;
bool     RCOutput::_sbus_enabled=0;
bool     RCOutput::_corked=0;
uint8_t  RCOutput::_used_channels=0;

uint8_t RCOutput::_servo_mask=0;

uint32_t RCOutput::_timer2_preload;
uint16_t RCOutput::_timer3_preload;

uint8_t  RCOutput::_pwm_type=0;

const timer_dev* RCOutput::out_timers[16]  IN_CCM;
uint8_t          RCOutput::num_out_timers  IN_CCM;


#define PWM_TIMER_KHZ          2000  // 1000 in cleanflight
#define ONESHOT125_TIMER_KHZ   8000  // 8000 in cleanflight
#define ONESHOT42_TIMER_KHZ   28000  // 24000 in cleanflight
#define PWM_BRUSHED_TIMER_KHZ 16000  // 8000 in cleanflight

#define _BV(bit) (1U << (bit))

void RCOutput::init()
{
    memset(&_period[0], 0, sizeof(_period));
    memset(&_initialized[0], 0, sizeof(_initialized));

    _used_channels=0;
    
}


void RCOutput::do_4way_if(AP_HAL::UARTDriver* uart) {
    esc4wayInit(output_channels, F4Light_OUT_CHANNELS);
    esc4wayProcess(uart);
}

#ifdef DEBUG_INT
extern "C" int printf(const char *msg, ...);

static void isr_handler(){
    uint32_t *sp;

#define FRAME_SHIFT 10 // uses IRQ handler itself

    asm volatile ("mov %0, sp\n\t"  : "=rm" (sp) );
    
    // Stack frame contains:
    // r0, r1, r2, r3, r12, r14, the return address and xPSR
    // - Stacked R0  = sp[0]
    // - Stacked R1  = sp[1]
    // - Stacked R2  = sp[2]
    // - Stacked R3  = sp[3]
    // - Stacked R12 = sp[4]
    // - Stacked LR  = sp[5]
    // - Stacked PC  = sp[6]
    // - Stacked xPSR= sp[7]
    printf("pc=%lx lr=%lx\n", sp[FRAME_SHIFT+6], sp[FRAME_SHIFT+5]);
}
#endif

void RCOutput::lateInit(){ // 2nd stage with loaded parameters

    uint8_t map = hal_param_helper->_motor_layout;
    _servo_mask = hal_param_helper->_servo_mask;
    _pwm_type   = hal_param_helper->_pwm_type;
    
    if(map >= ARRAY_SIZE(revo_motor_map)) return; // don't initialize if parameter is wrong
    output_channels = revo_motor_map[map];
    
    InitPWM();
    
#ifdef DEBUG_INT
    uint8_t spin = output_channels[DEBUG_INT]; // motor 6 as button
    GPIO::_pinMode(spin, INPUT_PULLUP);

    Revo_handler h = { .vp = isr_handler  };  
    GPIO::_attach_interrupt(spin, h.h, FALLING, 0);
#endif
}

void RCOutput::InitPWM()
{
    for(uint8_t i = 0; i < F4Light_MAX_OUTPUT_CHANNELS; i++) {
        _freq[i] = 50;
    }
    fill_timers();
    _set_output_mode(MODE_PWM_NORMAL); // init timers
}



// not from _freq to take channel dependency
uint16_t RCOutput::get_freq(uint8_t ch) {
    if(ch >= F4Light_MAX_OUTPUT_CHANNELS) return 0;
    
    const timer_dev *dev = PIN_MAP[output_channels[ch]].timer_device;

    /* transform to period by inverse of _time_period(icr) */
    return (uint16_t)(dev->state->freq / timer_get_reload(dev));
}

// fill array of used timers
void RCOutput::fill_timers(){
    memset(out_timers, 0, sizeof(out_timers)); // clear it first
    num_out_timers=0;

    for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
        if (!(_enabled_channels & _BV(ch))) continue;      // not enabled

        const timer_dev *tim = PIN_MAP[output_channels[ch]].timer_device;

        bool add=true;
        for(uint8_t i =0; i<num_out_timers;i++){
            if(out_timers[i]==tim){
                add=false;
                break;
            }
        }
        if(add) out_timers[num_out_timers++] = tim;
    }
}


void RCOutput::_set_output_mode(enum RCOutput::output_mode mode) {
    
    uint32_t period=0;
    uint32_t freq;

    _once_mode=false;
    

    switch(_pwm_type) {
    case 0:
    default:
        switch(mode){
        case MODE_PWM_NORMAL:
            _mode=BOARD_PWM_NORMAL;
            break;

        case MODE_PWM_BRUSHED:
            _mode=BOARD_PWM_BRUSHED;
            break;    
        
        default:
        case MODE_PWM_ONESHOT:
            _mode=BOARD_PWM_ONESHOT;
            break;
        }  
        break;
          
    case 1:
        _mode=BOARD_PWM_ONESHOT;
        break;
        
    case 2:
        _mode=BOARD_PWM_ONESHOT125;
        break;
    
    case 3:
        _mode=BOARD_PWM_ONESHOT42;
        break;

    case 4:
        _mode=BOARD_PWM_PWM125;
        break;
    }
    
// TODO: remove hardwiring timers
// TODO: we should change mode only for channels with freq > 50Hz

    switch(_mode){

    case BOARD_PWM_NORMAL:
    default:
// output uses timers 2 & 3 so let init them for PWM mode
        period    = ((PWM_TIMER_KHZ*1000UL) / 50); // 50Hz by default - will be corrected late per-channel in init_channel()
        freq      = PWM_TIMER_KHZ;        // 2MHz 0.5us ticks - for 50..490Hz PWM
        break;

    case BOARD_PWM_ONESHOT: // same as PWM but with manual restarting
// output uses timers 2 & 3 so let init them for PWM mode
        period    = (uint32_t)-1; // max possible
        freq      = PWM_TIMER_KHZ;       // 2MHz 0.5us ticks - for 50..490Hz PWM
        _once_mode=true;
        break;

    case BOARD_PWM_ONESHOT125:
        period    = (uint32_t)-1; // max possible
        freq      = ONESHOT125_TIMER_KHZ;       // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
//    at 16Mhz, period with 65536 will be 244Hz so even 16-bit timer will never overflows at 500Hz loop
        _once_mode=true;
        break;

   case BOARD_PWM_PWM125: // like Oneshot125 but PWM so not once
        period    = (uint32_t)-1; // max possible
        freq      = ONESHOT125_TIMER_KHZ;       // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
//    at 16Mhz, period with 65536 will be 244Hz so even 16-bit timer will never overflows at 500Hz loop
        break;

    case BOARD_PWM_ONESHOT42:
        period    = (uint32_t)-1; // max possible
        freq      = ONESHOT42_TIMER_KHZ;       // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
//    at 28Mhz, period with 65536 will be 427Hz so even 16-bit timer should not overflows at 500Hz loop, but very close to
        _once_mode=true;
        break;

    case BOARD_PWM_BRUSHED: 
                     // dev    period   freq, kHz
        period = 1000;
        freq   = PWM_BRUSHED_TIMER_KHZ;       // 16MHz  - 0..1 in 1000 steps
        break;
    }


#if 1
// correct code should init all timers used for outputs
        
        for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
            const timer_dev *tim = PIN_MAP[output_channels[ch]].timer_device;
            tim->state->update=false;  // reset flag first
        }

        for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
            if (!(_enabled_channels & _BV(ch))) continue;      // not enabled

            if(_freq[ch]>50){
                const timer_dev *tim = PIN_MAP[output_channels[ch]].timer_device;    
                tim->state->update=true; // set flag for update for needed timer
            }
        }

        for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
            uint8_t pin = output_channels[ch];
    
            const timer_dev *tim = PIN_MAP[pin].timer_device;
            if(tim->state->update) {
                configTimeBase(tim, period,  freq);
                tim->state->update = false;    // only once
                if(_mode == BOARD_PWM_BRUSHED) tim->state->freq_scale=1;
            }
        }

#else // raw and dirty way

                             // dev    period   freq, kHz                             
    configTimeBase(TIMER2, period,  freq);       // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
    configTimeBase(TIMER3, period,  freq);       // 16MHz 62.5ns ticks 

    if(_mode == BOARD_PWM_BRUSHED) {
        TIMER2->state->freq_scale=1;
        TIMER3->state->freq_scale=1;
    }

#endif

    init_channels();

#if 1 
    for(uint8_t i =0; i<num_out_timers;i++){
        timer_resume(out_timers[i]);
    }
#else
    timer_resume(TIMER2);
    timer_resume(TIMER3);
#endif
}

void RCOutput::_set_pin_output_mode(uint8_t ch) {
    
    uint32_t period=0;
    uint32_t freq;

    switch(_mode){

    case BOARD_PWM_NORMAL:
    default:
// output uses timers 2 & 3 so let init them for PWM mode
        period    = ((PWM_TIMER_KHZ*1000UL) / 50); // 50Hz by default - will be corrected late per-channel in init_channel()
        freq      = PWM_TIMER_KHZ;        // 2MHz 0.5us ticks - for 50..490Hz PWM
        break;

    case BOARD_PWM_ONESHOT: // same as PWM but with manual restarting
// output uses timers 2 & 3 so let init them for PWM mode
        period    = (uint32_t)-1; // max possible
        freq      = PWM_TIMER_KHZ;       // 2MHz 0.5us ticks - for 50..490Hz PWM
        break;

    case BOARD_PWM_ONESHOT125:
        period    = (uint32_t)-1; // max possible
        freq      = ONESHOT125_TIMER_KHZ;       // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
//    at 16Mhz, period with 65536 will be 244Hz so even 16-bit timer will never overflows at 500Hz loop
        break;

   case BOARD_PWM_PWM125: // like Oneshot125 but PWM so not once
        period    = (uint32_t)-1; // max possible
        freq      = ONESHOT125_TIMER_KHZ;       // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
//    at 16Mhz, period with 65536 will be 244Hz so even 16-bit timer will never overflows at 500Hz loop
        break;

    case BOARD_PWM_ONESHOT42:
        period    = (uint32_t)-1; // max possible
        freq      = ONESHOT42_TIMER_KHZ;       // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
//    at 28Mhz, period with 65536 will be 427Hz so even 16-bit timer should not overflows at 500Hz loop, but very close to
        break;

    case BOARD_PWM_BRUSHED: 
                     // dev    period   freq, kHz
        period = 1000;
        freq   = PWM_BRUSHED_TIMER_KHZ;       // 16MHz  - 0..1 in 1000 steps
        break;
    }


    uint8_t pin = output_channels[ch];
    
    const timer_dev *tim = PIN_MAP[pin].timer_device;
    configTimeBase(tim, period,  freq);
    if(_mode == BOARD_PWM_BRUSHED) tim->state->freq_scale=1;
}




bool RCOutput::is_servo_enabled(uint8_t ch){
    if(ch>=F4Light_OUT_CHANNELS){ // servos
        uint8_t sn = ch - F4Light_OUT_CHANNELS;
        if(!(_servo_mask & (1<<sn)) ) return false;
    }

    return true;
}

// for Oneshot125 
// [1000;2000] => [125;250]
// so frequency of timers should be 8 times more - 16MHz, but timers on 84MHz can give only 16.8MHz

// channels 1&2, 3&4&5&6 can has a different rates
void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) {
    uint32_t mask=1;
    
    uint16_t freq = freq_hz;
    
    for(uint8_t i=0; i< F4Light_MAX_OUTPUT_CHANNELS; i++) { // кто последний тот и папа
        if(chmask & mask) {
            if(!(_enabled_channels & mask) ) return;      // not enabled

// for true one-shot        if(_once_mode && freq_hz>50) continue; // no frequency in OneShoot modes
            _freq[i] = freq_hz;
            
            if(_once_mode && freq_hz>50) freq = freq_hz / 2; // divide frequency by 2 in OneShoot modes
            const uint8_t pin = output_channels[i];
            const timer_dev *dev = PIN_MAP[pin].timer_device;
            timer_set_reload(dev,  _timer_period(freq, dev)); 
        }
        mask <<= 1;
    }
}

void RCOutput::init_channel(uint8_t ch){
    if(ch>=F4Light_MAX_OUTPUT_CHANNELS) return;

    uint8_t pin = output_channels[ch];
    if (pin >= BOARD_NR_GPIO_PINS) return;

    const stm32_pin_info &p = PIN_MAP[pin];
    const timer_dev *dev = p.timer_device;

    timer_set_mode(   dev, p.timer_channel, TIMER_PWM);

    uint16_t freq = _freq[ch];
    if(_once_mode && freq>50) freq/=2;
    timer_set_reload(dev,  _timer_period(freq, dev));
    if(_once_mode) {
        timer_set_compare(dev, p.timer_channel, 0); // to prevent outputs in case of timer overflow
    }
}


void RCOutput::init_channels(){
    for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
        init_channel(ch);
    }
}

/* constrain pwm to be between min and max pulsewidth */
static inline uint16_t constrain_period(uint16_t p) {
    if (p > RC_INPUT_MAX_PULSEWIDTH) return RC_INPUT_MAX_PULSEWIDTH;
    if (p < RC_INPUT_MIN_PULSEWIDTH) return RC_INPUT_MIN_PULSEWIDTH;
    return p;
}

void RCOutput::set_pwm(uint8_t ch, uint16_t pwm){
    
    if(ch>=F4Light_MAX_OUTPUT_CHANNELS) return;

    if (!(_enabled_channels & _BV(ch))) return;      // not enabled

    if(!is_servo_enabled(ch)) return; // disabled servo

    uint8_t pin = output_channels[ch];
    if (pin >= BOARD_NR_GPIO_PINS) return;

    switch(_mode){
    case BOARD_PWM_BRUSHED:
        pwm -= 1000; // move from 1000..2000 to 0..1000
        break;

    case BOARD_PWM_ONESHOT42: // works at single freq
    case BOARD_PWM_ONESHOT125:
        break;

    default:
        pwm <<= 1; // frequency of timers 2MHz
        break;
    }

    const stm32_pin_info &p = PIN_MAP[pin];
    const timer_dev *dev = p.timer_device;

    pwm *= dev->state->freq_scale; // take into account the inaccuracy of setting the timer frequency for small prescalers
    timer_set_compare(dev, p.timer_channel, pwm); 
}

void RCOutput::write(uint8_t ch, uint16_t period_us)
{
    if(ch>=F4Light_MAX_OUTPUT_CHANNELS) return;

    if(_used_channels<ch) _used_channels=ch+1;
    
    uint16_t pwm = constrain_period(period_us);

    if(_period[ch]==pwm) return; // already so

    _period[ch]=pwm;
    
    if(_corked) return;

    set_pwm(ch, pwm);
}



void RCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)
{
    if(ch>=F4Light_MAX_OUTPUT_CHANNELS) return;

    for (int i = 0; i < len; i++) {
        write(i + ch, period_us[i]);
    }
}

uint16_t RCOutput::read(uint8_t ch)
{
    if(ch>=F4Light_MAX_OUTPUT_CHANNELS) return RC_INPUT_MIN_PULSEWIDTH;

    return _period[ch];
}

void RCOutput::read(uint16_t* period_us, uint8_t len)
{// here we don't need to limit channel count - all unsupported will be read as RC_INPUT_MIN_PULSEWIDT
    for (int i = 0; i < len; i++) {
        period_us[i] = read(i);
    }
}

void RCOutput::enable_ch(uint8_t ch)
{
    if (ch >= F4Light_MAX_OUTPUT_CHANNELS) 
        return;

    if(_enabled_channels & (1U<<ch) ) return; // already OK

    if(!is_servo_enabled(ch)) return; // disabled servo
            
    _enabled_channels |= (1U<<ch);
    if (_period[ch] == PWM_IGNORE_THIS_CHANNEL) {
        _period[ch] = 0;
    }
    
    if(!_initialized[ch]) {
    
        uint8_t pin = output_channels[ch];

        _set_pin_output_mode(ch);
        GPIO::_pinMode(pin, PWM);
        init_channel(ch);
        
        const timer_dev *dev = PIN_MAP[pin].timer_device;

        timer_resume(dev);
    
        _initialized[ch]=true;
    }

    fill_timers(); // re-calculate list of used timers

#ifdef DEBUG_INT
    uint8_t spin = output_channels[DEBUG_INT]; // motor 6
    GPIO::_pinMode(spin, INPUT_PULLUP);
#endif
    
}

void RCOutput::disable_ch(uint8_t ch)
{
    if (ch >= F4Light_MAX_OUTPUT_CHANNELS) {
        return;
    }

    if(!is_servo_enabled(ch)) return; // disabled servo

    _enabled_channels &= ~(1U<<ch);
    _period[ch] = PWM_IGNORE_THIS_CHANNEL;
    

    uint8_t pin = output_channels[ch];
    if (pin >= BOARD_NR_GPIO_PINS) return;

    GPIO::_pinMode(pin, OUTPUT);
    GPIO::_write(pin, 0);

    fill_timers(); // re-calculate list of used timers
}


void RCOutput::push()
{
#ifdef DEBUG_PWM
    uint8_t spin = output_channels[DEBUG_PWM]; // motor 6 as strobe
    GPIO::_pinMode(spin, OUTPUT);
    GPIO::_write(spin, 1);
#endif
    
    for (uint16_t ch = 0; ch < _used_channels; ch++) {
        set_pwm(ch, _period[ch]);
    }

    if(_once_mode){   // generate timer's update on ALL used pins, but only once per timer

#if 1
        for(uint8_t i =0; i<num_out_timers;i++){
            timer_generate_update(out_timers[i]);
        }

        for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
            const stm32_pin_info &p = PIN_MAP[output_channels[ch]];
            timer_set_compare(p.timer_device, p.timer_channel, 0); // to prevent outputs  in case of timer overflows
        }

#else

        for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {    
            const timer_dev *tim = PIN_MAP[output_channels[ch]].timer_device;
            tim->state->update=false;  // reset flag first
        }

        for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
            if (!(_enabled_channels & _BV(ch))) continue;      // not enabled
            const timer_dev *tim = PIN_MAP[output_channels[ch]].timer_device;
    
            tim->state->update=true; // set flag for update for needed timer
        }

        for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
            const timer_dev *tim = PIN_MAP[output_channels[ch]].timer_device;
            if(tim->state->update) {
                timer_generate_update(tim);
                tim->state->update = false;    // only once
            }
        }
#endif
    }

    _corked = false;
        
#ifdef DEBUG_PWM
    GPIO::_write(spin, 0);
#endif
}