b90ad218b759a264da739685faecfd328be0db40
[c64tapwav] / decode.cpp
1 // Copyright Steinar H. Gunderson <sgunderson@bigfoot.com>
2 // Licensed under the GPL, v2. (See the file COPYING.)
3
4 #include <stdio.h>
5 #include <string.h>
6 #include <math.h>
7 #include <assert.h>
8 #include <limits.h>
9 #include <getopt.h>
10 #include <vector>
11 #include <algorithm>
12
13 #include "audioreader.h"
14 #include "interpolate.h"
15 #include "level.h"
16 #include "tap.h"
17 #include "filter.h"
18
19 #define BUFSIZE 4096
20 #define C64_FREQUENCY 985248
21 #define SYNC_PULSE_START 1000
22 #define SYNC_PULSE_END 20000
23 #define SYNC_PULSE_LENGTH 378.0
24 #define SYNC_TEST_TOLERANCE 1.10
25
26 // SPSA options
27 #define NUM_FILTER_COEFF 32
28 #define NUM_ITER 5000
29 #define A NUM_ITER/10  // approx
30 #define INITIAL_A 0.005 // A bit of trial and error...
31 #define INITIAL_C 0.02  // This too.
32 #define GAMMA 0.166
33 #define ALPHA 1.0
34
35 static float hysteresis_upper_limit = 3000.0 / 32768.0;
36 static float hysteresis_lower_limit = -3000.0 / 32768.0;
37 static bool do_calibrate = true;
38 static bool output_cycles_plot = false;
39 static bool do_crop = false;
40 static float crop_start = 0.0f, crop_end = HUGE_VAL;
41
42 static bool use_fir_filter = false;
43 static float filter_coeff[NUM_FILTER_COEFF] = { 1.0f };  // The rest is filled with 0.
44 static bool use_rc_filter = false;
45 static float rc_filter_freq;
46 static bool output_filtered = false;
47
48 static bool quiet = false;
49 static bool do_auto_level = false;
50 static bool output_leveled = false;
51 static std::vector<float> train_snap_points;
52 static bool do_train = false;
53
54 // The frequency to filter on (for do_auto_level), in Hertz.
55 // Larger values makes the compressor react faster, but if it is too large,
56 // you'll ruin the waveforms themselves.
57 static float auto_level_freq = 200.0;
58
59 // The minimum estimated sound level (for do_auto_level) at any given point.
60 // If you decrease this, you'll be able to amplify really silent signals
61 // by more, but you'll also increase the level of silent (ie. noise-only) segments,
62 // possibly caused misdetected pulses in these segments.
63 static float min_level = 0.05f;
64
65 // search for the value <limit> between [x,x+1]
66 double find_crossing(const std::vector<float> &pcm, int x, float limit)
67 {
68         double upper = x;
69         double lower = x + 1;
70         while (lower - upper > 1e-3) {
71                 double mid = 0.5f * (upper + lower);
72                 if (lanczos_interpolate(pcm, mid) > limit) {
73                         upper = mid;
74                 } else {
75                         lower = mid;
76                 }
77         }
78
79         return 0.5f * (upper + lower);
80 }
81
82 struct pulse {
83         double time;  // in seconds from start
84         double len;   // in seconds
85 };
86
87 // Calibrate on the first ~25k pulses (skip a few, just to be sure).
88 double calibrate(const std::vector<pulse> &pulses) {
89         if (pulses.size() < SYNC_PULSE_END) {
90                 fprintf(stderr, "Too few pulses, not calibrating!\n");
91                 return 1.0;
92         }
93
94         int sync_pulse_end = -1;
95         double sync_pulse_stddev = -1.0;
96
97         // Compute the standard deviation (to check for uneven speeds).
98         // If it suddenly skyrockets, we assume that sync ended earlier
99         // than we thought (it should be 25000 cycles), and that we should
100         // calibrate on fewer cycles.
101         for (int try_end : { 2000, 4000, 5000, 7500, 10000, 15000, SYNC_PULSE_END }) {
102                 double sum2 = 0.0;
103                 for (int i = SYNC_PULSE_START; i < try_end; ++i) {
104                         double cycles = pulses[i].len * C64_FREQUENCY;
105                         sum2 += (cycles - SYNC_PULSE_LENGTH) * (cycles - SYNC_PULSE_LENGTH);
106                 }
107                 double stddev = sqrt(sum2 / (try_end - SYNC_PULSE_START - 1));
108                 if (sync_pulse_end != -1 && stddev > 5.0 && stddev / sync_pulse_stddev > 1.3) {
109                         fprintf(stderr, "Stopping at %d sync pulses because standard deviation would be too big (%.2f cycles); shorter-than-usual trailer?\n",
110                                 sync_pulse_end, stddev);
111                         break;
112                 }
113                 sync_pulse_end = try_end;
114                 sync_pulse_stddev = stddev;
115         }
116         if (!quiet) {
117                 fprintf(stderr, "Sync pulse length standard deviation: %.2f cycles\n",
118                         sync_pulse_stddev);
119         }
120
121         double sum = 0.0;
122         for (int i = SYNC_PULSE_START; i < sync_pulse_end; ++i) {
123                 sum += pulses[i].len;
124         }
125         double mean_length = C64_FREQUENCY * sum / (sync_pulse_end - SYNC_PULSE_START);
126         double calibration_factor = SYNC_PULSE_LENGTH / mean_length;
127         if (!quiet) {
128                 fprintf(stderr, "Calibrated sync pulse length: %.2f -> %.2f (change %+.2f%%)\n",
129                         mean_length, SYNC_PULSE_LENGTH, 100.0 * (calibration_factor - 1.0));
130         }
131
132         // Check for pulses outside +/- 10% (sign of misdetection).
133         for (int i = SYNC_PULSE_START; i < sync_pulse_end; ++i) {
134                 double cycles = pulses[i].len * calibration_factor * C64_FREQUENCY;
135                 if (cycles < SYNC_PULSE_LENGTH / SYNC_TEST_TOLERANCE || cycles > SYNC_PULSE_LENGTH * SYNC_TEST_TOLERANCE) {
136                         fprintf(stderr, "Sync cycle with downflank at %.6f was detected at %.0f cycles; misdetect?\n",
137                                 pulses[i].time, cycles);
138                 }
139         }
140
141         return calibration_factor;
142 }
143
144 void output_tap(const std::vector<pulse>& pulses, double calibration_factor)
145 {
146         std::vector<char> tap_data;
147         for (unsigned i = 0; i < pulses.size(); ++i) {
148                 double cycles = pulses[i].len * calibration_factor * C64_FREQUENCY;
149                 int len = lrintf(cycles / TAP_RESOLUTION);
150                 if (i > SYNC_PULSE_END && (cycles < 100 || cycles > 800)) {
151                         fprintf(stderr, "Cycle with downflank at %.6f was detected at %.0f cycles; misdetect?\n",
152                                         pulses[i].time, cycles);
153                 }
154                 if (len <= 255) {
155                         tap_data.push_back(len);
156                 } else {
157                         int overflow_len = lrintf(cycles);
158                         tap_data.push_back(0);
159                         tap_data.push_back(overflow_len & 0xff);
160                         tap_data.push_back((overflow_len >> 8) & 0xff);
161                         tap_data.push_back(overflow_len >> 16);
162                 }
163         }
164
165         tap_header hdr;
166         memcpy(hdr.identifier, "C64-TAPE-RAW", 12);
167         hdr.version = 1;
168         hdr.reserved[0] = hdr.reserved[1] = hdr.reserved[2] = 0;
169         hdr.data_len = tap_data.size();
170
171         fwrite(&hdr, sizeof(hdr), 1, stdout);
172         fwrite(tap_data.data(), tap_data.size(), 1, stdout);
173 }
174
175 static struct option long_options[] = {
176         {"auto-level",       0,                 0, 'a' },
177         {"auto-level-freq",  required_argument, 0, 'b' },
178         {"output-leveled",   0,                 0, 'A' },
179         {"min-level",        required_argument, 0, 'm' },
180         {"no-calibrate",     0,                 0, 's' },
181         {"plot-cycles",      0,                 0, 'p' },
182         {"hysteresis-limit", required_argument, 0, 'l' },
183         {"filter",           required_argument, 0, 'f' },
184         {"rc-filter",        required_argument, 0, 'r' },
185         {"output-filtered",  0,                 0, 'F' },
186         {"crop",             required_argument, 0, 'c' },
187         {"quiet",            0,                 0, 'q' },
188         {"help",             0,                 0, 'h' },
189         {0,                  0,                 0, 0   }
190 };
191
192 void help()
193 {
194         fprintf(stderr, "decode [OPTIONS] AUDIO-FILE > TAP-FILE\n");
195         fprintf(stderr, "\n");
196         fprintf(stderr, "  -a, --auto-level             automatically adjust amplitude levels throughout the file\n");
197         fprintf(stderr, "  -b, --auto-level-freq        minimum frequency in Hertz of corrected level changes (default 200 Hz)\n");
198         fprintf(stderr, "  -A, --output-leveled         output leveled waveform to leveled.raw\n");
199         fprintf(stderr, "  -m, --min-level              minimum estimated sound level (0..32768) for --auto-level\n");
200         fprintf(stderr, "  -s, --no-calibrate           do not try to calibrate on sync pulse length\n");
201         fprintf(stderr, "  -p, --plot-cycles            output debugging info to cycles.plot\n");
202         fprintf(stderr, "  -l, --hysteresis-limit VAL   change amplitude threshold for ignoring pulses (0..32768)\n");
203         fprintf(stderr, "  -f, --filter C1:C2:C3:...    specify FIR filter (up to %d coefficients)\n", NUM_FILTER_COEFF);
204         fprintf(stderr, "  -r, --rc-filter FREQ         send signal through a highpass RC filter with given frequency (in Hertz)\n");
205         fprintf(stderr, "  -F, --output-filtered        output filtered waveform to filtered.raw\n");
206         fprintf(stderr, "  -c, --crop START[:END]       use only the given part of the file\n");
207         fprintf(stderr, "  -t, --train LEN1:LEN2:...    train a filter for detecting any of the given number of cycles\n");
208         fprintf(stderr, "                               (implies --no-calibrate and --quiet unless overridden)\n");
209         fprintf(stderr, "  -q, --quiet                  suppress some informational messages\n");
210         fprintf(stderr, "  -h, --help                   display this help, then exit\n");
211         exit(1);
212 }
213
214 void parse_options(int argc, char **argv)
215 {
216         for ( ;; ) {
217                 int option_index = 0;
218                 int c = getopt_long(argc, argv, "ab:Am:spl:f:r:Fc:t:qh", long_options, &option_index);
219                 if (c == -1)
220                         break;
221
222                 switch (c) {
223                 case 'a':
224                         do_auto_level = true;
225                         break;
226
227                 case 'b':
228                         auto_level_freq = atof(optarg);
229                         break;
230
231                 case 'A':
232                         output_leveled = true;
233                         break;
234
235                 case 'm':
236                         min_level = atof(optarg) / 32768.0;
237                         break;
238
239                 case 's':
240                         do_calibrate = false;
241                         break;
242
243                 case 'p':
244                         output_cycles_plot = true;
245                         break;
246
247                 case 'l': {
248                         const char *hyststr = strtok(optarg, ": ");
249                         hysteresis_upper_limit = atof(hyststr) / 32768.0;
250                         hyststr = strtok(NULL, ": ");
251                         if (hyststr == NULL) {
252                                 hysteresis_lower_limit = -hysteresis_upper_limit;
253                         } else {
254                                 hysteresis_lower_limit = atof(hyststr) / 32768.0;
255                         }
256                         break;
257                 }
258
259                 case 'f': {
260                         const char *coeffstr = strtok(optarg, ": ");
261                         int coeff_index = 0;
262                         while (coeff_index < NUM_FILTER_COEFF && coeffstr != NULL) {
263                                 filter_coeff[coeff_index++] = atof(coeffstr);
264                                 coeffstr = strtok(NULL, ": ");
265                         }
266                         use_fir_filter = true;
267                         break;
268                 }
269
270                 case 'r':
271                         use_rc_filter = true;
272                         rc_filter_freq = atof(optarg);
273                         break;
274
275                 case 'F':
276                         output_filtered = true;
277                         break;
278
279                 case 'c': {
280                         const char *cropstr = strtok(optarg, ":");
281                         crop_start = atof(cropstr);
282                         cropstr = strtok(NULL, ":");
283                         if (cropstr == NULL) {
284                                 crop_end = HUGE_VAL;
285                         } else {
286                                 crop_end = atof(cropstr);
287                         }
288                         do_crop = true;
289                         break;
290                 }
291
292                 case 't': {
293                         const char *cyclestr = strtok(optarg, ":");
294                         while (cyclestr != NULL) {
295                                 train_snap_points.push_back(atof(cyclestr));
296                                 cyclestr = strtok(NULL, ":");
297                         }
298                         do_train = true;
299
300                         // Set reasonable defaults (can be overridden later on the command line).
301                         do_calibrate = false;
302                         quiet = true;
303                         break;
304                 }
305
306                 case 'q':
307                         quiet = true;
308                         break;
309
310                 case 'h':
311                 default:
312                         help();
313                         exit(1);
314                 }
315         }
316 }
317
318 std::vector<float> crop(const std::vector<float>& pcm, float crop_start, float crop_end, int sample_rate)
319 {
320         size_t start_sample, end_sample;
321         if (crop_start >= 0.0f) {
322                 start_sample = std::min<size_t>(lrintf(crop_start * sample_rate), pcm.size());
323         }
324         if (crop_end >= 0.0f) {
325                 end_sample = std::min<size_t>(lrintf(crop_end * sample_rate), pcm.size());
326         }
327         return std::vector<float>(pcm.begin() + start_sample, pcm.begin() + end_sample);
328 }
329
330 // TODO: Support AVX here.
331 std::vector<float> do_fir_filter(const std::vector<float>& pcm, const float* filter)
332 {
333         std::vector<float> filtered_pcm;
334         filtered_pcm.reserve(pcm.size());
335         for (unsigned i = NUM_FILTER_COEFF; i < pcm.size(); ++i) {
336                 float s = 0.0f;
337                 for (int j = 0; j < NUM_FILTER_COEFF; ++j) {
338                         s += filter[j] * pcm[i - j];
339                 }
340                 filtered_pcm.push_back(s);
341         }
342
343         if (output_filtered) {
344                 FILE *fp = fopen("filtered.raw", "wb");
345                 fwrite(filtered_pcm.data(), filtered_pcm.size() * sizeof(filtered_pcm[0]), 1, fp);
346                 fclose(fp);
347         }
348
349         return filtered_pcm;
350 }
351
352 std::vector<float> do_rc_filter(const std::vector<float>& pcm, float freq, int sample_rate)
353 {
354         // This is only a 6 dB/oct filter, which seemingly works better
355         // than the Filter class, which is a standard biquad (12 dB/oct).
356         // The b/c calculations come from libnyquist (atone.c);
357         // I haven't checked, but I suppose they fall out of the bilinear
358         // transform of the transfer function H(s) = s/(s + w).
359         std::vector<float> filtered_pcm;
360         filtered_pcm.resize(pcm.size());
361         const float b = 2.0f - cos(2.0 * M_PI * freq / sample_rate);
362         const float c = b - sqrt(b * b - 1.0f);
363         float prev_in = 0.0f;
364         float prev_out = 0.0f;
365         for (unsigned i = 0; i < pcm.size(); ++i) {
366                 float in = pcm[i];
367                 float out = c * (prev_out + in - prev_in);
368                 filtered_pcm[i] = out;
369                 prev_in = in;
370                 prev_out = out;
371         }
372
373         if (output_filtered) {
374                 FILE *fp = fopen("filtered.raw", "wb");
375                 fwrite(filtered_pcm.data(), filtered_pcm.size() * sizeof(filtered_pcm[0]), 1, fp);
376                 fclose(fp);
377         }
378
379         return filtered_pcm;
380 }
381
382 std::vector<pulse> detect_pulses(const std::vector<float> &pcm, int sample_rate)
383 {
384         std::vector<pulse> pulses;
385
386         // Find the flanks.
387         enum State { START, ABOVE, BELOW } state = START;
388         double last_downflank = -1;
389         for (unsigned i = 0; i < pcm.size(); ++i) {
390                 if (pcm[i] > hysteresis_upper_limit) {
391                         state = ABOVE;
392                 } else if (pcm[i] < hysteresis_lower_limit) {
393                         if (state == ABOVE) {
394                                 // down-flank!
395                                 double t = find_crossing(pcm, i - 1, hysteresis_lower_limit) * (1.0 / sample_rate) + crop_start;
396                                 if (last_downflank > 0) {
397                                         pulse p;
398                                         p.time = t;
399                                         p.len = t - last_downflank;
400                                         pulses.push_back(p);
401                                 }
402                                 last_downflank = t;
403                         }
404                         state = BELOW;
405                 }
406         }
407         return pulses;
408 }
409
410 void output_cycle_plot(const std::vector<pulse> &pulses, double calibration_factor)
411 {
412         FILE *fp = fopen("cycles.plot", "w");
413         for (unsigned i = 0; i < pulses.size(); ++i) {
414                 double cycles = pulses[i].len * calibration_factor * C64_FREQUENCY;
415                 fprintf(fp, "%f %f\n", pulses[i].time, cycles);
416         }
417         fclose(fp);
418 }
419
420 std::pair<int, double> find_closest_point(double x, const std::vector<float> &points)
421 {
422         int best_point = 0;
423         double best_dist = (x - points[0]) * (x - points[0]);
424         for (unsigned j = 1; j < train_snap_points.size(); ++j) {
425                 double dist = (x - points[j]) * (x - points[j]);
426                 if (dist < best_dist) {
427                         best_point = j;
428                         best_dist = dist;
429                 }
430         }
431         return std::make_pair(best_point, best_dist);
432 }
433
434 float eval_badness(const std::vector<pulse>& pulses, double calibration_factor)
435 {
436         double sum_badness = 0.0;
437         for (unsigned i = 0; i < pulses.size(); ++i) {
438                 double cycles = pulses[i].len * calibration_factor * C64_FREQUENCY;
439                 if (cycles > 2000.0) cycles = 2000.0;  // Don't make pauses arbitrarily bad.
440                 std::pair<int, double> selected_point_and_sq_dist = find_closest_point(cycles, train_snap_points);
441                 sum_badness += selected_point_and_sq_dist.second;
442         }
443         return sqrt(sum_badness / (pulses.size() - 1));
444 }
445
446 void find_kmeans(const std::vector<pulse> &pulses, double calibration_factor, const std::vector<float> &initial_centers)
447 {
448         std::vector<float> last_centers = initial_centers;
449         std::vector<float> sums;
450         std::vector<float> num;
451         sums.resize(initial_centers.size());
452         num.resize(initial_centers.size());
453         for ( ;; ) {
454                 for (unsigned i = 0; i < initial_centers.size(); ++i) {
455                         sums[i] = 0.0f;
456                         num[i] = 0;
457                 }
458                 for (unsigned i = 0; i < pulses.size(); ++i) {
459                         double cycles = pulses[i].len * calibration_factor * C64_FREQUENCY;
460                         // Ignore heavy outliers, which are almost always long pauses.
461                         if (cycles > 2000.0) {
462                                 continue;
463                         }
464                         std::pair<int, double> selected_point_and_sq_dist = find_closest_point(cycles, last_centers);
465                         int p = selected_point_and_sq_dist.first;
466                         sums[p] += cycles;
467                         ++num[p];
468                 }
469                 bool any_moved = false;
470                 for (unsigned i = 0; i < initial_centers.size(); ++i) {
471                         if (num[i] == 0) {
472                                 fprintf(stderr, "K-means broke down, can't output new reference training points\n");
473                                 return;
474                         }
475                         float new_center = sums[i] / num[i];
476                         if (fabs(new_center - last_centers[i]) > 1e-3) {
477                                 any_moved = true;
478                         }
479                         last_centers[i] = new_center;
480                 }
481                 if (!any_moved) {
482                         break;
483                 }
484         }
485         fprintf(stderr, "New reference training points:");
486         for (unsigned i = 0; i < last_centers.size(); ++i) {
487                 fprintf(stderr, " %.3f", last_centers[i]);
488         }
489         fprintf(stderr, "\n");
490 }
491
492 void spsa_train(const std::vector<float> &pcm, int sample_rate)
493 {
494         float filter[NUM_FILTER_COEFF] = { 1.0f };  // The rest is filled with 0.
495
496         float start_c = INITIAL_C;
497         double best_badness = HUGE_VAL;
498
499         for (int n = 1; n < NUM_ITER; ++n) {
500                 float a = INITIAL_A * pow(n + A, -ALPHA);
501                 float c = start_c * pow(n, -GAMMA);
502
503                 // find a random perturbation
504                 float p[NUM_FILTER_COEFF];
505                 float filter1[NUM_FILTER_COEFF], filter2[NUM_FILTER_COEFF];
506                 for (int i = 0; i < NUM_FILTER_COEFF; ++i) {
507                         p[i] = (rand() % 2) ? 1.0 : -1.0;
508                         filter1[i] = std::max(std::min(filter[i] - c * p[i], 1.0f), -1.0f);
509                         filter2[i] = std::max(std::min(filter[i] + c * p[i], 1.0f), -1.0f);
510                 }
511
512                 std::vector<pulse> pulses1 = detect_pulses(do_fir_filter(pcm, filter1), sample_rate);
513                 std::vector<pulse> pulses2 = detect_pulses(do_fir_filter(pcm, filter2), sample_rate);
514                 float badness1 = eval_badness(pulses1, 1.0);
515                 float badness2 = eval_badness(pulses2, 1.0);
516
517                 // Find the gradient estimator
518                 float g[NUM_FILTER_COEFF];
519                 for (int i = 0; i < NUM_FILTER_COEFF; ++i) {
520                         g[i] = (badness2 - badness1) / (2.0 * c * p[i]);
521                         filter[i] -= a * g[i];
522                         filter[i] = std::max(std::min(filter[i], 1.0f), -1.0f);
523                 }
524                 if (badness2 < badness1) {
525                         std::swap(badness1, badness2);
526                         std::swap(filter1, filter2);
527                         std::swap(pulses1, pulses2);
528                 }
529                 if (badness1 < best_badness) {
530                         printf("\nNew best filter (badness=%f):", badness1);
531                         for (int i = 0; i < NUM_FILTER_COEFF; ++i) {
532                                 printf(" %.5f", filter1[i]);
533                         }
534                         best_badness = badness1;
535                         printf("\n");
536
537                         find_kmeans(pulses1, 1.0, train_snap_points);
538
539                         if (output_cycles_plot) {
540                                 output_cycle_plot(pulses1, 1.0);
541                         }
542                 }
543                 printf("%d ", n);
544                 fflush(stdout);
545         }
546 }
547
548 int main(int argc, char **argv)
549 {
550         parse_options(argc, argv);
551
552         make_lanczos_weight_table();
553         std::vector<float> pcm;
554         int sample_rate;
555         if (!read_audio_file(argv[optind], &pcm, &sample_rate)) {
556                 exit(1);
557         }
558
559         if (do_crop) {
560                 pcm = crop(pcm, crop_start, crop_end, sample_rate);
561         }
562
563         if (use_fir_filter) {
564                 pcm = do_fir_filter(pcm, filter_coeff);
565         }
566
567         if (use_rc_filter) {
568                 pcm = do_rc_filter(pcm, rc_filter_freq, sample_rate);
569         }
570
571         if (do_auto_level) {
572                 pcm = level_samples(pcm, min_level, auto_level_freq, sample_rate);
573                 if (output_leveled) {
574                         FILE *fp = fopen("leveled.raw", "wb");
575                         fwrite(pcm.data(), pcm.size() * sizeof(pcm[0]), 1, fp);
576                         fclose(fp);
577                 }
578         }
579
580 #if 0
581         for (int i = 0; i < LEN; ++i) {
582                 in[i] += rand() % 10000;
583         }
584 #endif
585
586 #if 0
587         for (int i = 0; i < LEN; ++i) {
588                 printf("%d\n", in[i]);
589         }
590 #endif
591
592         if (do_train) {
593                 spsa_train(pcm, sample_rate);
594                 exit(0);
595         }
596
597         std::vector<pulse> pulses = detect_pulses(pcm, sample_rate);
598
599         double calibration_factor = 1.0;
600         if (do_calibrate) {
601                 calibration_factor = calibrate(pulses);
602         }
603
604         if (output_cycles_plot) {
605                 output_cycle_plot(pulses, calibration_factor);
606         }
607
608         output_tap(pulses, calibration_factor);
609 }