16-bit clean shift in lsp_to_lpc()
[speexdsp.git] / libspeex / stereo.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: stereo.c
3
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7    
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10    
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14    
15    - Neither the name of the Xiph.org Foundation nor the names of its
16    contributors may be used to endorse or promote products derived from
17    this software without specific prior written permission.
18    
19    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
23    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31
32 #ifdef HAVE_CONFIG_H
33 #include "config.h"
34 #endif
35
36 #include <speex/speex_stereo.h>
37 #include <speex/speex_callbacks.h>
38 #include "vq.h"
39 #include <math.h>
40
41 /*float e_ratio_quant[4] = {1, 1.26, 1.587, 2};*/
42 static const float e_ratio_quant[4] = {.25f, .315f, .397f, .5f};
43
44 void speex_encode_stereo(float *data, int frame_size, SpeexBits *bits)
45 {
46    int i, tmp;
47    float e_left=0, e_right=0, e_tot=0;
48    float balance, e_ratio;
49    for (i=0;i<frame_size;i++)
50    {
51       e_left  += ((float)data[2*i])*data[2*i];
52       e_right += ((float)data[2*i+1])*data[2*i+1];
53       data[i] =  .5*(((float)data[2*i])+data[2*i+1]);
54       e_tot   += ((float)data[i])*data[i];
55    }
56    balance=(e_left+1)/(e_right+1);
57    e_ratio = e_tot/(1+e_left+e_right);
58
59    /*Quantization*/
60    speex_bits_pack(bits, 14, 5);
61    speex_bits_pack(bits, SPEEX_INBAND_STEREO, 4);
62    
63    balance=4*log(balance);
64
65    /*Pack sign*/
66    if (balance>0)
67       speex_bits_pack(bits, 0, 1);
68    else
69       speex_bits_pack(bits, 1, 1);
70    balance=floor(.5+fabs(balance));
71    if (balance>30)
72       balance=31;
73    
74    speex_bits_pack(bits, (int)balance, 5);
75    
76    /*Quantize energy ratio*/
77    tmp=vq_index(&e_ratio, e_ratio_quant, 1, 4);
78    speex_bits_pack(bits, tmp, 2);
79 }
80
81 void speex_encode_stereo_int(spx_int16_t *data, int frame_size, SpeexBits *bits)
82 {
83    int i, tmp;
84    float e_left=0, e_right=0, e_tot=0;
85    float balance, e_ratio;
86    for (i=0;i<frame_size;i++)
87    {
88       e_left  += ((float)data[2*i])*data[2*i];
89       e_right += ((float)data[2*i+1])*data[2*i+1];
90       data[i] =  .5*(((float)data[2*i])+data[2*i+1]);
91       e_tot   += ((float)data[i])*data[i];
92    }
93    balance=(e_left+1)/(e_right+1);
94    e_ratio = e_tot/(1+e_left+e_right);
95
96    /*Quantization*/
97    speex_bits_pack(bits, 14, 5);
98    speex_bits_pack(bits, SPEEX_INBAND_STEREO, 4);
99    
100    balance=4*log(balance);
101
102    /*Pack sign*/
103    if (balance>0)
104       speex_bits_pack(bits, 0, 1);
105    else
106       speex_bits_pack(bits, 1, 1);
107    balance=floor(.5+fabs(balance));
108    if (balance>30)
109       balance=31;
110    
111    speex_bits_pack(bits, (int)balance, 5);
112    
113    /*Quantize energy ratio*/
114    tmp=vq_index(&e_ratio, e_ratio_quant, 1, 4);
115    speex_bits_pack(bits, tmp, 2);
116 }
117
118 void speex_decode_stereo(float *data, int frame_size, SpeexStereoState *stereo)
119 {
120    float balance, e_ratio;
121    int i;
122    float e_tot=0, e_left, e_right, e_sum;
123
124    balance=stereo->balance;
125    e_ratio=stereo->e_ratio;
126    for (i=frame_size-1;i>=0;i--)
127    {
128       e_tot += ((float)data[i])*data[i];
129    }
130    e_sum=e_tot/e_ratio;
131    e_left  = e_sum*balance / (1+balance);
132    e_right = e_sum-e_left;
133
134    e_left  = sqrt(e_left/(e_tot+.01));
135    e_right = sqrt(e_right/(e_tot+.01));
136
137    for (i=frame_size-1;i>=0;i--)
138    {
139       float ftmp=data[i];
140       stereo->smooth_left  = .98*stereo->smooth_left  + .02*e_left;
141       stereo->smooth_right = .98*stereo->smooth_right + .02*e_right;
142       data[2*i] = stereo->smooth_left*ftmp;
143       data[2*i+1] = stereo->smooth_right*ftmp;
144    }
145 }
146
147 void speex_decode_stereo_int(spx_int16_t *data, int frame_size, SpeexStereoState *stereo)
148 {
149    float balance, e_ratio;
150    int i;
151    float e_tot=0, e_left, e_right, e_sum;
152
153    balance=stereo->balance;
154    e_ratio=stereo->e_ratio;
155    for (i=frame_size-1;i>=0;i--)
156    {
157       e_tot += ((float)data[i])*data[i];
158    }
159    e_sum=e_tot/e_ratio;
160    e_left  = e_sum*balance / (1+balance);
161    e_right = e_sum-e_left;
162
163    e_left  = sqrt(e_left/(e_tot+.01));
164    e_right = sqrt(e_right/(e_tot+.01));
165
166    for (i=frame_size-1;i>=0;i--)
167    {
168       float ftmp=data[i];
169       stereo->smooth_left  = .98*stereo->smooth_left  + .02*e_left;
170       stereo->smooth_right = .98*stereo->smooth_right + .02*e_right;
171       data[2*i] = stereo->smooth_left*ftmp;
172       data[2*i+1] = stereo->smooth_right*ftmp;
173    }
174 }
175
176 int speex_std_stereo_request_handler(SpeexBits *bits, void *state, void *data)
177 {
178    SpeexStereoState *stereo;
179    float sign=1;
180    int tmp;
181
182    stereo = (SpeexStereoState*)data;
183    if (speex_bits_unpack_unsigned(bits, 1))
184       sign=-1;
185    tmp = speex_bits_unpack_unsigned(bits, 5);
186    stereo->balance = exp(sign*.25*tmp);
187
188    tmp = speex_bits_unpack_unsigned(bits, 2);
189    stereo->e_ratio = e_ratio_quant[tmp];
190
191    return 0;
192 }