/*
 * $Id: LowcFE.cpp,v 1.2 2004/06/29 09:45:54 rco Exp $
 *
 * Copyright (C) 2002-2003 Fhg Fokus
 *
 * This file is part of sems, a free SIP media server.
 *
 * sems 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 2 of the License, or
 * (at your option) any later version.
 *
 * For a license to use the sems software under conditions
 * other than those described here, or to purchase support for this
 * software, please contact iptel.org by e-mail at the following addresses:
 *    info@iptel.org
 *
 * sems 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, write to the Free Software 
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "LowcFE.h"

#include <math.h>
#include <stdio.h>
#include <stdlib.h>

void LowcFE::convertsf(short *f, Float *t, int cnt)
{
    for (int i = 0; i < cnt; i++)
	t[i] = (Float)f[i];
}

void LowcFE::convertfs(Float *f, short *t, int cnt)
{
    for (int i = 0; i < cnt; i++){
	t[i] = (short)f[i];
    }
}

void LowcFE::copyf(Float *f, Float *t, int cnt)
{
    for (int i = 0; i < cnt; i++)
	t[i] = f[i];
}

void LowcFE::copys(short *f, short *t, int cnt)
{
    for (int i = 0; i < cnt; i++)
	t[i] = f[i];
}

void LowcFE::zeros(short *s, int cnt)
{
    for (int i = 0; i < cnt; i++)
	s[i] = 0;
}

LowcFE::LowcFE()
    : erasecnt(0), pitchbufend(0)
{
    zeros(history, HISTORYLEN);
}

/*
 * Save a frames worth of new speech in the history buffer.
 * Return the output speech delayed by POVERLAPMAX.
 */
void LowcFE::savespeech(short *s)
{
    /* make room for new signal */
    copys(&history[FRAMESZ], history, HISTORYLEN - FRAMESZ);
    /* copy in the new frame */
    copys(s, &history[HISTORYLEN - FRAMESZ], FRAMESZ);
    /* copy out the delayed frame */
    copys(&history[HISTORYLEN - FRAMESZ - POVERLAPMAX], s, FRAMESZ);
}

/*
 * A good frame was received and decoded.
 * If right after an erasure, do an overlap add with the synthetic signal.
 * Add the frame to history buffer.
 */
void LowcFE::addtohistory(short *s)
{
    if (erasecnt) {
	short overlapbuf[FRAMESZ];
	/*
	 * longer erasures require longer overlaps
	 * to smooth the transition between the synthetic
	 * and real signal.
	 */
	int olen = poverlap + (erasecnt - 1) * EOVERLAPINCR;
	if (olen > FRAMESZ)
	    olen = FRAMESZ;
	getfespeech(overlapbuf, olen);
	overlapaddatend(s, overlapbuf, olen);
	erasecnt = 0;
    }
    savespeech(s);
}

/*
 * Generate the synthetic signal.
 * At the beginning of an erasure determine the pitch, and extract
 * one pitch period from the tail of the signal. Do an OLA for 1/4
 * of the pitch to smooth the signal. Then repeat the extracted signal
 * for the length of the erasure. If the erasure continues for more than
 * 10 ms, increase the number of periods in the pitchbuffer. At the end
 * of an erasure, do an OLA with the start of the first good frame.
 * The gain decays as the erasure gets longer.
 */
void LowcFE::dofe(short *out)
{
    pitchbufend = &pitchbuf[HISTORYLEN];

    if (erasecnt == 0) {
 	convertsf(history, pitchbuf, HISTORYLEN); /* get history */
	pitch = findpitch(); /* find pitch */
	poverlap = pitch >> 2; /* OLA 1/4 wavelength */
	/* save original last poverlap samples */
	copyf(pitchbufend - poverlap, lastq, poverlap);
	poffset = 0; /* create pitch buffer with 1 period */
	pitchblen = pitch;
	pitchbufstart = pitchbufend - pitchblen;
	overlapadd(lastq, pitchbufstart - poverlap,
		   pitchbufend - poverlap, poverlap);
	/* update last 1/4 wavelength in history buffer */
	convertfs(pitchbufend - poverlap, &history[HISTORYLEN-poverlap],
		  poverlap);
	getfespeech(out, FRAMESZ); /* get synthesized speech */
    } else if (erasecnt == 1 || erasecnt == 2) {
	/* tail of previous pitch estimate */
	short tmp[POVERLAPMAX];
	int saveoffset = poffset; /* save offset for OLA */
	getfespeech(tmp, poverlap); /* continue with old pitchbuf */
	/* add periods to the pitch buffer */
	poffset = saveoffset;
	while (poffset > pitch)
	    poffset -= pitch;
	pitchblen += pitch; /* add a period */
	pitchbufstart = pitchbufend - pitchblen;
	overlapadd(lastq, pitchbufstart - poverlap,
		   pitchbufend - poverlap, poverlap);
	/* overlap add old pitchbuffer with new */
	getfespeech(out, FRAMESZ);
	overlapadd(tmp, out, out, poverlap);
	scalespeech(out);
     } else if (erasecnt > 5) {
 	zeros(out, FRAMESZ);
    } else {
	getfespeech(out, FRAMESZ);
	scalespeech(out);
    }
    erasecnt++;
    savespeech(out);
}

/*
 * Estimate the pitch.
 * l - pointer to first sample in last 20 msec of speech.
 * r - points to the sample PITCH_MAX before l
 */
