git.sesse.net Git - audiosync/blob - estimate-skew.cpp

   1 #include <stdio.h>
   2 #include <string.h>
   3 #include <math.h>
   4 #include <stdlib.h>
   5
   6 double frac(double x)
   7 {
   8         return x - (int)x;
   9 }
  10
  11 class FlankDetector
  12 {
  13 private:
  14         static const unsigned NUM_SAMPLES = 10;
  15         static const int MIN_LEVEL = 8192;
  16
  17         bool status, flag;  // high/low
  18         unsigned counter;
  19
  20 public:
  21         FlankDetector() : status(false), flag(false), counter(0) {}
  22         void update(short x)
  23         {
  24                 if (abs(x) < MIN_LEVEL) {
  25                         counter = 0;
  26                         return;
  27                 }
  28
  29                 if (!status) {
  30                         // low
  31                         if (x >= MIN_LEVEL) {
  32                                 ++counter;
  33                         } else {
  34                                 counter = 0;
  35                         }
  36                 } else {
  37                         // high
  38                         if (x <= -MIN_LEVEL) {
  39                                 ++counter;
  40                         } else {
  41                                 counter = 0;
  42                         }
  43                 }
  44
  45                 if (counter >= NUM_SAMPLES) {
  46                         status = !status;
  47                         counter = 0;
  48                         flag = true;
  49                 }
  50         }
  51
  52         bool check_flag()
  53         {
  54                 bool ret = flag;
  55                 flag = false;
  56                 return ret;
  57         }
  58
  59         bool get_status() const
  60         {
  61                 return status;
  62         }
  63 };
  64
  65 class InterpolatingReader
  66 {
  67 private:
  68         FILE *f;
  69         double p;
  70         int in_pos;
  71         short prev_sample, sample;
  72
  73 public:
  74         InterpolatingReader(FILE *f) : f(f), p(0.0), in_pos(-1) {}
  75
  76         double read_sample(double speed)
  77         {
  78                 p += speed;
  79                 while ((int)(ceil(p)) > in_pos) {
  80                         prev_sample = sample;
  81                         if (fread(&sample, sizeof(short), 1, f) != 1) {
  82                                 exit(0);
  83                         }
  84                         ++in_pos;
  85                 }
  86
  87                 // linear interpolation (works OK since delta_p varies so slowly)
  88                 double t = frac(p);
  89                 return prev_sample * (1.0 - t) + sample * t;
  90         }
  91 };
  92
  93 double filter(double x)
  94 {
  95         static double l;
  96         static bool init = false;
  97
  98         if (init) {
  99                 l = 0.05 * x + 0.95 * l;
 100         } else {
 101                 init = true;
 102                 l = x;
 103         }
 104         return l;
 105 }
 106
 107 int main(int argc, char **argv)
 108 {
 109         FILE *in1, *in2, *skew;
 110
 111         // open input1 (reference)
 112         if (strcmp(argv[1], "-") == 0) {
 113                 in1 = stdin;
 114         } else {
 115                 in1 = fopen(argv[1], "rb");
 116                 if (in1 == NULL) {
 117                         perror(argv[1]);
 118                         exit(1);
 119                 }
 120         }
 121
 122         // open input2
 123         if (strcmp(argv[2], "-") == 0) {
 124                 in2 = stdin;
 125         } else {
 126                 in2 = fopen(argv[2], "rb");
 127                 if (in2 == NULL) {
 128                         perror(argv[2]);
 129                         exit(1);
 130                 }
 131         }
 132
 133         // open (estimated) skew
 134         if (strcmp(argv[3], "-") == 0) {
 135                 skew = stdout;
 136         } else {
 137                 skew = fopen(argv[3], "wb");
 138                 if (skew == NULL) {
 139                         perror(argv[3]);
 140                         exit(1);
 141                 }
 142         }
 143
 144         FlankDetector f1, f2;
 145         InterpolatingReader intp(in2);
 146         double speed = 1.000;
 147
 148         // initial sync, reference
 149         do {
 150                 short s;
 151                 if (fread(&s, sizeof(short), 1, in1) != 1) {
 152                         exit(0);
 153                 }
 154
 155                 f1.update(s);
 156         } while (!f1.check_flag() || !f1.get_status());
 157
 158         // initial sync, input
 159         do {
 160                 short s = (short)intp.read_sample(speed);
 161                 f2.update(s);
 162
 163                 fwrite(&speed, sizeof(double), 1, skew);
 164         } while (!f2.check_flag() || !f2.get_status());
 165
 166         while (!feof(in1) && !feof(in2)) {
 167                 short refs;
 168
 169                 // read reference until the next triggering
 170                 unsigned num_ref = 0;
 171                 while (!feof(in1) && !f1.check_flag()) {
 172                         if (fread(&refs, sizeof(short), 1, in1) != 1) {
 173                                 exit(0);
 174                         }
 175                         f1.update(refs);
 176                         ++num_ref;
 177                 }
 178
 179                 // same here
 180                 unsigned num_inp = 0;
 181                 while (!feof(in2) && !f2.check_flag()) {
 182                         double s = intp.read_sample(speed);
 183                         f2.update((short)s);
 184                         ++num_inp;
 185
 186                         fwrite(&speed, sizeof(double), 1, skew);
 187                 }
 188
 189                 double ns = filter(speed * double(num_inp) / double(num_ref));
 190                 fprintf(stderr, "%u vs. %u -- ratio %.3f, speed %.3f, filtered speed %.3f\n", num_inp, num_ref,
 191                         double(num_inp) / double(num_ref), speed * double(num_inp) / double(num_ref), ns);
 192                 speed = ns;
 193         }
 194 }