#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 obtain_button_deltas(volatile wb_mailbox_button_regs_t * pCurrentButtons, button_regs_t * pOldButtons, button_deltas_t * pDeltas)
{
    // differences will fit in the int32_t and then in int16_t
    int32_t inc_delta, dec_delta;
    int32_t inc_reg, dec_reg;

    #define GET_DELTA(REG_I, REG_D, RESULT) {\
        /* get reg */\
        inc_reg = pCurrentButtons->regs.REG_I;\
        dec_reg = pCurrentButtons->regs.REG_D;\
        \
        /* calc new press */\
        inc_delta = inc_reg - pOldButtons->REG_I;\
        dec_delta = dec_reg - pOldButtons->REG_D;\
        \
        /* save reg */\
        pOldButtons->REG_I = inc_reg;\
        pOldButtons->REG_D = dec_reg;\
        \
        RESULT = inc_delta - dec_delta;\
    }

    GET_DELTA(inc_f1 , dec_f1 , pDeltas->dt_f1 );
    GET_DELTA(inc_d1 , dec_d1 , pDeltas->dt_d1 );
    GET_DELTA(inc_d2 , dec_d2 , pDeltas->dt_d2 );
    GET_DELTA(inc_ph2, dec_ph2, pDeltas->dt_ph2);
    GET_DELTA(inc_ph3, dec_ph3, pDeltas->dt_ph3);
    GET_DELTA(inc_f3 , dec_f3 , pDeltas->dt_f3 );
    GET_DELTA(inc_d3 , dec_d3 , pDeltas->dt_d3 );
    GET_DELTA(inc_n3 , dec_n3 , pDeltas->dt_n3 );
}

void update_three_signal_values(tweaked_params_s * pParams, button_deltas_t * pDeltas)
{
    // 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
    if (pDeltas->dt_f1 > 0)
        inc_f1(pParams, pDeltas->dt_f1);
    else if (pDeltas->dt_f1 < 0)
        dec_f1(pParams, pDeltas->dt_f1);

    if (pDeltas->dt_d1 > 0)
        inc_D1(pParams, pDeltas->dt_d1);
    else if (pDeltas->dt_d1 < 0)
        dec_D1(pParams, pDeltas->dt_d1);

    if (pDeltas->dt_d2 > 0)
        inc_D2(pParams, pDeltas->dt_d2);
    else if (pDeltas->dt_d2 < 0)
        dec_D2(pParams, pDeltas->dt_d2);

    if (pDeltas->dt_ph2 > 0)
        inc_Ph2(pParams, pDeltas->dt_ph2);
    else if (pDeltas->dt_ph2 < 0)
        dec_Ph2(pParams, pDeltas->dt_ph2);

    if (pDeltas->dt_ph3 > 0)
        inc_Ph3(pParams, pDeltas->dt_ph3);
    else if (pDeltas->dt_ph3 < 0)
        dec_Ph3(pParams, pDeltas->dt_ph3);

    if (pDeltas->dt_f3 > 0)
        inc_f3(pParams, pDeltas->dt_f3);
    else if (pDeltas->dt_f3 < 0)
        dec_f3(pParams, pDeltas->dt_f3);

    if (pDeltas->dt_d3 > 0)
        inc_D3(pParams, pDeltas->dt_d3);
    else if (pDeltas->dt_d3 < 0)
        dec_D3(pParams, pDeltas->dt_d3);

    if (pDeltas->dt_n3 > 0)
        inc_N3(pParams, pDeltas->dt_n3);
    else if (pDeltas->dt_n3 < 0)
        dec_N3(pParams, pDeltas->dt_n3);
}


