Add table-based lookups for the Lanczos interpolation, and use that in the syncer.
[c64tapwav] / sync.cpp
1 // A program that tries to sync up the two channels, using Viterbi decoding
2 // to find the most likely misalignment as it changes throughout the file.
3
4 #define NUM_THEORIES 200
5 #define THEORY_FROM -20.0  /* in samples */
6 #define THEORY_TO 20.0  /* in samples */
7 #define SWITCH_COST 1000.0  /* pretty arbitrary */
8
9 #include <stdio.h>
10 #include <math.h>
11 #include <vector>
12 #include <algorithm>
13
14 #include "interpolate.h"
15
16 #define BUFSIZE 4096
17
18 struct stereo_sample {
19         short left, right;
20 };
21 struct double_stereo_sample {
22         double left, right;
23 };
24
25 inline short clip(int x)
26 {
27         if (x < -32768) {
28                 return x;
29         } else if (x > 32767) {
30                 return 32767;
31         } else {
32                 return short(x);
33         }
34 }
35
36 struct hypothesis {
37         int id;
38         double cost;
39         hypothesis *prev;
40 };
41
42 int main(int argc, char **argv)
43 {
44         make_lanczos_weight_table();
45         std::vector<stereo_sample> pcm;
46
47         while (!feof(stdin)) {
48                 stereo_sample buf[BUFSIZE];
49                 ssize_t ret = fread(buf, sizeof(stereo_sample), BUFSIZE, stdin);
50                 if (ret >= 0) {
51                         pcm.insert(pcm.end(), buf, buf + ret);
52                 }
53         }
54
55         double sum_left = 0.0, sum_right = 0.0;
56         for (unsigned i = 0; i < pcm.size(); ++i) {
57                 sum_left += pcm[i].left;
58                 sum_right += pcm[i].right;
59         }
60         double mean_left = sum_left / pcm.size();
61         double mean_right = sum_right / pcm.size();
62
63         //fprintf(stderr, "Mean: L=%f R=%f\n", mean_left, mean_right);
64
65         double sum2_left = 0.0, sum2_right = 0.0;
66         for (unsigned i = 0; i < pcm.size(); ++i) {
67                 sum2_left += (pcm[i].left - mean_left) * (pcm[i].left - mean_left);
68                 sum2_right += (pcm[i].right - mean_right) * (pcm[i].right - mean_right);
69         }
70         double var_left = sum2_left / (pcm.size() - 1);
71         double var_right = sum2_right / (pcm.size() - 1);
72
73         //fprintf(stderr, "Stddev: L=%f R=%f\n", sqrt(var_left), sqrt(var_right));
74
75         double inv_sd_left = 1.0 / sqrt(var_left);
76         double inv_sd_right = 1.0 / sqrt(var_right);
77
78         std::vector<double_stereo_sample> norm;
79         norm.resize(pcm.size());
80
81         for (unsigned i = 0; i < pcm.size(); ++i) {
82                 norm[i].left = (pcm[i].left - mean_left) * inv_sd_left;
83                 norm[i].right = (pcm[i].right - mean_right) * inv_sd_right;
84         }
85
86 #if 0
87         double offset = 0.0;
88         double old_diff = 0.0;
89         for (unsigned i = 0; i < pcm.size(); ++i) {
90                 double left = (pcm[i].left - mean_left) * inv_sd_left;
91                 double right = (interpolate(pcm, i + offset) - mean_right) * inv_sd_right;
92
93                 double diff = right - left;
94                 old_diff = old_diff * 0.9999 + diff * 0.0001;
95                 offset -= 0.1 * old_diff;
96
97                 if (i % 100 == 0) {
98                         fprintf(stderr, "%7.3f: %7.3f [diff=%8.3f lagged diff=%8.3f]\n", i / 44100.0, offset, diff, old_diff);
99                 }
100                 printf("%f %f %f\n", i / 44100.0, left, right);
101         }
102 #endif
103
104         double delays[NUM_THEORIES];
105         hypothesis *bases = new hypothesis[NUM_THEORIES];
106
107         for (int h = 0; h < NUM_THEORIES; ++h) {
108                 delays[h] = THEORY_FROM + h * (THEORY_TO - THEORY_FROM) / (NUM_THEORIES - 1);
109                 bases[h].id = h;
110                 bases[h].cost = 0.0;
111                 bases[h].prev = NULL;
112         }
113
114         fprintf(stderr, "Matching blocks... %7.2f", 0.0);
115         hypothesis *prev_hyp = bases;
116         size_t total_end = pcm.size();
117         //size_t total_end = 441000;
118         for (unsigned i = 0; i < total_end; i += BUFSIZE) {
119                 fprintf(stderr, "\b\b\b\b\b\b\b%7.2f", i / 44100.0);
120                 size_t end = std::min<size_t>(i + BUFSIZE, total_end);
121         
122                 hypothesis *hyp = new hypothesis[NUM_THEORIES];
123
124                 // evaluate all hypotheses
125                 for (int h = 0; h < NUM_THEORIES; ++h) {
126                         double sum = 0.0;
127                         double d = delays[h];
128                         for (unsigned s = i; s < end; ++s) {
129                                 double left = norm[s].left;
130                                 double right = linear_interpolate_right(norm, s + d);
131                                 double diff = (right - left) * (right - left);
132                                 sum += diff;
133                         }
134
135                         double best_cost = HUGE_VAL;
136                         hypothesis *best_prev = NULL;
137                         for (int hp = 0; hp < NUM_THEORIES; ++hp) {
138                                 double switch_cost = SWITCH_COST * fabs(delays[hp] - delays[h]);
139                                 double cost = prev_hyp[hp].cost + sum + switch_cost;
140                                 if (best_prev == NULL || cost < best_cost) {
141                                         best_cost = cost;
142                                         best_prev = &prev_hyp[hp];
143                                 }
144                         }
145
146                         hyp[h].id = h;
147                         hyp[h].cost = best_cost;
148                         hyp[h].prev = best_prev;
149                 }
150
151                 prev_hyp = hyp;
152         }
153         fprintf(stderr, "\b\b\b\b\b\b\b%7.2f\n", total_end / 44100.0);
154
155         // best winner
156         double best_cost = HUGE_VAL;
157         hypothesis *best_hyp = NULL;
158         for (int h = 0; h < NUM_THEORIES; ++h) {
159                 if (best_hyp == NULL || prev_hyp[h].cost < best_cost) {
160                         best_cost = prev_hyp[h].cost;
161                         best_hyp = &prev_hyp[h];
162                 }
163         }
164
165         // trace the path backwards
166         std::vector<double> best_path;
167         while (best_hyp != NULL) {
168                 best_path.push_back(delays[best_hyp->id]);
169                 best_hyp = best_hyp->prev;
170         }
171
172         reverse(best_path.begin(), best_path.end());
173
174         fprintf(stderr, "Writing misalignment graphs to misalignment.plot...\n");
175         FILE *fp = fopen("misalignment.plot", "w");
176         for (unsigned i = 0; i < best_path.size(); ++i) {
177                 fprintf(fp, "%f %f\n", i * BUFSIZE / 44100.0, best_path[i]);
178         }
179         fclose(fp);
180
181         fprintf(stderr, "Stretching right channel to match left... %7.2f%%", 0.0);
182         double inv_sd = sqrt(2.0) / sqrt(var_left + var_right);
183         std::vector<stereo_sample> aligned_pcm;
184         std::vector<short> mono_pcm;
185         for (unsigned i = 0; i < total_end; ++i) {
186                 double d = lanczos_interpolate(best_path, i / double(BUFSIZE));
187                 int left = pcm[i].left;
188                 int right = lanczos_interpolate_right(pcm, i + d);
189
190                 stereo_sample ss;
191                 ss.left = left;
192                 ss.right = clip(right);
193                 aligned_pcm.push_back(ss);
194
195                 mono_pcm.push_back(clip(lrintf(inv_sd * 4096.0 * (left + right))));
196
197                 if (i % 4096 == 0) {
198                         fprintf(stderr, "\b\b\b\b\b\b\b\b%7.2f%%", 100.0 * i / total_end);
199                 }
200         }
201         fprintf(stderr, "\b\b\b\b\b\b\b%7.2f%%\n", 100.0);
202
203         fprintf(stderr, "Writing realigned stereo channels to aligned.raw...\n");
204         fp = fopen("aligned.raw", "wb");
205         fwrite(aligned_pcm.data(), sizeof(stereo_sample) * aligned_pcm.size(), 1, fp);
206         fclose(fp);
207
208         fprintf(stderr, "Writing combined mono track to combined.raw...\n");
209         fp = fopen("combined.raw", "wb");
210         fwrite(mono_pcm.data(), sizeof(short) * mono_pcm.size(), 1, fp);
211         fclose(fp);
212
213         fprintf(stderr, "All done.\n");
214
215 #if 0
216
217         for (int sec = 0; sec < 2400; ++sec) {
218                 double from_offset = -128.0;
219                 double to_offset = 128.0;
220                 double best_offset = HUGE_VAL;
221                 for (int iter = 0; iter < 5; ++iter) {  
222                         //printf("from %f to %f\n", from_offset, to_offset);
223                         double best_sum = HUGE_VAL;
224                 
225                         for (int k = 0; k < 100; ++k) {
226                                 double offset = from_offset + k * 0.01 * (to_offset - from_offset);
227                                 double sum = 0.0;
228                                 for (unsigned i = sec * 4410; i < sec * 4410 + 4410; ++i) {
229                                 //for (unsigned i = 100000; i < 10NUM_THEORIES0; ++i) {
230                                         double left = norm[i].left;
231                                         double right = interpolate(norm, i + offset);
232                                         //double right = (norm[i + offset].right - mean_right) * inv_sd_right;
233                                         double diff = (right - left) * (right - left);
234                                         sum += diff;
235                                 }
236                 
237                                 if (sum < best_sum) {
238                                         best_sum = sum;
239                                         best_offset = offset;
240                                 }
241
242                                 //printf("%f %f\n", offset, sqrt(sum / NUM_THEORIES00.0f));
243                                 //fflush(stdout);
244                         }
245                 
246                         double range = 0.1 * (to_offset - from_offset);
247                         from_offset = best_offset - range;
248                         to_offset = best_offset + range;
249                 }
250                 printf("%d %f\n", sec, best_offset);
251                 fflush(stdout);
252         }
253 #endif
254         
255
256 }