#include "generation.h"

/* Default values of client's registers
 * f1: 16kHz
 * D1: 49.2%
 * Ph2: 0.0
 * D2: 49.2%
 * Ph3: 0.0
 * f3: 1.15MHz
 * N3: 10 impules
 * D3: 4%
 */
void init_params(tweaked_params_s * pParams) {
    pParams->f1 = 16000;
    pParams->D1 = 0.492 * FXP_SCALING;//49.2 %
    pParams->D2 = 0.492 * FXP_SCALING;//49.2 %

    pParams->Ph2 = 0.00 * FXP_SCALING;

    pParams->Ph3 = 0.00 * FXP_SCALING;
    pParams->f3 = 1150000;
    pParams->D3 = 40;//4%
    pParams->N3 = 10;

    pParams->ena = ENABLE_FORCE_NONE;

    pParams->_period1 = DIV_ROUND(BASE_FREQ, pParams->f1);
    pParams->_duty1 = pParams->_period1 * pParams->D1 / FXP_SCALING;
    pParams->_duty2 = pParams->_period1 * pParams->D2 / FXP_SCALING;
    pParams->_phase2 = pParams->Ph2 * pParams->_period1 / FXP_SCALING;
    pParams->_period3 = DIV_ROUND(BASE_FREQ, pParams->f3);
}

uint32_t validate_generation_values(tweaked_params_s * pParams)
{
    if (pParams->f1 < MIN_FREQ1 || pParams->f1 > MAX_FREQ1)
        return -1;

    if (pParams->D1 < MIN_D1D2 || pParams->D1 > MAX_D1D2)
        return -1;

    // TODO: missing ph2 restrictions?
    if (pParams->D2 < MIN_D1D2 || pParams->D2 > MAX_D1D2)
        return -1;

    if (pParams->Ph3 > MAX_PH3)
        return -1;

    if (pParams->f3 < MIN_FREQ3 || pParams->f3 > MAX_FREQ3)
        return -1;

    // TODO: missing restrictions for f3 and D3

    return 0;
}

void update_three_signal_values(tweaked_params_s * pParams, wb_mailbox_button_regs_t * pButtons)
{
    // TODO: I assume here that value of 1 means one button press - how to interpret long presses?
    // Optimization - we only call delta - increment and decrement
    int32_t delta = pButtons->regs.inc_f1 - pButtons->regs.dec_f1;
    if (delta > 0)
        inc_f1(pParams, delta);
    else if (delta < 0)
        dec_f1(pParams, -delta);

    // TODO: add handling of the rest
    //delta = pButtons->regs.inc_d1 - pButtons->regs.dec_d1;
}


// TODO: all of these have to be updated by a value read from the register (how to calibrate it?)
void inc_f1(tweaked_params_s * pParams, uint32_t val) {
    int32_t f1 = pParams->f1;
    int32_t _period1 = 0;

    while (val) {
        f1 += 5;
        if (f1 > MAX_FREQ1)
            return;
        _period1 = DIV_ROUND(BASE_FREQ, f1);

        uint32_t pulses_time = pParams->_period3*(pParams->N3+2)*N_PULSE_TRAINS;
        if (pulses_time > _period1)
        {
            // undo the last step
            f1 -= 5;
            _period1 = DIV_ROUND(BASE_FREQ, f1);
            break;
        }
        --val;
    }

    int32_t _duty1 = _period1*pParams->D1/4000;
    int32_t _duty2 = _period1*pParams->D2/4000;
    int32_t _phase2 = _period1*pParams->Ph2/4000;

    pParams->_duty1 = _duty1;
    pParams->_duty2 = _duty2;
    pParams->_phase2 = _phase2;
    pParams->_period1 = _period1;
    pParams->f1 = f1;
}

void dec_f1(tweaked_params_s * pParams, uint32_t val) {
    int32_t f1 = pParams->f1;
    int32_t _period1 = 0;

    while (val) {
        f1 -= 5;
        if (f1 < MIN_FREQ1)
        {
            f1 += 5;
            _period1 = DIV_ROUND(BASE_FREQ, f1);
            break;
        }
        _period1 = DIV_ROUND(BASE_FREQ, f1);

        --val;
    }

    int32_t _duty1 = _period1*pParams->D1/4000;
    int32_t _duty2 = _period1*pParams->D2/4000;
    int32_t _phase2 = _period1*pParams->Ph2/4000;

    pParams->_duty1 = _duty1;
    pParams->_duty2 = _duty2;
    pParams->_phase2 = _phase2;
    pParams->_period1 = _period1;
    pParams->f1 = f1;
}