int LowcFE::findpitch()
{
    int i, j, k;
    int bestmatch;
    Float bestcorr;
    Float corr; /* correlation */
    Float energy; /* running energy */
    Float scale; /* scale correlation by average power */
    Float *rp; /* segment to match */
    Float *l = pitchbufend - CORRLEN;
    Float *r = pitchbufend - CORRBUFLEN;

    /* coarse search */
    rp = r;
    energy = 0.f;
    corr = 0.f;
    for (i = 0; i < CORRLEN; i += NDEC) {
	energy += rp[i] * rp[i];
	corr += rp[i] * l[i];
    }
    scale = energy;
    if (scale < CORRMINPOWER){
	scale = CORRMINPOWER;
    }
    corr = corr / (Float)sqrt(scale);
    bestcorr = corr;
    bestmatch = 0;
    for (j = NDEC; j <= PITCHDIFF; j += NDEC) {
	energy -= rp[0] * rp[0];
	energy += rp[CORRLEN] * rp[CORRLEN];
	rp += NDEC;
	corr = 0.f;
	for (i = 0; i < CORRLEN; i += NDEC)
	    corr += rp[i] * l[i];
	scale = energy;
	if (scale < CORRMINPOWER)
	    scale = CORRMINPOWER;
	corr /= (Float)sqrt(scale);
	if (corr >= bestcorr) {
	    bestcorr = corr;
	    bestmatch = j;
	}
    }
    /* fine search */
    j = bestmatch - (NDEC - 1);
    if (j < 0)
	j = 0;
    k = bestmatch + (NDEC - 1);
    if (k > PITCHDIFF)
	k = PITCHDIFF;
    rp = &r[j];
    energy = 0.f;
    corr = 0.f;
    for (i = 0; i < CORRLEN; i++) {
	energy += rp[i] * rp[i];
	corr += rp[i] * l[i];
    }
    scale = energy;
    if (scale < CORRMINPOWER)
	scale = CORRMINPOWER;

    corr = corr / (Float)sqrt(scale);
    bestcorr = corr;
    bestmatch = j;
    for (j++; j <= k; j++) {
	energy -= rp[0] * rp[0];
	energy += rp[CORRLEN] * rp[CORRLEN];
	rp++;
	corr = 0.f;
	for (i = 0; i < CORRLEN; i++)
	    corr += rp[i] * l[i];
	scale = energy;
	if (scale < CORRMINPOWER)
	    scale = CORRMINPOWER;
	corr = corr / (Float)sqrt(scale);
	if (corr > bestcorr) {
	    bestcorr = corr;
	    bestmatch = j;
	}
    }
    return PITCH_MAX - bestmatch;
}

/*
 * Get samples from the circular pitch buffer. Update poffset so
 * when subsequent frames are erased the signal continues.
 */
void LowcFE::getfespeech(short *out, int sz)
{
    while (sz) {
	int cnt = pitchblen - poffset;
	if (cnt > sz)
	    cnt = sz;
	convertfs(&pitchbufstart[poffset], out, cnt);
	poffset += cnt;
	if (poffset == pitchblen)
	    poffset = 0;
	out += cnt;
	sz -= cnt;
    }
}

void LowcFE::scalespeech(short *out)
{
      Float g = (Float)1. - (erasecnt - 1) * ATTENFAC;
      for (int i = 0; i < FRAMESZ; i++) {
  	out[i] = (short)(out[i] * g);
  	g -= ATTENINCR;
      }
}

/*
 * Overlap add left and right sides
 */
void LowcFE::overlapadd(Float *l, Float *r, Float *o, int cnt)
{
    Float incr = (Float)1. / cnt;
    Float lw = (Float)1. - incr;
    Float rw = incr;
    for (int i = 0; i < cnt; i++) {
	Float t = lw * l[i] + rw * r[i];
	if (t > 32767.)
	    t = 32767.;
	else if (t < -32768.)
	    t = -32768.;
	o[i] = t;
	lw -= incr;
	rw += incr;
    }
}

void LowcFE::overlapadd(short *l, short *r, short *o, int cnt)
{
    Float incr = (Float)1. / cnt;
    Float lw = (Float)1. - incr;
    Float rw = incr;
    for (int i = 0; i < cnt; i++) {
	Float t = lw * l[i] + rw * r[i];
	if (t > 32767.)
	    t = 32767.;
	else if (t < -32768.)
	    t = -32768.;
	o[i] = (short)t;
	lw -= incr;
	rw += incr;
    }
}

/*
 * Overlap add the erasure tail with the start of the first good frame
 * Scale the synthetic speech by the gain factor before the OLA.
 */
void LowcFE::overlapaddatend(short *s, short *f, int cnt)
{
    Float incr = (Float)1. / cnt;
    Float gain = (Float)1. - (erasecnt - 1) * ATTENFAC;
    if (gain < 0.)
	gain = (Float)0.;
    Float incrg = incr * gain;
    Float lw = ((Float)1. - incr) * gain;
    Float rw = incr;
    for (int i = 0; i < cnt; i++) {
	Float t = lw * f[i] + rw * s[i];
	if (t > 32767.)
	    t = 32767.;
	else if (t < -32768.)
	    t = -32768.;
	s[i] = (short)t;
	lw -= incrg;
	rw += incr;
    }
}