/*
 * Project 25 IMBE Encoder/Decoder Fixed-Point implementation
 * Developed by Pavel Yazev E-mail: pyazev@gmail.com
 * Version 1.0 (c) Copyright 2009
 *
 * This 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, or (at your option)
 * any later version.
 *
 * The software 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; see the file COPYING.  If not, write to the Free
 * Software Foundation, Inc., 51 Franklin Street, Boston, MA
 * 02110-1301, USA.
 */

#include "vocoder/imbe/typedef.h"
#include "vocoder/imbe/basic_op.h"
#include "vocoder/imbe/imbe.h"
#include "vocoder/imbe/aux_sub.h"
#include "vocoder/imbe/math_sub.h"
#include "vocoder/imbe/rand_gen.h"
#include "vocoder/imbe/tbls.h"
#include "vocoder/imbe/imbe_vocoder.h"

// ---------------------------------------------------------------------------
//  Constants
// ---------------------------------------------------------------------------

#define CNST_0_1_Q1_15 0x0CCD

// ---------------------------------------------------------------------------
//  Private Class Members
// ---------------------------------------------------------------------------

void imbe_vocoder::v_synt_init(void)
{
    Word16 i;

    for (i = 0; i < NUM_HARMS_MAX; i++) {
        ph_mem[i] = L_deposit_h(rand_gen());
        vu_dsn_prev[i] = 0;
    }

    num_harms_prev3 = 0;
    fund_freq_prev = 0;
}

