libs/rubberband/src/StretcherProcess.cpp

   1 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
   2
   3 /*
   4     Rubber Band
   5     An audio time-stretching and pitch-shifting library.
   6     Copyright 2007-2008 Chris Cannam.
   7
   8     This program is free software; you can redistribute it and/or
   9     modify it under the terms of the GNU General Public License as
  10     published by the Free Software Foundation; either version 2 of the
  11     License, or (at your option) any later version.  See the file
  12     COPYING included with this distribution for more information.
  13 */
  14
  15 #include "StretcherImpl.h"
  16 #include "PercussiveAudioCurve.h"
  17 #include "HighFrequencyAudioCurve.h"
  18 #include "ConstantAudioCurve.h"
  19 #include "StretchCalculator.h"
  20 #include "StretcherChannelData.h"
  21 #include "Resampler.h"
  22 #include "Profiler.h"
  23
  24 #include <cstring>
  25 #include <cassert>
  26 #include <cmath>
  27 #include <set>
  28 #include <map>
  29 #include <deque>
  30
  31
  32 using std::cerr;
  33 using std::endl;
  34
  35 namespace RubberBand {
  36
  37 RubberBandStretcher::Impl::ProcessThread::ProcessThread(Impl *s, size_t c) :
  38     m_s(s),
  39     m_channel(c),
  40     m_dataAvailable(std::string("data ") + char('A' + c)),
  41     m_abandoning(false)
  42 { }
  43
  44 void
  45 RubberBandStretcher::Impl::ProcessThread::run()
  46 {
  47     if (m_s->m_debugLevel > 1) {
  48         cerr << "thread " << m_channel << " getting going" << endl;
  49     }
  50
  51     ChannelData &cd = *m_s->m_channelData[m_channel];
  52
  53     while (cd.inputSize == -1 ||
  54            cd.inbuf->getReadSpace() > 0) {
  55
  56 //        if (cd.inputSize != -1) {
  57 //            cerr << "inputSize == " << cd.inputSize
  58 //                 << ", readSpace == " << cd.inbuf->getReadSpace() << endl;
  59 //        }
  60
  61         bool any = false, last = false;
  62         m_s->processChunks(m_channel, any, last);
  63
  64         if (last) break;
  65
  66         if (any) m_s->m_spaceAvailable.signal();
  67
  68         m_dataAvailable.lock();
  69         if (!m_s->testInbufReadSpace(m_channel) && !m_abandoning) {
  70             m_dataAvailable.wait(50000); // bounded in case of abandonment
  71         } else {
  72             m_dataAvailable.unlock();
  73         }
  74
  75         if (m_abandoning) {
  76             if (m_s->m_debugLevel > 1) {
  77                 cerr << "thread " << m_channel << " abandoning" << endl;
  78             }
  79             return;
  80         }
  81     }
  82
  83     bool any = false, last = false;
  84     m_s->processChunks(m_channel, any, last);
  85     m_s->m_spaceAvailable.signal();
  86
  87     if (m_s->m_debugLevel > 1) {
  88         cerr << "thread " << m_channel << " done" << endl;
  89     }
  90 }
  91
  92 void
  93 RubberBandStretcher::Impl::ProcessThread::signalDataAvailable()
  94 {
  95     m_dataAvailable.signal();
  96 }
  97
  98 void
  99 RubberBandStretcher::Impl::ProcessThread::abandon()
 100 {
 101     m_abandoning = true;
 102 }
 103
 104 bool
 105 RubberBandStretcher::Impl::resampleBeforeStretching() const
 106 {
 107     // We can't resample before stretching in offline mode, because
 108     // the stretch calculation is based on doing it the other way
 109     // around.  It would take more work (and testing) to enable this.
 110     if (!m_realtime) return false;
 111
 112     if (m_options & OptionPitchHighQuality) {
 113         return (m_pitchScale < 1.0); // better sound
 114     } else if (m_options & OptionPitchHighConsistency) {
 115         return false;
 116     } else {
 117         return (m_pitchScale > 1.0); // better performance
 118     }
 119 }
 120
 121 size_t
 122 RubberBandStretcher::Impl::consumeChannel(size_t c, const float *input,
 123                                           size_t samples, bool final)
 124 {
 125     Profiler profiler("RubberBandStretcher::Impl::consumeChannel");
 126
 127     ChannelData &cd = *m_channelData[c];
 128     RingBuffer<float> &inbuf = *cd.inbuf;
 129
 130     size_t toWrite = samples;
 131     size_t writable = inbuf.getWriteSpace();
 132
 133     bool resampling = resampleBeforeStretching();
 134
 135     if (resampling) {
 136
 137         toWrite = int(ceil(samples / m_pitchScale));
 138         if (writable < toWrite) {
 139             samples = int(floor(writable * m_pitchScale));
 140             if (samples == 0) return 0;
 141         }
 142
 143         size_t reqSize = int(ceil(samples / m_pitchScale));
 144         if (reqSize > cd.resamplebufSize) {
 145             cerr << "WARNING: RubberBandStretcher::Impl::consumeChannel: resizing resampler buffer from "
 146                  << cd.resamplebufSize << " to " << reqSize << endl;
 147             cd.setResampleBufSize(reqSize);
 148         }
 149
 150
 151         toWrite = cd.resampler->resample(&input,
 152                                          &cd.resamplebuf,
 153                                          samples,
 154                                          1.0 / m_pitchScale,
 155                                          final);
 156
 157     }
 158
 159     if (writable < toWrite) {
 160         if (resampling) {
 161             return 0;
 162         }
 163         toWrite = writable;
 164     }
 165
 166     if (resampling) {
 167         inbuf.write(cd.resamplebuf, toWrite);
 168         cd.inCount += samples;
 169         return samples;
 170     } else {
 171         inbuf.write(input, toWrite);
 172         cd.inCount += toWrite;
 173         return toWrite;
 174     }
 175 }
 176
 177 void
 178 RubberBandStretcher::Impl::processChunks(size_t c, bool &any, bool &last)
 179 {
 180     Profiler profiler("RubberBandStretcher::Impl::processChunks");
 181
 182     // Process as many chunks as there are available on the input
 183     // buffer for channel c.  This requires that the increments have
 184     // already been calculated.
 185
 186     ChannelData &cd = *m_channelData[c];
 187
 188     last = false;
 189     any = false;
 190
 191     while (!last) {
 192
 193         if (!testInbufReadSpace(c)) {
 194 //            cerr << "not enough input" << endl;
 195             break;
 196         }
 197
 198         any = true;
 199
 200         if (!cd.draining) {
 201             size_t got = cd.inbuf->peek(cd.fltbuf, m_windowSize);
 202             assert(got == m_windowSize || cd.inputSize >= 0);
 203             got = 0;
 204             cd.inbuf->skip(m_increment);
 205             analyseChunk(c);
 206         }
 207
 208         bool phaseReset = false;
 209         size_t phaseIncrement, shiftIncrement;
 210         getIncrements(c, phaseIncrement, shiftIncrement, phaseReset);
 211
 212         last = processChunkForChannel(c, phaseIncrement, shiftIncrement, phaseReset);
 213         cd.chunkCount++;
 214         if (m_debugLevel > 2) {
 215             cerr << "channel " << c << ": last = " << last << ", chunkCount = " << cd.chunkCount << endl;
 216         }
 217     }
 218 }
 219
 220 bool
 221 RubberBandStretcher::Impl::processOneChunk()
 222 {
 223     Profiler profiler("RubberBandStretcher::Impl::processOneChunk");
 224
 225     // Process a single chunk for all channels, provided there is
 226     // enough data on each channel for at least one chunk.  This is
 227     // able to calculate increments as it goes along.
 228
 229     for (size_t c = 0; c < m_channels; ++c) {
 230         if (!testInbufReadSpace(c)) return false;
 231         ChannelData &cd = *m_channelData[c];
 232         if (!cd.draining) {
 233             size_t got = cd.inbuf->peek(cd.fltbuf, m_windowSize);
 234             got = 0;
 235             assert(got == m_windowSize || cd.inputSize >= 0);
 236             cd.inbuf->skip(m_increment);
 237             analyseChunk(c);
 238         }
 239     }
 240
 241     bool phaseReset = false;
 242     size_t phaseIncrement, shiftIncrement;
 243     if (!getIncrements(0, phaseIncrement, shiftIncrement, phaseReset)) {
 244         calculateIncrements(phaseIncrement, shiftIncrement, phaseReset);
 245     }
 246
 247     bool last = false;
 248     for (size_t c = 0; c < m_channels; ++c) {
 249         last = processChunkForChannel(c, phaseIncrement, shiftIncrement, phaseReset);
 250         m_channelData[c]->chunkCount++;
 251     }
 252
 253     return last;
 254 }
 255
 256 bool
 257 RubberBandStretcher::Impl::testInbufReadSpace(size_t c)
 258 {
 259     Profiler profiler("RubberBandStretcher::Impl::testInbufReadSpace");
 260
 261     ChannelData &cd = *m_channelData[c];
 262     RingBuffer<float> &inbuf = *cd.inbuf;
 263
 264     size_t rs = inbuf.getReadSpace();
 265
 266     if (rs < m_windowSize && !cd.draining) {
 267
 268         if (cd.inputSize == -1) {
 269
 270             // Not all the input data has been written to the inbuf
 271             // (that's why the input size is not yet set).  We can't
 272             // process, because we don't have a full chunk of data, so
 273             // our process chunk would contain some empty padding in
 274             // its input -- and that would give incorrect output, as
 275             // we know there is more input to come.
 276
 277             if (!m_threaded) {
 278 //                cerr << "WARNING: RubberBandStretcher: read space < chunk size ("
 279 //                          << inbuf.getReadSpace() << " < " << m_windowSize
 280 //                          << ") when not all input written, on processChunks for channel " << c << endl;
 281             }
 282             return false;
 283         }
 284
 285         if (rs == 0) {
 286
 287             if (m_debugLevel > 1) {
 288                 cerr << "read space = 0, giving up" << endl;
 289             }
 290             return false;
 291
 292         } else if (rs < m_windowSize/2) {
 293
 294             if (m_debugLevel > 1) {
 295                 cerr << "read space = " << rs << ", setting draining true" << endl;
 296             }
 297
 298             cd.draining = true;
 299         }
 300     }
 301
 302     return true;
 303 }
 304
 305 bool
 306 RubberBandStretcher::Impl::processChunkForChannel(size_t c,
 307                                                   size_t phaseIncrement,
 308                                                   size_t shiftIncrement,
 309                                                   bool phaseReset)
 310 {
 311     Profiler profiler("RubberBandStretcher::Impl::processChunkForChannel");
 312
 313     // Process a single chunk on a single channel.  This assumes
 314     // enough input data is available; caller must have tested this
 315     // using e.g. testInbufReadSpace first.  Return true if this is
 316     // the last chunk on the channel.
 317
 318     if (phaseReset && (m_debugLevel > 1)) {
 319         cerr << "processChunkForChannel: phase reset found, incrs "
 320                   << phaseIncrement << ":" << shiftIncrement << endl;
 321     }
 322
 323     ChannelData &cd = *m_channelData[c];
 324
 325     if (!cd.draining) {
 326
 327         // This is the normal processing case -- draining is only
 328         // set when all the input has been used and we only need
 329         // to write from the existing accumulator into the output.
 330
 331         // We know we have enough samples available in m_inbuf --
 332         // this is usually m_windowSize, but we know that if fewer
 333         // are available, it's OK to use zeroes for the rest
 334         // (which the ring buffer will provide) because we've
 335         // reached the true end of the data.
 336
 337         // We need to peek m_windowSize samples for processing, and
 338         // then skip m_increment to advance the read pointer.
 339
 340         modifyChunk(c, phaseIncrement, phaseReset);
 341         synthesiseChunk(c); // reads from cd.mag, cd.phase
 342
 343         if (m_debugLevel > 2) {
 344             if (phaseReset) {
 345                 for (int i = 0; i < 10; ++i) {
 346                     cd.accumulator[i] = 1.2f - (i % 3) * 1.2f;
 347                 }
 348             }
 349         }
 350     }
 351
 352     bool last = false;
 353
 354     if (cd.draining) {
 355         if (m_debugLevel > 1) {
 356             cerr << "draining: accumulator fill = " << cd.accumulatorFill << " (shiftIncrement = " << shiftIncrement << ")" <<  endl;
 357         }
 358         if (shiftIncrement == 0) {
 359             cerr << "WARNING: draining: shiftIncrement == 0, can't handle that in this context: setting to " << m_increment << endl;
 360             shiftIncrement = m_increment;
 361         }
 362         if (cd.accumulatorFill <= shiftIncrement) {
 363             if (m_debugLevel > 1) {
 364                 cerr << "reducing shift increment from " << shiftIncrement
 365                           << " to " << cd.accumulatorFill
 366                           << " and marking as last" << endl;
 367             }
 368             shiftIncrement = cd.accumulatorFill;
 369             last = true;
 370         }
 371     }
 372
 373     if (m_threaded) {
 374
 375         int required = shiftIncrement;
 376
 377         if (m_pitchScale != 1.0) {
 378             required = int(required / m_pitchScale) + 1;
 379         }
 380
 381         if (cd.outbuf->getWriteSpace() < required) {
 382             if (m_debugLevel > 0) {
 383                 cerr << "Buffer overrun on output for channel " << c << endl;
 384             }
 385
 386             //!!! The only correct thing we can do here is resize the
 387             // buffer.  We can't wait for the client thread to read
 388             // some data out from the buffer so as to make more space,
 389             // because the client thread is probably stuck in a
 390             // process() call waiting for us to stow away enough input
 391             // increments to allow the process() call to complete.
 392
 393         }
 394     }
 395
 396     writeChunk(c, shiftIncrement, last);
 397     return last;
 398 }
 399
 400 void
 401 RubberBandStretcher::Impl::calculateIncrements(size_t &phaseIncrementRtn,
 402                                                size_t &shiftIncrementRtn,
 403                                                bool &phaseReset)
 404 {
 405     Profiler profiler("RubberBandStretcher::Impl::calculateIncrements");
 406
 407 //    cerr << "calculateIncrements" << endl;
 408
 409     // Calculate the next upcoming phase and shift increment, on the
 410     // basis that both channels are in sync.  This is in contrast to
 411     // getIncrements, which requires that all the increments have been
 412     // calculated in advance but can then return increments
 413     // corresponding to different chunks in different channels.
 414
 415     // Requires frequency domain representations of channel data in
 416     // the mag and phase buffers in the channel.
 417
 418     // This function is only used in real-time mode.
 419
 420     phaseIncrementRtn = m_increment;
 421     shiftIncrementRtn = m_increment;
 422     phaseReset = false;
 423
 424     if (m_channels == 0) return;
 425
 426     ChannelData &cd = *m_channelData[0];
 427
 428     size_t bc = cd.chunkCount;
 429     for (size_t c = 1; c < m_channels; ++c) {
 430         if (m_channelData[c]->chunkCount != bc) {
 431             cerr << "ERROR: RubberBandStretcher::Impl::calculateIncrements: Channels are not in sync" << endl;
 432             return;
 433         }
 434     }
 435
 436     const int hs = m_windowSize/2 + 1;
 437
 438     // Normally we would mix down the time-domain signal and apply a
 439     // single FFT, or else mix down the Cartesian form of the
 440     // frequency-domain signal.  Both of those would be inefficient
 441     // from this position.  Fortunately, the onset detectors should
 442     // work reasonably well (maybe even better?) if we just sum the
 443     // magnitudes of the frequency-domain channel signals and forget
 444     // about phase entirely.  Normally we don't expect the channel
 445     // phases to cancel each other, and broadband effects will still
 446     // be apparent.
 447
 448     float df = 0.f;
 449     bool silent = false;
 450
 451     if (m_channels == 1) {
 452
 453         df = m_phaseResetAudioCurve->processDouble(cd.mag, m_increment);
 454         silent = (m_silentAudioCurve->processDouble(cd.mag, m_increment) > 0.f);
 455
 456     } else {
 457
 458         double *tmp = (double *)alloca(hs * sizeof(double));
 459
 460         for (int i = 0; i < hs; ++i) {
 461             tmp[i] = 0.0;
 462         }
 463         for (size_t c = 0; c < m_channels; ++c) {
 464             for (int i = 0; i < hs; ++i) {
 465                 tmp[i] += m_channelData[c]->mag[i];
 466             }
 467         }
 468
 469         df = m_phaseResetAudioCurve->processDouble(tmp, m_increment);
 470         silent = (m_silentAudioCurve->processDouble(tmp, m_increment) > 0.f);
 471     }
 472
 473     int incr = m_stretchCalculator->calculateSingle
 474             (getEffectiveRatio(), df, m_increment);
 475
 476     m_lastProcessPhaseResetDf.write(&df, 1);
 477     m_lastProcessOutputIncrements.write(&incr, 1);
 478
 479     if (incr < 0) {
 480         phaseReset = true;
 481         incr = -incr;
 482     }
 483
 484     // The returned increment is the phase increment.  The shift
 485     // increment for one chunk is the same as the phase increment for
 486     // the following chunk (see comment below).  This means we don't
 487     // actually know the shift increment until we see the following
 488     // phase increment... which is a bit of a problem.
 489
 490     // This implies we should use this increment for the shift
 491     // increment, and make the following phase increment the same as
 492     // it.  This means in RT mode we'll be one chunk later with our
 493     // phase reset than we would be in non-RT mode.  The sensitivity
 494     // of the broadband onset detector may mean that this isn't a
 495     // problem -- test it and see.
 496
 497     shiftIncrementRtn = incr;
 498
 499     if (cd.prevIncrement == 0) {
 500         phaseIncrementRtn = shiftIncrementRtn;
 501     } else {
 502         phaseIncrementRtn = cd.prevIncrement;
 503     }
 504
 505     cd.prevIncrement = shiftIncrementRtn;
 506
 507     if (silent) ++m_silentHistory;
 508     else m_silentHistory = 0;
 509
 510     if (m_silentHistory >= int(m_windowSize / m_increment) && !phaseReset) {
 511         phaseReset = true;
 512         if (m_debugLevel > 1) {
 513             cerr << "calculateIncrements: phase reset on silence (silent history == "
 514                  << m_silentHistory << ")" << endl;
 515         }
 516     }
 517 }
 518
 519 bool
 520 RubberBandStretcher::Impl::getIncrements(size_t channel,
 521                                          size_t &phaseIncrementRtn,
 522                                          size_t &shiftIncrementRtn,
 523                                          bool &phaseReset)
 524 {
 525     Profiler profiler("RubberBandStretcher::Impl::getIncrements");
 526
 527     if (channel >= m_channels) {
 528         phaseIncrementRtn = m_increment;
 529         shiftIncrementRtn = m_increment;
 530         phaseReset = false;
 531         return false;
 532     }
 533
 534     // There are two relevant output increments here.  The first is
 535     // the phase increment which we use when recalculating the phases
 536     // for the current chunk; the second is the shift increment used
 537     // to determine how far to shift the processing buffer after
 538     // writing the chunk.  The shift increment for one chunk is the
 539     // same as the phase increment for the following chunk.
 540
 541     // When an onset occurs for which we need to reset phases, the
 542     // increment given will be negative.
 543
 544     // When we reset phases, the previous shift increment (and so
 545     // current phase increments) must have been m_increment to ensure
 546     // consistency.
 547
 548     // m_outputIncrements stores phase increments.
 549
 550     ChannelData &cd = *m_channelData[channel];
 551     bool gotData = true;
 552
 553     if (cd.chunkCount >= m_outputIncrements.size()) {
 554 //        cerr << "WARNING: RubberBandStretcher::Impl::getIncrements:"
 555 //             << " chunk count " << cd.chunkCount << " >= "
 556 //             << m_outputIncrements.size() << endl;
 557         if (m_outputIncrements.size() == 0) {
 558             phaseIncrementRtn = m_increment;
 559             shiftIncrementRtn = m_increment;
 560             phaseReset = false;
 561             return false;
 562         } else {
 563             cd.chunkCount = m_outputIncrements.size()-1;
 564             gotData = false;
 565         }
 566     }
 567
 568     int phaseIncrement = m_outputIncrements[cd.chunkCount];
 569
 570     int shiftIncrement = phaseIncrement;
 571     if (cd.chunkCount + 1 < m_outputIncrements.size()) {
 572         shiftIncrement = m_outputIncrements[cd.chunkCount + 1];
 573     }
 574
 575     if (phaseIncrement < 0) {
 576         phaseIncrement = -phaseIncrement;
 577         phaseReset = true;
 578     }
 579
 580     if (shiftIncrement < 0) {
 581         shiftIncrement = -shiftIncrement;
 582     }
 583
 584     if (shiftIncrement >= int(m_windowSize)) {
 585         cerr << "*** ERROR: RubberBandStretcher::Impl::processChunks: shiftIncrement " << shiftIncrement << " >= windowSize " << m_windowSize << " at " << cd.chunkCount << " (of " << m_outputIncrements.size() << ")" << endl;
 586         shiftIncrement = m_windowSize;
 587     }
 588
 589     phaseIncrementRtn = phaseIncrement;
 590     shiftIncrementRtn = shiftIncrement;
 591     if (cd.chunkCount == 0) phaseReset = true; // don't mess with the first chunk
 592     return gotData;
 593 }
 594
 595 void
 596 RubberBandStretcher::Impl::analyseChunk(size_t channel)
 597 {
 598     Profiler profiler("RubberBandStretcher::Impl::analyseChunk");
 599
 600     int i;
 601
 602     ChannelData &cd = *m_channelData[channel];
 603
 604     double *const R__ dblbuf = cd.dblbuf;
 605     float *const R__ fltbuf = cd.fltbuf;
 606
 607     int sz = m_windowSize;
 608     int hs = m_windowSize/2;
 609
 610     // cd.fltbuf is known to contain m_windowSize samples
 611
 612     m_window->cut(fltbuf);
 613
 614     if (cd.oversample > 1) {
 615
 616         int bufsiz = sz * cd.oversample;
 617         int offset = (bufsiz - sz) / 2;
 618
 619         // eek
 620
 621         for (i = 0; i < offset; ++i) {
 622             dblbuf[i] = 0.0;
 623         }
 624         for (i = 0; i < offset; ++i) {
 625             dblbuf[bufsiz - i - 1] = 0.0;
 626         }
 627         for (i = 0; i < sz; ++i) {
 628             dblbuf[offset + i] = fltbuf[i];
 629         }
 630         for (i = 0; i < bufsiz / 2; ++i) {
 631             double tmp = dblbuf[i];
 632             dblbuf[i] = dblbuf[i + bufsiz/2];
 633             dblbuf[i + bufsiz/2] = tmp;
 634         }
 635     } else {
 636         for (i = 0; i < hs; ++i) {
 637             dblbuf[i] = fltbuf[i + hs];
 638             dblbuf[i + hs] = fltbuf[i];
 639         }
 640     }
 641
 642     cd.fft->forwardPolar(dblbuf, cd.mag, cd.phase);
 643 }
 644
 645 static inline double mod(double x, double y) { return x - (y * floor(x / y)); }
 646 static inline double princarg(double a) { return mod(a + M_PI, -2.0 * M_PI) + M_PI; }
 647
 648 void
 649 RubberBandStretcher::Impl::modifyChunk(size_t channel,
 650                                        size_t outputIncrement,
 651                                        bool phaseReset)
 652 {
 653     Profiler profiler("RubberBandStretcher::Impl::modifyChunk");
 654
 655     ChannelData &cd = *m_channelData[channel];
 656
 657     if (phaseReset && m_debugLevel > 1) {
 658         cerr << "phase reset: leaving phases unmodified" << endl;
 659     }
 660
 661     const double rate = m_sampleRate;
 662     const int sz = m_windowSize;
 663     const int count = (sz * cd.oversample) / 2;
 664
 665     bool unchanged = cd.unchanged && (outputIncrement == m_increment);
 666     bool fullReset = phaseReset;
 667     bool laminar = !(m_options & OptionPhaseIndependent);
 668     bool bandlimited = (m_options & OptionTransientsMixed);
 669     int bandlow = lrint((150 * sz * cd.oversample) / rate);
 670     int bandhigh = lrint((1000 * sz * cd.oversample) / rate);
 671
 672     float freq0 = m_freq0;
 673     float freq1 = m_freq1;
 674     float freq2 = m_freq2;
 675
 676     if (laminar) {
 677         float r = getEffectiveRatio();
 678         if (r > 1) {
 679             float rf0 = 600 + (600 * ((r-1)*(r-1)*(r-1)*2));
 680             float f1ratio = freq1 / freq0;
 681             float f2ratio = freq2 / freq0;
 682             freq0 = std::max(freq0, rf0);
 683             freq1 = freq0 * f1ratio;
 684             freq2 = freq0 * f2ratio;
 685         }
 686     }
 687
 688     int limit0 = lrint((freq0 * sz * cd.oversample) / rate);
 689     int limit1 = lrint((freq1 * sz * cd.oversample) / rate);
 690     int limit2 = lrint((freq2 * sz * cd.oversample) / rate);
 691
 692     if (limit1 < limit0) limit1 = limit0;
 693     if (limit2 < limit1) limit2 = limit1;
 694
 695     double prevInstability = 0.0;
 696     bool prevDirection = false;
 697
 698     double distance = 0.0;
 699     const double maxdist = 8.0;
 700
 701     const int lookback = 1;
 702
 703     double distacc = 0.0;
 704
 705     for (int i = count; i >= 0; i -= lookback) {
 706
 707         bool resetThis = phaseReset;
 708
 709         if (bandlimited) {
 710             if (resetThis) {
 711                 if (i > bandlow && i < bandhigh) {
 712                     resetThis = false;
 713                     fullReset = false;
 714                 }
 715             }
 716         }
 717
 718         double p = cd.phase[i];
 719         double perr = 0.0;
 720         double outphase = p;
 721
 722         double mi = maxdist;
 723         if (i <= limit0) mi = 0.0;
 724         else if (i <= limit1) mi = 1.0;
 725         else if (i <= limit2) mi = 3.0;
 726
 727         if (!resetThis) {
 728
 729             double omega = (2 * M_PI * m_increment * i) / (sz * cd.oversample);
 730
 731             double pp = cd.prevPhase[i];
 732             double ep = pp + omega;
 733             perr = princarg(p - ep);
 734
 735             double instability = fabs(perr - cd.prevError[i]);
 736             bool direction = (perr > cd.prevError[i]);
 737
 738             bool inherit = false;
 739
 740             if (laminar) {
 741                 if (distance >= mi || i == count) {
 742                     inherit = false;
 743                 } else if (bandlimited && (i == bandhigh || i == bandlow)) {
 744                     inherit = false;
 745                 } else if (instability > prevInstability &&
 746                            direction == prevDirection) {
 747                     inherit = true;
 748                 }
 749             }
 750
 751             double advance = outputIncrement * ((omega + perr) / m_increment);
 752
 753             if (inherit) {
 754                 double inherited =
 755                     cd.unwrappedPhase[i + lookback] - cd.prevPhase[i + lookback];
 756                 advance = ((advance * distance) +
 757                            (inherited * (maxdist - distance)))
 758                     / maxdist;
 759                 outphase = p + advance;
 760                 distacc += distance;
 761                 distance += 1.0;
 762             } else {
 763                 outphase = cd.unwrappedPhase[i] + advance;
 764                 distance = 0.0;
 765             }
 766
 767             prevInstability = instability;
 768             prevDirection = direction;
 769
 770         } else {
 771             distance = 0.0;
 772         }
 773
 774         cd.prevError[i] = perr;
 775         cd.prevPhase[i] = p;
 776         cd.phase[i] = outphase;
 777         cd.unwrappedPhase[i] = outphase;
 778     }
 779
 780     if (m_debugLevel > 1) {
 781         cerr << "mean inheritance distance = " << distacc / count << endl;
 782     }
 783
 784     if (fullReset) unchanged = true;
 785     cd.unchanged = unchanged;
 786
 787     if (unchanged && m_debugLevel > 1) {
 788         cerr << "frame unchanged on channel " << channel << endl;
 789     }
 790 }
 791
 792
 793 void
 794 RubberBandStretcher::Impl::formantShiftChunk(size_t channel)
 795 {
 796     Profiler profiler("RubberBandStretcher::Impl::formantShiftChunk");
 797
 798     ChannelData &cd = *m_channelData[channel];
 799
 800     double *const R__ mag = cd.mag;
 801     double *const R__ envelope = cd.envelope;
 802     double *const R__ dblbuf = cd.dblbuf;
 803
 804     const int sz = m_windowSize;
 805     const int hs = m_windowSize/2;
 806     const double denom = sz;
 807
 808
 809     cd.fft->inverseCepstral(mag, dblbuf);
 810
 811     for (int i = 0; i < sz; ++i) {
 812         dblbuf[i] /= denom;
 813     }
 814
 815     const int cutoff = m_sampleRate / 700;
 816
 817 //    cerr <<"cutoff = "<< cutoff << ", m_sampleRate/cutoff = " << m_sampleRate/cutoff << endl;
 818
 819     dblbuf[0] /= 2;
 820     dblbuf[cutoff-1] /= 2;
 821
 822     for (int i = cutoff; i < sz; ++i) {
 823         dblbuf[i] = 0.0;
 824     }
 825
 826     cd.fft->forward(dblbuf, envelope, 0);
 827
 828
 829     for (int i = 0; i <= hs; ++i) {
 830         envelope[i] = exp(envelope[i]);
 831     }
 832     for (int i = 0; i <= hs; ++i) {
 833         mag[i] /= envelope[i];
 834     }
 835
 836     if (m_pitchScale > 1.0) {
 837         // scaling up, we want a new envelope that is lower by the pitch factor
 838         for (int target = 0; target <= hs; ++target) {
 839             int source = lrint(target * m_pitchScale);
 840             if (source > int(m_windowSize)) {
 841                 envelope[target] = 0.0;
 842             } else {
 843                 envelope[target] = envelope[source];
 844             }
 845         }
 846     } else {
 847         // scaling down, we want a new envelope that is higher by the pitch factor
 848         for (int target = hs; target > 0; ) {
 849             --target;
 850             int source = lrint(target * m_pitchScale);
 851             envelope[target] = envelope[source];
 852         }
 853     }
 854
 855     for (int i = 0; i <= hs; ++i) {
 856         mag[i] *= envelope[i];
 857     }
 858
 859     cd.unchanged = false;
 860 }
 861
 862 void
 863 RubberBandStretcher::Impl::synthesiseChunk(size_t channel)
 864 {
 865     Profiler profiler("RubberBandStretcher::Impl::synthesiseChunk");
 866
 867
 868     if ((m_options & OptionFormantPreserved) &&
 869         (m_pitchScale != 1.0)) {
 870         formantShiftChunk(channel);
 871     }
 872
 873     ChannelData &cd = *m_channelData[channel];
 874
 875     double *const R__ dblbuf = cd.dblbuf;
 876     float *const R__ fltbuf = cd.fltbuf;
 877     float *const R__ accumulator = cd.accumulator;
 878     float *const R__ windowAccumulator = cd.windowAccumulator;
 879
 880     int sz = m_windowSize;
 881     int hs = m_windowSize/2;
 882     int i;
 883
 884
 885     if (!cd.unchanged) {
 886
 887         cd.fft->inversePolar(cd.mag, cd.phase, cd.dblbuf);
 888
 889         if (cd.oversample > 1) {
 890
 891             int bufsiz = sz * cd.oversample;
 892             int hbs = hs * cd.oversample;
 893             int offset = (bufsiz - sz) / 2;
 894
 895             for (i = 0; i < hbs; ++i) {
 896                 double tmp = dblbuf[i];
 897                 dblbuf[i] = dblbuf[i + hbs];
 898                 dblbuf[i + hbs] = tmp;
 899             }
 900             for (i = 0; i < sz; ++i) {
 901                 fltbuf[i] = float(dblbuf[i + offset]);
 902             }
 903         } else {
 904             for (i = 0; i < hs; ++i) {
 905                 fltbuf[i] = float(dblbuf[i + hs]);
 906             }
 907             for (i = 0; i < hs; ++i) {
 908                 fltbuf[i + hs] = float(dblbuf[i]);
 909             }
 910         }
 911
 912         float denom = float(sz * cd.oversample);
 913
 914         // our ffts produced unscaled results
 915         for (i = 0; i < sz; ++i) {
 916             fltbuf[i] = fltbuf[i] / denom;
 917         }
 918     }
 919
 920     m_window->cut(fltbuf);
 921
 922     for (i = 0; i < sz; ++i) {
 923         accumulator[i] += fltbuf[i];
 924     }
 925
 926     cd.accumulatorFill = m_windowSize;
 927
 928     float fixed = m_window->getArea() * 1.5f;
 929
 930     for (i = 0; i < sz; ++i) {
 931         float val = m_window->getValue(i);
 932         windowAccumulator[i] += val * fixed;
 933     }
 934 }
 935
 936 void
 937 RubberBandStretcher::Impl::writeChunk(size_t channel, size_t shiftIncrement, bool last)
 938 {
 939     Profiler profiler("RubberBandStretcher::Impl::writeChunk");
 940
 941     ChannelData &cd = *m_channelData[channel];
 942
 943     float *const R__ accumulator = cd.accumulator;
 944     float *const R__ windowAccumulator = cd.windowAccumulator;
 945
 946     const int sz = m_windowSize;
 947     const int si = shiftIncrement;
 948
 949     int i;
 950
 951     if (m_debugLevel > 2) {
 952         cerr << "writeChunk(" << channel << ", " << shiftIncrement << ", " << last << ")" << endl;
 953     }
 954
 955     for (i = 0; i < si; ++i) {
 956         if (windowAccumulator[i] > 0.f) {
 957             accumulator[i] /= windowAccumulator[i];
 958         }
 959     }
 960
 961     // for exact sample scaling (probably not meaningful if we
 962     // were running in RT mode)
 963     size_t theoreticalOut = 0;
 964     if (cd.inputSize >= 0) {
 965         theoreticalOut = lrint(cd.inputSize * m_timeRatio);
 966     }
 967
 968     bool resampledAlready = resampleBeforeStretching();
 969
 970     if (!resampledAlready &&
 971         (m_pitchScale != 1.0 || m_options & OptionPitchHighConsistency) &&
 972         cd.resampler) {
 973
 974         size_t reqSize = int(ceil(si / m_pitchScale));
 975         if (reqSize > cd.resamplebufSize) {
 976             // This shouldn't normally happen -- the buffer is
 977             // supposed to be initialised with enough space in the
 978             // first place.  But we retain this check in case the
 979             // pitch scale has changed since then, or the stretch
 980             // calculator has gone mad, or something.
 981             cerr << "WARNING: RubberBandStretcher::Impl::writeChunk: resizing resampler buffer from "
 982                       << cd.resamplebufSize << " to " << reqSize << endl;
 983             cd.setResampleBufSize(reqSize);
 984         }
 985
 986
 987         size_t outframes = cd.resampler->resample(&cd.accumulator,
 988                                                   &cd.resamplebuf,
 989                                                   si,
 990                                                   1.0 / m_pitchScale,
 991                                                   last);
 992
 993
 994         writeOutput(*cd.outbuf, cd.resamplebuf,
 995                     outframes, cd.outCount, theoreticalOut);
 996
 997     } else {
 998         writeOutput(*cd.outbuf, accumulator,
 999                     si, cd.outCount, theoreticalOut);
1000     }
1001
1002     for (i = 0; i < sz - si; ++i) {
1003         accumulator[i] = accumulator[i + si];
1004     }
1005
1006     for (i = sz - si; i < sz; ++i) {
1007         accumulator[i] = 0.0f;
1008     }
1009
1010     for (i = 0; i < sz - si; ++i) {
1011         windowAccumulator[i] = windowAccumulator[i + si];
1012     }
1013
1014     for (i = sz - si; i < sz; ++i) {
1015         windowAccumulator[i] = 0.0f;
1016     }
1017
1018     if (int(cd.accumulatorFill) > si) {
1019         cd.accumulatorFill -= si;
1020     } else {
1021         cd.accumulatorFill = 0;
1022         if (cd.draining) {
1023             if (m_debugLevel > 1) {
1024                 cerr << "RubberBandStretcher::Impl::processChunks: setting outputComplete to true" << endl;
1025             }
1026             cd.outputComplete = true;
1027         }
1028     }
1029 }
1030
1031 void
1032 RubberBandStretcher::Impl::writeOutput(RingBuffer<float> &to, float *from, size_t qty, size_t &outCount, size_t theoreticalOut)
1033 {
1034     Profiler profiler("RubberBandStretcher::Impl::writeOutput");
1035
1036     // In non-RT mode, we don't want to write the first startSkip
1037     // samples, because the first chunk is centred on the start of the
1038     // output.  In RT mode we didn't apply any pre-padding in
1039     // configure(), so we don't want to remove any here.
1040
1041     size_t startSkip = 0;
1042     if (!m_realtime) {
1043         startSkip = lrintf((m_windowSize/2) / m_pitchScale);
1044     }
1045
1046     if (outCount > startSkip) {
1047
1048         // this is the normal case
1049
1050         if (theoreticalOut > 0) {
1051             if (m_debugLevel > 1) {
1052                 cerr << "theoreticalOut = " << theoreticalOut
1053                      << ", outCount = " << outCount
1054                      << ", startSkip = " << startSkip
1055                      << ", qty = " << qty << endl;
1056             }
1057             if (outCount - startSkip <= theoreticalOut &&
1058                 outCount - startSkip + qty > theoreticalOut) {
1059                 qty = theoreticalOut - (outCount - startSkip);
1060                 if (m_debugLevel > 1) {
1061                     cerr << "reduce qty to " << qty << endl;
1062                 }
1063             }
1064         }
1065
1066         if (m_debugLevel > 2) {
1067             cerr << "writing " << qty << endl;
1068         }
1069
1070         size_t written = to.write(from, qty);
1071
1072         if (written < qty) {
1073             cerr << "WARNING: RubberBandStretcher::Impl::writeOutput: "
1074                  << "Buffer overrun on output: wrote " << written
1075                  << " of " << qty << " samples" << endl;
1076         }
1077
1078         outCount += written;
1079         return;
1080     }
1081
1082     // the rest of this is only used during the first startSkip samples
1083
1084     if (outCount + qty <= startSkip) {
1085         if (m_debugLevel > 1) {
1086             cerr << "qty = " << qty << ", startSkip = "
1087                  << startSkip << ", outCount = " << outCount
1088                  << ", discarding" << endl;
1089         }
1090         outCount += qty;
1091         return;
1092     }
1093
1094     size_t off = startSkip - outCount;
1095     if (m_debugLevel > 1) {
1096         cerr << "qty = " << qty << ", startSkip = "
1097              << startSkip << ", outCount = " << outCount
1098              << ", writing " << qty - off
1099              << " from start offset " << off << endl;
1100     }
1101     to.write(from + off, qty - off);
1102     outCount += qty;
1103 }
1104
1105 int
1106 RubberBandStretcher::Impl::available() const
1107 {
1108     Profiler profiler("RubberBandStretcher::Impl::available");
1109
1110     if (m_threaded) {
1111         MutexLocker locker(&m_threadSetMutex);
1112         if (m_channelData.empty()) return 0;
1113     } else {
1114         if (m_channelData.empty()) return 0;
1115     }
1116
1117     if (!m_threaded) {
1118         for (size_t c = 0; c < m_channels; ++c) {
1119             if (m_channelData[c]->inputSize >= 0) {
1120 //                cerr << "available: m_done true" << endl;
1121                 if (m_channelData[c]->inbuf->getReadSpace() > 0) {
1122 //                    cerr << "calling processChunks(" << c << ") from available" << endl;
1123                     //!!! do we ever actually do this? if so, this method should not be const
1124                     // ^^^ yes, we do sometimes -- e.g. when fed a very short file
1125                     bool any = false, last = false;
1126                     ((RubberBandStretcher::Impl *)this)->processChunks(c, any, last);
1127                 }
1128             }
1129         }
1130     }
1131
1132     size_t min = 0;
1133     bool consumed = true;
1134     bool haveResamplers = false;
1135
1136     for (size_t i = 0; i < m_channels; ++i) {
1137         size_t availIn = m_channelData[i]->inbuf->getReadSpace();
1138         size_t availOut = m_channelData[i]->outbuf->getReadSpace();
1139         if (m_debugLevel > 2) {
1140             cerr << "available on channel " << i << ": " << availOut << " (waiting: " << availIn << ")" << endl;
1141         }
1142         if (i == 0 || availOut < min) min = availOut;
1143         if (!m_channelData[i]->outputComplete) consumed = false;
1144         if (m_channelData[i]->resampler) haveResamplers = true;
1145     }
1146
1147     if (min == 0 && consumed) return -1;
1148     if (m_pitchScale == 1.0) return min;
1149
1150     if (haveResamplers) return min; // resampling has already happened
1151     return int(floor(min / m_pitchScale));
1152 }
1153
1154 size_t
1155 RubberBandStretcher::Impl::retrieve(float *const *output, size_t samples) const
1156 {
1157     Profiler profiler("RubberBandStretcher::Impl::retrieve");
1158
1159     size_t got = samples;
1160
1161     for (size_t c = 0; c < m_channels; ++c) {
1162         size_t gotHere = m_channelData[c]->outbuf->read(output[c], got);
1163         if (gotHere < got) {
1164             if (c > 0) {
1165                 if (m_debugLevel > 0) {
1166                     cerr << "RubberBandStretcher::Impl::retrieve: WARNING: channel imbalance detected" << endl;
1167                 }
1168             }
1169             got = gotHere;
1170         }
1171     }
1172
1173     return got;
1174 }
1175
1176 }
1177