void inc_D1(tweaked_params_s * pParams) {
    int32_t D1 = pParams->D1;

    D1++;
    if(D1>MAX_D1D2)
        return;
    int32_t _duty1 = pParams->_period1*D1/4000;
    if(_duty1+pParams->_phase2 >= pParams->_period1/2)
        return;

    pParams->D1 = D1;
    pParams->_duty1 = _duty1;
}

void dec_D1(tweaked_params_s * pParams) {
    int32_t D1 = pParams->D1;

    D1--;
    if(D1<MIN_D1D2)
        return;
    int32_t _duty1 = pParams->_period1*D1/4000;

    pParams->D1 = D1;
    pParams->_duty1 = _duty1;
}


void inc_D2(tweaked_params_s * pParams) {
    int32_t D2 = pParams->D2;

    D2++;
    if(D2>MAX_D1D2)
        return;
    int32_t _duty2 = pParams->_period1*D2/4000;

    pParams->D2 = D2;
    pParams->_duty2 = _duty2;
}

void dec_D2(tweaked_params_s * pParams) {
    int32_t D2 = pParams->D2;

    D2--;
    if(D2<MIN_D1D2)
        return;
    int32_t _duty2 = pParams->_period1*D2/4000;

    pParams->D2 = D2;
    pParams->_duty2 = _duty2;
}

void inc_Ph2(tweaked_params_s * pParams) {
    int32_t Ph2 = pParams->Ph2;

    Ph2++;
    int32_t _phase2 = Ph2*pParams->_period1/4000;
    if(pParams->_duty1+_phase2 >= pParams->_period1/2)
        return;

    pParams->Ph2 = Ph2;
    pParams->_phase2 = _phase2;
}

void dec_Ph2(tweaked_params_s * pParams) {
    int32_t Ph2 = pParams->Ph2;

    Ph2--;
    if(Ph2<0)
        return;//limit value
    int32_t _phase2 = Ph2*pParams->_period1/4000;

    pParams->Ph2 = Ph2;
    pParams->_phase2 = _phase2;
}

void inc_Ph3(tweaked_params_s * pParams) {
    int32_t Ph3 = pParams->Ph3;

    Ph3++;
    if(Ph3>MAX_PH3)
        return;//limit value

    pParams->Ph3 = Ph3;
}

void dec_Ph3(tweaked_params_s * pParams) {
    int32_t Ph3 = pParams->Ph3;

    Ph3--;
    if(Ph3<0)
        return;//limit value

    pParams->Ph3 = Ph3;
}

void inc_f3(tweaked_params_s * pParams) {
    int32_t f3 = pParams->f3;

    f3 += 10000;
    if(f3>MAX_FREQ3)
        return;
    int32_t _period3 = DIV_ROUND(BASE_FREQ, f3);

    pParams->f3 = f3;
    pParams->_period3 = _period3;
}

void dec_f3(tweaked_params_s * pParams) {
    int32_t f3 = pParams->f3;

    f3 -= 10000;
    if(f3<MIN_FREQ3)
        return;
    int32_t _period3 = DIV_ROUND(BASE_FREQ, f3);

    uint32_t pulses_time = _period3*(pParams->N3+2)*N_PULSE_TRAINS;
    if(pulses_time>pParams->_period1)
        return;

    pParams->f3 = f3;
    pParams->_period3 = _period3;
}

void inc_D3(tweaked_params_s * pParams) {
    int32_t D3 = pParams->D3;

    D3 += 5;
    if(D3>1000)
        return;

    pParams->D3 = D3;
}

void dec_D3(tweaked_params_s * pParams) {
    int32_t D3 = pParams->D3;

    D3 -= 5;
    if(D3<0)
        return;

    pParams->D3 = D3;
}

void inc_N3(tweaked_params_s * pParams) {
    int32_t N3 = pParams->N3;

    N3++;
    if(N3>254)
        return;

    uint32_t pulses_time = pParams->_period3*(N3+2)*N_PULSE_TRAINS;
    if(pulses_time>pParams->_period1)
        return;

    pParams->N3 = N3;
}

void dec_N3(tweaked_params_s * pParams) {
    int32_t N3 = pParams->N3;

    N3--;
    if(N3<1)
        return;

    pParams->N3 = N3;
}

void ena_force_toggle(tweaked_params_s * pParams) {
    enable_force_t ena = pParams->ena;

    ena = ena==ENABLE_FORCE_DISABLE?ENABLE_FORCE_ENABLE:ENABLE_FORCE_DISABLE;

    pParams->ena = ena;
}