void inc_f1(tweaked_params_s * pParams, int16_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/FXP_SCALING;
    int32_t _duty2 = _period1*pParams->D2/FXP_SCALING;
    int32_t _phase2 = _period1*pParams->Ph2/FXP_SCALING;

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

void dec_f1(tweaked_params_s * pParams, int16_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/FXP_SCALING;
    int32_t _duty2 = _period1*pParams->D2/FXP_SCALING;
    int32_t _phase2 = _period1*pParams->Ph2/FXP_SCALING;

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

void inc_D1(tweaked_params_s * pParams, int16_t val) {
    int32_t D1 = pParams->D1;
    int32_t _duty1 = pParams->_duty1;

    while (val) {
        D1++;
        if (D1 > MAX_D1D2)
            return;
        _duty1 = pParams->_period1*D1/FXP_SCALING;
        if (_duty1 + pParams->_phase2 >= pParams->_period1/2)
        {
            // undo the last step
            D1--;
            _duty1 = pParams->_period1*D1/FXP_SCALING;
            break;
        }
        --val;
    }

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

void dec_D1(tweaked_params_s * pParams, int16_t val) {
    int32_t D1 = pParams->D1;
    int32_t _duty1 = pParams->_duty1;

    while (val) {
        D1--;
        if (D1 < MIN_D1D2)
            return;
        _duty1 = pParams->_period1*D1/FXP_SCALING;
        --val;
    }

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


void inc_D2(tweaked_params_s * pParams, int16_t val) {
    int32_t D2 = pParams->D2;
    int32_t _duty2 = pParams->_duty2;

    while (val) {
        D2++;
        if (D2 > MAX_D1D2)
            return;
        _duty2 = pParams->_period1*D2/FXP_SCALING;
        --val;
    }

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

void dec_D2(tweaked_params_s * pParams, int16_t val) {
    int32_t D2 = pParams->D2;
    int32_t _duty2 = pParams->_duty2;

    while (val) {
        D2--;
        if (D2 < MIN_D1D2)
            return;
        _duty2 = pParams->_period1*D2/FXP_SCALING;
        --val;
    }

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

void inc_Ph2(tweaked_params_s * pParams, int16_t val) {
    int32_t Ph2 = pParams->Ph2;
    int32_t _phase2 = pParams->_phase2;

    while (val) {
        Ph2++;
        _phase2 = Ph2*pParams->_period1/FXP_SCALING;
        if (pParams->_duty1 + _phase2 >= pParams->_period1/2)
        {
            // undo the last step
            Ph2--;
            _phase2 = Ph2*pParams->_period1/FXP_SCALING;
            break;
        }
        --val;
    }

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

void dec_Ph2(tweaked_params_s * pParams, int16_t val) {
    int32_t Ph2 = pParams->Ph2;
    int32_t _phase2 = pParams->_phase2;

    while (val) {
        Ph2--;
        if (Ph2 < 0)
            return;
        _phase2 = Ph2*pParams->_period1/FXP_SCALING;
        --val;
    }

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

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

    while (val) {
        Ph3++;
        if (Ph3 > MAX_PH3)
            return;
        --val;
    }

    pParams->Ph3 = Ph3;
}

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

    while (val) {
        Ph3--;
        if (Ph3 < 0)
            return;
        --val;
    }

    pParams->Ph3 = Ph3;
}

void inc_f3(tweaked_params_s * pParams, int16_t val) {
    int32_t f3 = pParams->f3;
    int32_t _period3 = 0;

    while (val) {
        f3 += 10000;
        if (f3 > MAX_FREQ3)
            return;
        _period3 = DIV_ROUND(BASE_FREQ, f3);
        --val;
    }

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

void dec_f3(tweaked_params_s * pParams, int16_t val) {
    int32_t f3 = pParams->f3;
    int32_t _period3 = 0;

    while (val) {
        f3 -= 10000;
        if (f3 < MIN_FREQ3)
            return;
        _period3 = DIV_ROUND(BASE_FREQ, f3);

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

        --val;
    }

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

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

    while (val) {
        D3 += 5;
        if (D3 > 1000)
            return;
        --val;
    }

    pParams->D3 = D3;
}

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

    while (val) {
        D3 -= 5;
        if (D3 < 0)
            return;
        --val;
    }

    pParams->D3 = D3;
}

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

    while (val) {
        N3++;
        if (N3 > 254)
            return;

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

        --val;
    }

    pParams->N3 = N3;
}

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

    while (val) {
        N3--;
        if (N3 < 1)
            return;

        --val;
    }

    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;
}