void imbe_vocoder::v_synt(IMBE_PARAM* imbe_param, Word16* snd)
{
    Word32 L_tmp, L_tmp1, fund_freq, L_snd[FRAME], L_ph_acc, L_ph_step;
    Word32 L_ph_acc_aux, L_ph_step_prev, L_amp_acc, L_amp_step, L_ph_step_aux;
    Word16 num_harms, i, j, *vu_dsn, *sa, *s_ptr, *s_ptr_aux, num_harms_max, num_harms_max_4;
    UWord32 ph_mem_prev[NUM_HARMS_MAX], dph[NUM_HARMS_MAX];
    Word16 num_harms_inv, num_harms_sh, num_uv;
    Word16 freq_flag;


    fund_freq = imbe_param->fund_freq;
    num_harms = imbe_param->num_harms;
    vu_dsn = imbe_param->v_uv_dsn;
    sa = imbe_param->sa;
    num_harms_inv = imbe_param->div_one_by_num_harm;
    num_harms_sh = imbe_param->div_one_by_num_harm_sh;
    num_uv = imbe_param->l_uv;

    for (i = 0; i < FRAME; i++)
        L_snd[i] = 0;

    // Update phases (calculated phase value correspond to bound of frame)
    L_tmp = (((fund_freq_prev + fund_freq) >> 7) * FRAME / 2) << 7;  // It is performed integer multiplication by mod 1

    L_ph_acc = 0;
    for (i = 0; i < NUM_HARMS_MAX; i++) {
        ph_mem_prev[i] = ph_mem[i];
        L_ph_acc += L_tmp;
        ph_mem[i] += L_ph_acc;
        dph[i] = 0;
    }

    num_harms_max = (num_harms >= num_harms_prev3) ? num_harms : num_harms_prev3;
    num_harms_max_4 = num_harms_max >> 2;

    if (L_abs(L_sub(fund_freq, fund_freq_prev)) >= L_mpy_ls(fund_freq, CNST_0_1_Q1_15))
        freq_flag = 1;
    else
        freq_flag = 0;

    L_ph_step = L_ph_step_prev = 0;
    for (i = 0; i < num_harms_max; i++) {
        L_ph_step += fund_freq;
        L_ph_step_prev += fund_freq_prev;


        if (i > num_harms_max_4) {
            if (num_uv == num_harms) {
                dph[i] = L_deposit_h(rand_gen());
            }
            else {
                L_tmp = L_mult(rand_gen(), num_harms_inv);
                dph[i] = L_shr(L_tmp, 15 - num_harms_sh) * num_uv;
            }
            ph_mem[i] += dph[i];
        }

        if (vu_dsn[i] == 0 && vu_dsn_prev[i] == 0)
            continue;

        if (vu_dsn[i] == 1 && vu_dsn_prev[i] == 0)  // unvoiced => voiced
        {
            s_ptr = (Word16*)ws;
            L_ph_acc = ph_mem[i] - (((L_ph_step >> 7) * 104) << 7);
            for (j = 56; j <= 104; j++) {
                L_tmp = L_mult(*s_ptr++, sa[i]);
                L_tmp = L_mpy_ls(L_tmp, cos_fxp(extract_h(L_ph_acc)));
                L_tmp = L_shr(L_tmp, 1);
                L_snd[j] = L_add(L_snd[j], L_tmp);
                L_ph_acc += L_ph_step;
            }

            for (j = 105; j <= 159; j++) {
                L_tmp = L_mult(sa[i], cos_fxp(extract_h(L_ph_acc)));
                L_tmp = L_shr(L_tmp, 1);
                L_snd[j] = L_add(L_snd[j], L_tmp);
                L_ph_acc += L_ph_step;
            }
            continue;
        }


        if (vu_dsn[i] == 0 && vu_dsn_prev[i] == 1)  // voiced => unvoiced
        {
            s_ptr = (Word16*)&ws[48];
            L_ph_acc = ph_mem_prev[i];

            for (j = 0; j <= 55; j++) {
                L_tmp = L_mult(sa_prev3[i], cos_fxp(extract_h(L_ph_acc)));
                L_tmp = L_shr(L_tmp, 1);
                L_snd[j] = L_add(L_snd[j], L_tmp);
                L_ph_acc += L_ph_step_prev;
            }

            for (j = 56; j <= 104; j++) {
                L_tmp = L_mult(*s_ptr--, sa_prev3[i]);
                L_tmp = L_mpy_ls(L_tmp, cos_fxp(extract_h(L_ph_acc)));
                L_tmp = L_shr(L_tmp, 1);
                L_snd[j] = L_add(L_snd[j], L_tmp);
                L_ph_acc += L_ph_step_prev;
            }
            continue;
        }

        if (i >= 7 || freq_flag) {
            s_ptr_aux = (Word16*)&ws[48];
            L_ph_acc_aux = ph_mem_prev[i];

            s_ptr = (Word16*)ws;
            L_ph_acc = ph_mem[i] - (((L_ph_step >> 7) * 104) << 7);

            for (j = 0; j <= 55; j++) {
                L_tmp = L_mult(sa_prev3[i], cos_fxp(extract_h(L_ph_acc_aux)));
                L_tmp = L_shr(L_tmp, 1);
                L_snd[j] = L_add(L_snd[j], L_tmp);
                L_ph_acc_aux += L_ph_step_prev;
            }

            for (j = 56; j <= 104; j++) {
                L_tmp = L_mult(*s_ptr_aux--, sa_prev3[i]);
                L_tmp = L_mpy_ls(L_tmp, cos_fxp(extract_h(L_ph_acc_aux)));
                L_tmp = L_shr(L_tmp, 1);
                L_snd[j] = L_add(L_snd[j], L_tmp);

                L_tmp = L_mult(*s_ptr++, sa[i]);
                L_tmp = L_mpy_ls(L_tmp, cos_fxp(extract_h(L_ph_acc)));
                L_tmp = L_shr(L_tmp, 1);
                L_snd[j] = L_add(L_snd[j], L_tmp);

                L_ph_acc_aux += L_ph_step_prev;
                L_ph_acc += L_ph_step;
            }

            for (j = 105; j <= 159; j++) {
                L_tmp = L_mult(sa[i], cos_fxp(extract_h(L_ph_acc)));
                L_tmp = L_shr(L_tmp, 1);
                L_snd[j] = L_add(L_snd[j], L_tmp);
                L_ph_acc += L_ph_step;
            }
            continue;
        }

        L_amp_step = L_mpy_ls(L_shr(L_deposit_h(sub(sa[i], sa_prev3[i])), 4 + 1), CNST_0_1_Q1_15); // (sa[i] - sa_prev3[i]) / 160, 1/160 = 0.1/16 
        L_amp_acc = L_shr(L_deposit_h(sa_prev3[i]), 1);


        L_ph_step_aux = L_mpy_ls(L_shr(fund_freq - fund_freq_prev, 4 + 1), CNST_0_1_Q1_15);       // (fund_freq - fund_freq_prev)/(2*160)
        L_ph_step_aux = ((L_ph_step_aux >> 7) * (i + 1)) << 7;

        L_ph_acc = ph_mem_prev[i];

        L_tmp1 = L_mpy_ls(L_shr(dph[i], 4), CNST_0_1_Q1_15);  // dph[i] / 160

        for (j = 0; j < 160; j++) {
            L_ph_acc_aux = ((L_ph_step_aux >> 9) * j) << 9;
            L_ph_acc_aux = ((L_ph_acc_aux >> 9) * j) << 9;

            L_tmp = L_mpy_ls(L_amp_acc, cos_fxp(extract_h(L_ph_acc + L_ph_acc_aux)));
            L_snd[j] = L_add(L_snd[j], L_tmp);

            L_amp_acc = L_add(L_amp_acc, L_amp_step);
            L_ph_acc += L_ph_step_prev;
            L_ph_acc += L_tmp1;
        }
    }

    for (i = 0; i < FRAME; i++)
        *snd++ = extract_h(L_snd[i]);

    v_zap(vu_dsn_prev, NUM_HARMS_MAX);
    v_equ(vu_dsn_prev, imbe_param->v_uv_dsn, num_harms);
    v_equ(sa_prev3, imbe_param->sa, num_harms);

    num_harms_prev3 = num_harms;
    fund_freq_prev = fund_freq;
}
