1/*****************************************************************************\
2 Snes9x - Portable Super Nintendo Entertainment System (TM) emulator.
3 This file is licensed under the Snes9x License.
4 For further information, consult the LICENSE file in the root directory.
5\*****************************************************************************/
6
7#ifndef __NEW_RESAMPLER_H
8#define __NEW_RESAMPLER_H
9
10#include <cstring>
11#include <cassert>
12#if __cplusplus >= 201103L
13#include <cstdint>
14#else
15#include <stdint.h>
16#endif
17#include <cmath>
18
19class Resampler
20{
21 public:
22 int size;
23 int buffer_size;
24 int start;
25 int16_t *buffer;
26
27 float r_step;
28 float r_frac;
29 int r_left[4], r_right[4];
30
31 static inline int16_t short_clamp(int n)
32 {
33 return (int16_t)(((int16_t)n != n) ? (n >> 31) ^ 0x7fff : n);
34 }
35
36 static inline int min(int a, int b)
37 {
38 return ((a) < (b) ? (a) : (b));
39 }
40
41 static inline float hermite(float mu1, float a, float b, float c, float d)
42 {
43 float mu2, mu3, m0, m1, a0, a1, a2, a3;
44
45 mu2 = mu1 * mu1;
46 mu3 = mu2 * mu1;
47
48 m0 = (c - a) * 0.5;
49 m1 = (d - b) * 0.5;
50
51 a0 = +2 * mu3 - 3 * mu2 + 1;
52 a1 = mu3 - 2 * mu2 + mu1;
53 a2 = mu3 - mu2;
54 a3 = -2 * mu3 + 3 * mu2;
55
56 return (a0 * b) + (a1 * m0) + (a2 * m1) + (a3 * c);
57 }
58
59 Resampler()
60 {
61 this->buffer_size = 0;
62 buffer = NULL;
63 r_step = 1.0;
64 }
65
66 Resampler(int num_samples)
67 {
68 this->buffer_size = num_samples;
69 buffer = new int16_t[this->buffer_size];
70 r_step = 1.0;
71 clear();
72 }
73
74 ~Resampler()
75 {
76 delete[] buffer;
77 buffer = NULL;
78 }
79
80 inline void time_ratio(double ratio)
81 {
82 r_step = ratio;
83 }
84
85 inline void clear(void)
86 {
87 if (!buffer)
88 return;
89
90 start = 0;
91 size = 0;
92 memset(buffer, 0, buffer_size * 2);
93
94 r_frac = 0.0;
95 r_left[0] = r_left[1] = r_left[2] = r_left[3] = 0;
96 r_right[0] = r_right[1] = r_right[2] = r_right[3] = 0;
97 }
98
99 inline bool pull(int16_t *dst, int num_samples)
100 {
101 if (space_filled() < num_samples)
102 return false;
103
104 memcpy(dst, buffer + start, min(num_samples, buffer_size - start) * 2);
105
106 if (num_samples > (buffer_size - start))
107 memcpy(dst + (buffer_size - start), buffer, (num_samples - (buffer_size - start)) * 2);
108
109 start = (start + num_samples) % buffer_size;
110 size -= num_samples;
111
112 return true;
113 }
114
115 inline void push_sample(int16_t l, int16_t r)
116 {
117 if (space_empty() >= 2)
118 {
119 int end = start + size;
120 if (end >= buffer_size)
121 end -= buffer_size;
122 buffer[end] = l;
123 buffer[end + 1] = r;
124 size += 2;
125 }
126 }
127
128 inline bool push(int16_t *src, int num_samples)
129 {
130 if (space_empty() < num_samples)
131 return false;
132
133 int end = start + size;
134 if (end > buffer_size)
135 end -= buffer_size;
136 int first_write_size = min(num_samples, buffer_size - end);
137
138 memcpy(buffer + end, src, first_write_size * 2);
139
140 if (num_samples > first_write_size)
141 memcpy(buffer, src + first_write_size, (num_samples - first_write_size) * 2);
142
143 size += num_samples;
144
145 return true;
146 }
147
148 void read(int16_t *data, int num_samples)
149 {
150 //If we are outputting the exact same ratio as the input, pull directly from the input buffer
151 if (r_step == 1.0)
152 {
153 pull(data, num_samples);
154 return;
155 }
156
157 assert((num_samples & 1) == 0); // resampler always processes both stereo samples
158 int o_position = 0;
159
160 while (o_position < num_samples && size > 0)
161 {
162 int s_left = buffer[start];
163 int s_right = buffer[start + 1];
164 int hermite_val[2];
165
166 while (r_frac <= 1.0 && o_position < num_samples)
167 {
168 hermite_val[0] = (int)hermite(r_frac, (float)r_left[0], (float)r_left[1], (float)r_left[2], (float)r_left[3]);
169 hermite_val[1] = (int)hermite(r_frac, (float)r_right[0], (float)r_right[1], (float)r_right[2], (float)r_right[3]);
170 data[o_position] = short_clamp(hermite_val[0]);
171 data[o_position + 1] = short_clamp(hermite_val[1]);
172
173 o_position += 2;
174
175 r_frac += r_step;
176 }
177
178 if (r_frac > 1.0)
179 {
180 r_left[0] = r_left[1];
181 r_left[1] = r_left[2];
182 r_left[2] = r_left[3];
183 r_left[3] = s_left;
184
185 r_right[0] = r_right[1];
186 r_right[1] = r_right[2];
187 r_right[2] = r_right[3];
188 r_right[3] = s_right;
189
190 r_frac -= 1.0;
191
192 start += 2;
193 if (start >= buffer_size)
194 start -= buffer_size;
195 size -= 2;
196 }
197 }
198 }
199
200 inline int space_empty(void) const
201 {
202 return buffer_size - size;
203 }
204
205 inline int space_filled(void) const
206 {
207 return size;
208 }
209
210 inline int avail(void)
211 {
212 //If we are outputting the exact same ratio as the input, find out directly from the input buffer
213 if (r_step == 1.0)
214 return size;
215
216 return (int)trunc(((size >> 1) - r_frac) / r_step) * 2;
217 }
218
219 void resize(int num_samples)
220 {
221 if (buffer)
222 delete[] buffer;
223 buffer_size = num_samples;
224 buffer = new int16_t[buffer_size];
225 clear();
226 }
227};
228
229#endif /* __NEW_RESAMPLER_H */